Visualization of the maximum distance point in rviz

edit

Hello

I create and build own node that calculate the maximal distance of the obstacle (based on laser data). I would like to visualize that point in rviz and distinguished with some colour(for example green). This is my launch file

<launch>
<param name="/use_sim_time" value="true"/>

<node name="rosplay" pkg="rosbag" type="play" args=" /Data/p1-20.bag --loop  --clock"/>

<node pkg="tf" type="static_transform_publisher" name="baselink_laser" args="0 0 0 0 0 0 /base_link /laser 10"/>
<node pkg="tf" type="static_transform_publisher" name="laser_imu" args="0 0 0 0 0 0 /laser /base_imu 10"/>
<node pkg="tf" type="static_transform_publisher" name="baselink_camera" args="0 0 0 0 0 0 /base_link /camera 10"/>

<!-- Start the map server node and specify the map file (*.pgm) and the map resolution in metres/pixel -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl_listener)/maps/pow_real_time.yaml"         output="screen"/>

<!--Start the Laser_scan_matcher package, to provide odometry from laser data (ICP)-->
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node" 
    name="laser_scan_matcher_node" output="screen">
    <param name="use_alpha_beta" value="true"/>
    <param name="max_iterations" value="10"/>
</node>

<node pkg="amcl" type="amcl" name="amcl" respawn="true" output="screen">
    <param name="base_frame_id" value="base_link"/>
    <param name="odom_frame_id" value="world"/>
    <param name="global_frame_id" value="map"/>
    <param name="update_min_d" value="0.09"/>   
    <param name="update_min_a" value="0.09"/> 
    <param name="initial_pose_x" value="-1"/>
    <param name="initial_pose_y" value="0"/>
    <param name="initial_pose_a" value="-0.1"/>
    <param name="min_particles" value="6000"/> 
    <param name="max_particles" value="8000"/>
    <param name="odom_model_type" value="diff"/>
    <param name="kld_err" value="0.1"/> 
    <param name="resample_interval" value="1"/>
    <param name="odom_alpha1" value="0.2"/>   
    <param name="odom_alpha2" value="0.2"/>
    <param name="odom_alpha3" value="0.5"/>
    <param name="odom_alpha4" value="0.5"/>
    <param name="laser_max_beams" value="30"/>
</node>

<!--Start the "analyzer_packages"-->
<!-- acml_listener broadcasts markers for the position of the wheelchair, to visualize in Rviz-->
<node pkg="amcl_listener" type="amcl_pose_listener" name="amcl_listener" output="screen"/>
<!-- Object node records minimal ranges from laser, speed_node will record travel speed of wheelchair -->
<!--node pkg="pow_analyzer" type="object_node" name="object_node" output="screen"/> -->
<node pkg="pow_analyzer" type="max_range_node" name="max_range_node" output="screen"/>
<!--node pkg="pow_analyzer" type="speed_node" name="speed_node" output="screen"/> -->

<!-- Start an rviz node with a custom configuration for the viewpoint, map_server, trajectory, laser scans, etc -->
<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find pow_analyzer)/launch/pow_rviz.vcg"/> 

</launch>

This is the source code of the max_range_node

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/LaserScan.h"
#include <vector>

sensor_msgs::LaserScan laser_scan;
float min_range;
float max_range;

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{

    std::vector<float> laser;
    laser = msg->ranges;

    int size_laser = laser.size();
    for (int i=0;i<size_laser;i++){
        if (laser[i] < 0.01){
            laser[i] = 99999;
        }
        if (laser[i] > 45){
            laser[i] = 99999;
        }
    }

    min_range = 2; //Init closest point 
    max_range = 1; // Init maximal distance point
    int index_min; 
    int index_max; 

//find the closest point to the obstacle and the maximal distance to the //obstacle


    for (int i=0;i<size_laser;i++){
        for (int j=0;j<size_laser;j++){

        if (laser[i] < min_range ...