bebop_driver_nodelet.cpp
Go to the documentation of this file.
00001 
00025 #include <ros/ros.h>
00026 #include <pluginlib/class_list_macros.h>
00027 #include <nodelet/nodelet.h>
00028 #include <nav_msgs/Odometry.h>
00029 #include <sensor_msgs/JointState.h>
00030 #include <tf2/LinearMath/Quaternion.h>
00031 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00032 #include <tf2_ros/transform_broadcaster.h>
00033 #include <sensor_msgs/NavSatFix.h>
00034 
00035 #include <boost/bind.hpp>
00036 #include <boost/make_shared.hpp>
00037 #include <boost/date_time/posix_time/posix_time.hpp>
00038 #include <cmath>
00039 #include <algorithm>
00040 #include <string>
00041 #include <cstdio>
00042 
00043 #include <bebop_driver/bebop_driver_nodelet.h>
00044 #include <bebop_driver/BebopArdrone3Config.h>
00045 
00046 // For AuxThread() - without the following, callback wrapper types are incomplete to the compiler
00047 #include "bebop_driver/autogenerated/ardrone3_state_callbacks.h"
00048 
00049 PLUGINLIB_EXPORT_CLASS(bebop_driver::BebopDriverNodelet, nodelet::Nodelet)
00050 
00051 namespace bebop_driver
00052 {
00053 
00054 namespace util
00055 {
00056 
00057 int BebopPrintToROSLogCB(eARSAL_PRINT_LEVEL level, const char *tag, const char *format, va_list va)
00058 {
00059   const int32_t sz = vsnprintf(bebop_err_str, BEBOP_ERR_STR_SZ, format, va);
00060   bebop_err_str[std::min(BEBOP_ERR_STR_SZ, sz) - 1] = '\0';
00061   // We can't use variable names with ROS_*_NAMED macros
00062   static const std::string logger_name = std::string(ROSCONSOLE_NAME_PREFIX) + "." +
00063       ros::this_node::getName() + ".bebopsdk";
00064   // Use tag inline
00065   ROS_LOG(util::arsal_level_to_ros[level], logger_name, "[%s] %s", tag, bebop_err_str);
00066   return 1;
00067 }
00068 
00069 }  // namespace util
00070 
00071 BebopDriverNodelet::BebopDriverNodelet()
00072  : bebop_ptr_(new bebop_driver::Bebop(util::BebopPrintToROSLogCB))
00073 {
00074   NODELET_INFO("Nodelet Cstr");
00075 }
00076 
00077 void BebopDriverNodelet::onInit()
00078 {
00079   ros::NodeHandle& nh = getNodeHandle();
00080   ros::NodeHandle& private_nh = getPrivateNodeHandle();
00081   util::ResetTwist(camera_twist_);
00082   {
00083     boost::unique_lock<boost::mutex> twist_lock(twist_mutex_);
00084     util::ResetTwist(prev_bebop_twist_);
00085     util::ResetTwist(prev_camera_twist_);
00086     prev_twist_stamp_ = ros::Time(0);
00087   }
00088 
00089   // Params (not dynamically reconfigurable, local)
00090   // TODO(mani-monaj): Wrap all calls to .param() in a function call to enable logging
00091   const bool param_reset_settings = private_nh.param("reset_settings", false);
00092   const bool param_sync_time = private_nh.param("sync_time", false);
00093   const std::string& param_camera_info_url = private_nh.param<std::string>("camera_info_url", "");
00094   const std::string& param_bebop_ip = private_nh.param<std::string>("bebop_ip", "192.168.42.1");
00095 
00096   param_camera_frame_id_ = private_nh.param<std::string>("camera_frame_id", "camera_optical");
00097   param_odom_frame_id_ = private_nh.param<std::string>("odom_frame_id", "odom");
00098   param_publish_odom_tf_ = private_nh.param<bool>("publish_odom_tf", true);
00099   param_cmd_vel_timeout_ = private_nh.param<double>("cmd_vel_timeout", 0.2);
00100 
00101   NODELET_INFO("Connecting to Bebop ...");
00102   try
00103   {
00104     bebop_ptr_->Connect(nh, private_nh, param_bebop_ip);
00105 
00106     if (param_reset_settings)
00107     {
00108       NODELET_WARN("Resetting all settings ...");
00109       bebop_ptr_->ResetAllSettings();
00110       // Wait for 3 seconds
00111       ros::Rate(ros::Duration(3.0)).sleep();
00112     }
00113 
00114     if (param_sync_time)
00115     {
00116       NODELET_WARN("Syncing system and bebop time ...");
00117       boost::posix_time::ptime system_time = ros::Time::now().toBoost();
00118       std::string iso_time_str = boost::posix_time::to_iso_extended_string(system_time);
00119       bebop_ptr_->SetDate(iso_time_str);
00120       NODELET_INFO_STREAM("Current time: " << iso_time_str);
00121       ros::Rate(ros::Duration(3.0)).sleep();
00122     }
00123 
00124     NODELET_INFO("Fetching all settings from the Drone ...");
00125     bebop_ptr_->RequestAllSettings();
00126     ros::Rate(ros::Duration(3.0)).sleep();
00127   }
00128   catch (const std::runtime_error& e)
00129   {
00130     NODELET_FATAL_STREAM("Init failed: " << e.what());
00131     // TODO(mani-monaj): Retry mechanism
00132     throw e;
00133   }
00134 
00135   cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &BebopDriverNodelet::CmdVelCallback, this);
00136   camera_move_sub_ = nh.subscribe("camera_control", 1, &BebopDriverNodelet::CameraMoveCallback, this);
00137   takeoff_sub_ = nh.subscribe("takeoff", 1, &BebopDriverNodelet::TakeoffCallback, this);
00138   land_sub_ = nh.subscribe("land", 1, &BebopDriverNodelet::LandCallback, this);
00139   reset_sub_ = nh.subscribe("reset", 1, &BebopDriverNodelet::EmergencyCallback, this);
00140   flattrim_sub_ = nh.subscribe("flattrim", 1, &BebopDriverNodelet::FlatTrimCallback, this);
00141   navigatehome_sub_ = nh.subscribe("autoflight/navigate_home", 1, &BebopDriverNodelet::NavigateHomeCallback, this);
00142   start_autoflight_sub_ = nh.subscribe("autoflight/start", 1, &BebopDriverNodelet::StartAutonomousFlightCallback, this);
00143   pause_autoflight_sub_ = nh.subscribe("autoflight/pause", 1, &BebopDriverNodelet::PauseAutonomousFlightCallback, this);
00144   stop_autoflight_sub_ = nh.subscribe("autoflight/stop", 1, &BebopDriverNodelet::StopAutonomousFlightCallback, this);
00145   animation_sub_ = nh.subscribe("flip", 1, &BebopDriverNodelet::FlipAnimationCallback, this);
00146   snapshot_sub_ = nh.subscribe("snapshot", 10, &BebopDriverNodelet::TakeSnapshotCallback, this);
00147   exposure_sub_ = nh.subscribe("set_exposure", 10, &BebopDriverNodelet::SetExposureCallback, this);
00148   toggle_recording_sub_ = nh.subscribe("record", 10, &BebopDriverNodelet::ToggleRecordingCallback, this);
00149 
00150   odom_pub_ = nh.advertise<nav_msgs::Odometry>("odom", 30);
00151   camera_joint_pub_ = nh.advertise<sensor_msgs::JointState>("joint_states", 10, true);
00152   gps_fix_pub_ = nh.advertise<sensor_msgs::NavSatFix>("fix", 10, true);
00153 
00154   cinfo_manager_ptr_.reset(new camera_info_manager::CameraInfoManager(nh, "bebop_front", param_camera_info_url));
00155   image_transport_ptr_.reset(new image_transport::ImageTransport(nh));
00156   image_transport_pub_ = image_transport_ptr_->advertiseCamera("image_raw", 60);
00157 
00158   camera_info_msg_ptr_.reset(new sensor_msgs::CameraInfo());
00159 
00160   dynr_serv_ptr_.reset(new dynamic_reconfigure::Server<bebop_driver::BebopArdrone3Config>(private_nh));
00161   dynamic_reconfigure::Server<bebop_driver::BebopArdrone3Config>::CallbackType cb =
00162       boost::bind(&bebop_driver::BebopDriverNodelet::ParamCallback, this, _1, _2);
00163 
00164   dynr_serv_ptr_->setCallback(cb);
00165 
00166   try
00167   {
00168     NODELET_INFO("Enabling video stream ...");
00169     bebop_ptr_->StartStreaming();
00170     if (bebop_ptr_->IsStreamingStarted())
00171     {
00172       camera_pub_thread_ptr_ = boost::make_shared<boost::thread>(
00173             boost::bind(&bebop_driver::BebopDriverNodelet::BebopDriverNodelet::CameraPublisherThread, this));
00174     }
00175   }
00176   catch (const::std::runtime_error& e)
00177   {
00178     NODELET_ERROR_STREAM("Start() failed: " << e.what());
00179     // TODO(mani-monaj): Retry mechanism
00180   }
00181 
00182   aux_thread_ptr_ = boost::make_shared<boost::thread>(
00183         boost::bind(&bebop_driver::BebopDriverNodelet::BebopDriverNodelet::AuxThread, this));
00184 
00185   NODELET_INFO_STREAM("Nodelet lwp_id: " << util::GetLWPId());
00186 }
00187 
00188 BebopDriverNodelet::~BebopDriverNodelet()
00189 {
00190   NODELET_INFO_STREAM("Bebop Nodelet Dstr: " << bebop_ptr_->IsConnected());
00191   NODELET_INFO_STREAM("Killing Camera Thread ...");
00192   if (camera_pub_thread_ptr_)
00193   {
00194     camera_pub_thread_ptr_->interrupt();
00195     camera_pub_thread_ptr_->join();
00196   }
00197   NODELET_INFO_STREAM("Killing Aux Thread ...");
00198   if (aux_thread_ptr_)
00199   {
00200     aux_thread_ptr_->interrupt();
00201     aux_thread_ptr_->join();
00202   }
00203   if (bebop_ptr_->IsStreamingStarted()) bebop_ptr_->StopStreaming();
00204   if (bebop_ptr_->IsConnected()) bebop_ptr_->Disconnect();
00205 }
00206 
00207 void BebopDriverNodelet::CmdVelCallback(const geometry_msgs::TwistConstPtr& twist_ptr)
00208 {
00209   try
00210   {
00211     const geometry_msgs::Twist& bebop_twist_ = *twist_ptr;
00212     bool is_bebop_twist_changed = false;
00213     {
00214       boost::unique_lock<boost::mutex> twist_lock(twist_mutex_);
00215       is_bebop_twist_changed = !util::CompareTwists(bebop_twist_, prev_bebop_twist_);
00216       prev_twist_stamp_ = ros::Time::now();
00217       prev_bebop_twist_ = bebop_twist_;
00218     }
00219 
00220     // TODO: Always apply zero after non-zero values
00221     if (is_bebop_twist_changed)
00222     {
00223       bebop_ptr_->Move(CLAMP(-bebop_twist_.linear.y, -1.0, 1.0),
00224                        CLAMP(bebop_twist_.linear.x, -1.0, 1.0),
00225                        CLAMP(bebop_twist_.linear.z, -1.0, 1.0),
00226                        CLAMP(-bebop_twist_.angular.z, -1.0, 1.0));
00227     }
00228   }
00229   catch (const std::runtime_error& e)
00230   {
00231     ROS_ERROR_STREAM(e.what());
00232   }
00233 }
00234 
00235 void BebopDriverNodelet::TakeoffCallback(const std_msgs::EmptyConstPtr& empty_ptr)
00236 {
00237   try
00238   {
00239     bebop_ptr_->Takeoff();
00240   }
00241   catch (const std::runtime_error& e)
00242   {
00243     ROS_ERROR_STREAM(e.what());
00244   }
00245 }
00246 
00247 void BebopDriverNodelet::LandCallback(const std_msgs::EmptyConstPtr& empty_ptr)
00248 {
00249   try
00250   {
00251     bebop_ptr_->Land();
00252   }
00253   catch (const std::runtime_error& e)
00254   {
00255     ROS_ERROR_STREAM(e.what());
00256   }
00257 }
00258 
00259 // We shoudld probably switch to sensor_msgs/JointState instead of Twist
00260 void BebopDriverNodelet::CameraMoveCallback(const geometry_msgs::TwistConstPtr& twist_ptr)
00261 {
00262   try
00263   {
00264     camera_twist_ = *twist_ptr;
00265     const bool is_camera_twist_changed = !util::CompareTwists(camera_twist_, prev_camera_twist_);
00266     if (is_camera_twist_changed)
00267     {
00268       bebop_ptr_->MoveCamera(CLAMP(camera_twist_.angular.y, -83.0, 17.0),
00269                              CLAMP(camera_twist_.angular.z, -35.0, 35.0));
00270       prev_camera_twist_ = camera_twist_;
00271     }
00272   }
00273   catch (const std::runtime_error& e)
00274   {
00275     ROS_ERROR_STREAM(e.what());
00276   }
00277 }
00278 
00279 void BebopDriverNodelet::EmergencyCallback(const std_msgs::EmptyConstPtr& empty_ptr)
00280 {
00281   try
00282   {
00283     bebop_ptr_->Emergency();
00284   }
00285   catch (const std::runtime_error& e)
00286   {
00287     ROS_ERROR_STREAM(e.what());
00288   }
00289 }
00290 
00291 void BebopDriverNodelet::FlatTrimCallback(const std_msgs::EmptyConstPtr &empty_ptr)
00292 {
00293   try
00294   {
00295     // TODO(mani-monaj): Check if landed
00296     ROS_INFO("Flat Trim");
00297     bebop_ptr_->FlatTrim();
00298   }
00299   catch (const std::runtime_error& e)
00300   {
00301     ROS_ERROR_STREAM(e.what());
00302   }
00303 }
00304 
00305 void BebopDriverNodelet::NavigateHomeCallback(const std_msgs::BoolConstPtr &start_stop_ptr)
00306 {
00307   try
00308   {
00309     ROS_INFO("%sing navigate home behavior ...", start_stop_ptr->data ? "Start" : "Stopp");
00310     bebop_ptr_->NavigateHome(start_stop_ptr->data);
00311   }
00312   catch (const std::runtime_error& e)
00313   {
00314     ROS_ERROR_STREAM(e.what());
00315   }
00316 }
00317 
00318 void BebopDriverNodelet::StartAutonomousFlightCallback(const std_msgs::StringConstPtr& filepath_ptr)
00319 {
00320   std::string filepath;
00321   if (filepath_ptr->data.empty())
00322   {
00323     ROS_WARN("No flight plan provided. Using default: 'flightplan.mavlink'");
00324     filepath = "flightplan.mavlink";
00325   }
00326   else
00327   {
00328     filepath = filepath_ptr->data;
00329   }
00330 
00331   try
00332   {
00333     ROS_INFO("Starting autonomous flight path: %s", filepath.c_str());
00334     bebop_ptr_->StartAutonomousFlight(filepath);
00335   }
00336   catch (const std::runtime_error& e)
00337   {
00338     ROS_ERROR_STREAM(e.what());
00339   }
00340 }
00341 
00342 void BebopDriverNodelet::PauseAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr)
00343 {
00344   try
00345   {
00346     ROS_INFO("Pausing autonomous flight");
00347     bebop_ptr_->PauseAutonomousFlight();
00348   }
00349   catch (const std::runtime_error& e)
00350   {
00351     ROS_ERROR_STREAM(e.what());
00352   }
00353 }
00354 
00355 void BebopDriverNodelet::StopAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr)
00356 {
00357   try
00358   {
00359     ROS_INFO("Stopping autonomous flight");
00360     bebop_ptr_->StopAutonomousFlight();
00361   }
00362   catch (const std::runtime_error& e)
00363   {
00364     ROS_ERROR_STREAM(e.what());
00365   }
00366 }
00367 
00368 void BebopDriverNodelet::FlipAnimationCallback(const std_msgs::UInt8ConstPtr &animid_ptr)
00369 {
00370   try
00371   {
00372     // TODO(mani-monaj): Check if flying
00373     ROS_INFO("Performing flip animation %d ...", animid_ptr->data);
00374     bebop_ptr_->AnimationFlip(animid_ptr->data);
00375   }
00376   catch (const std::runtime_error& e)
00377   {
00378     ROS_ERROR_STREAM(e.what());
00379   }
00380 }
00381 
00382 void BebopDriverNodelet::TakeSnapshotCallback(const std_msgs::EmptyConstPtr &empty_ptr)
00383 {
00384   try
00385   {
00386     ROS_INFO("Taking a high-res snapshot on-board");
00387     bebop_ptr_->TakeSnapshot();
00388   }
00389   catch (const std::runtime_error& e)
00390   {
00391     ROS_ERROR_STREAM(e.what());
00392   }
00393 }
00394 
00395 void BebopDriverNodelet::SetExposureCallback(const std_msgs::Float32ConstPtr& exposure_ptr)
00396 {
00397   try
00398   {
00399     ROS_INFO("Setting exposure to %f", exposure_ptr->data);
00400     bebop_ptr_->SetExposure(exposure_ptr->data);
00401   }
00402   catch (const std::runtime_error& e)
00403   {
00404     ROS_ERROR_STREAM(e.what());
00405   }
00406 }
00407 
00408 void BebopDriverNodelet::ToggleRecordingCallback(const std_msgs::BoolConstPtr &toggle_ptr)
00409 {
00410   const bool& start_record = toggle_ptr->data;
00411   try
00412   {
00413     ROS_INFO_STREAM("Sending request to " << (start_record ? "start" : "stop")
00414                     << " on board video recording");
00415     bebop_ptr_->ToggleVideoRecording(start_record);
00416   }
00417   catch (const std::runtime_error& e)
00418   {
00419     ROS_ERROR_STREAM(e.what());
00420   }
00421 }
00422 
00423 void BebopDriverNodelet::ParamCallback(BebopArdrone3Config &config, uint32_t level)
00424 {
00425   NODELET_INFO("Dynamic reconfigure callback with level: %d", level);
00426   bebop_ptr_->UpdateSettings(config);
00427 }
00428 
00429 // Runs its own context
00430 void BebopDriverNodelet::CameraPublisherThread()
00431 {
00432   uint32_t frame_w = 0;
00433   uint32_t frame_h = 0;
00434   NODELET_INFO_STREAM("[CameraThread] thread lwp_id: " << util::GetLWPId());
00435 
00436   while (!boost::this_thread::interruption_requested())
00437   {
00438     try
00439     {
00440       sensor_msgs::ImagePtr image_msg_ptr_(new sensor_msgs::Image());
00441       const ros::Time t_now = ros::Time::now();
00442 
00443       NODELET_DEBUG_STREAM("Grabbing a frame from Bebop");
00444       // This is blocking
00445       bebop_ptr_->GetFrontCameraFrame(image_msg_ptr_->data, frame_w, frame_h);
00446 
00447       NODELET_DEBUG_STREAM("Frame grabbed: " << frame_w << " , " << frame_h);
00448       camera_info_msg_ptr_.reset(new sensor_msgs::CameraInfo(cinfo_manager_ptr_->getCameraInfo()));
00449       camera_info_msg_ptr_->header.stamp = t_now;
00450       camera_info_msg_ptr_->header.frame_id = param_camera_frame_id_;
00451       camera_info_msg_ptr_->width = frame_w;
00452       camera_info_msg_ptr_->height = frame_h;
00453 
00454       if (image_transport_pub_.getNumSubscribers() > 0)
00455       {
00456         image_msg_ptr_->encoding = "rgb8";
00457         image_msg_ptr_->is_bigendian = false;
00458         image_msg_ptr_->header.frame_id = param_camera_frame_id_;
00459         image_msg_ptr_->header.stamp = t_now;
00460         image_msg_ptr_->width = frame_w;
00461         image_msg_ptr_->height = frame_h;
00462         image_msg_ptr_->step = image_msg_ptr_->width * 3;
00463 
00464         image_transport_pub_.publish(image_msg_ptr_, camera_info_msg_ptr_);
00465       }
00466     }
00467     catch (const std::runtime_error& e)
00468     {
00469       NODELET_ERROR_STREAM("[CameraThread] " << e.what());
00470     }
00471   }
00472 
00473   NODELET_INFO("Camera publisher thread died.");
00474 }
00475 
00476 void BebopDriverNodelet::AuxThread()
00477 {
00478   NODELET_INFO_STREAM("[AuxThread] thread lwp_id: " << util::GetLWPId());
00479   // Since the update rate of bebop states is 5Hz, 15Hz seems fine for this thread
00480   ros::Rate loop_rate(15.0);
00481   geometry_msgs::Twist zero_twist;
00482   util::ResetTwist(zero_twist);
00483 
00484   // Camera Pan/Tilt State
00485   bebop_msgs::Ardrone3CameraStateOrientation::ConstPtr camera_state_ptr;
00486 
00487   // East-South-Down
00488   bebop_msgs::Ardrone3PilotingStateSpeedChanged::ConstPtr speed_esd_ptr;
00489 
00490   // Inertial frame
00491   bebop_msgs::Ardrone3PilotingStateAttitudeChanged::ConstPtr attitude_ptr;
00492 
00493   // GPS
00494   bebop_msgs::Ardrone3PilotingStatePositionChanged::ConstPtr gps_state_ptr;
00495 
00496   // REP-103
00497   double beb_roll_rad = 0.0;
00498   double beb_pitch_rad = 0.0;
00499   double beb_yaw_rad = 0.0;
00500   double beb_vx_m = 0.0;
00501   double beb_vy_m = 0.0;
00502   double beb_vz_m = 0.0;
00503 
00504   // TF2, Integerator
00505   ros::Time last_odom_time(ros::Time::now());
00506   geometry_msgs::TransformStamped odom_to_base_tf;
00507   odom_to_base_tf.header.frame_id = param_odom_frame_id_;
00508   odom_to_base_tf.child_frame_id = "base_link";
00509   tf2_ros::TransformBroadcaster tf_broad;
00510   tf2::Vector3 odom_to_base_trans_v3(0.0, 0.0, 0.0);
00511   tf2::Quaternion odom_to_base_rot_q;
00512 
00513   // Detect new messages
00514   // TODO(mani-monaj): Wrap this functionality into a class to remove duplicate code
00515   ros::Time last_speed_time(0);
00516   ros::Time last_att_time(0);
00517   ros::Time last_camerastate_time(0);
00518   ros::Time last_gps_time(0);
00519   bool new_speed_data = false;
00520   bool new_attitude_data = false;
00521   bool new_camera_state = false;
00522   bool new_gps_state = false;
00523 
00524   // We do not publish JointState in a nodelet friendly way
00525   // These names should match the joint name defined by bebop_description
00526   sensor_msgs::JointState js_msg;
00527   js_msg.name.push_back("camera_pan_joint");
00528   js_msg.name.push_back("camera_tilt_joint");
00529   js_msg.position.resize(2);
00530 
00531   sensor_msgs::NavSatFix gps_msg;
00532   gps_msg.header.frame_id = "/gps";
00533   gps_msg.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
00534   gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS | sensor_msgs::NavSatStatus::SERVICE_GLONASS;
00535   gps_msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
00536 
00537   while (!boost::this_thread::interruption_requested())
00538   {
00539     try
00540     {
00541       if (!bebop_ptr_->IsConnected())
00542       {
00543         loop_rate.sleep();
00544         continue;
00545       }
00546       {
00547         const ros::Time t_now = ros::Time::now();
00548         // cmd_vel safety
00549         boost::unique_lock<boost::mutex> twist_lock(twist_mutex_);
00550         if ( !util::CompareTwists(prev_bebop_twist_, zero_twist) &&
00551             ((t_now - prev_twist_stamp_).toSec() > param_cmd_vel_timeout_)
00552            )
00553         {
00554           NODELET_WARN("[AuxThread] Input cmd_vel timeout, reseting cmd_vel ...");
00555           util::ResetTwist(prev_bebop_twist_);
00556           bebop_ptr_->Move(0.0, 0.0, 0.0, 0.0);
00557         }
00558       }
00559 
00560       // Experimental
00561       if (bebop_ptr_->ardrone3_pilotingstate_positionchanged_ptr)
00562       {
00563         gps_state_ptr = bebop_ptr_->ardrone3_pilotingstate_positionchanged_ptr->GetDataCstPtr();
00564         if ((gps_state_ptr->header.stamp - last_gps_time).toSec() > util::eps)
00565         {
00566           last_gps_time = gps_state_ptr->header.stamp;
00567           new_gps_state = true;
00568         }
00569       }
00570 
00571       if (bebop_ptr_->ardrone3_camerastate_orientation_ptr)
00572       {
00573         camera_state_ptr = bebop_ptr_->ardrone3_camerastate_orientation_ptr->GetDataCstPtr();
00574 
00575         if ((camera_state_ptr->header.stamp - last_camerastate_time).toSec() > util::eps)
00576         {
00577           last_camerastate_time = camera_state_ptr->header.stamp;
00578           new_camera_state = true;
00579         }
00580       }
00581 
00582       if (bebop_ptr_->ardrone3_pilotingstate_speedchanged_ptr)
00583       {
00584         speed_esd_ptr = bebop_ptr_->ardrone3_pilotingstate_speedchanged_ptr->GetDataCstPtr();
00585 
00586         // conside new data only
00587         if ((speed_esd_ptr->header.stamp - last_speed_time).toSec() > util::eps)
00588         {
00589           last_speed_time = speed_esd_ptr->header.stamp;
00590           new_speed_data = true;
00591         }
00592       }
00593 
00594       if (bebop_ptr_->ardrone3_pilotingstate_attitudechanged_ptr)
00595       {
00596         attitude_ptr = bebop_ptr_->ardrone3_pilotingstate_attitudechanged_ptr->GetDataCstPtr();
00597 
00598         // conside new data only
00599         if ((attitude_ptr->header.stamp - last_att_time).toSec() > util::eps)
00600         {
00601           last_att_time = attitude_ptr->header.stamp;
00602           beb_roll_rad = attitude_ptr->roll;
00603           beb_pitch_rad = -attitude_ptr->pitch;
00604           beb_yaw_rad = -attitude_ptr->yaw;
00605 
00606           odom_to_base_rot_q.setRPY(beb_roll_rad, beb_pitch_rad, beb_yaw_rad);
00607           new_attitude_data = true;
00608         }
00609       }
00610 
00611       const double sync_diff_s = fabs((last_att_time - last_speed_time).toSec());
00612       // When new data (speed and rpy) is available and they are approx. synced
00613       if (new_speed_data && new_attitude_data &&
00614           speed_esd_ptr && attitude_ptr &&
00615           (sync_diff_s < 0.2))
00616       {
00617         ros::Time stamp = std::max(speed_esd_ptr->header.stamp, attitude_ptr->header.stamp);
00618 
00619         const double beb_vx_enu = speed_esd_ptr->speedX;
00620         const double beb_vy_enu = -speed_esd_ptr->speedY;
00621         const double beb_vz_enu = -speed_esd_ptr->speedZ;
00622         beb_vx_m = cos(beb_yaw_rad) * beb_vx_enu + sin(beb_yaw_rad) * beb_vy_enu;
00623         beb_vy_m = -sin(beb_yaw_rad) * beb_vx_enu + cos(beb_yaw_rad) * beb_vy_enu;
00624         beb_vz_m = beb_vz_enu;
00625 
00626         const double dt = (ros::Time::now() - last_odom_time).toSec();
00627         odom_to_base_trans_v3 += tf2::Vector3(beb_vx_enu * dt, beb_vy_enu * dt, beb_vz_enu * dt);
00628 
00629         nav_msgs::OdometryPtr odom_msg_ptr(new nav_msgs::Odometry());
00630         odom_msg_ptr->header.stamp = stamp;
00631         odom_msg_ptr->header.frame_id = param_odom_frame_id_;
00632         odom_msg_ptr->child_frame_id = "base_link";
00633         odom_msg_ptr->twist.twist.linear.x = beb_vx_m;
00634         odom_msg_ptr->twist.twist.linear.y = beb_vy_m;
00635         odom_msg_ptr->twist.twist.linear.z = beb_vz_m;
00636 
00637         // TODO(mani-monaj): Optimize this
00638         odom_msg_ptr->pose.pose.position.x = odom_to_base_trans_v3.x();
00639         odom_msg_ptr->pose.pose.position.y = odom_to_base_trans_v3.y();
00640         odom_msg_ptr->pose.pose.position.z = odom_to_base_trans_v3.z();
00641         tf2::convert(odom_to_base_rot_q, odom_msg_ptr->pose.pose.orientation);
00642         odom_pub_.publish(odom_msg_ptr);
00643 
00644         if (param_publish_odom_tf_)
00645         {
00646           odom_to_base_tf.header.stamp = stamp;
00647           tf2::convert(tf2::Transform(odom_to_base_rot_q, odom_to_base_trans_v3), odom_to_base_tf.transform);
00648           tf_broad.sendTransform(odom_to_base_tf);
00649         }
00650 
00651         last_odom_time = ros::Time::now();
00652         new_speed_data = false;
00653         new_attitude_data = false;
00654       }
00655 
00656       if (new_gps_state && gps_state_ptr)
00657       {
00658         // The SDK reports 500, 500, 500 when there is no GPS fix
00659         const bool is_valid_gps = (fabs(gps_state_ptr->latitude - 500.0) > util::eps) &&
00660             (fabs(gps_state_ptr->longitude - 500.0) > util::eps) &&
00661             (fabs(gps_state_ptr->altitude - 500.0) > util::eps);
00662 
00663         gps_msg.header.stamp = gps_state_ptr->header.stamp;
00664         gps_msg.status.status = is_valid_gps ? static_cast<int8_t>(sensor_msgs::NavSatStatus::STATUS_FIX):
00665                                                static_cast<int8_t>(sensor_msgs::NavSatStatus::STATUS_NO_FIX);
00666         gps_msg.latitude = gps_state_ptr->latitude;
00667         gps_msg.longitude = gps_state_ptr->longitude;
00668         gps_msg.altitude = gps_state_ptr->altitude;
00669         gps_fix_pub_.publish(gps_msg);
00670         new_gps_state = false;
00671       }
00672 
00673       if (new_camera_state && camera_state_ptr)
00674       {
00675         js_msg.header.stamp = camera_state_ptr->header.stamp;
00676         js_msg.position[0] = -camera_state_ptr->pan * util::deg2rad;
00677         js_msg.position[1] = -camera_state_ptr->tilt * util::deg2rad;
00678         camera_joint_pub_.publish(js_msg);
00679         new_camera_state = false;
00680       }
00681 
00682       loop_rate.sleep();
00683     }
00684     catch (const std::runtime_error& e)
00685     {
00686       NODELET_ERROR_STREAM("[AuxThread] " << e.what());
00687     }
00688   }
00689 }
00690 
00691 }  // namespace bebop_driver


bebop_driver
Author(s): Mani Monajjemi
autogenerated on Sat Jun 8 2019 20:37:45