("/ardrone/reset", 1); /* Message queue length is just 1 */
35 |
36 | while (ros::ok()) {
37 | double time_start=(double)ros::Time::now().toSec();
38 |
39 | while ((double)ros::Time::now().toSec()< time_start+1.0 || drone_state == 0)
40 | {
41 | pub_empty.publish(emp_msg); //launches the droe
42 | ROS_INFO("Sending Reset Signal");
43 | ros::spinOnce();
44 | loop_rate.sleep();
45 | if((double)ros::Time::now().toSec()> time_start+3.0){
46 |
47 | ROS_ERROR("Time limit reached, unable to set state of ardrone");
48 | exit(0);
49 | }//end if
50 |
51 | }//while time or state
52 | system("rosservice call /ardrone/setledanimation 1 5 2");
53 | ROS_INFO("ARdrone reset");
54 | exit(0);
55 | }//ros::ok
56 | }//main
57 |
--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.4.6)
2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3 |
4 | # Set the build type. Options are:
5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
6 | # Debug : w/ debug symbols, w/o optimization
7 | # Release : w/o debug symbols, w/ optimization
8 | # RelWithDebInfo : w/ debug symbols, w/ optimization
9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
10 | #set(ROS_BUILD_TYPE RelWithDebInfo)
11 |
12 | rosbuild_init()
13 |
14 | #set the default path for built executables to the "bin" directory
15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
16 | #set the default path for built libraries to the "lib" directory
17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
18 |
19 | #uncomment if you have defined messages
20 | #rosbuild_genmsg()
21 | #uncomment if you have defined services
22 | #rosbuild_gensrv()
23 |
24 | #common commands for building c++ executables and libraries
25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
26 | #target_link_libraries(${PROJECT_NAME} another_library)
27 | #rosbuild_add_boost_directories()
28 | #rosbuild_link_boost(${PROJECT_NAME} thread)
29 | rosbuild_add_executable(led_dance src/control_leds.cpp)
30 | rosbuild_add_executable(ardrone_test src/test_fly.cpp)
31 | rosbuild_add_executable(ardrone_test_back_forth src/fly_back_forth.cpp)
32 | rosbuild_add_executable(reset src/reset.cpp)
33 | rosbuild_add_executable(land src/land.cpp)
34 | rosbuild_add_executable(takeoff src/takeoff.cpp)
35 | rosbuild_add_executable(test_find_K src/test_fly_find_K.cpp)
36 | rosbuild_add_executable(joy_test_find_K src/fly_from_joy_test_K.cpp)
37 | #rosbuild_add_executable(pid_point src/pid_to_point.cpp)
38 | rosbuild_add_executable(fly_joy src/fly_from_joy.cpp)
39 | #target_link_libraries(example ${PROJECT_NAME})
40 |
--------------------------------------------------------------------------------
/src/readme:
--------------------------------------------------------------------------------
1 | The ardrone examples package features a variety of nodes to help users get the AR.Drone quadrotor robot flying quickly. This code has been tested on the the AR.Drone 1 and 2. Example code is provided for: taking off, landing, resetting, flying for joystick, and open loop flying.
2 |
3 | These nodes rely on the AR.Drone Autonomy package (https://github.com/AutonomyLab/ardrone_autonomy) which act as the drivers for the robot. The drivers accept two types of commands, velocity inputs via twist messages (http://www.ros.org/doc/api/geometry_msgs/html/msg/Twist.html), and mode changes via empty type messages (http://ros.org/wiki/std_msgs). These commands can be given from either the command line (rostopic echo) or through a compiled node.
4 |
5 | Setup:
6 | git the AR.Drone autonomy package and put it in your ROS workspace. Because of the video codecs, it needs to be installed as shown in the readme (https://github.com/AutonomyLab/ardrone_autonomy).
7 | git this package and put it in your ROS workspace, no install needed.
8 | Launch the drivers and battery monitor using the launch file:
9 | roslaunch enviroment.launch
10 | Run the take off node!
11 | rosrun takeoff
12 |
13 | To use the joystick flying node, one must install the ros joy package if it is not already on the system (http://www.ros.org/wiki/joy). Joystick nodes generally use a joy type message, which the fly_from_joy node expects. Please dig into the source code (.cpp files) to see how it is conducted further.
14 |
15 | This package was developed because I found myself (at the time a beginner to ROS) a little overwhelmed when beginning to use the AR.Drone in a linux environment. After talking to other users, I decided to formally release a package of the sample nodes I created for my laboratory. Explicit use of these nodes isn't really expected, as they are more of a jumping off point for more functional code. Feel free to contact me through the github page with additions to the package or questions.
16 |
17 |
18 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | ROS AR.Drone Examples
2 | ARLab @ University of Utah
3 | http://arl.cs.utah.edu
4 | http://www.cs.utah.edu/~parcon/
5 | Parker Conroy
6 |
7 | -------------------------
8 | The ardrone examples package features a variety of nodes to help users get the AR.Drone quadrotor robot flying quickly. This code has been tested on the the AR.Drone 1 and 2. Example code is provided for: taking off, landing, resetting, flying for joystick, and open loop flying.
9 |
10 | These nodes rely on the AR.Drone Autonomy package (https://github.com/AutonomyLab/ardrone_autonomy) which act as the drivers for the robot. The drivers accept two types of commands, velocity inputs via twist messages (http://www.ros.org/doc/api/geometry_msgs/html/msg/Twist.html), and mode changes via empty type messages (http://ros.org/wiki/std_msgs). These commands can be given from either the command line (rostopic echo) or through a compiled node.
11 |
12 | Setup:
13 | git the AR.Drone autonomy package and put it in your ROS workspace. Because of the video codecs, it needs to be installed as shown in the readme (https://github.com/AutonomyLab/ardrone_autonomy).
14 | git this package and put it in your ROS workspace, no install needed.
15 | Launch the drivers and battery monitor using the launch file:
16 | roslaunch enviroment.launch
17 | Run the take off node!
18 | rosrun takeoff
19 |
20 | To use the joystick flying node, one must install the ros joy package if it is not already on the system (http://www.ros.org/wiki/joy). Joystick nodes generally use a joy type message, which the fly_from_joy node expects. Please dig into the source code (.cpp files) to see how it is conducted further.
21 |
22 | This package was developed because I found myself (at the time a beginner to ROS) a little overwhelmed when beginning to use the AR.Drone in a linux environment. After talking to other users, I decided to formally release a package of the sample nodes I created for my laboratory. Explicit use of these nodes isn't really expected, as they are more of a jumping off point for more functional code. Feel free to contact me through the github page with additions to the package or questions.
23 |
24 |
25 |
--------------------------------------------------------------------------------
/launch/enviroment.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
22 |
23 |
24 | [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]
25 | [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
26 | [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 100000.0]
27 |
28 |
29 |
30 |
--------------------------------------------------------------------------------
/src/fly_from_joy.cpp:
--------------------------------------------------------------------------------
1 | /*
2 | Parker Conroy
3 | Algorithmic Robotics Lab @ University of Utah
4 |
5 |
6 | This code actuates the ARdrone from a generic joystick message. It is open loop.
7 | It is intended as a simple example for those starting with the AR Drone platform.
8 | */
9 | #include
10 | #include
11 | #include
12 | #include
13 | #include
14 |
15 | double joy_x_,joy_y_,joy_z_;
16 | double joy_x,joy_y,joy_z;
17 | int new_msg=0;
18 | float forget =0.99;
19 | double joy_x_old,joy_y_old,joy_z_old;
20 | geometry_msgs::Twist twist_msg;
21 | std_msgs::Empty emp_msg;
22 | geometry_msgs::Vector3 v3_msg; //[x, y,z]
23 | sensor_msgs::Joy joy_msg_in;
24 |
25 |
26 | void joy_callback(const sensor_msgs::Joy& joy_msg_in)
27 | {
28 | //Take in joystick
29 | joy_x_=joy_msg_in.axes[0];
30 | joy_y_=joy_msg_in.axes[1];
31 | joy_z_=joy_msg_in.axes[2];
32 |
33 | //Take in time
34 | //msg_time=(double)ros::Time::now().toNSec();
35 | new_msg=1;
36 | }
37 |
38 | float map(float value, float in_min, float in_max, float out_min, float out_max) {
39 | return (float)((value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min);
40 | }
41 |
42 | int main(int argc, char** argv)
43 | {
44 | ros::init(argc, argv,"ARDrone_fly_from_joy");
45 | ros::NodeHandle node;
46 | ros::Rate loop_rate(50);
47 | ros::Publisher pub_twist;
48 | ros::Publisher pub_empty;
49 | ros::Publisher pub_v3;
50 | ros::Subscriber joy_sub;
51 |
52 | pub_twist = node.advertise("cmd_vel", 1); //send robot input on /cmd_vel topic
53 | pub_v3 = node.advertise("joy_vel", 1); //send velocity for graphing on /joy_vel topic
54 | joy_sub = node.subscribe("/joy", 1, joy_callback); //suscribe to the joystick message
55 |
56 |
57 | ROS_INFO("Waiting for joystick message");
58 | while (!new_msg){
59 | ros::spinOnce();
60 | loop_rate.sleep();
61 | }
62 |
63 | ROS_INFO("Starting Joy --> cmd_vel Node");
64 | while (ros::ok() && new_msg) { //start the node when a messge comes in
65 | //pub_empty.publish(emp_msg); //launches the drone
66 |
67 | joy_x=map(joy_x_,-1024,1024,-1,1); //map the joy input (generally 10 bit to what the AR.Drone drivers expect)
68 | joy_y=map(joy_y_,-1024,1024,-1,1);
69 | joy_z=map(joy_z_,-1024,1024,-1,1);
70 |
71 | if (fabs(joy_x)<0.01) {joy_x =0;}
72 | //else {joy_x=joy_x*forget+joy_x_old*(1-forget);} //This line can smoothing the input signal via the float forget.
73 |
74 | if (fabs(joy_y)<0.01) {joy_y =0;}
75 | //else {joy_y=joy_y*forget+joy_y_old*(1-forget);}
76 |
77 | if (fabs(joy_z)<0.01) {joy_z =0;}
78 | //else {joy_z=joy_z*forget+joy_z_old*(1-forget);}
79 |
80 | ROS_INFO("new message");
81 |
82 | twist_msg.linear.x=joy_x;
83 | twist_msg.linear.y=joy_y;
84 | twist_msg.linear.z=joy_z;
85 | twist_msg.angular.z=0.0; //YAW IS DISABLED
86 |
87 | v3_msg.x=joy_x;
88 | v3_msg.y=joy_y;
89 | v3_msg.z=joy_z;
90 |
91 | new_msg=0;
92 | pub_v3.publish(v3_msg); //message is posted for easy graphing
93 | pub_twist.publish(twist_msg); //send message to the robot
94 |
95 | ros::spinOnce();
96 | loop_rate.sleep();
97 |
98 | }//ros::ok
99 | ROS_ERROR("ROS::ok failed- Node Closing");
100 | }//main
101 |
--------------------------------------------------------------------------------
/src/test_fly.cpp:
--------------------------------------------------------------------------------
1 | /*
2 | Parker Conroy
3 | Algorithmic Robotics Lab @ University of Utah
4 |
5 | This code sends simple commands to fly the AR Drone.
6 | It is intended as a simple example for those starting with the AR Drone platform.
7 | */
8 | #include
9 | #include
10 | #include
11 |
12 | geometry_msgs::Twist twist_msg;
13 | geometry_msgs::Twist twist_msg_hover;
14 | geometry_msgs::Twist twist_msg_pshover;
15 | std_msgs::Empty emp_msg;
16 |
17 |
18 | int main(int argc, char** argv)
19 | {
20 |
21 | printf("Manual Test Node Starting");
22 | ros::init(argc, argv,"ARDrone_manual_test");
23 | ros::NodeHandle node;
24 | ros::Rate loop_rate(50);
25 |
26 | ros::Publisher pub_empty_land;
27 | ros::Publisher pub_twist;
28 | ros::Publisher pub_empty_takeoff;
29 | ros::Publisher pub_empty_reset;
30 | double time;
31 |
32 | //hover message
33 | twist_msg_hover.linear.x=0.0;
34 | twist_msg_hover.linear.y=0.0;
35 | twist_msg_hover.linear.z=0.0;
36 | twist_msg_hover.angular.x=0.0;
37 | twist_msg_hover.angular.y=0.0;
38 | twist_msg_hover.angular.z=0.0;
39 |
40 | //psudo-hover message
41 | twist_msg_pshover.linear.x=0.00001; // lowest value for psudo hover to work
42 | twist_msg_pshover.linear.y=0.0;
43 | twist_msg_pshover.linear.z=0.0;
44 | twist_msg_pshover.angular.x=0.0;
45 | twist_msg_pshover.angular.y=0.0;
46 | twist_msg_pshover.angular.z=0.0;
47 |
48 | //command message
49 | float fly_time=9.0;
50 | float land_time=5.0;
51 | float kill_time =2.0;
52 |
53 | twist_msg.linear.x=0.0;
54 | twist_msg.linear.y=-0.0001;
55 | twist_msg.linear.z=0.0;
56 | twist_msg.angular.x=0.0;
57 | twist_msg.angular.y=0.0;
58 | twist_msg.angular.z=0.0;
59 |
60 |
61 |
62 | pub_twist = node.advertise("/cmd_vel", 1); /* Message queue length is just 1 */
63 | pub_empty_takeoff = node.advertise("/ardrone/takeoff", 1); /* Message queue length is just 1 */
64 | pub_empty_land = node.advertise("/ardrone/land", 1); /* Message queue length is just 1 */
65 | pub_empty_reset = node.advertise("/ardrone/reset", 1); /* Message queue length is just 1 */
66 |
67 |
68 | time =(double)ros::Time::now().toSec();
69 | ROS_INFO("Starting ARdrone_test loop");
70 |
71 |
72 | while (ros::ok()) {
73 |
74 | if ( (double)ros::Time::now().toSec()< time+5.0){
75 |
76 | pub_empty_takeoff.publish(emp_msg); //launches the drone
77 | pub_twist.publish(twist_msg_hover); //drone is flat
78 | ROS_INFO("Taking off");
79 | }//takeoff before t+5
80 |
81 | else if ( (double)ros::Time::now().toSec()> time+fly_time+land_time+kill_time){
82 |
83 | ROS_INFO("Closing Node");
84 | pub_empty_reset.publish(emp_msg); //kills the drone
85 | break;
86 |
87 | }//kill node
88 |
89 | else if ( ((double)ros::Time::now().toSec()> time+fly_time+land_time)){
90 |
91 | pub_twist.publish(twist_msg_hover); //drone is flat
92 | pub_empty_land.publish(emp_msg); //lands the drone
93 | ROS_INFO("Landing");
94 | }//land after t+15
95 |
96 | else
97 | {
98 | //pub_twist.publish(twist_msg_pshover);
99 | pub_twist.publish(twist_msg_hover);
100 | ROS_INFO("Flying");
101 | }//fly according to desired twist
102 |
103 | ros::spinOnce();
104 | loop_rate.sleep();
105 |
106 | }//ros::ok
107 |
108 | }//main
109 |
--------------------------------------------------------------------------------
/src/fly_back_and_forth.cpp:
--------------------------------------------------------------------------------
1 | /*
2 | Parker Conroy
3 | Algorithmic Robotics Lab @ University of Utah
4 |
5 |
6 | This code actuates the AR Drone back and forth.
7 | It is intended as a simple example for those starting with the AR Drone platform.
8 | */
9 | #include
10 | #include
11 | #include
12 |
13 | geometry_msgs::Twist twist_msg;
14 | geometry_msgs::Twist twist_msg_neg;
15 | geometry_msgs::Twist twist_msg_hover;
16 | geometry_msgs::Twist twist_msg_up;
17 | std_msgs::Empty emp_msg;
18 |
19 |
20 | int main(int argc, char** argv)
21 | {
22 |
23 | ROS_INFO("ARdrone Test Back and Forth Starting");
24 | ros::init(argc, argv,"ARDrone_test");
25 | ros::NodeHandle node;
26 | ros::Rate loop_rate(50);
27 |
28 | ros::Publisher pub_empty_land;
29 | ros::Publisher pub_twist;
30 | ros::Publisher pub_empty_takeoff;
31 | ros::Publisher pub_empty_reset;
32 | double start_time;
33 |
34 | //hover message
35 | twist_msg_hover.linear.x=0.0;
36 | twist_msg_hover.linear.y=0.0;
37 | twist_msg_hover.linear.z=0.0;
38 | twist_msg_hover.angular.x=0.0;
39 | twist_msg_hover.angular.y=0.0;
40 | twist_msg_hover.angular.z=0.0;
41 | //up message
42 | twist_msg_up.linear.x=0.0;
43 | twist_msg_up.linear.y=0.0;
44 | twist_msg_up.linear.z=0.5;
45 | twist_msg_up.angular.x=0.0;
46 | twist_msg_up.angular.y=0.0;
47 | twist_msg_up.angular.z=0.0;
48 | //command message
49 | float takeoff_time=5.0;
50 | float fly_time=7.0;
51 | float land_time=3.0;
52 | float kill_time =2.0;
53 |
54 |
55 | twist_msg.linear.x=0.0;
56 | twist_msg.linear.y=0.25;
57 | twist_msg.linear.z=0.0;
58 | twist_msg.angular.x=0.0;
59 | twist_msg.angular.y=0.0;
60 | twist_msg.angular.z=0.0;
61 |
62 | twist_msg_neg.linear.x=-twist_msg.linear.x;
63 | twist_msg_neg.linear.y=-twist_msg.linear.y;
64 | twist_msg_neg.linear.z=-twist_msg.linear.z;
65 | twist_msg_neg.angular.x=-twist_msg.angular.x;
66 | twist_msg_neg.angular.y=-twist_msg.angular.y;
67 | twist_msg_neg.angular.z=-twist_msg.angular.z;
68 |
69 |
70 |
71 | pub_twist = node.advertise("/cmd_vel", 1); /* Message queue length is just 1 */
72 | pub_empty_takeoff = node.advertise("/ardrone/takeoff", 1); /* Message queue length is just 1 */
73 | pub_empty_land = node.advertise("/ardrone/land", 1); /* Message queue length is just 1 */
74 | pub_empty_reset = node.advertise("/ardrone/reset", 1); /* Message queue length is just 1 */
75 |
76 |
77 | start_time =(double)ros::Time::now().toSec();
78 | ROS_INFO("Starting ARdrone_test loop");
79 |
80 |
81 | while (ros::ok()) {
82 | while ((double)ros::Time::now().toSec()< start_time+takeoff_time){ //takeoff
83 |
84 | pub_empty_takeoff.publish(emp_msg); //launches the drone
85 | pub_twist.publish(twist_msg_hover); //drone is flat
86 | ROS_INFO("Taking off");
87 | ros::spinOnce();
88 | loop_rate.sleep();
89 | }//while takeoff
90 |
91 | while ((double)ros::Time::now().toSec()> start_time+takeoff_time+fly_time){
92 |
93 | pub_twist.publish(twist_msg_hover); //drone is flat
94 | pub_empty_land.publish(emp_msg); //lands the drone
95 | ROS_INFO("Landing");
96 |
97 |
98 | if ((double)ros::Time::now().toSec()> takeoff_time+start_time+fly_time+land_time+kill_time){
99 |
100 | ROS_INFO("Closing Node");
101 | //pub_empty_reset.publish(emp_msg); //kills the drone
102 | exit(0); }//kill node
103 | ros::spinOnce();
104 | loop_rate.sleep();
105 | }//while land
106 |
107 | while ( (double)ros::Time::now().toSec()> start_time+takeoff_time && (double)ros::Time::now().toSec()< start_time+takeoff_time+fly_time){
108 |
109 |
110 | if((double)ros::Time::now().toSec()< start_time+takeoff_time+fly_time/2){
111 | pub_twist.publish(twist_msg);
112 | ROS_INFO("Flying +ve");
113 |
114 | }//fly according to desired twist
115 |
116 | if((double)ros::Time::now().toSec()> start_time+takeoff_time+fly_time/2){
117 | pub_twist.publish(twist_msg_neg);
118 | ROS_INFO("Flying -ve");
119 |
120 | }//fly according to desired twist
121 |
122 | ros::spinOnce();
123 | loop_rate.sleep();
124 | }
125 |
126 | ros::spinOnce();
127 | loop_rate.sleep();
128 |
129 | }//ros::ok
130 |
131 | }//main
132 |
--------------------------------------------------------------------------------
/src/test_fly_find_K.cpp:
--------------------------------------------------------------------------------
1 | /*
2 | Parker Conroy
3 | ARLab
4 |
5 | */
6 | #include
7 | #include
8 | #include
9 | #include
10 |
11 | geometry_msgs::Twist twist_msg;
12 | geometry_msgs::Twist twist_msg_hover;
13 | geometry_msgs::Twist twist_msg_neg;
14 | geometry_msgs::Twist twist_msg_pshover;
15 | geometry_msgs::Twist twist_msg_up;
16 | std_msgs::Empty emp_msg;
17 | double vx_=0.0;
18 | double vy_=0.0;
19 | double vz_=0.0;
20 | float takeoff_time=8.0;
21 | float fly_time=3.0;
22 | float land_time=3.0;
23 | float kill_time =4.0;
24 |
25 | void nav_callback(const ardrone_autonomy::Navdata& msg_in)
26 | {
27 | //Take in state of ardrone
28 | vx_=msg_in.vx*0.001;
29 | vy_=msg_in.vy*0.001;
30 | vz_=msg_in.vz*0.001;
31 | //ROS_INFO("getting sensor reading");
32 | }
33 |
34 | geometry_msgs::Twist test_controller(double vx_des,double vy_des,double vz_des,double K)
35 | {
36 | geometry_msgs::Twist twist_msg_gen;
37 |
38 | twist_msg_gen.linear.x=K*(vx_des-vx_); //{-1 to 1}=K*( m/s - m/s)
39 | //ROS_INFO("vx des- vx sensor &f",vx_des-vx_);
40 | twist_msg_gen.linear.y=K*(vy_des-vy_);
41 | twist_msg_gen.linear.z=K*(vz_des-vz_);
42 | twist_msg_gen.angular.x=1.0;
43 | twist_msg_gen.angular.y=1.0;
44 | twist_msg_gen.angular.z=0.0;
45 | return twist_msg_gen;
46 | }
47 |
48 | int main(int argc, char** argv)
49 | {
50 |
51 | printf("Manual Test Node Starting");
52 | ros::init(argc, argv,"ARDrone_manual_test");
53 | ros::NodeHandle node;
54 | ros::Rate loop_rate(50);
55 |
56 | ros::Publisher pub_empty_land;
57 | ros::Publisher pub_twist;
58 | ros::Publisher pub_empty_takeoff;
59 | ros::Publisher pub_empty_reset;
60 | ros::Subscriber nav_sub;
61 | double start_time;
62 |
63 | //hover message
64 | twist_msg_hover.linear.x=0.0;
65 | twist_msg_hover.linear.y=0.0;
66 | twist_msg_hover.linear.z=0.0;
67 | twist_msg_hover.angular.x=0.0;
68 | twist_msg_hover.angular.y=0.0;
69 | twist_msg_hover.angular.z=0.0;
70 | //fly up
71 | twist_msg_up.linear.x=0.0;
72 | twist_msg_up.linear.y=0.0;
73 | twist_msg_up.linear.z=0.5;
74 | twist_msg_up.angular.x=0.0;
75 | twist_msg_up.angular.y=0.0;
76 | twist_msg_up.angular.z=0.0;
77 | //command message
78 | twist_msg.linear.x=0.0;
79 | twist_msg.linear.y=0.0;
80 | twist_msg.linear.z=0.0;
81 | twist_msg.angular.x=0.0;
82 | twist_msg.angular.y=0.0;
83 | twist_msg.angular.z=0.0;
84 |
85 |
86 | nav_sub = node.subscribe("/ardrone/navdata", 1, nav_callback);
87 | pub_twist = node.advertise("/cmd_vel", 1);
88 | pub_empty_takeoff = node.advertise("/ardrone/takeoff", 1);
89 | pub_empty_land = node.advertise("/ardrone/land", 1);
90 | pub_empty_reset = node.advertise("/ardrone/reset", 1);
91 |
92 | start_time =(double)ros::Time::now().toSec();
93 | ROS_INFO("Starting ARdrone_test loop");
94 |
95 | double desired_vx=0.75; // [m/s]
96 | //double desired_vx=0.0; // [m/s]
97 | double K = .75; // []
98 |
99 | while (ros::ok()) {
100 | while ((double)ros::Time::now().toSec()< start_time+takeoff_time){ //takeoff
101 |
102 | pub_empty_takeoff.publish(emp_msg); //launches the drone
103 | pub_twist.publish(twist_msg_hover); //drone is flat
104 | ROS_INFO("Taking off");
105 | ros::spinOnce();
106 | loop_rate.sleep();
107 | }//while takeoff
108 |
109 | while ((double)ros::Time::now().toSec()> start_time+takeoff_time+fly_time){
110 |
111 | pub_twist.publish(twist_msg_hover); //drone is flat
112 |
113 | ROS_INFO("Landing");
114 |
115 |
116 | if ((double)ros::Time::now().toSec()> takeoff_time+start_time+fly_time+land_time+kill_time){
117 | pub_empty_land.publish(emp_msg); //lands the drone
118 | ROS_INFO("Closing Node");
119 | exit(0); }//kill node
120 | ros::spinOnce();
121 | loop_rate.sleep();
122 | }//while land
123 |
124 | while ( (double)ros::Time::now().toSec()> start_time+takeoff_time && (double)ros::Time::now().toSec()< start_time+takeoff_time+fly_time){
125 |
126 | twist_msg=test_controller(desired_vx,0.0,0.0,K);
127 |
128 | if((double)ros::Time::now().toSec()< start_time+takeoff_time+fly_time){
129 | pub_twist.publish(twist_msg);
130 | ROS_INFO("Flying +ve");
131 |
132 | }//fly according to desired twist
133 |
134 | if((double)ros::Time::now().toSec()> start_time+takeoff_time+fly_time){
135 |
136 | desired_vx=-desired_vx;
137 | pub_twist.publish(twist_msg);
138 | ROS_INFO("Flying -ve");
139 |
140 | }//fly according to desired twist
141 |
142 | ros::spinOnce();
143 | loop_rate.sleep();
144 | }
145 |
146 | ros::spinOnce();
147 | loop_rate.sleep();
148 |
149 | }//ros::ok
150 |
151 | }//main
152 |
--------------------------------------------------------------------------------
/src/fly_from_joy_test_K.cpp:
--------------------------------------------------------------------------------
1 | /*
2 | Parker Conroy
3 | Algorithmic Robotics Lab @ University of Utah
4 |
5 |
6 | This code actuates the ARdrone from a generic joystick message. It is open loop.
7 | It is intended as a simple example for those starting with the AR Drone platform.
8 | */
9 | #include
10 | #include
11 | #include
12 | #include
13 | #include
14 | #include
15 |
16 | double max_speed = 0.5; //[m/s]
17 | double Kp= 0.75;
18 | double Kd= 0.75;
19 |
20 | double joy_x_,joy_y_,joy_z_;
21 | int joy_a_,joy_b_,joy_xbox_;
22 | double joy_x,joy_y,joy_z;
23 | int joy_a,joy_b,joy_xbox;
24 |
25 | double drone_vx_, drone_vy_ , drone_vz_;
26 | double drone_ax_, drone_ay_ , drone_az_;
27 | double drone_vx, drone_vy , drone_vz;
28 | double drone_ax, drone_ay , drone_az;
29 |
30 | double cmd_x,cmd_y,cmd_z;
31 | int new_msg=0;
32 | int drone_state =0;
33 | // state: {0 is failure, 2 is landed, 3 is flying, 4 is hovering, 6 taking off, 8 landing}
34 | float forget =0.99;
35 | //double joy_x_old,joy_y_old,joy_z_old;
36 | geometry_msgs::Twist twist_msg;
37 | std_msgs::Empty emp_msg;
38 | geometry_msgs::Vector3 v3_msg; //[x, y,z]
39 | sensor_msgs::Joy joy_msg_in;
40 |
41 |
42 | void joy_callback(const sensor_msgs::Joy& joy_msg_in)
43 | {
44 | //Take in xbox controller
45 | joy_x_=joy_msg_in.axes[1]; //left stick up-down
46 | joy_y_=joy_msg_in.axes[0]; //left stick left-right
47 | joy_a_=joy_msg_in.buttons[0]; //a button
48 | joy_b_=joy_msg_in.buttons[1]; //b button
49 | joy_xbox_=joy_msg_in.buttons[8]; //xbox button
50 |
51 | //Take in time
52 | //msg_time=(double)ros::Time::now().toNSec();
53 | new_msg=1;
54 | }
55 |
56 | void nav_callback(const ardrone_autonomy::Navdata& msg_in)
57 | {
58 | //Take in state of ardrone
59 | drone_vx_=msg_in.vx*0.001; //[mm/s] to [m/s]
60 | drone_vy_=msg_in.vy*0.001;
61 | drone_vz_=msg_in.vz*0.001;
62 |
63 | drone_ax_=msg_in.ax*9.8; //[g] to [m/s2]
64 | drone_ay_=msg_in.ay*9.8;
65 | drone_az_=msg_in.az*9.8;
66 |
67 | drone_state=msg_in.state;
68 | //ROS_INFO("getting sensor reading");
69 | }
70 |
71 | void test_controller(double vx_des,double vy_des,double vz_des,double Kp, double Kd)
72 | {
73 | geometry_msgs::Twist twist_msg_gen;
74 |
75 | cmd_x=Kp*(vx_des-drone_vx_); //-Kd *drone_vx_ ; //{-1 to 1}=K*( m/s - m/s)
76 | cmd_y=Kp*(vy_des-drone_vy_);
77 | cmd_z=Kp*(vz_des-drone_vz_);
78 | twist_msg_gen.angular.x=1.0;
79 | twist_msg_gen.angular.y=1.0;
80 | twist_msg_gen.angular.z=0.0;
81 | }
82 |
83 | /*
84 | geometry_msgs::Twist test_controller(double vx_des,double vy_des,double vz_des,double K)
85 | {
86 | geometry_msgs::Twist twist_msg_gen;
87 |
88 | twist_msg_gen.linear.x=K*(vx_des-drone_vx_); //{-1 to 1}=K*( m/s - m/s)
89 | twist_msg_gen.linear.y=K*(vy_des-drone_vy_);
90 | twist_msg_gen.linear.z=K*(vz_des-drone_vz_);
91 | twist_msg_gen.angular.x=1.0;
92 | twist_msg_gen.angular.y=1.0;
93 | twist_msg_gen.angular.z=0.0;
94 |
95 | return twist_msg_gen;
96 | }
97 | */
98 | double map(double value, double in_min, double in_max, double out_min, double out_max) {
99 | return (double)((value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min);
100 | }
101 |
102 | void merge_new_mgs(void){
103 | joy_x=joy_x_;
104 | joy_y=joy_y_;
105 | joy_z=joy_z_;
106 | joy_a=joy_a_;
107 | joy_b=joy_b_;
108 | joy_xbox=joy_xbox_;
109 | drone_vx=drone_vx_;
110 | drone_vy=drone_vy_;
111 | drone_vz=drone_vz_;
112 | drone_ax=drone_ax_;
113 | drone_ay=drone_ay_;
114 | drone_az=drone_az_;
115 | }
116 |
117 | int main(int argc, char** argv)
118 | {
119 | ros::init(argc, argv,"ARDrone_fly_from_joy");
120 | ros::NodeHandle node;
121 | ros::Rate loop_rate(50);
122 | ros::Publisher pub_twist;
123 | ros::Publisher pub_empty_reset;
124 | ros::Publisher pub_empty_land;
125 | ros::Publisher pub_empty_takeoff;
126 | ros::Publisher pub_v3;
127 | ros::Subscriber joy_sub;
128 | ros::Subscriber nav_sub;
129 |
130 | pub_twist = node.advertise("/cmd_vel", 1);
131 | pub_v3 = node.advertise("/joy_vel", 1);
132 | joy_sub = node.subscribe("/joy", 1, joy_callback);
133 | nav_sub = node.subscribe("/ardrone/navdata", 1, nav_callback);
134 | pub_empty_reset = node.advertise("/ardrone/reset", 1);
135 | pub_empty_takeoff = node.advertise("/ardrone/takeoff", 1);
136 | pub_empty_land = node.advertise("/ardrone/land", 1);
137 |
138 | ROS_INFO("Starting Test Node, /cmd_vel = f(joy,quad velocity)");
139 | while (ros::ok()) {
140 | merge_new_mgs();
141 |
142 | //commands to change state of drone
143 | if (joy_a){
144 | while (drone_state ==2){
145 | ROS_INFO("Launching drone");
146 | pub_empty_takeoff.publish(emp_msg); //launches the drone
147 | ros::spinOnce();
148 | loop_rate.sleep();
149 | }//drone take off
150 | }
151 | if (joy_b){
152 | while (drone_state ==3 || drone_state ==4){
153 | ROS_INFO("landing drone");
154 | pub_empty_land.publish(emp_msg); //launches the drone
155 | ros::spinOnce();
156 | loop_rate.sleep();
157 | }//drone land
158 | }
159 | if (joy_xbox){
160 | double time_start=(double)ros::Time::now().toSec();
161 | while (drone_state ==0 ){
162 | ROS_INFO("landing drone");
163 | pub_empty_reset.publish(emp_msg); //launches the drone
164 | ros::spinOnce();
165 | loop_rate.sleep();
166 | if((double)ros::Time::now().toSec()> time_start+3.0){
167 | ROS_ERROR("Time limit reached, unable reset ardrone");
168 | break; //exit loop
169 | }
170 | }//drone take off
171 | }
172 |
173 | if (fabs(joy_x)<0.01) {joy_x =0;}
174 | //else {joy_x=joy_x*forget+joy_x_old*(1-forget);} //smoothing via forget
175 |
176 | if (fabs(joy_y)<0.01) {joy_y =0;}
177 | //else {joy_y=joy_y*forget+joy_y_old*(1-forget);}
178 |
179 | if (fabs(joy_z)<0.01) {joy_z =0;}
180 | //else {joy_z=joy_z*forget+joy_z_old*(1-forget);}
181 |
182 | cmd_x= joy_x*max_speed;
183 | cmd_y= joy_y*max_speed;
184 | cmd_z= joy_z*max_speed;
185 |
186 | test_controller(cmd_x,cmd_x,cmd_x,Kp,Kd); //modifies cmd_x,cmd_y,cmd_z proportinal to quad_speed
187 |
188 | twist_msg.linear.x=cmd_x;
189 | twist_msg.linear.y=cmd_y;
190 | twist_msg.linear.z=0.0;//THRUST AND YAW ARE DISABLED
191 | twist_msg.angular.z=0.0;
192 |
193 | v3_msg.x=cmd_x;
194 | v3_msg.y=cmd_y;
195 | v3_msg.z=cmd_z;
196 |
197 | new_msg=0;
198 | pub_v3.publish(v3_msg);
199 | pub_twist.publish(twist_msg);
200 |
201 | ros::spinOnce();
202 | loop_rate.sleep();
203 |
204 | }//ros::ok
205 | ROS_ERROR("ROS::ok failed- Node Closing");
206 | }//main
207 |
--------------------------------------------------------------------------------