DASL Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
====== Getting the Bottom Camera View from the Ar.Drone ======

===== Introduction =====

In the [[ardrone_brown_ros|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.

===== 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 ===
<code c++ ardrone_driver.cpp>
#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;
}
</code>
**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.

<code c++ ardronedriver.h>
#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
</code>
**save and close the file.**

==== 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.

<code c++ teleop_twist.cpp>
#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
};
</code>
**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.

<code c++ teleop_twist.h>
#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
</code>
**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.


  roscd ardrone_brown

  sh build_sdk.sh

  cmake .

  rosmake

 
**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
    
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.

<code python drone_teleop.py>
#!/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)

</code>
**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".**