Skip to content

added metadata topic #139

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
tnaegeli wants to merge 11 commits into
base: indigo-devel
Choose a base branch
from
Open

added metadata topic #139

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
6bb5163
Added publisher for for Altitude as std_msgs
lumeier Nov 8, 2016
48daea0
Started to integrate Metadata extraction
lumeier Dec 14, 2016
959b557
print struct
lumeier Dec 15, 2016
54f01c3
Added publishers for camera and drone quaternion metadata
lumeier Dec 20, 2016
bfb367d
Merge pull request #1 from lumeier/metadata
tnaegeli Jan 4, 2017
18cbd33
Added capability to use the new sdk feature for float camera control
lumeier Jan 5, 2017
3beb349
Merge pull request #1 from lumeier/metadata
lumeier Jan 5, 2017
15ba685
Removed int camera movement since it overwrites float commands
Jan 6, 2017
561d97b
Merge pull request #2 from lumeier/indigo-devel
tnaegeli Jan 6, 2017
1ac3d53
wip
Aug 14, 2017
aebf8a5
included metadata height and lookat quaternion
Sep 28, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion bebop_driver/include/bebop_driver/bebop.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
#include <boost/atomic.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition_variable.hpp>
#include "bebop_driver/metadata_struct.h"

extern "C"
{
Expand Down Expand Up @@ -94,6 +95,8 @@ class Bebop
ARSAL_Sem_t state_sem_;
boost::shared_ptr<VideoDecoder> video_decoder_ptr_;
std::string bebop_ip_;
const uint8_t *metadata_ptr_;
MetadataV2Base_t metadata_;

// boost::mutex callback_map_mutex_;
typedef std::map<eARCONTROLLER_DICTIONARY_KEY, boost::shared_ptr<cb::AbstractCommand> > callback_map_t;
Expand Down Expand Up @@ -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<uint8_t>& buffer, uint32_t &width, uint32_t &height) const;
bool GetFrontCameraFrame(std::vector<uint8_t>& buffer, uint32_t &width, uint32_t &height, MetadataV2Base_t &metadata_);
uint32_t GetFrontCameraFrameWidth() const;
uint32_t GetFrontCameraFrameHeight() const;
};
Expand Down
27 changes: 27 additions & 0 deletions bebop_driver/include/bebop_driver/bebop_driver_nodelet.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
#include <boost/shared_ptr.hpp>

#include "bebop_driver/bebop.h"
#include "geometry_msgs/QuaternionStamped.h"

#define CLAMP(x, l, h) (((x) > (h)) ? (h) : (((x) < (l)) ? (l) : (x)))

Expand Down Expand Up @@ -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_driver::Bebop> bebop_ptr_;
boost::shared_ptr<boost::thread> camera_pub_thread_ptr_;
boost::shared_ptr<boost::thread> aux_thread_ptr_;
Expand All @@ -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_;

Expand All @@ -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<camera_info_manager::CameraInfoManager> cinfo_manager_ptr_;
boost::shared_ptr<image_transport::ImageTransport> 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<dynamic_reconfigure::Server<bebop_driver::BebopArdrone3Config> > dynr_serv_ptr_;
Expand Down
3 changes: 3 additions & 0 deletions bebop_driver/include/bebop_driver/bebop_video_decoder.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
31 changes: 31 additions & 0 deletions bebop_driver/include/bebop_driver/metadata_struct.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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;
27 changes: 27 additions & 0 deletions bebop_driver/launch/bebop1.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
<?xml version="1.0"?>
<launch>
<arg name="namespace" default="q1/real" />
<arg name="ip" default="192.168.101.1" />
<arg name="config_file" default="$(find bebop_driver)/config/defaults.yaml" />
<arg name="camera_info_url" default="package://bebop_driver/data/bebop_camera_calib.yaml" />
<!--Start Joy node-->
<group>
<node pkg="joy" name="joy" type="joy_node" output="screen">
<param name="autorepeat_rate" value="15"/>
</node>
<!--Start Vicon Bridge-->
<include file="$(find vicon_bridge)/launch/vicon.launch" />
</group>


<!--Start Bebop Autonomy-->
<group ns="q1/real">
<node pkg="bebop_driver" name="bebop_driver" type="bebop_driver_node" output="screen">
<param name="camera_info_url" value="$(arg camera_info_url)" />
<param name="bebop_ip" value="192.168.101.1" />
<rosparam command="load" file="$(arg config_file)" />
</node>
<include file="$(find bebop_description)/launch/description.launch" />
</group>

