teleop_twist.cpp
Go to the documentation of this file.
00001 #include "teleop_twist.h"
00002 #include "ardrone_autonomy/LedAnim.h"
00003 #include "utils/ardrone_date.h"
00004 
00005 inline float max(float a, float b) { return a > b ? a : b; }
00006 inline float min(float a, float b) { return a < b ? a : b; }
00007 
00008 bool needs_takeoff = false;
00009 bool needs_land = false;
00010 bool needs_reset = false;
00011 geometry_msgs::Twist cmd_vel;
00012 float old_left_right = -10.0;
00013 float old_front_back = -10.0;
00014 float old_up_down = -10.0;
00015 float old_turn = -10.0;
00016 
00017 int cam_state = DEFAULT_CAM_STATE; // 0 for forward and 1 for vertical, change to enum later
00018 int set_navdata_demo_value = DEFAULT_NAVDATA_DEMO; 
00019 int32_t detect_enemy_color = ARDRONE_DETECTION_COLOR_ORANGE_YELLOW;
00020 int32_t detect_dtype = CAD_TYPE_MULTIPLE_DETECTION_MODE;
00021 int32_t detect_hori_type = TAG_TYPE_MASK(TAG_TYPE_SHELL_TAG_V2);
00022 int32_t detect_vert_type = TAG_TYPE_MASK(TAG_TYPE_BLACK_ROUNDEL);
00023 int32_t detect_indoor_hull = 0;
00024 int32_t detect_disable_placeholder = 0;
00025 int32_t detect_enable_placeholder = 1;
00026 
00027 const LED_ANIMATION_IDS ledAnimMap[14] = {
00028         BLINK_GREEN_RED, BLINK_GREEN, BLINK_RED, BLINK_ORANGE,
00029         SNAKE_GREEN_RED, FIRE, STANDARD, RED, GREEN, RED_SNAKE,BLANK,
00030         LEFT_GREEN_RIGHT_RED, LEFT_RED_RIGHT_GREEN, BLINK_STANDARD};
00031 
00032 //ros service callback to set the camera channel
00033 bool setCamChannelCallback(ardrone_autonomy::CamSelect::Request& request, ardrone_autonomy::CamSelect::Response& response)
00034 {
00035     const int _modes = (IS_ARDRONE1) ? 4 : 2;
00036     cam_state = request.channel % _modes;
00037     ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
00038     fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
00039     response.result = true;
00040     return true;
00041 }
00042 // ros service callback function for toggling Cam
00043 bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
00044 {
00045     const int _modes = (IS_ARDRONE1) ? 4 : 2;
00046     cam_state = (cam_state + 1) % _modes;
00047     ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
00048     fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
00049     return true;
00050 }
00051 
00052 // ros service callback to turn on and off camera recording
00053 bool setRecordCallback(ardrone_autonomy::RecordEnable::Request &request, ardrone_autonomy::RecordEnable::Response& response)
00054 {
00055   char record_command[ARDRONE_DATE_MAXSIZE + 64];
00056   int32_t new_codec;
00057 
00058   if( request.enable == true ) {
00059     char date[ARDRONE_DATE_MAXSIZE];
00060     time_t t = time(NULL);
00061     // For some reason the linker can't find this, so we'll just do it manually, cutting and pasting
00062     //    ardrone_time2date(t, ARDRONE_FILE_DATE_FORMAT, date);
00063     strftime(date, ARDRONE_DATE_MAXSIZE, ARDRONE_FILE_DATE_FORMAT, localtime(&t));
00064     snprintf(record_command, sizeof(record_command), "%d,%s", USERBOX_CMD_START, date);
00065     new_codec = MP4_360P_H264_720P_CODEC;
00066   } else {
00067     snprintf(record_command, sizeof(record_command), "%d", USERBOX_CMD_STOP );
00068     new_codec = H264_360P_CODEC;
00069   }
00070 
00071   vp_os_mutex_lock(&twist_lock);
00072   ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_codec, &new_codec, NULL );
00073   ARDRONE_TOOL_CONFIGURATION_ADDEVENT (userbox_cmd, record_command, NULL );
00074   vp_os_mutex_unlock(&twist_lock);
00075 
00076   response.result = true;
00077   return true;
00078 }    
00079 
00080 bool setLedAnimationCallback(ardrone_autonomy::LedAnim::Request& request, ardrone_autonomy::LedAnim::Response& response)
00081 {
00082     LED_ANIMATION_IDS anim_id = ledAnimMap[request.type % 14]; // Don't trick me
00083     vp_os_mutex_lock(&twist_lock);
00084     ardrone_at_set_led_animation(anim_id, (float) fabs(request.freq), (uint32_t) abs(request.duration));
00085     vp_os_mutex_unlock(&twist_lock);
00086     response.result = true;
00087     return true;
00088 }
00089 
00090 bool setFlightAnimationCallback(ardrone_autonomy::FlightAnim::Request &request, ardrone_autonomy::FlightAnim::Response &response)
00091 {
00092     char param[20];
00093     const int anim_type = request.type % ARDRONE_NB_ANIM_MAYDAY;
00094     const int anim_duration = (request.duration > 0) ? request.duration : MAYDAY_TIMEOUT[anim_type];
00095     snprintf(param, sizeof (param), "%d,%d", anim_type, anim_duration);
00096     vp_os_mutex_lock(&twist_lock);
00097     ARDRONE_TOOL_CONFIGURATION_ADDEVENT(flight_anim, param, NULL);
00098     vp_os_mutex_unlock(&twist_lock);
00099     response.result = true;
00100     return true;
00101 }
00102 
00103 bool flatTrimCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
00104 {
00105     vp_os_mutex_lock(&twist_lock);
00106     ardrone_at_set_flat_trim();
00107     vp_os_mutex_unlock(&twist_lock);
00108     fprintf(stderr, "\nFlat Trim Set.\n");
00109 }
00110 
00111 void cmdVelCallback(const geometry_msgs::TwistConstPtr &msg)
00112 {
00113     vp_os_mutex_lock(&twist_lock);
00114     // Main 4DOF
00115     cmd_vel.linear.x  = max(min(-msg->linear.x, 1.0), -1.0);
00116     cmd_vel.linear.y  = max(min(-msg->linear.y, 1.0), -1.0);
00117         cmd_vel.linear.z  = max(min(msg->linear.z, 1.0), -1.0);
00118         cmd_vel.angular.z = max(min(-msg->angular.z, 1.0), -1.0);
00119     // These 2DOF just change the auto hover behaviour
00120     // No bound() required
00121     cmd_vel.angular.x = msg->angular.x;
00122     cmd_vel.angular.y = msg->angular.y;
00123     vp_os_mutex_unlock(&twist_lock);
00124 }
00125 
00126 void landCallback(const std_msgs::Empty &msg)
00127 {
00128     vp_os_mutex_lock(&twist_lock);
00129     needs_land = true;
00130     vp_os_mutex_unlock(&twist_lock);
00131 }
00132 
00133 void resetCallback(const std_msgs::Empty &msg)
00134 {       
00135     vp_os_mutex_lock(&twist_lock);
00136     needs_reset = true;
00137     vp_os_mutex_unlock(&twist_lock);
00138 }
00139 
00140 void takeoffCallback(const std_msgs::Empty &msg)
00141 {
00142     vp_os_mutex_lock(&twist_lock);
00143     needs_takeoff = true;
00144     vp_os_mutex_unlock(&twist_lock);
00145 }
00146 
00147 C_RESULT open_teleop(void)
00148 {
00149         return C_OK;
00150 }
00151 
00152 C_RESULT update_teleop(void)
00153 {
00154         // This function *toggles* the emergency state, so we only want to toggle the emergency
00155         // state when we are in the emergency state (because we want to get out of it).
00156     vp_os_mutex_lock(&twist_lock);
00157     if (needs_reset)
00158     {
00159         ardrone_tool_set_ui_pad_select(1);
00160         needs_reset = false;
00161     }
00162     else if (needs_takeoff)
00163     {
00164         ardrone_tool_set_ui_pad_start(1);
00165         needs_takeoff = false;
00166     }
00167     else if (needs_land)
00168     {
00169         ardrone_tool_set_ui_pad_start(0);
00170         needs_land = false;
00171     }
00172     else
00173     {
00174 
00175         float left_right = (float) cmd_vel.linear.y;
00176         float front_back = (float) cmd_vel.linear.x;
00177         float up_down = (float) cmd_vel.linear.z;
00178         float turn = (float) cmd_vel.angular.z;
00179         
00180         bool is_changed = !(
00181                 (fabs(left_right - old_left_right) < _EPS) && 
00182                 (fabs(front_back - old_front_back) < _EPS) && 
00183                 (fabs(up_down - old_up_down) < _EPS) && 
00184                 (fabs(turn - old_turn) < _EPS)
00185                 );
00186         
00187         // These lines are for testing, they should be moved to configurations
00188         // Bit 0 of control_flag == 0: should we hover?
00189         // Bit 1 of control_flag == 1: should we use combined yaw mode?
00190         
00191         int32_t control_flag = 0x00;
00192         int32_t combined_yaw = 0x00;
00193         
00194         // Auto hover detection based on ~0 values for 4DOF cmd_vel
00195         int32_t hover = (int32_t)
00196                 (
00197                 (fabs(left_right) < _EPS) && 
00198                 (fabs(front_back) < _EPS) && 
00199                 (fabs(up_down) < _EPS) && 
00200                 (fabs(turn) < _EPS) &&
00201                 // Set angular.x or angular.y to a non-zero value to disable entering hover
00202                 // even when 4DOF control command is ~0
00203                 (fabs(cmd_vel.angular.x) < _EPS) &&
00204                 (fabs(cmd_vel.angular.y) < _EPS)
00205                 );
00206 
00207         control_flag |= ((1 - hover) << 0);
00208         control_flag |= (combined_yaw << 1);
00209         //ROS_INFO (">>> Control Flag: %d", control_flag);
00210         
00211         old_left_right = left_right;
00212         old_front_back = front_back;
00213         old_up_down = up_down;
00214         old_turn = turn;
00215         //is_changed = true;
00216         if ((is_changed) || (hover))
00217         {
00218             ardrone_tool_set_progressive_cmd(control_flag, left_right, front_back, up_down, turn, 0.0, 0.0);
00219         }
00220 
00221     }
00222     vp_os_mutex_unlock(&twist_lock);
00223         return C_OK;
00224 }
00225 
00226 C_RESULT close_teleop(void)
00227 {
00228         return C_OK;
00229 }
00230 
00231 input_device_t teleop = {
00232         "Teleop",
00233         open_teleop,
00234         update_teleop,
00235         close_teleop
00236 };
00237 


ardrone_autonomy
Author(s): Mani Monajjemi
autogenerated on Mon Jan 6 2014 11:03:00