This is an old revision of the document!
Getting the Bottom Camera View from the Ar.Drone
Introduction
In the last tutorial we learnt how to fly the ardrone using ROS (http://dasl.mem.drexel.edu/wiki/index.php/Controlling_the_ARDRONE_with_the_Brown-ROS_package). In this tutorial we will learn to access the bottom camera of the ARDRONE and also to toggle the between the two cameras.
Make the necessary changes
ARDRONE_DRIVER.CPP
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.
Code
<source lang=“C”> #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;
}
</source>
'Save and close the file.'
===ardrone_driver.h===
In the same folder “src” look for the file “ardrone_driver.h”.Open up the file and replace the code with the code below.
<source lang=“C”>
#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
</source>
'save and close the file.'
<br>
<br>
===teleop_twist.cpp===
In the same folder “src” look for the file “teleop_twist.cpp”.Open up the file and replace the code with the code below.
<source lang=“C”>
#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 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
};
</source>
'Save and close the file.'
teleop_twist.h
Likewise in the same “src” folder open up the file and replace the code with the code below.
<source lang=“C”> #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
</source>
'Save and close the file.'
==Compile the ardrone_brown package again==
Open up a new terminal and enter the following commands one after the other.
<source lang=“C”>
1. roscd ardrone_brown
2. sh build_sdk.sh
3. cmake .
4. rosmake
</source>
'Make sure you get 0 failures while making this package.'
==Adding the toggle cam option in the drone_teleop package==
Open up a new terminal and enter
roscd drone_teleop
<br>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.
<source lang=“python”>
#!/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)
</source>
'Save and close the file.'
==Test toggling between the frontal view and bottom view of the ARDRONE.==
===Set up the connection with the ardrone===
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”.'