From 6bb516397b61ab4c4b7b9dd2a103a2dbe7139ce5 Mon Sep 17 00:00:00 2001 From: Lukas Meier Date: Tue, 8 Nov 2016 10:14:08 +0100 Subject: [PATCH 1/8] Added publisher for for Altitude as std_msgs --- .../bebop_driver/bebop_driver_nodelet.h | 1 + bebop_driver/launch/bebop101.launch | 29 ++++++++++++++++++ bebop_driver/launch/bebop421.launch | 15 ++++++++++ bebop_driver/src/bebop_driver_nodelet.cpp | 30 +++++++++++++++++++ 4 files changed, 75 insertions(+) create mode 100644 bebop_driver/launch/bebop101.launch create mode 100644 bebop_driver/launch/bebop421.launch diff --git a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h index c0dbdb4..61c5d29 100644 --- a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h +++ b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h @@ -126,6 +126,7 @@ class BebopDriverNodelet : public nodelet::Nodelet ros::Publisher odom_pub_; ros::Publisher camera_joint_pub_; ros::Publisher gps_fix_pub_; + ros::Publisher altitude_pub_; boost::shared_ptr cinfo_manager_ptr_; boost::shared_ptr image_transport_ptr_; diff --git a/bebop_driver/launch/bebop101.launch b/bebop_driver/launch/bebop101.launch new file mode 100644 index 0000000..fc58e55 --- /dev/null +++ b/bebop_driver/launch/bebop101.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/launch/bebop421.launch b/bebop_driver/launch/bebop421.launch new file mode 100644 index 0000000..df462ab --- /dev/null +++ b/bebop_driver/launch/bebop421.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index e0a32a2..b673c91 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 @@ -137,6 +138,7 @@ 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); 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)); @@ -468,6 +470,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 +480,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 +495,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 +517,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 +593,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 && From 48daea07ed54d1f8e44f423b2067365f7758d9c5 Mon Sep 17 00:00:00 2001 From: Lukas Meier Date: Wed, 14 Dec 2016 13:53:29 +0100 Subject: [PATCH 2/8] Started to integrate Metadata extraction --- .../bebop_driver/bebop_video_decoder.h | 3 + .../include/bebop_driver/metadata_struct.h | 52 +++++++++++++++ bebop_driver/launch/bebop103.launch | 24 +++++++ bebop_driver/launch/bebop_all.launch | 23 +++++++ bebop_driver/src/bebop_video_decoder.cpp | 64 +++++++++++++++++++ 5 files changed, 166 insertions(+) create mode 100644 bebop_driver/include/bebop_driver/metadata_struct.h create mode 100644 bebop_driver/launch/bebop103.launch create mode 100755 bebop_driver/launch/bebop_all.launch diff --git a/bebop_driver/include/bebop_driver/bebop_video_decoder.h b/bebop_driver/include/bebop_driver/bebop_video_decoder.h index ec86c78..dfa68f5 100644 --- a/bebop_driver/include/bebop_driver/bebop_video_decoder.h +++ b/bebop_driver/include/bebop_driver/bebop_video_decoder.h @@ -96,6 +96,9 @@ class VideoDecoder inline uint32_t GetFrameHeight() const {return codec_initialized_ ? codec_ctx_ptr_->height : 0;} inline const uint8_t* GetFrameRGBRawCstPtr() const {return frame_rgb_raw_ptr_;} + uint16_t uint16Swap(uint16_t s); + int16_t int16Swap(int16_t s); + }; } // 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..cde88bb --- /dev/null +++ b/bebop_driver/include/bebop_driver/metadata_struct.h @@ -0,0 +1,52 @@ +// Define the structs to extract the Metadata from the Bebop Video Stream + + +// Basic metadata struct +typedef struct +{ + uint16_t specific; /**< Identifier = 0x5031 */ + uint16_t length; /**< Size in 32 bits words = 6 */ + int16_t droneYaw; /**< Drone yaw/psi (rad), Q4.12 */ + int16_t dronePitch; /**< Drone pitch/theta (rad), Q4.12 */ + int16_t droneRoll; /**< Drone roll/phi (rad), Q4.12 */ + int16_t cameraPan; /**< Camera pan (rad), Q4.12 */ + int16_t cameraTilt; /**< Camera tilt (rad), Q4.12 */ + int16_t frameW; /**< Frame view quaternion W, Q4.12 */ + int16_t frameX; /**< Frame view quaternion X, Q4.12 */ + int16_t frameY; /**< Frame view quaternion Y, Q4.12 */ + int16_t frameZ; /**< Frame view quaternion Z, Q4.12 */ + int16_t exposureTime; /**< Frame exposure time (ms), Q8.8 */ + int16_t gain; /**< Frame ISO gain */ + int8_t wifiRssi; /**< Wifi RSSI (dBm) */ + uint8_t batteryPercentage; /**< Battery charge percentage */ +} StreamingMetadataV1Basic_t; + +// Extended metadata structure +typedef struct +{ + uint16_t specific; /**< Identifier = 0x5031 */ + uint16_t length; /**< Size in 32 bits words = 13 */ + int16_t droneYaw; /**< Drone yaw/psi (rad), Q4.12 */ + int16_t dronePitch; /**< Drone pitch/theta (rad), Q4.12 */ + int16_t droneRoll; /**< Drone roll/phi (rad), Q4.12 */ + int16_t cameraPan; /**< Camera pan (rad), Q4.12 */ + int16_t cameraTilt; /**< Camera tilt (rad), Q4.12 */ + int16_t frameW; /**< Frame view quaternion W, Q4.12 */ + int16_t frameX; /**< Frame view quaternion X, Q4.12 */ + int16_t frameY; /**< Frame view quaternion Y, Q4.12 */ + int16_t frameZ; /**< Frame view quaternion Z, Q4.12 */ + int16_t exposureTime; /**< Frame exposure time (ms), Q8.8 */ + int16_t gain; /**< Frame ISO gain */ + int8_t wifiRssi; /**< Wifi RSSI (dBm) */ + uint8_t batteryPercentage; /**< Battery charge percentage */ + int32_t gpsLatitude; /**< GPS latitude (deg), Q12.20 */ + int32_t gpsLongitude; /**< GPS longitude (deg), Q12.20 */ + int32_t gpsAltitudeAndSv; /**< Bits 31..8 = GPS altitude (m) Q16.8, bits 7..0 = SV count */ + int32_t altitude; /**< Altitude relative to take-off (m), Q16.16 */ + uint32_t distanceFromHome; /**< Distance from home (m), Q16.16 */ + int16_t xSpeed; /**< X speed (m/s), Q8.8 */ + int16_t ySpeed; /**< Y speed (m/s), Q8.8 */ + int16_t zSpeed; /**< Z speed (m/s), Q8.8 */ + uint8_t state; /**< Bit 7 = binning, bits 6..0 = flyingState */ + uint8_t mode; /**< Bit 7 = animation, bits 6..0 = pilotingMode */ +} StreamingMetadataV1Extended_t; diff --git a/bebop_driver/launch/bebop103.launch b/bebop_driver/launch/bebop103.launch new file mode 100644 index 0000000..28b729e --- /dev/null +++ b/bebop_driver/launch/bebop103.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/launch/bebop_all.launch b/bebop_driver/launch/bebop_all.launch new file mode 100755 index 0000000..07cb551 --- /dev/null +++ b/bebop_driver/launch/bebop_all.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/src/bebop_video_decoder.cpp b/bebop_driver/src/bebop_video_decoder.cpp index ba634af..7a4ac16 100644 --- a/bebop_driver/src/bebop_video_decoder.cpp +++ b/bebop_driver/src/bebop_video_decoder.cpp @@ -23,10 +23,15 @@ 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 @@ -37,6 +42,8 @@ extern "C" namespace bebop_driver { + StreamingMetadataV1Extended_t metadata; + void *meta; const char* VideoDecoder::LOG_TAG = "Decoder"; @@ -61,6 +68,7 @@ VideoDecoder::VideoDecoder() update_codec_params_(false) {} + bool VideoDecoder::InitCodec() { if (codec_initialized_) @@ -286,6 +294,44 @@ bool VideoDecoder::Decode(const ARCONTROLLER_Frame_t *bebop_frame_ptr_) packet_.data = bebop_frame_ptr_->data; packet_.size = bebop_frame_ptr_->used; + + printf("Check for NULL Pointer!!!!!\n"); + + if (bebop_frame_ptr_->metadata == NULL) + {printf("NULL Pointer!!!!!");} + //printf("Pointer Adress: %p\n",bebop_frame_ptr_->metadata); + printf("Metadata Size: %d \n",bebop_frame_ptr_->metadataSize); + //printf("Size of metadata: %ld\n",sizeof(metadata)); + + memcpy(&metadata,bebop_frame_ptr_->metadata,bebop_frame_ptr_->metadataSize); + //memcpy(&metadata,bebop_frame_ptr_->metadata,4); + printf("Size of Metadata: %ld\n",sizeof(metadata)); + printf("memcpy successful!\n"); + printf("Length of Metadata in 32 bit words: %d\n",VideoDecoder::uint16Swap(metadata.length)); + // printf("cameraPan: %d\n",VideoDecoder::int16Swap(metadata.cameraPan)); + printf("cameraPan: %d\n",metadata.cameraPan); + //std::bitset<16> int_bit(VideoDecoder::int16Swap(metadata.exposureTime)); + std::bitset<8> int_bit(metadata.batteryPercentage); + + std::cout << "Drone Yaw: "<< int_bit << "\n"; + + + // int *test; + // *test==2; + // int test2; + // memcpy(&test2,test,sizeof(int)); + // printf("test2: %d\n ",test2); + //Debug Print + + + //printf("Battery: %" PRIu16 "\n",metadata->batteryPercentage); + //printf("Battery Percentage: %d\n",metadata->length); + + //printf("droneYaw: %d\n",metadata.droneYaw); + //printf("Metadata_size: %d\n",sizeof(metadata)); + //printf("Timestamp: %ld\n",bebop_frame_ptr_->); + + const uint32_t width_prev = GetFrameWidth(); const uint32_t height_prev = GetFrameHeight(); @@ -322,4 +368,22 @@ bool VideoDecoder::Decode(const ARCONTROLLER_Frame_t *bebop_frame_ptr_) return true; } + + +uint16_t VideoDecoder::uint16Swap(uint16_t s) + { + unsigned char b1, b2; + b1 = s & 255; + b2 = (s >> 8) & 255; + return (b1 << 8) + b2; + } + +int16_t VideoDecoder::int16Swap(int16_t s) + { + unsigned char b1, b2; + b1 = s & 255; + b2 = (s >> 8) & 255; + return (b1 << 8) + b2; + } + } // namespace bebop_driver From 959b5571a4a1cee424f9ceecccd663b7b261e446 Mon Sep 17 00:00:00 2001 From: Lukas Meier Date: Thu, 15 Dec 2016 19:00:17 +0100 Subject: [PATCH 3/8] print struct --- .../include/bebop_driver/metadata_struct.h | 29 ++++ bebop_driver/src/bebop_video_decoder.cpp | 130 ++++++++++++++++-- 2 files changed, 149 insertions(+), 10 deletions(-) diff --git a/bebop_driver/include/bebop_driver/metadata_struct.h b/bebop_driver/include/bebop_driver/metadata_struct.h index cde88bb..695c492 100644 --- a/bebop_driver/include/bebop_driver/metadata_struct.h +++ b/bebop_driver/include/bebop_driver/metadata_struct.h @@ -50,3 +50,32 @@ typedef struct uint8_t state; /**< Bit 7 = binning, bits 6..0 = flyingState */ uint8_t mode; /**< Bit 7 = animation, bits 6..0 = pilotingMode */ } StreamingMetadataV1Extended_t; + +// // Extended metadata structure +// typedef struct +// { +// uint16_t specific; /**< Identifier = 0x5031 */ +// uint16_t length; /**< Size in 32 bits words = 13 */ +// int32_t alt; /**< Drone yaw/psi (rad), Q4.12 */ +// int16_t droneRoll; /**< Drone roll/phi (rad), Q4.12 */ +// int16_t cameraPan; /**< Camera pan (rad), Q4.12 */ +// int16_t cameraTilt; /**< Camera tilt (rad), Q4.12 */ +// int16_t frameW; /**< Frame view quaternion W, Q4.12 */ +// int16_t frameX; /**< Frame view quaternion X, Q4.12 */ +// int16_t frameY; /**< Frame view quaternion Y, Q4.12 */ +// int16_t frameZ; /**< Frame view quaternion Z, Q4.12 */ +// int16_t exposureTime; /**< Frame exposure time (ms), Q8.8 */ +// int16_t gain; /**< Frame ISO gain */ +// int8_t wifiRssi; /**< Wifi RSSI (dBm) */ +// uint8_t batteryPercentage; /**< Battery charge percentage */ +// int32_t gpsLatitude; /**< GPS latitude (deg), Q12.20 */ +// int32_t gpsLongitude; /**< GPS longitude (deg), Q12.20 */ +// int32_t gpsAltitudeAndSv; /**< Bits 31..8 = GPS altitude (m) Q16.8, bits 7..0 = SV count */ +// int32_t altitude; /**< Altitude relative to take-off (m), Q16.16 */ +// uint32_t distanceFromHome; /**< Distance from home (m), Q16.16 */ +// int16_t xSpeed; /**< X speed (m/s), Q8.8 */ +// int16_t ySpeed; /**< Y speed (m/s), Q8.8 */ +// int16_t zSpeed; /**< Z speed (m/s), Q8.8 */ +// uint8_t state; /**< Bit 7 = binning, bits 6..0 = flyingState */ +// uint8_t mode; /**< Bit 7 = animation, bits 6..0 = pilotingMode */ +// } StreamingMetadataV1Extended_t; diff --git a/bebop_driver/src/bebop_video_decoder.cpp b/bebop_driver/src/bebop_video_decoder.cpp index 7a4ac16..ef69f80 100644 --- a/bebop_driver/src/bebop_video_decoder.cpp +++ b/bebop_driver/src/bebop_video_decoder.cpp @@ -24,6 +24,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI */ #include "bebop_driver/bebop_video_decoder.h" #include "bebop_driver/metadata_struct.h" +#include #include #include @@ -43,7 +44,7 @@ extern "C" namespace bebop_driver { StreamingMetadataV1Extended_t metadata; - void *meta; + StreamingMetadataV1Extended_t metadata_temp; const char* VideoDecoder::LOG_TAG = "Decoder"; @@ -299,21 +300,130 @@ bool VideoDecoder::Decode(const ARCONTROLLER_Frame_t *bebop_frame_ptr_) if (bebop_frame_ptr_->metadata == NULL) {printf("NULL Pointer!!!!!");} + + //metadata_temp = ntohl(metadata); + //metadata=metadata_temp; + //printf("Pointer Adress: %p\n",bebop_frame_ptr_->metadata); printf("Metadata Size: %d \n",bebop_frame_ptr_->metadataSize); - //printf("Size of metadata: %ld\n",sizeof(metadata)); - memcpy(&metadata,bebop_frame_ptr_->metadata,bebop_frame_ptr_->metadataSize); - //memcpy(&metadata,bebop_frame_ptr_->metadata,4); - printf("Size of Metadata: %ld\n",sizeof(metadata)); - printf("memcpy successful!\n"); - printf("Length of Metadata in 32 bit words: %d\n",VideoDecoder::uint16Swap(metadata.length)); + memcpy(&metadata,bebop_frame_ptr_->metadata,bebop_frame_ptr_->metadataSize); + + printf("\nStreamingMetadataV1Extended_t:\n"); + printf("specific: \t\t0x%x\n",ntohs(metadata.specific)); + printf("length: \t\t%d\n\n",ntohs(metadata.length)); + + // std::bitset<16> spec_bit(metadata.specific); + // std::bitset<16> len_bit(metadata.length); + // + // std::string s1=spec_bit.to_string(); + // std::string s2=len_bit.to_string(); + // std::bitset<32> firstbits(s2 + s1); + // int bits_int=(int)(firstbits.to_ulong()); + // std::bitset<32> ntohled(ntohl(bits_int)); + // std::cout << "First 32 Bits: " << spec_bit << len_bit << "\nconcatenated: " << firstbits << "\n" << ntohled << "\n"; + + // std::bitset<32> alt2_bit(ntohl(metadata.alt)); + // std::string alt2_string(alt2_bit.to_string()); + // std::string alt2_m=alt2_string.substr(0,15); + // std::string alt2_cm=alt2_string.substr(16,31); + // std::bitset<16> alt2_m_bit(alt2_m); + // std::bitset<16> alt2_cm_bit(alt2_cm); + // + // std::cout << "altitude: " << alt2_m_bit.to_ulong() << "." << alt2_cm_bit.to_ulong() << "m\n"<< alt2_m_bit << alt2_cm_bit << "\n"; + + + printf("droneYaw: \t\t%d\t",ntohs(metadata.droneYaw)); + std::bitset<16> droneyaw_bit(ntohs(metadata.droneYaw)); + std::cout << droneyaw_bit << "\n"; + printf("dronePitch: \t\t%d\t",ntohs(metadata.dronePitch)); + std::bitset<16> dronepitch_bit(ntohs(metadata.dronePitch)); + std::cout << dronepitch_bit << "\n"; + + //Reverse engineered height + // std::string yaw_string=droneyaw_bit.to_string(); + // std::string pitch_string=dronepitch_bit.to_string(); + // std::string meters=yaw_string; + // std::string cms=pitch_string; + // std::bitset mbit= + //std::cout << "altitude: " << droneyaw_bit.to_ulong() << "." << dronepitch_bit.to_ulong() << "m\n"; + + + printf("droneRoll: \t\t%d\t",ntohs(metadata.droneRoll)); + std::bitset<16> droneroll_bit(ntohs(metadata.droneRoll)); + std::cout << droneroll_bit << "\n\n"; + + printf("cameraPan: \t\t%d\t",VideoDecoder::int16Swap(metadata.cameraPan)); + std::bitset<16> camerapan_bit(VideoDecoder::int16Swap(metadata.cameraPan)); + std::cout << camerapan_bit << "\n"; + printf("cameraTilt: \t\t%d\t",VideoDecoder::int16Swap(metadata.cameraTilt)); + std::bitset<16> cameratilt_bit(VideoDecoder::int16Swap(metadata.cameraTilt)); + std::cout << cameratilt_bit << "\n\n"; + + printf("frameW: \t\t%d\t",VideoDecoder::int16Swap(metadata.frameW)); + std::bitset<16> framew_bit(VideoDecoder::int16Swap(metadata.frameW)); + std::cout << framew_bit << "\n"; + printf("frameX: \t\t%d\t",VideoDecoder::int16Swap(metadata.frameX)); + std::bitset<16> framex_bit(VideoDecoder::int16Swap(metadata.frameX)); + std::cout << framex_bit << "\n"; + printf("frameY: \t\t%d\t",VideoDecoder::int16Swap(metadata.frameY)); + std::bitset<16> framey_bit(VideoDecoder::int16Swap(metadata.frameY)); + std::cout << framey_bit << "\n"; + printf("frameZ: \t\t%d\t",VideoDecoder::int16Swap(metadata.frameZ)); + std::bitset<16> framez_bit(VideoDecoder::int16Swap(metadata.frameZ)); + std::cout << framez_bit << "\n\n"; + + printf("exposureTime: \t\t%d\n",VideoDecoder::int16Swap(metadata.exposureTime)); + printf("gain: \t\t\t%d\n",VideoDecoder::int16Swap(metadata.gain)); + + std::bitset<8> wifi_bit(metadata.wifiRssi); + std::cout << "wifiRssi (bits): \t\t" << wifi_bit << "\n"; + std::bitset<8> bat_bit(metadata.batteryPercentage); + std::cout << "batteryPercentage (bits):\t"<< bat_bit << "\n\n"; + + std::bitset<32> lat_bit(metadata.gpsLatitude); + std::cout << "gpsLatitude (bits): \t\t" << lat_bit << "\n"; + std::bitset<32> long_bit(metadata.gpsLongitude); + std::cout << "gpsLongitude (bits): \t\t" << long_bit << "\n"; + std::bitset<32> gpsalt_bit(metadata.gpsAltitudeAndSv); + std::cout << "gpsAltitudeAndSv (bits): \t" << gpsalt_bit << "\n"; + std::bitset<32> alt_bit(metadata.altitude); + std::cout << "altitude (bits): \t\t" << alt_bit << "\n"; + std::bitset<32> home_bit(metadata.distanceFromHome); + std::cout << "distanceFromHome (bits): \t" << home_bit << "\n\n"; + + + //printf("\n. . .\n\n"); + + printf("xSpeed: \t\t%d\t",VideoDecoder::int16Swap(metadata.xSpeed)); + std::bitset<16> xspeed_bit(VideoDecoder::int16Swap(metadata.xSpeed)); + std::cout << xspeed_bit << "\n"; + printf("ySpeed: \t\t%d\t",VideoDecoder::int16Swap(metadata.ySpeed)); + std::bitset<16> yspeed_bit(VideoDecoder::int16Swap(metadata.ySpeed)); + std::cout << yspeed_bit << "\n"; + printf("zSpeed: \t\t%d\t",VideoDecoder::int16Swap(metadata.zSpeed)); + std::bitset<16> zspeed_bit(VideoDecoder::int16Swap(metadata.zSpeed)); + std::cout << zspeed_bit << "\n\n"; + + std::bitset<8> state_bit(metadata.state); + std::cout << "State (bits): \t\t\t" << state_bit << "\n"; + std::bitset<8> mode_bit(metadata.state); + std::cout << "Mode (bits): \t\t\t" << mode_bit << "\n\n"; + // int deadbeef = 0xDEADBEEF; + //printf("TESTING DEADBEEF %x %x\n", deadbeef, ntohl(deadbeef)); + + + //printf("\t\t\t.\n\t\t\t.\n\t\t\t.\n"); + + + + // printf("cameraPan: %d\n",VideoDecoder::int16Swap(metadata.cameraPan)); - printf("cameraPan: %d\n",metadata.cameraPan); + //printf("cameraPan: %d\n",metadata.cameraPan); //std::bitset<16> int_bit(VideoDecoder::int16Swap(metadata.exposureTime)); - std::bitset<8> int_bit(metadata.batteryPercentage); + //std::bitset<8> int_bit(metadata.batteryPercentage); - std::cout << "Drone Yaw: "<< int_bit << "\n"; + //std::cout << "Drone Yaw: "<< int_bit << "\n"; // int *test; From 54f01c3b8330b6708ecf834e8883fe19735ce2fe Mon Sep 17 00:00:00 2001 From: Lukas Meier Date: Tue, 20 Dec 2016 14:50:00 +0100 Subject: [PATCH 4/8] Added publishers for camera and drone quaternion metadata --- bebop_driver/include/bebop_driver/bebop.h | 5 +- .../bebop_driver/bebop_driver_nodelet.h | 24 +++ .../bebop_driver/bebop_video_decoder.h | 4 +- .../include/bebop_driver/metadata_struct.h | 106 +++-------- bebop_driver/src/bebop.cpp | 12 +- bebop_driver/src/bebop_driver_nodelet.cpp | 39 +++- bebop_driver/src/bebop_video_decoder.cpp | 168 +----------------- 7 files changed, 108 insertions(+), 250 deletions(-) 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 61c5d29..94095e3 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,9 @@ 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_; + geometry_msgs::Twist camera_twist_; geometry_msgs::Twist prev_camera_twist_; @@ -127,12 +145,18 @@ class BebopDriverNodelet : public nodelet::Nodelet 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_; + + 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 dfa68f5..d85b714 100644 --- a/bebop_driver/include/bebop_driver/bebop_video_decoder.h +++ b/bebop_driver/include/bebop_driver/bebop_video_decoder.h @@ -95,9 +95,9 @@ 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_;} - uint16_t uint16Swap(uint16_t s); - int16_t int16Swap(int16_t s); + uint8_t *metadata_ptr_; }; diff --git a/bebop_driver/include/bebop_driver/metadata_struct.h b/bebop_driver/include/bebop_driver/metadata_struct.h index 695c492..168f498 100644 --- a/bebop_driver/include/bebop_driver/metadata_struct.h +++ b/bebop_driver/include/bebop_driver/metadata_struct.h @@ -1,81 +1,31 @@ // Define the structs to extract the Metadata from the Bebop Video Stream - - -// Basic metadata struct typedef struct { - uint16_t specific; /**< Identifier = 0x5031 */ - uint16_t length; /**< Size in 32 bits words = 6 */ - int16_t droneYaw; /**< Drone yaw/psi (rad), Q4.12 */ - int16_t dronePitch; /**< Drone pitch/theta (rad), Q4.12 */ - int16_t droneRoll; /**< Drone roll/phi (rad), Q4.12 */ - int16_t cameraPan; /**< Camera pan (rad), Q4.12 */ - int16_t cameraTilt; /**< Camera tilt (rad), Q4.12 */ - int16_t frameW; /**< Frame view quaternion W, Q4.12 */ - int16_t frameX; /**< Frame view quaternion X, Q4.12 */ - int16_t frameY; /**< Frame view quaternion Y, Q4.12 */ - int16_t frameZ; /**< Frame view quaternion Z, Q4.12 */ - int16_t exposureTime; /**< Frame exposure time (ms), Q8.8 */ - int16_t gain; /**< Frame ISO gain */ - int8_t wifiRssi; /**< Wifi RSSI (dBm) */ - uint8_t batteryPercentage; /**< Battery charge percentage */ -} StreamingMetadataV1Basic_t; - -// Extended metadata structure -typedef struct -{ - uint16_t specific; /**< Identifier = 0x5031 */ - uint16_t length; /**< Size in 32 bits words = 13 */ - int16_t droneYaw; /**< Drone yaw/psi (rad), Q4.12 */ - int16_t dronePitch; /**< Drone pitch/theta (rad), Q4.12 */ - int16_t droneRoll; /**< Drone roll/phi (rad), Q4.12 */ - int16_t cameraPan; /**< Camera pan (rad), Q4.12 */ - int16_t cameraTilt; /**< Camera tilt (rad), Q4.12 */ - int16_t frameW; /**< Frame view quaternion W, Q4.12 */ - int16_t frameX; /**< Frame view quaternion X, Q4.12 */ - int16_t frameY; /**< Frame view quaternion Y, Q4.12 */ - int16_t frameZ; /**< Frame view quaternion Z, Q4.12 */ - int16_t exposureTime; /**< Frame exposure time (ms), Q8.8 */ - int16_t gain; /**< Frame ISO gain */ - int8_t wifiRssi; /**< Wifi RSSI (dBm) */ - uint8_t batteryPercentage; /**< Battery charge percentage */ - int32_t gpsLatitude; /**< GPS latitude (deg), Q12.20 */ - int32_t gpsLongitude; /**< GPS longitude (deg), Q12.20 */ - int32_t gpsAltitudeAndSv; /**< Bits 31..8 = GPS altitude (m) Q16.8, bits 7..0 = SV count */ - int32_t altitude; /**< Altitude relative to take-off (m), Q16.16 */ - uint32_t distanceFromHome; /**< Distance from home (m), Q16.16 */ - int16_t xSpeed; /**< X speed (m/s), Q8.8 */ - int16_t ySpeed; /**< Y speed (m/s), Q8.8 */ - int16_t zSpeed; /**< Z speed (m/s), Q8.8 */ - uint8_t state; /**< Bit 7 = binning, bits 6..0 = flyingState */ - uint8_t mode; /**< Bit 7 = animation, bits 6..0 = pilotingMode */ -} StreamingMetadataV1Extended_t; - -// // Extended metadata structure -// typedef struct -// { -// uint16_t specific; /**< Identifier = 0x5031 */ -// uint16_t length; /**< Size in 32 bits words = 13 */ -// int32_t alt; /**< Drone yaw/psi (rad), Q4.12 */ -// int16_t droneRoll; /**< Drone roll/phi (rad), Q4.12 */ -// int16_t cameraPan; /**< Camera pan (rad), Q4.12 */ -// int16_t cameraTilt; /**< Camera tilt (rad), Q4.12 */ -// int16_t frameW; /**< Frame view quaternion W, Q4.12 */ -// int16_t frameX; /**< Frame view quaternion X, Q4.12 */ -// int16_t frameY; /**< Frame view quaternion Y, Q4.12 */ -// int16_t frameZ; /**< Frame view quaternion Z, Q4.12 */ -// int16_t exposureTime; /**< Frame exposure time (ms), Q8.8 */ -// int16_t gain; /**< Frame ISO gain */ -// int8_t wifiRssi; /**< Wifi RSSI (dBm) */ -// uint8_t batteryPercentage; /**< Battery charge percentage */ -// int32_t gpsLatitude; /**< GPS latitude (deg), Q12.20 */ -// int32_t gpsLongitude; /**< GPS longitude (deg), Q12.20 */ -// int32_t gpsAltitudeAndSv; /**< Bits 31..8 = GPS altitude (m) Q16.8, bits 7..0 = SV count */ -// int32_t altitude; /**< Altitude relative to take-off (m), Q16.16 */ -// uint32_t distanceFromHome; /**< Distance from home (m), Q16.16 */ -// int16_t xSpeed; /**< X speed (m/s), Q8.8 */ -// int16_t ySpeed; /**< Y speed (m/s), Q8.8 */ -// int16_t zSpeed; /**< Z speed (m/s), Q8.8 */ -// uint8_t state; /**< Bit 7 = binning, bits 6..0 = flyingState */ -// uint8_t mode; /**< Bit 7 = animation, bits 6..0 = pilotingMode */ -// } StreamingMetadataV1Extended_t; +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/src/bebop.cpp b/bebop_driver/src/bebop.cpp index 7285318..f979647 100644 --- a/bebop_driver/src/bebop.cpp +++ b/bebop_driver/src/bebop.cpp @@ -561,7 +561,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 +586,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 b673c91..899303c 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/bebop_driver/src/bebop_driver_nodelet.cpp @@ -45,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) @@ -139,6 +140,9 @@ void BebopDriverNodelet::onInit() 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); + 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)); @@ -419,7 +423,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())); @@ -430,6 +434,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_; @@ -440,7 +445,39 @@ 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); } + catch (const std::runtime_error& e) { NODELET_ERROR_STREAM("[CameraThread] " << e.what()); diff --git a/bebop_driver/src/bebop_video_decoder.cpp b/bebop_driver/src/bebop_video_decoder.cpp index ef69f80..d26267c 100644 --- a/bebop_driver/src/bebop_video_decoder.cpp +++ b/bebop_driver/src/bebop_video_decoder.cpp @@ -43,9 +43,6 @@ extern "C" namespace bebop_driver { - StreamingMetadataV1Extended_t metadata; - StreamingMetadataV1Extended_t metadata_temp; - const char* VideoDecoder::LOG_TAG = "Decoder"; // TODO(mani-monaj): Move to util, inline @@ -294,153 +291,7 @@ bool VideoDecoder::Decode(const ARCONTROLLER_Frame_t *bebop_frame_ptr_) packet_.data = bebop_frame_ptr_->data; packet_.size = bebop_frame_ptr_->used; - - - printf("Check for NULL Pointer!!!!!\n"); - - if (bebop_frame_ptr_->metadata == NULL) - {printf("NULL Pointer!!!!!");} - - //metadata_temp = ntohl(metadata); - //metadata=metadata_temp; - - //printf("Pointer Adress: %p\n",bebop_frame_ptr_->metadata); - printf("Metadata Size: %d \n",bebop_frame_ptr_->metadataSize); - - memcpy(&metadata,bebop_frame_ptr_->metadata,bebop_frame_ptr_->metadataSize); - - printf("\nStreamingMetadataV1Extended_t:\n"); - printf("specific: \t\t0x%x\n",ntohs(metadata.specific)); - printf("length: \t\t%d\n\n",ntohs(metadata.length)); - - // std::bitset<16> spec_bit(metadata.specific); - // std::bitset<16> len_bit(metadata.length); - // - // std::string s1=spec_bit.to_string(); - // std::string s2=len_bit.to_string(); - // std::bitset<32> firstbits(s2 + s1); - // int bits_int=(int)(firstbits.to_ulong()); - // std::bitset<32> ntohled(ntohl(bits_int)); - // std::cout << "First 32 Bits: " << spec_bit << len_bit << "\nconcatenated: " << firstbits << "\n" << ntohled << "\n"; - - // std::bitset<32> alt2_bit(ntohl(metadata.alt)); - // std::string alt2_string(alt2_bit.to_string()); - // std::string alt2_m=alt2_string.substr(0,15); - // std::string alt2_cm=alt2_string.substr(16,31); - // std::bitset<16> alt2_m_bit(alt2_m); - // std::bitset<16> alt2_cm_bit(alt2_cm); - // - // std::cout << "altitude: " << alt2_m_bit.to_ulong() << "." << alt2_cm_bit.to_ulong() << "m\n"<< alt2_m_bit << alt2_cm_bit << "\n"; - - - printf("droneYaw: \t\t%d\t",ntohs(metadata.droneYaw)); - std::bitset<16> droneyaw_bit(ntohs(metadata.droneYaw)); - std::cout << droneyaw_bit << "\n"; - printf("dronePitch: \t\t%d\t",ntohs(metadata.dronePitch)); - std::bitset<16> dronepitch_bit(ntohs(metadata.dronePitch)); - std::cout << dronepitch_bit << "\n"; - - //Reverse engineered height - // std::string yaw_string=droneyaw_bit.to_string(); - // std::string pitch_string=dronepitch_bit.to_string(); - // std::string meters=yaw_string; - // std::string cms=pitch_string; - // std::bitset mbit= - //std::cout << "altitude: " << droneyaw_bit.to_ulong() << "." << dronepitch_bit.to_ulong() << "m\n"; - - - printf("droneRoll: \t\t%d\t",ntohs(metadata.droneRoll)); - std::bitset<16> droneroll_bit(ntohs(metadata.droneRoll)); - std::cout << droneroll_bit << "\n\n"; - - printf("cameraPan: \t\t%d\t",VideoDecoder::int16Swap(metadata.cameraPan)); - std::bitset<16> camerapan_bit(VideoDecoder::int16Swap(metadata.cameraPan)); - std::cout << camerapan_bit << "\n"; - printf("cameraTilt: \t\t%d\t",VideoDecoder::int16Swap(metadata.cameraTilt)); - std::bitset<16> cameratilt_bit(VideoDecoder::int16Swap(metadata.cameraTilt)); - std::cout << cameratilt_bit << "\n\n"; - - printf("frameW: \t\t%d\t",VideoDecoder::int16Swap(metadata.frameW)); - std::bitset<16> framew_bit(VideoDecoder::int16Swap(metadata.frameW)); - std::cout << framew_bit << "\n"; - printf("frameX: \t\t%d\t",VideoDecoder::int16Swap(metadata.frameX)); - std::bitset<16> framex_bit(VideoDecoder::int16Swap(metadata.frameX)); - std::cout << framex_bit << "\n"; - printf("frameY: \t\t%d\t",VideoDecoder::int16Swap(metadata.frameY)); - std::bitset<16> framey_bit(VideoDecoder::int16Swap(metadata.frameY)); - std::cout << framey_bit << "\n"; - printf("frameZ: \t\t%d\t",VideoDecoder::int16Swap(metadata.frameZ)); - std::bitset<16> framez_bit(VideoDecoder::int16Swap(metadata.frameZ)); - std::cout << framez_bit << "\n\n"; - - printf("exposureTime: \t\t%d\n",VideoDecoder::int16Swap(metadata.exposureTime)); - printf("gain: \t\t\t%d\n",VideoDecoder::int16Swap(metadata.gain)); - - std::bitset<8> wifi_bit(metadata.wifiRssi); - std::cout << "wifiRssi (bits): \t\t" << wifi_bit << "\n"; - std::bitset<8> bat_bit(metadata.batteryPercentage); - std::cout << "batteryPercentage (bits):\t"<< bat_bit << "\n\n"; - - std::bitset<32> lat_bit(metadata.gpsLatitude); - std::cout << "gpsLatitude (bits): \t\t" << lat_bit << "\n"; - std::bitset<32> long_bit(metadata.gpsLongitude); - std::cout << "gpsLongitude (bits): \t\t" << long_bit << "\n"; - std::bitset<32> gpsalt_bit(metadata.gpsAltitudeAndSv); - std::cout << "gpsAltitudeAndSv (bits): \t" << gpsalt_bit << "\n"; - std::bitset<32> alt_bit(metadata.altitude); - std::cout << "altitude (bits): \t\t" << alt_bit << "\n"; - std::bitset<32> home_bit(metadata.distanceFromHome); - std::cout << "distanceFromHome (bits): \t" << home_bit << "\n\n"; - - - //printf("\n. . .\n\n"); - - printf("xSpeed: \t\t%d\t",VideoDecoder::int16Swap(metadata.xSpeed)); - std::bitset<16> xspeed_bit(VideoDecoder::int16Swap(metadata.xSpeed)); - std::cout << xspeed_bit << "\n"; - printf("ySpeed: \t\t%d\t",VideoDecoder::int16Swap(metadata.ySpeed)); - std::bitset<16> yspeed_bit(VideoDecoder::int16Swap(metadata.ySpeed)); - std::cout << yspeed_bit << "\n"; - printf("zSpeed: \t\t%d\t",VideoDecoder::int16Swap(metadata.zSpeed)); - std::bitset<16> zspeed_bit(VideoDecoder::int16Swap(metadata.zSpeed)); - std::cout << zspeed_bit << "\n\n"; - - std::bitset<8> state_bit(metadata.state); - std::cout << "State (bits): \t\t\t" << state_bit << "\n"; - std::bitset<8> mode_bit(metadata.state); - std::cout << "Mode (bits): \t\t\t" << mode_bit << "\n\n"; - // int deadbeef = 0xDEADBEEF; - //printf("TESTING DEADBEEF %x %x\n", deadbeef, ntohl(deadbeef)); - - - //printf("\t\t\t.\n\t\t\t.\n\t\t\t.\n"); - - - - - // printf("cameraPan: %d\n",VideoDecoder::int16Swap(metadata.cameraPan)); - //printf("cameraPan: %d\n",metadata.cameraPan); - //std::bitset<16> int_bit(VideoDecoder::int16Swap(metadata.exposureTime)); - //std::bitset<8> int_bit(metadata.batteryPercentage); - - //std::cout << "Drone Yaw: "<< int_bit << "\n"; - - - // int *test; - // *test==2; - // int test2; - // memcpy(&test2,test,sizeof(int)); - // printf("test2: %d\n ",test2); - //Debug Print - - - //printf("Battery: %" PRIu16 "\n",metadata->batteryPercentage); - //printf("Battery Percentage: %d\n",metadata->length); - - //printf("droneYaw: %d\n",metadata.droneYaw); - //printf("Metadata_size: %d\n",sizeof(metadata)); - //printf("Timestamp: %ld\n",bebop_frame_ptr_->); - + metadata_ptr_= bebop_frame_ptr_->metadata; const uint32_t width_prev = GetFrameWidth(); const uint32_t height_prev = GetFrameHeight(); @@ -479,21 +330,4 @@ bool VideoDecoder::Decode(const ARCONTROLLER_Frame_t *bebop_frame_ptr_) } - -uint16_t VideoDecoder::uint16Swap(uint16_t s) - { - unsigned char b1, b2; - b1 = s & 255; - b2 = (s >> 8) & 255; - return (b1 << 8) + b2; - } - -int16_t VideoDecoder::int16Swap(int16_t s) - { - unsigned char b1, b2; - b1 = s & 255; - b2 = (s >> 8) & 255; - return (b1 << 8) + b2; - } - } // namespace bebop_driver From 18cbd330fe99cc5c900301707718b8ff26e0e487 Mon Sep 17 00:00:00 2001 From: Lukas Meier Date: Thu, 5 Jan 2017 17:20:48 +0100 Subject: [PATCH 5/8] Added capability to use the new sdk feature for float camera control --- bebop_driver/src/bebop.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp index f979647..8a22d5e 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"); + //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->sendCameraOrientationV2( + device_controller_ptr_->aRDrone3, + static_cast(tilt), + static_cast(pan))); } uint32_t Bebop::GetFrontCameraFrameWidth() const From 15ba685238236e9f46ea1df0925692819c481a33 Mon Sep 17 00:00:00 2001 From: Tobias Naegeli Date: Fri, 6 Jan 2017 13:59:21 +0100 Subject: [PATCH 6/8] Removed int camera movement since it overwrites float commands --- bebop_driver/src/bebop.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp index 8a22d5e..423ad10 100644 --- a/bebop_driver/src/bebop.cpp +++ b/bebop_driver/src/bebop.cpp @@ -546,10 +546,10 @@ void Bebop::MoveCamera(const double &tilt, const double &pan) { ThrowOnInternalError("Camera Move Failure"); //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))); +// 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->sendCameraOrientationV2( From 1ac3d53a97c29af5705a554cd4b0bdaf040e98cf Mon Sep 17 00:00:00 2001 From: Tobias Naegeli Date: Mon, 14 Aug 2017 11:25:20 +0200 Subject: [PATCH 7/8] wip --- bebop_driver/launch/bebop1.launch | 27 +++++++++++++++ bebop_driver/launch/bebop14all_stick.launch | 34 +++++++++++++++++++ bebop_driver/launch/bebop1allspace.launch | 31 +++++++++++++++++ .../launch/bebop1allspaceTarget.launch | 32 +++++++++++++++++ bebop_driver/launch/bebop3all_stick.launch | 27 +++++++++++++++ bebop_driver/launch/bebop4all.launch | 26 ++++++++++++++ bebop_driver/launch/bebop4all_stick.launch | 27 +++++++++++++++ bebop_driver/launch/bebop5all_stick.launch | 29 ++++++++++++++++ bebop_driver/launch/bebop5allspace.launch | 31 +++++++++++++++++ bebop_driver/launch/bebop_all.launch | 0 bebop_driver/scripts/meta/generate.py | 0 bebop_driver/scripts/meta/install.sh | 0 bebop_driver/src/bebop_driver_nodelet.cpp | 8 +++-- 13 files changed, 270 insertions(+), 2 deletions(-) create mode 100644 bebop_driver/launch/bebop1.launch create mode 100644 bebop_driver/launch/bebop14all_stick.launch create mode 100644 bebop_driver/launch/bebop1allspace.launch create mode 100644 bebop_driver/launch/bebop1allspaceTarget.launch create mode 100644 bebop_driver/launch/bebop3all_stick.launch create mode 100644 bebop_driver/launch/bebop4all.launch create mode 100644 bebop_driver/launch/bebop4all_stick.launch create mode 100644 bebop_driver/launch/bebop5all_stick.launch create mode 100644 bebop_driver/launch/bebop5allspace.launch mode change 100755 => 100644 bebop_driver/launch/bebop_all.launch mode change 100755 => 100644 bebop_driver/scripts/meta/generate.py mode change 100755 => 100644 bebop_driver/scripts/meta/install.sh diff --git a/bebop_driver/launch/bebop1.launch b/bebop_driver/launch/bebop1.launch new file mode 100644 index 0000000..9dc59ab --- /dev/null +++ b/bebop_driver/launch/bebop1.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_driver/launch/bebop14all_stick.launch b/bebop_driver/launch/bebop14all_stick.launch new file mode 100644 index 0000000..9565388 --- /dev/null +++ b/bebop_driver/launch/bebop14all_stick.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 old mode 100755 new mode 100644 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_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index 899303c..5c30a4d 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/bebop_driver/src/bebop_driver_nodelet.cpp @@ -213,10 +213,12 @@ void BebopDriverNodelet::CmdVelCallback(const geometry_msgs::TwistConstPtr& twis // TODO: Always apply zero after non-zero values if (is_bebop_twist_changed) { - bebop_ptr_->Move(CLAMP(-bebop_twist_.linear.y, -1.0, 1.0), + for(int i=0;i<10;i++){ + bebop_ptr_->Move(CLAMP(-bebop_twist_.linear.y, -1.0, 1.0), CLAMP(bebop_twist_.linear.x, -1.0, 1.0), CLAMP(bebop_twist_.linear.z, -1.0, 1.0), CLAMP(-bebop_twist_.angular.z, -1.0, 1.0)); + } } } catch (const std::runtime_error& e) @@ -256,12 +258,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|??) + //for(int i=0;i<20;i++){ bebop_ptr_->MoveCamera(CLAMP(camera_twist_.angular.y, -35.0, 35.0), CLAMP(camera_twist_.angular.z, -35.0, 35.0)); prev_camera_twist_ = camera_twist_; +//} } } catch (const std::runtime_error& e) From aebf8a53dd36ddff58a71aa1ecb2cbba3f663519 Mon Sep 17 00:00:00 2001 From: Tobias Naegeli Date: Thu, 28 Sep 2017 10:44:09 +0200 Subject: [PATCH 8/8] included metadata height and lookat quaternion --- .../bebop_driver/bebop_driver_nodelet.h | 4 ++- bebop_driver/launch/bebop1.launch | 2 +- bebop_driver/launch/bebop101.launch | 29 ---------------- bebop_driver/launch/bebop103.launch | 24 ------------- bebop_driver/launch/bebop14all_stick.launch | 34 ------------------- bebop_driver/launch/bebop421.launch | 15 -------- bebop_driver/src/bebop.cpp | 2 +- bebop_driver/src/bebop_driver_nodelet.cpp | 14 ++++---- 8 files changed, 13 insertions(+), 111 deletions(-) delete mode 100644 bebop_driver/launch/bebop101.launch delete mode 100644 bebop_driver/launch/bebop103.launch delete mode 100644 bebop_driver/launch/bebop14all_stick.launch delete mode 100644 bebop_driver/launch/bebop421.launch diff --git a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h index 94095e3..ead5797 100644 --- a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h +++ b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h @@ -124,6 +124,8 @@ class BebopDriverNodelet : public nodelet::Nodelet 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_; @@ -147,7 +149,7 @@ class BebopDriverNodelet : public nodelet::Nodelet 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_; diff --git a/bebop_driver/launch/bebop1.launch b/bebop_driver/launch/bebop1.launch index 9dc59ab..9f1369e 100644 --- a/bebop_driver/launch/bebop1.launch +++ b/bebop_driver/launch/bebop1.launch @@ -1,7 +1,7 @@ - + diff --git a/bebop_driver/launch/bebop101.launch b/bebop_driver/launch/bebop101.launch deleted file mode 100644 index fc58e55..0000000 --- a/bebop_driver/launch/bebop101.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/bebop_driver/launch/bebop103.launch b/bebop_driver/launch/bebop103.launch deleted file mode 100644 index 28b729e..0000000 --- a/bebop_driver/launch/bebop103.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/bebop_driver/launch/bebop14all_stick.launch b/bebop_driver/launch/bebop14all_stick.launch deleted file mode 100644 index 9565388..0000000 --- a/bebop_driver/launch/bebop14all_stick.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/bebop_driver/launch/bebop421.launch b/bebop_driver/launch/bebop421.launch deleted file mode 100644 index df462ab..0000000 --- a/bebop_driver/launch/bebop421.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp index 423ad10..4c600ad 100644 --- a/bebop_driver/src/bebop.cpp +++ b/bebop_driver/src/bebop.cpp @@ -552,7 +552,7 @@ void Bebop::MoveCamera(const double &tilt, const double &pan) // static_cast(pan))); //Send camera command for new sdk as float - ThrowOnCtrlError(device_controller_ptr_->aRDrone3->sendCameraOrientationV2( + ThrowOnCtrlError(device_controller_ptr_->aRDrone3->setCameraOrientationV2( device_controller_ptr_->aRDrone3, static_cast(tilt), static_cast(pan))); diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index 5c30a4d..5e264e7 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/bebop_driver/src/bebop_driver_nodelet.cpp @@ -142,7 +142,7 @@ void BebopDriverNodelet::onInit() 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)); @@ -213,12 +213,10 @@ void BebopDriverNodelet::CmdVelCallback(const geometry_msgs::TwistConstPtr& twis // TODO: Always apply zero after non-zero values if (is_bebop_twist_changed) { - for(int i=0;i<10;i++){ - bebop_ptr_->Move(CLAMP(-bebop_twist_.linear.y, -1.0, 1.0), + bebop_ptr_->Move(CLAMP(-bebop_twist_.linear.y, -1.0, 1.0), CLAMP(bebop_twist_.linear.x, -1.0, 1.0), CLAMP(bebop_twist_.linear.z, -1.0, 1.0), CLAMP(-bebop_twist_.angular.z, -1.0, 1.0)); - } } } catch (const std::runtime_error& e) @@ -261,8 +259,8 @@ void BebopDriverNodelet::CameraMoveCallback(const geometry_msgs::TwistConstPtr& if (1) { // TODO(mani-monaj): Set |90| limit to appropriate value (|45|??) - //for(int i=0;i<20;i++){ - 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_; //} @@ -480,6 +478,10 @@ void BebopDriverNodelet::CameraPublisherThread() // 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)