Costmap_2d does not subscribe to LaserScan or PointCloud topic

asked 2020-06-05 12:13:07 -0500

stefanoferaco
1 ●1 ●2 ●1

Hello everyone! I will illustrate the project: I would build a local 2D costmap from a .bag file that contains 3D LIDAR point clouds in the topic /velodyne_points . I would do this task without any further mapping stage (no gmapping or similar), but I want just to obtain a local 2D cost map with inflated areas around obstacles, by using the costmap_2D package. Thus, after having processed the raw point_cloud with some custom nodes, I can obtain a point cloud from the ground (topic: /filtered_points_no_ground) and a laser scan topic (topic: /laserscan). Both of the topic are published wrt to the velodyne tf that is connected to odom by declaring this command "rosrun tf static_transform_publisher 0 0 0 0 0 0 odom velodyne 100".

The problem is that costmap_2d does not subscibe to any of the two topics I would have to provide (laser scans or filtered point cloud). Therefore, I can obtain only a blank output as I do "rostopic echo /costmap/voxel_grid" after launching the costmap_2D node with: roslaunch costmap_2d example.launch .

I attach here the .yaml file and the .launch file for the task I have described.

example_params.yaml

costmap:
  global_frame: velodyne
  robot_base_frame: base_link
  rolling_window: true
  static_map: false
  resolution: 0.05
  publish_frequency: 0.1
  update_frequency: 0.1
  plugins:
  - {name: obstacle_layer,   type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer,   type: "costmap_2d::InflationLayer"}
footprint: [[0.5,0.5],[0.5,-0.5],[-0.5,-0.3],[-0.5,0.5]]
raytrace_range: 15.0
obstacle_range: 20.0
inflation_layer:
  enabled:              true
  cost_scaling_factor:  5.0  
  inflation_radius:     0.5  
obstacle_layer:
  enabled:              true
  max_obstacle_height:  1.5
  track_unknown_space:  false    
  obstacle_range: 25
  raytrace_range: 30.0
  origin_z: 0.0
  publish_voxel_map: false
observation_sources:  velodyne
velodyne: {sensor_frame: velodyne, data_type: PointCloud2, topic: filtered_points_no_ground, marking: true, clearing: true, expected_update_rate: 0.1, min_obstacle_height: 0.0, max_obstacle_height: 1.5}
(in the case I use the laser scans, I would write this:
observation_sources:  laser_scan_sensor
laser_scan_sensor: {sensor_frame: velodyne, data_type: LaserScan, topic: laserscan, marking: true, clearing: true, expected_update_rate: 0.1, min_obstacle_height: 0.0, max_obstacle_height: 1.5}
)

example.launch

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

  <!-- Publishes the voxel grid to rviz for display -->
  <node pkg="costmap_2d" type="costmap_2d_markers" name="voxel_visualizer">
    <remap from="voxel_grid" to="costmap/voxel_grid"/>
  </node>

  <!-- Run the costmap node -->
  <node name="costmap_node" pkg="costmap_2d" type="costmap_2d_node" >
    <rosparam file="$(find costmap_2d)/launch/example_params.yaml" command="load" ns="costmap" />
  </node>

</launch>

How can I solve this problem? I also tried to check with "rostopic info" on the subscription of the topics but laserscan topic or filtered_points_no_ground are not subscribed. Additional information: Hardware: nVidia Jetson Xavier computing platform; Ubuntu 18.04; ROS: Melodic

Thanks.

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2020-06-09 08:22:45 -0500

stefanoferaco
1 ●1 ●2 ●1

updated 2020-06-09 08:52:38 -0500

UPDATE: Using "use_simtime = true" as suggested by @Dragonslayer , I can actually have a published map, but it is completely blank. I am using the two files I attach below. Could someone please help me in understanding why I cannot obtain a 2D local costmap?

Launch file

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

  <!-- Publishes the voxel grid to rviz for display -->
  <node pkg="costmap_2d" type="costmap_2d_markers" name="voxel_visualizer">
    <remap from="voxel_grid" to="costmap/voxel_grid"/>
  </node>

  <!-- Run the costmap node -->
  <node name="costmap_node" pkg="costmap_2d" type="costmap_2d_node" >
    <rosparam file="$(find costmap_2d)/launch/example_params.yaml" command="load" ns="costmap" />
  </node>

</launch>

.yaml file

global_frame: velodyne
robot_base_frame: velodyne
footprint: [
    # tail
    [-0.25, -0.25],
    [-0.25, 0.25],
    # nose
    [0.25, 0.5],
    [0.25, 0.00],
    [0.25, -0.5]
    ]

transform_tolerance: 5
rolling_window: true

resolution: 0.05
publish_frequency: 1.0
update_frequency: 1.0

plugins:
    - {name: inflation,        type: "costmap_2d::InflationLayer"}
    - {name: obstacles,       type: "costmap_2d::ObstacleLayer"}

inflation:
  enabled:              true
  cost_scaling_factor:  5.0  
  inflation_radius:     0.5 

obstacles:
  enabled: true
  track_unkown_space: false
  observation_sources: velodyne
  velodyne: {
    observation_persistance: 0.5,
    sensor_frame: velodyne,
    data_type: PointCloud2,
    topic: velodyne_points,
    marking: true,
    clearing: true,
    expected_update_rate: 0.1,
    min_obstacle_height: 0.0,
    max_obstacle_height: 5
  }

Thank you.

edit flag offensive delete link

Comments

try adding "map_type: costmap" into the first line of the yaml. Is the pointcloud actually publishing?

Dragonslayer ( 2020-06-09 09:08:41 -0500 )edit

I only know costmap2d from the navigation stack, but think you should get a costmap from costmap2d for rviz without your "voxel_visualizer" node. What does rostpic list output when this all is running? Are you sure you try to visualize the correct map?

