companion_process_status.cpp

Go to the documentation of this file.

/*
 * Copyright 2018 Tanja Baumann.
 *
 * 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/CompanionProcessStatus.h>

namespace mavros {
namespace extra_plugins {
using mavlink::minimal::MAV_TYPE;
using mavlink::minimal::MAV_STATE;
using mavlink::minimal::MAV_COMPONENT;
using mavlink::minimal::MAV_AUTOPILOT;
using mavlink::minimal::MAV_MODE_FLAG;
using utils::enum_value;

class CompanionProcessStatusPlugin : public plugin::PluginBase {
public:
        CompanionProcessStatusPlugin() : PluginBase(),
                status_nh("~companion_process")
        { }

        void initialize(UAS &uas_) override
        {
                PluginBase::initialize(uas_);

                status_sub = status_nh.subscribe("status", 10, &CompanionProcessStatusPlugin::status_cb, this);
        }

        Subscriptions get_subscriptions() override
        {
                return { /* Rx disabled */ };
        }

private:
        ros::NodeHandle status_nh;
        ros::Subscriber status_sub;

        void status_cb(const mavros_msgs::CompanionProcessStatus::ConstPtr &req)
        {
                mavlink::minimal::msg::HEARTBEAT heartbeat {};

                heartbeat.type = enum_value(MAV_TYPE::ONBOARD_CONTROLLER);
                heartbeat.autopilot = enum_value(MAV_AUTOPILOT::PX4);
                heartbeat.base_mode     = enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED);
                heartbeat.system_status = req->state;   //enum="MAV_STATE" System status flag

                ROS_DEBUG_STREAM_NAMED("companion_process_status", "companion process component id: " <<
                                utils::to_string_enum<MAV_COMPONENT>(req->component) << " companion process status: " <<
                                utils::to_string_enum<MAV_STATE>(heartbeat.system_status) << std::endl << heartbeat.to_yaml());

                UAS_FCU(m_uas)->send_message_ignore_drop(heartbeat, req->component);
        }
};
}       // namespace extra_plugins
}       // namespace mavros

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::CompanionProcessStatusPlugin, mavros::plugin::PluginBase)