range.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

/*---------------------------------------------------------------
        APPLICATION: mrpt_ros bridge
        FILE: range.cpp
        AUTHOR: Raghavender Sahdev <raghavendersahdev@gmail.com>
  ---------------------------------------------------------------*/
#include "mrpt_bridge/range.h"
#include <iostream>
using namespace std;

namespace mrpt_bridge
{
namespace range
{
/************************************************************************
 *                                              ros2mrpt                                                                *
 ************************************************************************/
bool ros2mrpt(const sensor_msgs::Range& msg, CObservationRange& obj)
{
        obj.minSensorDistance = msg.min_range;
        obj.maxSensorDistance = msg.max_range;
        obj.sensorConeApperture = msg.field_of_view;

        obj.sensedData.at(0).sensedDistance = msg.range;
        return true;
}

/************************************************************************
 *                                              mrpt2ros                                                                *
 ************************************************************************/
bool mrpt2ros(
        const CObservationRange& obj, const std_msgs::Header& msg_header,
        sensor_msgs::Range* msg)
{
        long num_range = obj.sensedData.size();

        // 1) sensor_msgs::Range:: header
        for (int i = 0; i < num_range; i++) msg[i].header = msg_header;

        // 2) sensor_msg::Range parameters
        for (int i = 0; i < num_range; i++)
        {
                msg[i].max_range = obj.maxSensorDistance;
                msg[i].min_range = obj.minSensorDistance;
                msg[i].field_of_view = obj.sensorConeApperture;
        }

        for (int i = 0; i < num_range; i++)
                msg[i].range = obj.sensedData.at(i).sensedDistance;

        // msg.radiation_type
        return true;
}
}  // namespace range
}  // namespace mrpt_bridge

/*
uint8 ULTRASOUND=0
uint8 INFRARED=1
std_msgs/Header header
uint8 radiation_type
float32 field_of_view
float32 min_range
float32 max_range
float32 range
*/