obstacle_distance.cpp

Go to the documentation of this file.

/*
 * Copyright 2017 Nuno Marques.
 *
 * 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.mdA
 */

#include <mavros/mavros_plugin.h>
#include <mavros/utils.h>

#include <sensor_msgs/LaserScan.h>

namespace mavros {
namespace extra_plugins {
static constexpr double RAD_TO_DEG = 180.0 / M_PI;
using mavlink::common::MAV_DISTANCE_SENSOR;

class ObstacleDistancePlugin : public plugin::PluginBase {
public:
        ObstacleDistancePlugin() : PluginBase(),
                obstacle_nh("~obstacle")
        { }

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

                std::string mav_frame;
                obstacle_nh.param<std::string>("mav_frame", mav_frame, "GLOBAL");
                frame = utils::mav_frame_from_str(mav_frame);

                obstacle_sub = obstacle_nh.subscribe("send", 10, &ObstacleDistancePlugin::obstacle_cb, this);
        }

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

private:
        ros::NodeHandle obstacle_nh;
        ros::Subscriber obstacle_sub;

        mavlink::common::MAV_FRAME frame;

        void obstacle_cb(const sensor_msgs::LaserScan::ConstPtr &req)
        {
                mavlink::common::msg::OBSTACLE_DISTANCE obstacle {};

                if (req->ranges.size() <= obstacle.distances.size()) {
                        // all distances from sensor will fit in obstacle distance message
                        for (int i = 0; i < req->ranges.size(); i++) {
                                float distance_cm = req->ranges[i] * 1e2;
                                if (std::isnan(distance_cm) || distance_cm >= UINT16_MAX || distance_cm < 0) {
                                        obstacle.distances[i] = UINT16_MAX;
                                } else {
                                        obstacle.distances[i] = static_cast<uint16_t>(distance_cm);
                                }
                        }
                        std::fill(obstacle.distances.begin() + req->ranges.size(), obstacle.distances.end(), UINT16_MAX);       

                        const float increment_deg = req->angle_increment * RAD_TO_DEG;
                        obstacle.increment = static_cast<uint8_t>(increment_deg + 0.5f);  
                        obstacle.increment_f = increment_deg;
                } else {
                        // all distances from sensor will not fit so we combine adjacent distances always taking the shortest distance
                        size_t scale_factor = ceil(double(req->ranges.size()) / obstacle.distances.size());
                        for (size_t i = 0; i < obstacle.distances.size(); i++) {
                                obstacle.distances[i] = UINT16_MAX;
                                for (size_t j = 0; j < scale_factor; j++) {
                                        size_t req_index = i * scale_factor + j;
                                        if (req_index < req->ranges.size()) {
                                                float distance_cm = req->ranges[req_index] * 1e2;
                                                if (!std::isnan(distance_cm) && distance_cm < UINT16_MAX && distance_cm > 0) {
                                                        obstacle.distances[i] = std::min(obstacle.distances[i], static_cast<uint16_t>(distance_cm));
                                                }
                                        }
                                }
                        }
                        obstacle.increment = ceil(req->angle_increment * RAD_TO_DEG * scale_factor);    
                }

                obstacle.time_usec = req->header.stamp.toNSec() / 1000;                                 
                obstacle.sensor_type = utils::enum_value(MAV_DISTANCE_SENSOR::LASER);   
                obstacle.min_distance = req->range_min * 1e2;                                                   
                obstacle.max_distance = req->range_max * 1e2;                                                   
                obstacle.frame = utils::enum_value(frame);
                // Assume angle_increment is positive and incoming message is in a FRD/NED frame
                obstacle.angle_offset = req->angle_min * RAD_TO_DEG;                                            

                ROS_DEBUG_STREAM_NAMED("obstacle_distance", "OBSDIST: sensor type: " << utils::to_string_enum<MAV_DISTANCE_SENSOR>(obstacle.sensor_type)
                                                                                     << std::endl << obstacle.to_yaml());

                UAS_FCU(m_uas)->send_message_ignore_drop(obstacle);
        }
};
}       // namespace extra_plugins
}       // namespace mavros

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