diff --git a/bebop_driver/config/defaults.yaml b/bebop_driver/config/defaults.yaml index 4ce12e2..cf2fd3e 100644 --- a/bebop_driver/config/defaults.yaml +++ b/bebop_driver/config/defaults.yaml @@ -19,6 +19,7 @@ states: enable_autotakeoffmodechanged: true enable_mediastreamingstate_videoenablechanged: true enable_camerastate_orientation: true + enable_camerastate_orientationv2: true enable_gpsstate_numberofsatellitechanged: true enable_numberofsatellitechanged: true diff --git a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h index 87c63cc..f633508 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h +++ b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h @@ -1605,6 +1605,9 @@ class Ardrone3CameraStateOrientationV2 : public AbstractState { ARSAL_PRINT(ARSAL_PRINT_INFO, "CB" , "[STATES] Enabling %s", topic.c_str()); ros_pub_ = nh.advertise(topic, 10, true); + ::boost::lock_guard lock(mutex_); + msg_ptr.reset(new ::bebop_msgs::Ardrone3CameraStateOrientationV2()); + ros_pub_.publish(msg_ptr); } // pub_enabled_ is false } diff --git a/bebop_driver/launch/bebop_node.launch b/bebop_driver/launch/bebop_node.launch index f28797e..f74005e 100644 --- a/bebop_driver/launch/bebop_node.launch +++ b/bebop_driver/launch/bebop_node.launch @@ -2,7 +2,7 @@ - + diff --git a/bebop_driver/launch/bebop_nodelet.launch b/bebop_driver/launch/bebop_nodelet.launch index 048fcf6..87ddbbe 100644 --- a/bebop_driver/launch/bebop_nodelet.launch +++ b/bebop_driver/launch/bebop_nodelet.launch @@ -2,7 +2,7 @@ - + diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp index d2664c1..5564109 100644 --- a/bebop_driver/src/bebop.cpp +++ b/bebop_driver/src/bebop.cpp @@ -553,10 +553,10 @@ void Bebop::Move(const double &roll, const double &pitch, const double &gaz_spee void Bebop::MoveCamera(const double &tilt, const double &pan) { ThrowOnInternalError("Camera Move Failure"); - ThrowOnCtrlError(device_controller_ptr_->aRDrone3->setCameraOrientation( + ThrowOnCtrlError(device_controller_ptr_->aRDrone3->setCameraOrientationV2( device_controller_ptr_->aRDrone3, - static_cast(tilt), - static_cast(pan))); + static_cast(tilt), + static_cast(pan))); } uint32_t Bebop::GetFrontCameraFrameWidth() const diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index 1deca22..2d1dc60 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/bebop_driver/src/bebop_driver_nodelet.cpp @@ -482,7 +482,8 @@ void BebopDriverNodelet::AuxThread() util::ResetTwist(zero_twist); // Camera Pan/Tilt State - bebop_msgs::Ardrone3CameraStateOrientation::ConstPtr camera_state_ptr; + bebop_msgs::Ardrone3CameraStateOrientationV2::ConstPtr camera_state_ptr; + bebop_ptr_->MoveCamera(0.0, 0.0); // East-South-Down bebop_msgs::Ardrone3PilotingStateSpeedChanged::ConstPtr speed_esd_ptr; @@ -568,9 +569,9 @@ void BebopDriverNodelet::AuxThread() } } - if (bebop_ptr_->ardrone3_camerastate_orientation_ptr) + if (bebop_ptr_->ardrone3_camerastate_orientationv2_ptr) { - camera_state_ptr = bebop_ptr_->ardrone3_camerastate_orientation_ptr->GetDataCstPtr(); + camera_state_ptr = bebop_ptr_->ardrone3_camerastate_orientationv2_ptr->GetDataCstPtr(); if ((camera_state_ptr->header.stamp - last_camerastate_time).toSec() > util::eps) {