Time, camera exposure and video stabilization mode (#97)

* Add parameter sync_time (true => sync / false => no sync) to allow syncing bebop time on start. * Add simple visual model to urdf description * Add topic for setting camera exposure and video stabilization mode. Also included geometric camera calibration for bebop2 * Rename calibration file for bebop1 * Add parameter for setting drone type, updated documentation to include sync_time parameter and set_exposure topic.
AutonomyLab · Mar 24, 2017 · 8a9d6c7 · 8a9d6c7
1 parent 8297873
commit 8a9d6c7
Show file tree

Hide file tree

Showing 12 changed files with 95 additions and 6 deletions.
diff --git a/bebop_description/meshes/bebop_model.stl b/bebop_description/meshes/bebop_model.stl
diff --git a/bebop_description/urdf/bebop_base.urdf.xacro b/bebop_description/urdf/bebop_base.urdf.xacro
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<!-- Kinematic model only, no visuals, no collisions -->
+<!-- Kinematic model, simplified visual, no collisions -->
 <robot name="bebop" xmlns:xacro="http://www.ros.org/wiki/xacro">
   <xacro:property name="PI" value="3.1415926535897931" />
   <xacro:property name="camera_offset_len" value="0.1" />
@@ -12,7 +12,13 @@
   <xacro:property name="tilt_vel" value="2.0" />
   <xacro:property name="joint_effort" value="50" />
 
-  <link name="base_link"></link>
+  <link name="base_link">
+   <visual>
+      <geometry>
+        <mesh scale=".001 .001 .001" filename="package://bebop_description/meshes/bebop_model.stl"/>
+      </geometry>
+   </visual>
+  </link>
   <link name="camera_base_link"></link>
   <link name="camera_pan_link"></link>
   <link name="camera_tilt_link"></link>
@@ -50,4 +56,4 @@
 
   <!-- [nice to have] -->
   <!-- xacro:include filename="bebop.gazebo" /-->
-</robot>
+</robot>
diff --git a/bebop_driver/data/bebop_camera_calib.yaml → bebop_driver/data/bebop1_camera_calib.yaml b/bebop_driver/data/bebop_camera_calib.yaml → bebop_driver/data/bebop1_camera_calib.yaml
diff --git a/bebop_driver/data/bebop2_camera_calib.yaml b/bebop_driver/data/bebop2_camera_calib.yaml
@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 368
+camera_name: bebop_front
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [395.006308, 0.000000, 314.697857, 0.000000, 404.392711, 174.704342, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [0.009430, -0.001107, -0.011436, -0.002274, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [400.769714, 0.000000, 312.419488, 0.000000, 0.000000, 407.883575, 169.238191, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/bebop_driver/include/bebop_driver/bebop.h b/bebop_driver/include/bebop_driver/bebop.h
@@ -152,6 +152,7 @@ class Bebop
   // nothrow
   void Disconnect();
 
+  void SetDate(const std::string &date);
   void RequestAllSettings();
   void ResetAllSettings();
   void UpdateSettings(const bebop_driver::BebopArdrone3Config& config);
@@ -172,6 +173,8 @@ class Bebop
   void MoveCamera(const double& tilt, const double& pan);
 
   void TakeSnapshot();
+  // exposure should be between -3.0 and +3.0
+  void SetExposure(const float& exposure);
   // true: start, false: stop
   void ToggleVideoRecording(const bool start);
 

diff --git a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h
@@ -34,6 +34,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 #include <geometry_msgs/Twist.h>
 #include <std_msgs/Empty.h>
 #include <std_msgs/UInt8.h>
+#include <std_msgs/Float32.h>
 #include <std_msgs/Bool.h>
 #include <std_msgs/String.h>
 #include <camera_info_manager/camera_info_manager.h>
@@ -121,6 +122,7 @@ class BebopDriverNodelet : public nodelet::Nodelet
   ros::Subscriber stop_autoflight_sub_;
   ros::Subscriber animation_sub_;
   ros::Subscriber snapshot_sub_;
+  ros::Subscriber exposure_sub_;
   ros::Subscriber toggle_recording_sub_;
 
   ros::Publisher odom_pub_;
@@ -160,6 +162,7 @@ class BebopDriverNodelet : public nodelet::Nodelet
   void StopAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr);
   void FlipAnimationCallback(const std_msgs::UInt8ConstPtr& animid_ptr);
   void TakeSnapshotCallback(const std_msgs::EmptyConstPtr& empty_ptr);
+  void SetExposureCallback(const std_msgs::Float32ConstPtr& exposure_ptr);
   void ToggleRecordingCallback(const std_msgs::BoolConstPtr& toggle_ptr);
 
   void ParamCallback(bebop_driver::BebopArdrone3Config &config, uint32_t level);

diff --git a/bebop_driver/launch/bebop_node.launch b/bebop_driver/launch/bebop_node.launch
@@ -2,8 +2,9 @@
 <launch>
     <arg name="namespace" default="bebop" />
     <arg name="ip" default="192.168.42.1" />
+    <arg name="drone_type" default="bebop1" /> <!-- available drone types: bebop1, bebop2 -->
     <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" />
+    <arg name="camera_info_url" default="package://bebop_driver/data/$(arg drone_type)_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)" />

diff --git a/bebop_driver/launch/bebop_nodelet.launch b/bebop_driver/launch/bebop_nodelet.launch
@@ -2,8 +2,9 @@
 <launch>
     <arg name="namespace" default="bebop" />
     <arg name="ip" default="192.168.42.1" />
+    <arg name="drone_type" default="bebop1" /> <!-- available drone types: bebop1, bebop2 -->
     <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" />
+    <arg name="camera_info_url" default="package://bebop_driver/data/$(arg drone_type)_camera_calib.yaml" />
     <group ns="$(arg namespace)">
         <!-- nodelet manager -->
         <node pkg="nodelet" type="nodelet" name="bebop_nodelet_manager" args="manager" output="screen"/>

diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp
@@ -391,6 +391,14 @@ void Bebop::Disconnect()
   ARSAL_PRINT(ARSAL_PRINT_INFO, LOG_TAG, "Disconnected from Bebop ...");
 }
 
+void Bebop::SetDate(const std::string &date)
+{
+  ThrowOnInternalError("Setting Date Failed");
+  ThrowOnCtrlError(
+        device_controller_ptr_->common->sendCommonCurrentDate(device_controller_ptr_->common, const_cast<char*>(date.c_str())),
+        "Setting Date Failed");
+}
+
 void Bebop::RequestAllSettings()
 {
   ThrowOnInternalError("Request Settings Failed");
@@ -599,6 +607,15 @@ void Bebop::TakeSnapshot()
           device_controller_ptr_->aRDrone3));
 }
 