Dragonslayer ( 2020-06-09 10:53:13 -0500 )edit

@Dragonslayer I tried adding "map_type: costmap" in the beginning does not change anything. The point cloud is actually publishing.

I share here the pointcloud I am using (from University of Edinburgh): https://uoe-my.sharepoint.com/:u:/g/p...

Could you try to run your navigation stack (costmap_2d) node on this .bag file? Thank you indeed.

The topics I have by running "rostopic list" are these ones:

/all_points
/clock
/costmap/voxel_grid
/costmap_node/costmap/costmap
/costmap_node/costmap/costmap_updates
/costmap_node/costmap/footprint
/costmap_node/costmap/inflation/parameter_descriptions
/costmap_node/costmap/inflation/parameter_updates
/costmap_node/costmap/obstacles/parameter_descriptions
/costmap_node/costmap/obstacles/parameter_updates
/costmap_node/costmap/parameter_descriptions
/costmap_node/costmap/parameter_updates
/filtered_points_ground
/filtered_points_no_ground
/fix
/imu/data
/imu/mag

...others

stefanoferaco ( 2020-06-09 11:38:46 -0500 )edit

...continue topics:

/imu_data_str
/left/image_raw_color/compressed
/odom
/right/image_raw_color/compressed
/ros_can/state
/ros_can/state_str
/ros_can/wheel_speeds
/rosout
/rosout_agg
/tf
/tf_static
/velocity
/velodyne_points
/visualization_marker

If I try to echo /costmap/voxel_grid , it shows me that it is not computed. If I try to echo /costmap_node/costmap/costmap , it shows me a fully blank matrix (zeros everywhere).

Could you try with the provided dataset? Thanks a lot @Dragonslayer since this issue is driving me mad.

stefanoferaco ( 2020-06-09 11:41:41 -0500 )edit

What I would like to see is the now in use .yaml, did you ad "map_type: costmap"? Also your "global_frame: velodyne" might give you trouble, you dont get any transform errors? A rosrun tf view_frames print would be nice. Your voxel_grid isnt published because that whole node doesnt make sense to me at the moment. Launch without it. What node is publishing "all_points"? I find it suspicious that there seem to be more then one point_cloud source, the velodyne and the others. Sorry, at the moment Iam not setup to check the bag file.

Dragonslayer ( 2020-06-09 11:47:22 -0500 )edit

@Dragonslayer here it is my current .yaml : https://gofile.io/d/lzkzQT I dont' get any transform error and here it is the output of view_frames: https://gofile.io/d/NXbVVl

/all_points is published by the ground_plane_filter node and the topic is equivalent to /velodyne_points (actually it is ground points + no ground points). Trying to subscribe to /all_points does not solve the problem.

Thanks for your next reply.

stefanoferaco ( 2020-06-10 03:54:36 -0500 )edit

I dont see a map specifications in the yaml. Try adding (and replacing the datapoints with data that works for you) this below transform_tolerance parameter in your yaml.

static_map: false
rolling_window: true
width: 20.0
height: 20.0
resolution: 0.05
track_unknown_space: false
always_send_full_costmap: true

You could also try roswtf --all while running the bag and look for issues there, posting that roport would be nice. What is the velodyne actually outputing 3d pointcloud or 2d? Is it type pointcloud2 or just pointcloud? This is all running on the same computer? Can rviz display the pointcloud itself?

Dragonslayer ( 2020-06-10 10:00:17 -0500 )edit

@Dragonslayer I firstly left the default map specifications (10, 10, 0.05 for width, height and resolution), now I changed to (20, 20, 0.04) and I added the parameters you have suggested, but nothing new happened. I post here the last .yaml file. https://gofile.io/d/0Z8y5X The velodyne is actually publishing a 3D pointcloud and the topic type is PointCloud2. All is running in the same computer (nVidia Jetson Xavier). The Rviz can publish all the other topics, except from the ones related to costmap (it publishes a costmap, but it is actually blank). Now I post below the result of 'roswtf' in the navigation folder: https://gofile.io/d/maObWT Do you think that from this result I can solve the problem? Thanks.

stefanoferaco ( 2020-06-10 11:06:53 -0500 )edit

see more comments

answered 2020-06-06 09:34:47 -0500

Dragonslayer
574 ●18 ●78 ●33

I would suggest, in exampel_params.yaml, to indent observation_source and the lines below. yaml is format sensitive. observation_source is out of scope obstacle_layer.

edit flag offensive delete link

Comments

Hello @Dragonslayer, thanks for your reply. I have rewritten the .yaml file as you suggested but nothing has changed, so I tried setting use_sim_time to false (currently I am using a recorded .bag file), is this ok? Anyway, now while the node is running I can read a warning "Costmap2DROS transform timeout. Current time: 151XXX.XXX, global_pose stamp: 0.0000, tolerance 5.0" and nothing is published... How could I solve this? I am not able to use any odometry message from the .bag file. Thanks.

stefanoferaco ( 2020-06-08 16:10:11 -0500 )edit

You get the timeout because the timestamps of the bagfile dont match the blocktime(actual time), use_simtime = true, when using a bag file is indicated.

Dragonslayer ( 2020-06-09 08:50:22 -0500 )edit

Thanks. I added an update.

stefanoferaco ( 2020-06-09 08:52:57 -0500 )edit

I dont see the updated yaml file.

Dragonslayer ( 2020-06-09 09:40:08 -0500 )edit

add a comment

Costmap_2d does not subscribe to LaserScan or PointCloud topic

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Costmap_2d does not subscribe to LaserScan or PointCloud topic edit

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Costmap_2d does not subscribe to LaserScan or PointCloud topic