Visualization of the maximum distance point in rviz
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 ...
What is max_range_node doing and how does it provide its data? What's the output of
rosnode info max_range_node
? By just seeing the launch file, it is hard to infer what all your nodes are doing without even having the documentation/sourcecode of these nodes.Ok. Just add the source code of the node. Max range just calculate the max distance of the obstacle of the laser range scan data,.
I just want to mark (with some colour, for example green) this Maximum range, which is max_range calculated in the node I just post it.
I just the max_range_node visualize. The other nodes are at the moment not important. Ok?
Your code seems quite obscure. Are you sure it does what you want? You walk over the laser in nested loops. If you just want to get the max/min that is wrong.
Ok. yes than its wrong. What should I do to get max/min?
Well, usually it's: max=0; for(uint i ... laser.range) if(laser.range[i] > max: max=laser.range[i]