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
                        const float scale_factor = 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 = floor(i * scale_factor + j);
                                        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));
                                        }
                                }
                        }
                        const float increment_deg = req->angle_increment * RAD_TO_DEG * scale_factor;
                        obstacle.increment = static_cast<uint8_t>(increment_deg + 0.5f);        
                        obstacle.increment_f = increment_deg;
 
                }
 
                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.hpp>
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::ObstacleDistancePlugin, mavros::plugin::PluginBase)