</launch>
31 changes: 31 additions & 0 deletions bebop_driver/launch/bebop1allspace.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<launch>
<arg name="namespace" default="q5/real" />
<arg name="ip" default="192.168.105.1" />
<arg name="config_file" default="$(find bebop_driver)/config/defaults.yaml" />
<arg name="camera_info_url" default="package://bebop_driver/data/bebop_camera_calib.yaml" />
<!--Start Joy node-->
<group>
<node pkg="joy" name="joy" type="joy_node" output="screen">
<param name="autorepeat_rate" value="15"/>
<param name="deadzone" value="0"/>
<param name="dev" value="/dev/input/js0"/>
</node>
<!--Start Vicon Bridge-->
<include file="$(find vicon_bridge)/launch/vicon.launch" />
<include file="$(find spacenav_node)/launch/static_deadband.launch" />
</group>


<!--Start Bebop Autonomy-->

<group ns="q1/real">
<node pkg="bebop_driver" name="bebop_driver" type="bebop_driver_node" output="screen">
<param name="camera_info_url" value="$(arg camera_info_url)" />
<param name="bebop_ip" value="192.168.101.1" />
<rosparam command="load" file="$(arg config_file)" />
</node>
<include file="$(find bebop_description)/launch/description.launch" />
</group>

</launch>
32 changes: 32 additions & 0 deletions bebop_driver/launch/bebop1allspaceTarget.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
<?xml version="1.0"?>
<launch>
<arg name="namespace" default="q5/real" />
<arg name="ip" default="192.168.101.1" />
<arg name="config_file" default="$(find bebop_driver)/config/defaults.yaml" />
<arg name="camera_info_url" default="package://bebop_driver/data/bebop_camera_calib.yaml" />
<!--Start Joy node-->
<group>
<node pkg="joy" name="joy" type="joy_node" output="screen">
<param name="autorepeat_rate" value="15"/>
<param name="deadzone" value="0"/>
<param name="dev" value="/dev/input/js0"/>
</node>
<!--Start Vicon Bridge-->
<include file="$(find vicon_bridge)/launch/vicon.launch" />
<include file="$(find spacenav_node)/launch/static_deadband.launch" />
<include file="$(find target_node)/launch/camera_target.launch" />
</group>


<!--Start Bebop Autonomy-->

<group ns="q1/real">
<node pkg="bebop_driver" name="bebop_driver" type="bebop_driver_node" output="screen">
<param name="camera_info_url" value="$(arg camera_info_url)" />
<param name="bebop_ip" value="192.168.101.1" />
<rosparam command="load" file="$(arg config_file)" />
</node>
<include file="$(find bebop_description)/launch/description.launch" />
</group>

</launch>
27 changes: 27 additions & 0 deletions bebop_driver/launch/bebop3all_stick.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
<?xml version="1.0"?>
<launch>
<arg name="namespace" default="q3/real" />
<arg name="ip" default="192.168.1.104" />
<arg name="config_file" default="$(find bebop_driver)/config/defaults.yaml" />
<arg name="camera_info_url" default="package://bebop_driver/data/bebop_camera_calib.yaml" />
<!--Start Joy node-->
<group>
<node pkg="joy" name="joy" type="joy_node" output="screen">
<param name="autorepeat_rate" value="15"/>
</node>
<!--Start Vicon Bridge-->
<include file="$(find vicon_bridge)/launch/vicon.launch" />
</group>


<!--Start Bebop Autonomy-->
<group ns="q3/real">
<node pkg="bebop_driver" name="bebop_driver" type="bebop_driver_node" output="screen">
<param name="camera_info_url" value="$(arg camera_info_url)" />
<param name="bebop_ip" value="192.168.103.1" />
<rosparam command="load" file="$(arg config_file)" />
</node>
<include file="$(find bebop_description)/launch/description.launch" />
</group>

</launch>
26 changes: 26 additions & 0 deletions bebop_driver/launch/bebop4all.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
<?xml version="1.0"?>
<launch>
<arg name="namespace" default="q4/real" />
<arg name="ip" default="192.168.1.104" />
<arg name="config_file" default="$(find bebop_driver)/config/defaults.yaml" />
<arg name="camera_info_url" default="package://bebop_driver/data/bebop_camera_calib.yaml" />
<!--Start Joy node-->
<group>
<node pkg="joy" name="joy" type="joy_node" output="screen">
<param name="autorepeat_rate" value="15"/>
</node>
<!--Start Vicon Bridge-->
<include file="$(find vicon_bridge)/launch/vicon.launch" />
</group>


