In the last tutorial we learnt how to fly the ardrone using ROS. In this tutorial we will learn to access the bottom camera of the ARDRONE and also to toggle the between the two cameras.
1.Open up a terminal and enter :
roscd ardrone_brown
2.Now go to folder named “src” and open up the file in 'ardrone_driver.cpp
' in a editor and replace the whole code with the code below.
#include "ardrone_driver.h" #include "teleop_twist.h" #include "video.h" //////////////////////////////////////////////////////////////////////////////// // class ARDroneDriver //////////////////////////////////////////////////////////////////////////////// ARDroneDriver::ARDroneDriver() : image_transport(node_handle) { toggleCam_sub = node_handle.subscribe("/ardrone/togglecam", 10, &toggleCamCallback); cmd_vel_sub = node_handle.subscribe("/cmd_vel", 1, &cmdVelCallback); takeoff_sub = node_handle.subscribe("/ardrone/takeoff", 1, &takeoffCallback); reset_sub = node_handle.subscribe("/ardrone/reset", 1, &resetCallback); land_sub = node_handle.subscribe("/ardrone/land", 1, &landCallback); image_pub = image_transport.advertiseCamera("/ardrone/image_raw", 1); navdata_pub = node_handle.advertise<ardrone_brown::Navdata>("/ardrone/navdata", 1); //toggleCam_service = node_handle.advertiseService("/ardrone/togglecam", toggleCamCallback); } ARDroneDriver::~ARDroneDriver() { } void ARDroneDriver::run() { ros::Rate loop_rate(40); while (node_handle.ok()) { if (current_frame_id != last_frame_id) { publish_video(); publish_navdata(); last_frame_id = current_frame_id; } ardrone_tool_update(); ros::spinOnce(); loop_rate.sleep(); } } void ARDroneDriver::publish_video() { sensor_msgs::Image image_msg; sensor_msgs::CameraInfo cinfo_msg; image_msg.width = STREAM_WIDTH; image_msg.height = STREAM_HEIGHT; image_msg.encoding = "rgb8"; image_msg.is_bigendian = false; image_msg.step = STREAM_WIDTH*3; image_msg.data.resize(STREAM_WIDTH*STREAM_HEIGHT*3); std::copy(buffer, buffer+(STREAM_WIDTH*STREAM_HEIGHT*3), image_msg.data.begin()); // We only put the width and height in here. cinfo_msg.width = STREAM_WIDTH; cinfo_msg.height = STREAM_HEIGHT; image_pub.publish(image_msg, cinfo_msg); } void ARDroneDriver::publish_navdata() { ardrone_brown::Navdata msg; msg.batteryPercent = navdata.vbat_flying_percentage; // positive means counterclockwise rotation around axis msg.rotX = navdata.phi / 1000.0; // tilt left/right msg.rotY = -navdata.theta / 1000.0; // tilt forward/backward msg.rotZ = -navdata.psi / 1000.0; // orientation msg.altd = navdata.altitude; // cm msg.vx = navdata.vx; // mm/sec msg.vy = -navdata.vy; // mm/sec msg.vz = -navdata.vz; // mm/sec msg.tm = arnavtime.time; // TODO: Ideally we would be able to figure out whether we are in an emergency state // using the navdata.ctrl_state bitfield with the ARDRONE_EMERGENCY_MASK flag, but // it seems to always be 0. The emergency state seems to be correlated with the // inverse of the ARDRONE_TIMER_ELAPSED flag, but that just makes so little sense // that I don't want to use it because it's probably wrong. So we'll just use a // manual reset for now. navdata_pub.publish(msg); } //////////////////////////////////////////////////////////////////////////////// // custom_main //////////////////////////////////////////////////////////////////////////////// extern "C" int custom_main(int argc, char** argv) { int res = ardrone_tool_setup_com( NULL ); if( FAILED(res) ) { printf("Wifi initialization failed. It means either:\n"); printf("\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n"); printf("\t* wifi device is not present (on your pc or on your card)\n"); printf("\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n"); printf("\t* ap is not up (reboot card or remove wifi usb dongle)\n"); printf("\t* wifi device has no antenna\n"); } else { ardrone_tool_init(argc, argv); ros::init(argc, argv, "ardrone_driver"); ARDroneDriver().run(); } return 0; }
Save and close the file.
In the same folder “src” look for the file “ardrone_driver.h”. Open up the file and replace the code with the code below.
#ifndef _ARDRONE_DRIVER_H_ #define _ARDRONE_DRIVER_H_ #include <ros/ros.h> #include <image_transport/image_transport.h> #include <sensor_msgs/Image.h> #include <ardrone_brown/Navdata.h> #include "ardrone_sdk.h" class ARDroneDriver { public: ARDroneDriver(); ~ARDroneDriver(); void run(); private: void publish_video(); void publish_navdata(); ros::NodeHandle node_handle; ros::Subscriber cmd_vel_sub; ros::Subscriber takeoff_sub; ros::Subscriber reset_sub; ros::Subscriber land_sub; ros::Subscriber toggleCam_sub; image_transport::ImageTransport image_transport; image_transport::CameraPublisher image_pub; ros::Publisher navdata_pub; ros::ServiceServer toggleCam_service; int last_frame_id; int flying_state; }; #endif
save and close the file.
In the same folder “src” look for the file “teleop_twist.cpp”.Open up the file and replace the code with the code below.
#include "teleop_twist.h" inline float max(float a, float b) { return a > b ? a : b; } inline float min(float a, float b) { return a < b ? a : b; } bool is_flying = false; bool needs_reset = false; geometry_msgs::Twist cmd_vel; int cam_state=0; // 0 for forward and 1 for vertical, change to enum later // ros service callback function for toggling Cam /*bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { if (cam_state == 0) // toggle to 1, the vertical camera { cam_state = 1; #ifdef _USING_SDK_1_7_ ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL); #else ardrone_at_set_toy_configuration("video:video_channel","1"); #endif fprintf(stderr, "\nToggling from frontal camera to vertical camera.\n"); } else if (cam_state == 1) // toggle to the forward camera { cam_state = 0; #ifdef _USING_SDK_1_7_ ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL); #else ardrone_at_set_toy_configuration("video:video_channel","0"); #endif fprintf(stderr, "\nToggling from vertical camera to frontal camera.\n"); } return true; }*/ // Older rostopic callback function for toggling Cam void toggleCamCallback(const std_msgs::Empty &msg) { if (cam_state == 0) // toggle to 1, the vertical camera { cam_state = 1; ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL); //ardrone_at_set_toy_configuration("video:video_channel","1"); fprintf(stderr, "\nToggling from frontal camera to vertical camera.\n"); } else if (cam_state == 1) // toggle to the forward camera { cam_state = 0; ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL); //ardrone_at_set_toy_configuration("video:video_channel","0"); fprintf(stderr, "\nToggling from vertical camera to frontal camera.\n"); } } void cmdVelCallback(const geometry_msgs::TwistConstPtr &msg) { const float maxHorizontalSpeed = 1; // use 0.1f for testing and 1 for the real thing cmd_vel.linear.x = max(min(-msg->linear.x, maxHorizontalSpeed), -maxHorizontalSpeed); cmd_vel.linear.y = max(min(-msg->linear.y, maxHorizontalSpeed), -maxHorizontalSpeed); cmd_vel.linear.z = max(min(msg->linear.z, 1), -1); cmd_vel.angular.x = 0; cmd_vel.angular.y = 0; cmd_vel.angular.z = max(min(-msg->angular.z, 1), -1); } void landCallback(const std_msgs::Empty &msg) { is_flying = false; } void resetCallback(const std_msgs::Empty &msg) { needs_reset = true; } void takeoffCallback(const std_msgs::Empty &msg) { is_flying = true; } C_RESULT open_teleop(void) { return C_OK; } C_RESULT update_teleop(void) { // This function *toggles* the emergency state, so we only want to toggle the emergency // state when we are in the emergency state (because we want to get out of it). ardrone_tool_set_ui_pad_select(needs_reset); needs_reset = false; // This function sets whether or not the robot should be flying. If it is flying and you // send 0, the robot will slow down the motors and slowly descend to the floor. ardrone_tool_set_ui_pad_start(is_flying); float left_right = cmd_vel.linear.y; float front_back = cmd_vel.linear.x; float up_down = cmd_vel.linear.z; float turn = cmd_vel.angular.z; ardrone_at_set_progress_cmd(1, left_right, front_back, up_down, turn); return C_OK; } C_RESULT close_teleop(void) { return C_OK; } input_device_t teleop = { "Teleop", open_teleop, update_teleop, close_teleop };
Save and close the file.
Likewise in the same “src” folder open up the file and replace the code with the code below.
#ifndef _TELEOP_TWIST_H_ #define _TELEOP_TWIST_H_ #include "ardrone_sdk.h" #include <geometry_msgs/Twist.h> #include <std_msgs/Empty.h> #include <std_srvs/Empty.h> #define _USING_SDK_1_7_ extern input_device_t teleop; void cmdVelCallback(const geometry_msgs::TwistConstPtr &msg); void landCallback(const std_msgs::Empty &msg); void resetCallback(const std_msgs::Empty &msg); void takeoffCallback(const std_msgs::Empty &msg); void toggleCamCallback(const std_msgs::Empty &msg); //bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); #endif
Save and close the file.
Open up a new terminal and enter the following commands one after the other.
roscd ardrone_brown
sh build_sdk.sh
cmake .
rosmake
Make sure you get 0 failures while making this package.
Open up a new terminal and enter
roscd drone_teleop
After you enter the drone_teleop package go to folder “bin” and open up the file drone_teleop.py and replace the code in the file with the code below.
#!/usr/bin/env python import roslib; roslib.load_manifest('drone_teleop') import rospy from geometry_msgs.msg import Twist from std_msgs.msg import Empty import sys, select, termios, tty msg = """ Reading from the keyboard and Publishing to Twist! --------------------------- up/down: move forward/backward left/right: move left/right w/s: increase/decrease altitude a/d: turn left/right t/l: takeoff/land r: reset (toggle emergency state) c: toggle_cam_view(newly added) anything else: stop please don't have caps lock on. CTRL+c to quit """ #control portal for the user move_bindings = { 68:('linear', 'y', 0.1), #left 67:('linear', 'y', -0.1), #right 65:('linear', 'x', 0.1), #forward 66:('linear', 'x', -0.1), #back 'w':('linear', 'z', 0.1), 's':('linear', 'z', -0.1), 'a':('angular', 'z', 1), 'd':('angular', 'z', -1), } def getKey(): tty.setraw(sys.stdin.fileno()) select.select([sys.stdin], [], [], 0) key = sys.stdin.read(1) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) return key if __name__=="__main__": settings = termios.tcgetattr(sys.stdin) print msg pub = rospy.Publisher('cmd_vel', Twist) land_pub = rospy.Publisher('/ardrone/land', Empty) reset_pub = rospy.Publisher('/ardrone/reset', Empty) toggle_pub=rospy.Publisher('/ardrone/togglecam', Empty) takeoff_pub =rospy.Publisher('/ardrone/takeoff', Empty) rospy.init_node('drone_teleop') try: while(True): key = getKey() # takeoff and landing if key == 'l': land_pub.publish(Empty()) if key == 'r': reset_pub.publish(Empty()) if key == 't': takeoff_pub.publish(Empty()) if key == 'c': toggle_pub.publish(Empty()) twist = Twist() if ord(key) in move_bindings.keys(): key = ord(key) if key in move_bindings.keys(): (lin_ang, xyz, speed) = move_bindings[key] setattr(getattr(twist, lin_ang), xyz, speed) else: if (key == '\x03'): break print twist #print pub.publish(twist) except Exception as e: print e print repr(e) finally: twist = Twist() twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 pub.publish(twist) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
Save and close the file.
Turn on the ARDRONE and make sure the led lights on the ARDRONE turns green.
Make sure you connect to the ARDRONE by synchronizing with the wi-fi of your Computer.
1.Open up a new terminal and enter the commands (each of them must be entered in a new terminal one after the other):
roscore rosrun ardrone_brown ardrone_driver rosrun image_view image_view image:=/ardrone/image_raw rosrun drone_teleop drone_teleop.py
Now you can toggle between the bottom and frontal camera by hitting the letter “C”.