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


ardrone_autonomy
Author(s): Mani Monajjemi, Mani Monajjemi
autogenerated on Fri Dec 9 2016 03:36:59