Camera IMU synchronisation support added

mavros/CMakeLists.txt

@@ -90,6 +90,7 @@ add_service_files(
   CommandBool.srv
   CommandHome.srv
   CommandTOL.srv
+  CommandTriggerControl.srv
   StreamRate.srv
   SetMode.srv
   FileOpen.srv

mavros/src/plugins/command.cpp

@@ -24,6 +24,7 @@
 #include <mavros/CommandBool.h>
 #include <mavros/CommandHome.h>
 #include <mavros/CommandTOL.h>
+#include <mavros/CommandTriggerControl.h>
 
 namespace mavplugin {
 class CommandTransaction {
@@ -67,6 +68,7 @@ class CommandPlugin : public MavRosPlugin {
 		takeoff_srv = cmd_nh.advertiseService("takeoff", &CommandPlugin::takeoff_cb, this);
 		land_srv = cmd_nh.advertiseService("land", &CommandPlugin::land_cb, this);
 		guided_srv = cmd_nh.advertiseService("guided_enable", &CommandPlugin::guided_cb, this);
+		trigger_srv = cmd_nh.advertiseService("trigger_control", &CommandPlugin::trigger_control_cb, this);
 	}
 
 	const message_map get_rx_handlers() {
@@ -87,6 +89,7 @@ class CommandPlugin : public MavRosPlugin {
 	ros::ServiceServer takeoff_srv;
 	ros::ServiceServer land_srv;
 	ros::ServiceServer guided_srv;
+	ros::ServiceServer trigger_srv;
 
 	bool use_comp_id_system_control;
 
@@ -316,6 +319,14 @@ class CommandPlugin : public MavRosPlugin {
 				0, 0, 0, 0, 0, 0,
 				res.success, res.result);
 	}
+
+        bool trigger_control_cb(mavros::CommandTriggerControl::Request &req,
+			mavros::CommandTriggerControl::Response &res) {
+		return send_command_long_and_wait(MAV_CMD_DO_TRIGGER_CONTROL, (req.trigger_enable)? 1.0 : 0.0,
+				req.integration_time,
+				0, 0, 0, 0, 0, 0,
+				res.success, res.result);
+	}
 };
 };	// namespace mavplugin
 

mavros/srv/CommandLong.srv

@@ -22,6 +22,7 @@ uint16 CMD_MISSION_START = 300
 uint16 CMD_COMPONENT_ARM_DISARM = 400
 uint16 CMD_START_RX_PAIR = 500
 uint16 CMD_REQUEST_AUTOPILOT_CAPABILITIES = 520
+uint16 CMD_DO_TRIGGER_CONTROL = 2003
 
 uint16 command
 uint8 confirmation

mavros/srv/CommandTriggerControl.srv

@@ -0,0 +1,7 @@
+# Type for controlling onboard camera trigerring system
+
+bool    trigger_enable		# Trigger on/off control
+float32 integration_time	# Shutter integration time. Zero to use current onboard value.
+---
+bool success
+uint8 result

mavros_extras/CMakeLists.txt

@@ -54,6 +54,7 @@ add_message_files(
   FILES
   OpticalFlowRad.msg
   Vibration.msg
+  CamIMUStamp.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -116,6 +117,7 @@ add_library(mavros_extras
   src/plugins/vision_speed_estimate.cpp
   src/plugins/visualization.cpp
   src/plugins/vibration.cpp
+  src/plugins/cam_imu_sync.cpp
 )
 add_dependencies(mavros_extras
   mavros_extras_generate_messages_cpp

mavros_extras/mavros_plugins.xml

@@ -23,4 +23,7 @@
 	<class name="vibration" type="mavplugin::VibrationPlugin" base_class_type="mavplugin::MavRosPlugin">
 		<description>Publish VIBRATION message data from FCU.</description>
 	</class>
+        <class name="cam_imu_sync" type="mavplugin::CamIMUSyncPlugin" base_class_type="mavplugin::MavRosPlugin">
+		<description>Publish camera trigger data for synchronisation of IMU and camera frames</description>
+	</class>
 </library>

mavros_extras/msg/CamIMUStamp.msg

@@ -0,0 +1,4 @@
+# IMU-Camera synchronisation data
+
+time frame_stamp		# Timestamp when the camera was triggered
+int32 frame_seq_id		# Sequence number of the image frame

mavros_extras/src/plugins/cam_imu_sync.cpp

@@ -0,0 +1,70 @@
+/**
+ * @brief Camera IMU synchronisation plugin
+ * @file cam_imu_sync.cpp
+ * @author Mohammed Kabir < mhkabir98@gmail.com >
+ *
+ * @addtogroup plugin
+ * @{
+ */
+/*
+ * Copyright 2015 Mohammed Kabir.
+ *
+ * This file is part of the mavros package and subject to the license terms
+ * in the top-level LICENSE file of the mavros repository.
+ * https://github.com/mavlink/mavros/tree/master/LICENSE.md
+ */
+
+#include <mavros/mavros_plugin.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <mavros_extras/CamIMUStamp.h>
+
+namespace mavplugin {
+/**
+ * @brief Camera IMU synchronisation plugin
+ *
+ * This plugin publishes a timestamp for when a external camera system was 
+ * triggered by the FCU. Sequence ID from the message and the image sequence from
+ * camera can be corellated to get the exact shutter trigger time.
+ */
+class CamIMUSyncPlugin : public MavRosPlugin {
+public:
+	CamIMUSyncPlugin() :
+		cam_imu_sync_nh("~cam_imu_sync"),
+		uas(nullptr)
+	{ };
+
+	void initialize(UAS &uas_)
+	{
+		uas = &uas_;
+
+		cam_imu_pub = cam_imu_sync_nh.advertise<mavros_extras::CamIMUStamp>("cam_imu_stamp", 10);
+	}
+
+	const message_map get_rx_handlers() {
+		return {
+			       MESSAGE_HANDLER(MAVLINK_MSG_ID_CAMERA_TRIGGER, &CamIMUSyncPlugin::handle_cam_trig)
+		};
+	}
+
+private:
+	ros::NodeHandle cam_imu_sync_nh;
+	UAS *uas;
+
+	ros::Publisher cam_imu_pub;
+
+	void handle_cam_trig(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
+		mavlink_camera_trigger_t ctrig;
+		mavlink_msg_camera_trigger_decode(msg, &ctrig);
+
+		auto sync_msg = boost::make_shared<mavros_extras::CamIMUStamp>();
+
+		sync_msg->frame_stamp = uas->synchronise_stamp(ctrig.time_usec);
+		sync_msg->frame_seq_id = ctrig.seq;
+
+		cam_imu_pub.publish(sync_msg);
+	}
+};
+};	// namespace mavplugin
+
+PLUGINLIB_EXPORT_CLASS(mavplugin::CamIMUSyncPlugin, mavplugin::MavRosPlugin)