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
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
00062 static const std::string logger_name = std::string(ROSCONSOLE_NAME_PREFIX) + "." +
00063 ros::this_node::getName() + ".bebopsdk";
00064
00065 ROS_LOG(util::arsal_level_to_ros[level], logger_name, "[%s] %s", tag, bebop_err_str);
00066 return 1;
00067 }
00068
00069 }
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
00090
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
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
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
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
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
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
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
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
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
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
00480 ros::Rate loop_rate(15.0);
00481 geometry_msgs::Twist zero_twist;
00482 util::ResetTwist(zero_twist);
00483
00484
00485 bebop_msgs::Ardrone3CameraStateOrientation::ConstPtr camera_state_ptr;
00486
00487
00488 bebop_msgs::Ardrone3PilotingStateSpeedChanged::ConstPtr speed_esd_ptr;
00489
00490
00491 bebop_msgs::Ardrone3PilotingStateAttitudeChanged::ConstPtr attitude_ptr;
00492
00493
00494 bebop_msgs::Ardrone3PilotingStatePositionChanged::ConstPtr gps_state_ptr;
00495
00496
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
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
00514
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
00525
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
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
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
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
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
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
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
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 }