cam_imu_sync.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2015 Mohammed Kabir.
00011  *
00012  * This file is part of the mavros package and subject to the license terms
00013  * in the top-level LICENSE file of the mavros repository.
00014  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00015  */
00016 
00017 #include <mavros/mavros_plugin.h>
00018 #include <pluginlib/class_list_macros.h>
00019 
00020 #include <mavros_msgs/CamIMUStamp.h>
00021 
00022 namespace mavplugin {
00030 class CamIMUSyncPlugin : public MavRosPlugin {
00031 public:
00032         CamIMUSyncPlugin() :
00033                 cam_imu_sync_nh("~cam_imu_sync"),
00034                 uas(nullptr)
00035         { };
00036 
00037         void initialize(UAS &uas_)
00038         {
00039                 uas = &uas_;
00040 
00041                 cam_imu_pub = cam_imu_sync_nh.advertise<mavros_msgs::CamIMUStamp>("cam_imu_stamp", 10);
00042         }
00043 
00044         const message_map get_rx_handlers() {
00045                 return {
00046                                MESSAGE_HANDLER(MAVLINK_MSG_ID_CAMERA_TRIGGER, &CamIMUSyncPlugin::handle_cam_trig)
00047                 };
00048         }
00049 
00050 private:
00051         ros::NodeHandle cam_imu_sync_nh;
00052         UAS *uas;
00053 
00054         ros::Publisher cam_imu_pub;
00055 
00056         void handle_cam_trig(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00057                 mavlink_camera_trigger_t ctrig;
00058                 mavlink_msg_camera_trigger_decode(msg, &ctrig);
00059 
00060                 auto sync_msg = boost::make_shared<mavros_msgs::CamIMUStamp>();
00061 
00062                 sync_msg->frame_stamp = uas->synchronise_stamp(ctrig.time_usec);
00063                 sync_msg->frame_seq_id = ctrig.seq;
00064 
00065                 cam_imu_pub.publish(sync_msg);
00066         }
00067 };
00068 };      // namespace mavplugin
00069 
00070 PLUGINLIB_EXPORT_CLASS(mavplugin::CamIMUSyncPlugin, mavplugin::MavRosPlugin)


mavros_extras
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:27