+void Bebop::SetExposure(const float& exposure)
+{
+  ThrowOnInternalError("Failed to set exposure");
+  // TODO fairf4x: Check bounds ?
+  ThrowOnCtrlError(
+    device_controller_ptr_->aRDrone3->sendPictureSettingsExpositionSelection(device_controller_ptr_->aRDrone3, (float)exposure)
+  );
+}
+
 void Bebop::ToggleVideoRecording(const bool start)
 {
   ThrowOnInternalError("Video Toggle Failure");

diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp
@@ -34,6 +34,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 
 #include <boost/bind.hpp>
 #include <boost/make_shared.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
 #include <cmath>
 #include <algorithm>
 #include <string>
@@ -88,6 +89,7 @@ void BebopDriverNodelet::onInit()
   // Params (not dynamically reconfigurable, local)
   // TODO(mani-monaj): Wrap all calls to .param() in a function call to enable logging
   const bool param_reset_settings = private_nh.param("reset_settings", false);
+  const bool param_sync_time = private_nh.param("sync_time", false);
   const std::string& param_camera_info_url = private_nh.param<std::string>("camera_info_url", "");
   const std::string& param_bebop_ip = private_nh.param<std::string>("bebop_ip", "192.168.42.1");
 
@@ -105,7 +107,17 @@ void BebopDriverNodelet::onInit()
     {
       NODELET_WARN("Resetting all settings ...");
       bebop_ptr_->ResetAllSettings();
-      // Wait for 5 seconds
+      // Wait for 3 seconds
+      ros::Rate(ros::Duration(3.0)).sleep();
+    }
+
+    if (param_sync_time)
+    {
+      NODELET_WARN("Syncing system and bebop time ...");
+      boost::posix_time::ptime system_time = ros::Time::now().toBoost();
+      std::string iso_time_str = boost::posix_time::to_iso_extended_string(system_time);
+      bebop_ptr_->SetDate(iso_time_str);
+      NODELET_INFO_STREAM("Current time: " << iso_time_str);
       ros::Rate(ros::Duration(3.0)).sleep();
     }
 
@@ -132,6 +144,7 @@ void BebopDriverNodelet::onInit()
   stop_autoflight_sub_ = nh.subscribe("autoflight/stop", 1, &BebopDriverNodelet::StopAutonomousFlightCallback, this);
   animation_sub_ = nh.subscribe("flip", 1, &BebopDriverNodelet::FlipAnimationCallback, this);
   snapshot_sub_ = nh.subscribe("snapshot", 10, &BebopDriverNodelet::TakeSnapshotCallback, this);
+  exposure_sub_ = nh.subscribe("set_exposure", 10, &BebopDriverNodelet::SetExposureCallback, this);
   toggle_recording_sub_ = nh.subscribe("record", 10, &BebopDriverNodelet::ToggleRecordingCallback, this);
 
   odom_pub_ = nh.advertise<nav_msgs::Odometry>("odom", 30);
@@ -379,6 +392,19 @@ void BebopDriverNodelet::TakeSnapshotCallback(const std_msgs::EmptyConstPtr &emp
   }
 }
 
