diff --git a/bebop_driver/include/bebop_driver/bebop.h b/bebop_driver/include/bebop_driver/bebop.h index 1381ff9..444e2db 100644 --- a/bebop_driver/include/bebop_driver/bebop.h +++ b/bebop_driver/include/bebop_driver/bebop.h @@ -39,6 +39,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include #include #include +#include "bebop_driver/metadata_struct.h" extern "C" { @@ -94,6 +95,8 @@ class Bebop ARSAL_Sem_t state_sem_; boost::shared_ptr video_decoder_ptr_; std::string bebop_ip_; + const uint8_t *metadata_ptr_; + MetadataV2Base_t metadata_; // boost::mutex callback_map_mutex_; typedef std::map > callback_map_t; @@ -177,7 +180,7 @@ class Bebop // This function is blocking and runs in the caller's thread's context // which is different from FrameReceivedCallback's context - bool GetFrontCameraFrame(std::vector& buffer, uint32_t &width, uint32_t &height) const; + bool GetFrontCameraFrame(std::vector& buffer, uint32_t &width, uint32_t &height, MetadataV2Base_t &metadata_); uint32_t GetFrontCameraFrameWidth() const; uint32_t GetFrontCameraFrameHeight() const; }; diff --git a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h index c0dbdb4..ead5797 100644 --- a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h +++ b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h @@ -44,6 +44,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include #include "bebop_driver/bebop.h" +#include "geometry_msgs/QuaternionStamped.h" #define CLAMP(x, l, h) (((x) > (h)) ? (h) : (((x) < (l)) ? (l) : (x))) @@ -98,6 +99,20 @@ class Bebop; class BebopDriverNodelet : public nodelet::Nodelet { private: + float frameX_; + float frameY_; + float frameZ_; + float frameW_; + + float droneX_; + float droneY_; + float droneZ_; + float droneW_; + + float northSpeed_; + float eastSpeed_; + float downSpeed_; + boost::shared_ptr bebop_ptr_; boost::shared_ptr camera_pub_thread_ptr_; boost::shared_ptr aux_thread_ptr_; @@ -106,6 +121,11 @@ class BebopDriverNodelet : public nodelet::Nodelet ros::Time prev_twist_stamp_; boost::mutex twist_mutex_; + geometry_msgs::QuaternionStamped drone_quat_msg_; + geometry_msgs::QuaternionStamped camera_quat_msg_; + + std_msgs::Float64 above_ground_msg_; + geometry_msgs::Twist camera_twist_; geometry_msgs::Twist prev_camera_twist_; @@ -126,12 +146,19 @@ class BebopDriverNodelet : public nodelet::Nodelet ros::Publisher odom_pub_; ros::Publisher camera_joint_pub_; ros::Publisher gps_fix_pub_; + ros::Publisher altitude_pub_; + ros::Publisher meta_camera_frame_pub_; + ros::Publisher meta_drone_frame_pub_; + ros::Publisher meta_aboveground_pub_; + boost::shared_ptr cinfo_manager_ptr_; boost::shared_ptr image_transport_ptr_; image_transport::CameraPublisher image_transport_pub_; sensor_msgs::CameraInfoPtr camera_info_msg_ptr_; + uint8_t *metadata_ptr_; + MetadataV2Base_t metadata_; // Dynamic Reconfigure boost::shared_ptr > dynr_serv_ptr_; diff --git a/bebop_driver/include/bebop_driver/bebop_video_decoder.h b/bebop_driver/include/bebop_driver/bebop_video_decoder.h index ec86c78..d85b714 100644 --- a/bebop_driver/include/bebop_driver/bebop_video_decoder.h +++ b/bebop_driver/include/bebop_driver/bebop_video_decoder.h @@ -95,7 +95,10 @@ class VideoDecoder inline uint32_t GetFrameWidth() const {return codec_initialized_ ? codec_ctx_ptr_->width : 0;} inline uint32_t GetFrameHeight() const {return codec_initialized_ ? codec_ctx_ptr_->height : 0;} + inline const uint8_t* GetFrameMetadataPtr() const {return metadata_ptr_;} inline const uint8_t* GetFrameRGBRawCstPtr() const {return frame_rgb_raw_ptr_;} + uint8_t *metadata_ptr_; + }; } // namespace bebop_driver diff --git a/bebop_driver/include/bebop_driver/metadata_struct.h b/bebop_driver/include/bebop_driver/metadata_struct.h new file mode 100644 index 0000000..168f498 --- /dev/null +++ b/bebop_driver/include/bebop_driver/metadata_struct.h @@ -0,0 +1,31 @@ +// Define the structs to extract the Metadata from the Bebop Video Stream +typedef struct +{ +uint16_t id; /**< Identifier = 0x5032 */ +uint16_t length; /**< Structure size in 32 bits words excluding the id and length +fields and including extensions */ +int32_t groundDistance; /**< Best ground distance estimation (m), Q16.16 */ +int32_t latitude; /**< Absolute latitude (deg), Q10.22 */ +int32_t longitude; /**< Absolute longitude (deg), Q10.22 */ +int32_t altitudeAndSv; /**< Bits 31..8 = altitude (m) Q16.8, bits 7..0 = GPS SV count */ +int16_t northSpeed; /**< North speed (m/s), Q8.8 */ +int16_t eastSpeed; /**< East speed (m/s), Q8.8 */ +int16_t downSpeed; /**< Down speed (m/s), Q8.8 */ +int16_t airSpeed; /**< Speed relative to air (m/s), negative means no data, Q8.8 */ +int16_t droneW; /**< Drone quaternion W, Q2.14 */ +int16_t droneX; /**< Drone quaternion X, Q2.14 */ +int16_t droneY; /**< Drone quaternion Y, Q2.14 */ +int16_t droneZ; /**< Drone quaternion Z, Q2.14 */ +int16_t frameW; /**< Frame view quaternion W, Q2.14 */ +int16_t frameX; /**< Frame view quaternion X, Q2.14 */ +int16_t frameY; /**< Frame view quaternion Y, Q2.14 */ +int16_t frameZ; /**< Frame view quaternion Z, Q2.14 */ +int16_t cameraPan; /**< Camera pan (rad), Q4.12 */ +int16_t cameraTilt; /**< Camera tilt (rad), Q4.12 */ +uint16_t exposureTime; /**< Frame exposure time (ms), Q8.8 */ +uint16_t gain; /**< Frame ISO gain */ +uint8_t state; /**< Bit 7 = binning, bits 6..0 = flyingState */ +uint8_t mode; /**< Bit 7 = animation, bits 6..0 = pilotingMode */ +int8_t wifiRssi; /**< Wifi RSSI (dBm) */ +uint8_t batteryPercentage; /**< Battery charge percentage */ +} MetadataV2Base_t; diff --git a/bebop_driver/launch/bebop1.launch b/bebop_driver/launch/bebop1.launch new file mode 100644 index 0000000..9f1369e --- /dev/null +++ b/bebop_driver/launch/bebop1.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/launch/bebop1allspace.launch b/bebop_driver/launch/bebop1allspace.launch new file mode 100644 index 0000000..ee67ab0 --- /dev/null +++ b/bebop_driver/launch/bebop1allspace.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/launch/bebop1allspaceTarget.launch b/bebop_driver/launch/bebop1allspaceTarget.launch new file mode 100644 index 0000000..a08c619 --- /dev/null +++ b/bebop_driver/launch/bebop1allspaceTarget.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/launch/bebop3all_stick.launch b/bebop_driver/launch/bebop3all_stick.launch new file mode 100644 index 0000000..1f44bf6 --- /dev/null +++ b/bebop_driver/launch/bebop3all_stick.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/launch/bebop4all.launch b/bebop_driver/launch/bebop4all.launch new file mode 100644 index 0000000..d742a92 --- /dev/null +++ b/bebop_driver/launch/bebop4all.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/launch/bebop4all_stick.launch b/bebop_driver/launch/bebop4all_stick.launch new file mode 100644 index 0000000..49f77fa --- /dev/null +++ b/bebop_driver/launch/bebop4all_stick.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/launch/bebop5all_stick.launch b/bebop_driver/launch/bebop5all_stick.launch new file mode 100644 index 0000000..032d015 --- /dev/null +++ b/bebop_driver/launch/bebop5all_stick.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/launch/bebop5allspace.launch b/bebop_driver/launch/bebop5allspace.launch new file mode 100644 index 0000000..c363503 --- /dev/null +++ b/bebop_driver/launch/bebop5allspace.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/launch/bebop_all.launch b/bebop_driver/launch/bebop_all.launch new file mode 100644 index 0000000..07cb551 --- /dev/null +++ b/bebop_driver/launch/bebop_all.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/scripts/meta/generate.py b/bebop_driver/scripts/meta/generate.py old mode 100755 new mode 100644 diff --git a/bebop_driver/scripts/meta/install.sh b/bebop_driver/scripts/meta/install.sh old mode 100755 new mode 100644 diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp index 7285318..4c600ad 100644 --- a/bebop_driver/src/bebop.cpp +++ b/bebop_driver/src/bebop.cpp @@ -545,10 +545,17 @@ 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( + //Send camera command for old sdk as int8_t +// ThrowOnCtrlError(device_controller_ptr_->aRDrone3->setCameraOrientation( +// device_controller_ptr_->aRDrone3, +// static_cast(tilt), +// static_cast(pan))); + + //Send camera command for new sdk as float + 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 @@ -561,7 +568,7 @@ uint32_t Bebop::GetFrontCameraFrameHeight() const return video_decoder_ptr_->GetFrameHeight(); } -bool Bebop::GetFrontCameraFrame(std::vector &buffer, uint32_t& width, uint32_t& height) const +bool Bebop::GetFrontCameraFrame(std::vector &buffer, uint32_t& width, uint32_t& height,MetadataV2Base_t& metadata_) { boost::unique_lock lock(frame_avail_mutex_); @@ -586,6 +593,16 @@ bool Bebop::GetFrontCameraFrame(std::vector &buffer, uint32_t& width, u width = video_decoder_ptr_->GetFrameWidth(); height = video_decoder_ptr_->GetFrameHeight(); + metadata_ptr_=video_decoder_ptr_->GetFrameMetadataPtr(); + + if(metadata_ptr_==NULL) + { + printf("metadata_ptr_ is NULL!!\n"); + } + else + { + memcpy(&metadata_,metadata_ptr_,sizeof(metadata_)); + } is_frame_avail_ = false; // ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "COPY ENDED"); return true; diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index e0a32a2..5e264e7 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/bebop_driver/src/bebop_driver_nodelet.cpp @@ -31,6 +31,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include #include #include +#include #include #include @@ -44,6 +45,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI // For AuxThread() - without the following, callback wrapper types are incomplete to the compiler #include "bebop_driver/autogenerated/ardrone3_state_callbacks.h" +#include "inttypes.h" PLUGINLIB_EXPORT_CLASS(bebop_driver::BebopDriverNodelet, nodelet::Nodelet) @@ -137,6 +139,10 @@ void BebopDriverNodelet::onInit() odom_pub_ = nh.advertise("odom", 30); camera_joint_pub_ = nh.advertise("joint_states", 10, true); gps_fix_pub_ = nh.advertise("fix", 10, true); + altitude_pub_ = nh.advertise("altitude",10); + meta_camera_frame_pub_=nh.advertise("metadata/camera_quat",10); + meta_drone_frame_pub_ =nh.advertise("metadata/drone_quat",10); + meta_aboveground_pub_ = nh.advertise("metadata/above_ground",10); cinfo_manager_ptr_.reset(new camera_info_manager::CameraInfoManager(nh, "bebop_front", param_camera_info_url)); image_transport_ptr_.reset(new image_transport::ImageTransport(nh)); @@ -250,12 +256,14 @@ void BebopDriverNodelet::CameraMoveCallback(const geometry_msgs::TwistConstPtr& { camera_twist_ = *twist_ptr; const bool is_camera_twist_changed = !util::CompareTwists(camera_twist_, prev_camera_twist_); - if (is_camera_twist_changed) + if (1) { // TODO(mani-monaj): Set |90| limit to appropriate value (|45|??) - bebop_ptr_->MoveCamera(CLAMP(camera_twist_.angular.y, -35.0, 35.0), +// for(int i=0;i<20;i++){ + bebop_ptr_->MoveCamera(CLAMP(camera_twist_.angular.y, -83.0, 17.0), CLAMP(camera_twist_.angular.z, -35.0, 35.0)); prev_camera_twist_ = camera_twist_; +//} } } catch (const std::runtime_error& e) @@ -417,7 +425,7 @@ void BebopDriverNodelet::CameraPublisherThread() NODELET_DEBUG_STREAM("Grabbing a frame from Bebop"); // This is blocking - bebop_ptr_->GetFrontCameraFrame(image_msg_ptr_->data, frame_w, frame_h); + bebop_ptr_->GetFrontCameraFrame(image_msg_ptr_->data, frame_w, frame_h, metadata_); NODELET_DEBUG_STREAM("Frame grabbed: " << frame_w << " , " << frame_h); camera_info_msg_ptr_.reset(new sensor_msgs::CameraInfo(cinfo_manager_ptr_->getCameraInfo())); @@ -428,6 +436,7 @@ void BebopDriverNodelet::CameraPublisherThread() if (image_transport_pub_.getNumSubscribers() > 0) { + //Publish image data image_msg_ptr_->encoding = "rgb8"; image_msg_ptr_->is_bigendian = false; image_msg_ptr_->header.frame_id = param_camera_frame_id_; @@ -438,7 +447,43 @@ void BebopDriverNodelet::CameraPublisherThread() image_transport_pub_.publish(image_msg_ptr_, camera_info_msg_ptr_); } + + //Parse & publish camera frame quaternion and drone quaternion metadata + float frameX_ = ((float) ((short) ntohs(metadata_.frameX)))/(16384.0); + float frameY_ = -((float) ((short) ntohs(metadata_.frameY)))/(16384.0); + float frameZ_ = -((float) ((short) ntohs(metadata_.frameZ)))/(16384.0); + float frameW_ = ((float) ((short) ntohs(metadata_.frameW)))/(16384.0); + + camera_quat_msg_.header.frame_id = param_camera_frame_id_; + camera_quat_msg_.header.stamp = t_now; + camera_quat_msg_.quaternion.x = frameX_; + camera_quat_msg_.quaternion.y = frameY_; + camera_quat_msg_.quaternion.z = frameZ_; + camera_quat_msg_.quaternion.w = frameW_; + meta_camera_frame_pub_.publish(camera_quat_msg_); + + float droneX_ = ((float) ((short) ntohs(metadata_.droneX)))/(16384.0); + float droneY_ = -((float) ((short) ntohs(metadata_.droneY)))/(16384.0); + float droneZ_ = -((float) ((short) ntohs(metadata_.droneZ)))/(16384.0); + float droneW_ = ((float) ((short) ntohs(metadata_.droneW)))/(16384.0); + + drone_quat_msg_.header.frame_id = param_camera_frame_id_; + drone_quat_msg_.header.stamp = t_now; + drone_quat_msg_.quaternion.x = droneX_; + drone_quat_msg_.quaternion.y = droneY_; + drone_quat_msg_.quaternion.z = droneZ_; + drone_quat_msg_.quaternion.w = droneW_; + meta_drone_frame_pub_.publish(drone_quat_msg_); + + // float northSpeed_ = ((float) ((short) ntohs(metadata_.northSpeed)))/(256.0); + // float eastSpeed_ = ((float) ((short) ntohs(metadata_.eastSpeed)))/(256.0); + // float downSpeed_ = ((float) ((short) ntohs(metadata_.downSpeed)))/(256.0); + + float aboveground_ = ((float)(ntohl(metadata_.groundDistance)))/(65536.0); + above_ground_msg_.data = aboveground_; + meta_aboveground_pub_.publish(above_ground_msg_); } + catch (const std::runtime_error& e) { NODELET_ERROR_STREAM("[CameraThread] " << e.what()); @@ -468,6 +513,9 @@ void BebopDriverNodelet::AuxThread() // GPS bebop_msgs::Ardrone3PilotingStatePositionChanged::ConstPtr gps_state_ptr; + //Altitude (Altitude above starting point) + bebop_msgs::Ardrone3PilotingStateAltitudeChanged::ConstPtr altitude_ptr; + // REP-103 double beb_roll_rad = 0.0; double beb_pitch_rad = 0.0; @@ -475,6 +523,7 @@ void BebopDriverNodelet::AuxThread() double beb_vx_m = 0.0; double beb_vy_m = 0.0; double beb_vz_m = 0.0; + double beb_altitude=0.0; // TF2, Integerator ros::Time last_odom_time(ros::Time::now()); @@ -489,10 +538,12 @@ void BebopDriverNodelet::AuxThread() // TODO(mani-monaj): Wrap this functionality into a class to remove duplicate code ros::Time last_speed_time(0); ros::Time last_att_time(0); + ros::Time last_alt_time(0); ros::Time last_camerastate_time(0); ros::Time last_gps_time(0); bool new_speed_data = false; bool new_attitude_data = false; + bool new_altitude_data = false; bool new_camera_state = false; bool new_gps_state = false; @@ -509,6 +560,8 @@ void BebopDriverNodelet::AuxThread() gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS | sensor_msgs::NavSatStatus::SERVICE_GLONASS; gps_msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; + std_msgs::Float64 alt_msg; + while (!boost::this_thread::interruption_requested()) { try @@ -583,6 +636,26 @@ void BebopDriverNodelet::AuxThread() } } + if (bebop_ptr_->ardrone3_pilotingstate_altitudechanged_ptr) + { + altitude_ptr = bebop_ptr_->ardrone3_pilotingstate_altitudechanged_ptr->GetDataCstPtr(); + + // conside new data only + if ((altitude_ptr->header.stamp - last_alt_time).toSec() > util::eps) + { + last_alt_time = altitude_ptr->header.stamp; + beb_altitude = altitude_ptr->altitude; + new_altitude_data = true; + } + } + + if (new_altitude_data==true && altitude_ptr) + { + alt_msg.data=altitude_ptr->altitude; + altitude_pub_.publish(alt_msg); + new_altitude_data = false; + } + const double sync_diff_s = fabs((last_att_time - last_speed_time).toSec()); // When new data (speed and rpy) is available and they are approx. synced if (new_speed_data && new_attitude_data && diff --git a/bebop_driver/src/bebop_video_decoder.cpp b/bebop_driver/src/bebop_video_decoder.cpp index ba634af..d26267c 100644 --- a/bebop_driver/src/bebop_video_decoder.cpp +++ b/bebop_driver/src/bebop_video_decoder.cpp @@ -23,10 +23,16 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "bebop_driver/bebop_video_decoder.h" +#include "bebop_driver/metadata_struct.h" +#include #include #include #include +#include + +#include +#include #include @@ -37,7 +43,6 @@ extern "C" namespace bebop_driver { - const char* VideoDecoder::LOG_TAG = "Decoder"; // TODO(mani-monaj): Move to util, inline @@ -61,6 +66,7 @@ VideoDecoder::VideoDecoder() update_codec_params_(false) {} + bool VideoDecoder::InitCodec() { if (codec_initialized_) @@ -285,6 +291,7 @@ bool VideoDecoder::Decode(const ARCONTROLLER_Frame_t *bebop_frame_ptr_) packet_.data = bebop_frame_ptr_->data; packet_.size = bebop_frame_ptr_->used; + metadata_ptr_= bebop_frame_ptr_->metadata; const uint32_t width_prev = GetFrameWidth(); const uint32_t height_prev = GetFrameHeight(); @@ -322,4 +329,5 @@ bool VideoDecoder::Decode(const ARCONTROLLER_Frame_t *bebop_frame_ptr_) return true; } + } // namespace bebop_driver