<!--Start Bebop Autonomy-->
<group ns="$(arg namespace)">
<node pkg="bebop_driver" name="bebop_driver" type="bebop_driver_node" output="screen">
<param name="camera_info_url" value="$(arg camera_info_url)" />
<param name="bebop_ip" value="$(arg ip)" />
<rosparam command="load" file="$(arg config_file)" />
</node>
<include file="$(find bebop_description)/launch/description.launch" />
</group>
</launch>
27 changes: 27 additions & 0 deletions bebop_driver/launch/bebop4all_stick.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
<?xml version="1.0"?>
<launch>
<arg name="namespace" default="q4/real" />
<arg name="ip" default="192.168.1.104" />
<arg name="config_file" default="$(find bebop_driver)/config/defaults.yaml" />
<arg name="camera_info_url" default="package://bebop_driver/data/bebop_camera_calib.yaml" />
<!--Start Joy node-->
<group>
<node pkg="joy" name="joy" type="joy_node" output="screen">
<param name="autorepeat_rate" value="15"/>
</node>
<!--Start Vicon Bridge-->
<include file="$(find vicon_bridge)/launch/vicon.launch" />
</group>


<!--Start Bebop Autonomy-->
<group ns="q4/real">
<node pkg="bebop_driver" name="bebop_driver" type="bebop_driver_node" output="screen">
<param name="camera_info_url" value="$(arg camera_info_url)" />
<param name="bebop_ip" value="192.168.104.1" />
<rosparam command="load" file="$(arg config_file)" />
</node>
<include file="$(find bebop_description)/launch/description.launch" />
</group>

</launch>
29 changes: 29 additions & 0 deletions bebop_driver/launch/bebop5all_stick.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
<?xml version="1.0"?>
<launch>
<arg name="namespace" default="q5/real" />
<arg name="ip" default="192.168.105.1" />
<arg name="config_file" default="$(find bebop_driver)/config/defaults.yaml" />
<arg name="camera_info_url" default="package://bebop_driver/data/bebop_camera_calib.yaml" />
<!--Start Joy node-->
<group>
<node pkg="joy" name="joy" type="joy_node" output="screen">
<param name="autorepeat_rate" value="15"/>
<param name="deadzone" value="0"/>
<param name="dev" value="/dev/input/js0"/>
</node>
<!--Start Vicon Bridge-->
<include file="$(find vicon_bridge)/launch/vicon.launch" />
</group>


<!--Start Bebop Autonomy-->
<group ns="q5/real">
<node pkg="bebop_driver" name="bebop_driver" type="bebop_driver_node" output="screen">
<param name="camera_info_url" value="$(arg camera_info_url)" />
<param name="bebop_ip" value="192.168.105.1" />
<rosparam command="load" file="$(arg config_file)" />
</node>
<include file="$(find bebop_description)/launch/description.launch" />
</group>

</launch>
31 changes: 31 additions & 0 deletions bebop_driver/launch/bebop5allspace.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<launch>
<arg name="namespace" default="q5/real" />
<arg name="ip" default="192.168.105.1" />
<arg name="config_file" default="$(find bebop_driver)/config/defaults.yaml" />
<arg name="camera_info_url" default="package://bebop_driver/data/bebop_camera_calib.yaml" />
<!--Start Joy node-->
<group>
<node pkg="joy" name="joy" type="joy_node" output="screen">
<param name="autorepeat_rate" value="15"/>
<param name="deadzone" value="0"/>
<param name="dev" value="/dev/input/js0"/>
</node>
<!--Start Vicon Bridge-->
<include file="$(find vicon_bridge)/launch/vicon.launch" />
<include file="$(find spacenav_node)/launch/static_deadband.launch" />
</group>


<!--Start Bebop Autonomy-->
<group ns="q5/real">
<node pkg="bebop_driver" name="bebop_driver" type="bebop_driver_node" output="screen">
<param name="camera_info_url" value="$(arg camera_info_url)" />
<param name="bebop_ip" value="192.168.105.1" />
<rosparam command="load" file="$(arg config_file)" />
</node>
<include file="$(find bebop_description)/launch/description.launch" />
</group>


</launch>
23 changes: 23 additions & 0 deletions bebop_driver/launch/bebop_all.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<launch>
<arg name="namespace" default="q1/real" />
<arg name="ip" default="192.168.42.1" />
<arg name="config_file" default="$(find bebop_driver)/config/defaults.yaml" />
<arg name="camera_info_url" default="package://bebop_driver/data/bebop_camera_calib.yaml" />

<group ns="$(arg namespace)">
<node pkg="bebop_driver" name="bebop_driver" type="bebop_driver_node" output="screen">
<param name="camera_info_url" value="$(arg camera_info_url)" />
<param name="bebop_ip" value="$(arg ip)" />
<rosparam command="load" file="$(arg config_file)" />
</node>
<include file="$(find bebop_description)/launch/description.launch" />
</group>

<group>
<node pkg="joy" name="joy" type="joy_node" output="screen">
<param name="autorepeat_rate" value="15" />
</node>
<include file="$(find target_node)/launch/camera_target.launch" />
</group>
</launch>
Empty file modified bebop_driver/scripts/meta/generate.py
100755 → 100644
View file
Open in desktop
Empty file.
Empty file modified bebop_driver/scripts/meta/install.sh
100755 → 100644
View file
Open in desktop
Empty file.
Loading