rangefinder.cpp

Go to the documentation of this file.

 
/*
 * Copyright 2016 Ardupilot.
 *
 * 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 <sensor_msgs/Range.h>
 
namespace mavros {
namespace extra_plugins {
class RangefinderPlugin : public plugin::PluginBase {
public:
        RangefinderPlugin() : PluginBase(),
                rangefinder_nh("~rangefinder")
        { }
 
        void initialize(UAS &uas_) override
        {
                PluginBase::initialize(uas_);
 
                rangefinder_pub = rangefinder_nh.advertise<sensor_msgs::Range>("rangefinder", 10);
        }
 
        Subscriptions get_subscriptions() override
        {
                return {
                        make_handler(&RangefinderPlugin::handle_rangefinder)
                };
        }
 
private:
        ros::NodeHandle rangefinder_nh;
 
        ros::Publisher rangefinder_pub;
 
        void handle_rangefinder(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::RANGEFINDER &rangefinder) {
                auto rangefinder_msg = boost::make_shared<sensor_msgs::Range>();
                rangefinder_msg->header.stamp = ros::Time::now();
                rangefinder_msg->header.frame_id = "/rangefinder";
                rangefinder_msg->radiation_type = sensor_msgs::Range::INFRARED;
                rangefinder_msg->field_of_view = 0;
                rangefinder_msg->min_range = 0;
                rangefinder_msg->max_range = 1000;
                rangefinder_msg->range = rangefinder.distance;
 
                rangefinder_pub.publish(rangefinder_msg);
        }
};
}       // namespace extra_plugins
}       // namespace mavros
 
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::RangefinderPlugin, mavros::plugin::PluginBase)