+void BebopDriverNodelet::SetExposureCallback(const std_msgs::Float32ConstPtr& exposure_ptr)
+{
+  try
+  {
+    ROS_INFO("Setting exposure to %f", exposure_ptr->data);
+    bebop_ptr_->SetExposure(exposure_ptr->data);
+  }
+  catch (const std::runtime_error& e)
+  {
+    ROS_ERROR_STREAM(e.what());
+  }
+}
+
 void BebopDriverNodelet::ToggleRecordingCallback(const std_msgs::BoolConstPtr &toggle_ptr)
 {
   const bool& start_record = toggle_ptr->data;

diff --git a/docs/configuration.rst b/docs/configuration.rst
@@ -19,6 +19,11 @@ Sets the IP addres of the Bebop. The default value is ``192.168.42.1``.
 
 Setting this parameter to ``true`` will reset all Bebop configurations to factory defaults. Default value is ``false``.
 
+~sync_time
+----------
+
+Setting this parameter to ``true`` will synchronize drone time with your ROS system time. Default value is ``false``. 
+
 ~camera_info_url
 ----------------
 

diff --git a/docs/piloting.rst b/docs/piloting.rst
@@ -174,6 +174,13 @@ To take a high resolution on-board snapshot, publish a ``std_msgs/Empty`` messag
 * Username: ``anonymous``
 * Password: *<no password>*
 
+Set camera exposure
+===================
+
+It is possible to set camera exposure by publishing ``std_msgs/Float32`` message on ``set_exposure`` topic. Note that this functionality is not supported in Bebop1 Fw 3.3.0.  
+
+* Exposure value range: ``-3.0 .. +3.0``
+
 Toggle on-board Video Recording
 ===============================