camera.cpp

Go to the documentation of this file.

 
/*
 * Copyright 2021 Jaeyoung Lim.
 *
 * 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 <mavros_msgs/CameraImageCaptured.h>
 
namespace mavros {
namespace extra_plugins {
 
using mavlink::common::MAV_CMD;
using utils::enum_value;
 
class CameraPlugin : public plugin::PluginBase {
public:
        CameraPlugin() : PluginBase(),
        nh("~"),
        camera_nh("~camera")
        { }
 
        void initialize(UAS &uas_) override
        {
                PluginBase::initialize(uas_);
 
                camera_image_captured_pub = camera_nh.advertise<mavros_msgs::CameraImageCaptured>("image_captured", 10);
 
        }
 
        Subscriptions get_subscriptions() override
        {
                return {
                        make_handler(&CameraPlugin::handle_camera_image_captured)
                };
        }
 
private:
        ros::NodeHandle nh;
        ros::NodeHandle camera_nh;
        ros::Publisher camera_image_captured_pub;
 
        void handle_camera_image_captured(const mavlink::mavlink_message_t *msg, mavlink::common::msg::CAMERA_IMAGE_CAPTURED &mo)
        {
                auto ic = boost::make_shared<mavros_msgs::CameraImageCaptured>();
 
                ic->header.stamp = m_uas->synchronise_stamp(mo.time_boot_ms);
                ic->geo.latitude = mo.lat/ 1E7;
                ic->geo.longitude = mo.lon / 1E7;               // deg
                ic->geo.altitude = mo.alt / 1E3 + m_uas->geoid_to_ellipsoid_height(&ic->geo);   // in meters
                ic->relative_alt = mo.relative_alt / 1E3;
                auto q = ftf::mavlink_to_quaternion(mo.q);
                tf::quaternionEigenToMsg(q, ic->orientation);
                ic->image_index = mo.image_index;
                ic->capture_result = mo.capture_result;
                ic->file_url = mavlink::to_string(mo.file_url);
 
                camera_image_captured_pub.publish(ic);
        }
 
};
}       // namespace extra_plugins
}       // namespace mavros
 
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::CameraPlugin, mavros::plugin::PluginBase)