Costmap converter visualization - teb_local_planner

asked 2016-05-11 05:50:56 -0600

schultza
232 ●23 ●31 ●40

updated 2016-05-20 07:25:20 -0600

Hi,

I am using the teb_local_planner for my ackermann robot at the moment. After noticing that my performane is pretty bad (like 1Hz of control update) I wanted to try the costmap converter plugin with the planner. As someone mentioned without the converter the obstacles in the costmap are treated as a single point which leads to a huge amount of distance calculation.

The problem I have is, I don't know how to visualize the output of the costmap_converter plugin.

I followed this tutorial and when running my robot specific move_base launch file it says it loaded the plugin for example like here in the output of the launch file:

[ INFO] [1462963729.309633367, 443.699000000]: Costmap conversion plugin costmap_converter::CostmapToPolygonsDBSConcaveHull loaded.

How do I visualize the created conversions like polygons as seen in the package description of the costmap_converter plugin here?

EDIT

After adding Marker Display type in Rviz this was the output:

image description

The loaded converter was following with following parameters:

[ INFO] [1463730376.455696206, 722.323000000]: Costmap conversion plugin costmap_converter::CostmapToPolygonsDBSMCCH loaded.

the used .yaml file and launch file:

costmap_converter_params.yaml

TebLocalPlannerROS:
 costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5
 costmap_converter/CostmapToPolygonsDBSMCCH:
    cluster_max_distance: 0.4
    cluster_min_pts: 2
    cluster_max_pts: 30
    convex_hull_min_pt_separation: 0.1

move_base.launch

<?xml version="1.0"?>
<launch>
    <master auto="start"/>
    <arg name="map_file" default="$(find my_maps)/random_test.yaml"/>
    <!--<arg name="map_file" default="$(find my_maps)/first_map6.yaml"/>-->
    <!-- Run the map server -->
    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)"/>

    <!--- Run AMCL -->
    <!--- We load ACML here with diff=true to support our differential drive robot -->
    <include file="$(find robot_2d_nav)/launch/amcl_example.launch" />

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find robot_2d_nav)/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find robot_2d_nav)/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find robot_2d_nav)/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find robot_2d_nav)/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find robot_2d_nav)/base_local_planner_params.yaml" command="load" />

        <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"/>    
        <param name="base_global_planer" value="global_planner/GlobalPlanner"/>
        <param name="controller_frequency" value="10.0" />

        <!-- LOAD COSTMAP_CONVERTER PARAMETERS HERE -->
        <rosparam file="$(find robot_2d_nav)/costmap_converter_params.yaml" command="load" />
    </node>

    <node pkg="teb_local_planner" type="cmd_vel_to_ackermann_drive.py" name="cmd_vel_to_ackermann"> 
        <param name="wheelbase" value="1.811" />
    </node>
</launch>

Hi Christoph, I will change the resolution on monday and try it again, the current resolution is 0.5 so you say i should go to 0.1 or something in this area? Or should i change cluster_max_distance?

local_costmap_params.yaml

local_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  publish_voxel_map: true
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 10.0
  height: 10.0
  resolution: 0.5

If I have enough spare time I will push everything to my personal git and give you access to it also on Monday, so you can check the simulation (see below) and the planner and converter.

The specs are following: Laptop #1 (Just for gazebo simulation)

i7-4712MQ
16GB RAM
Nvidia 860GTX
512GB SSD

Laptop #2 (running the planner and converter)

i5-5300U
8GB RAM
Onboard ...

(more)

edit retag flag offensive close merge delete

add a comment

answered 2016-05-11 06:23:45 -0600

croesmann
2531 ●21 ●36 ●38

updated 2016-05-20 06:48:03 -0600

Hi schultza,

the extracted obstacles are published as Marker type from the teb_local_planner on topic teb_markers. Just add the marker display to your rviz configuration.

Note, the conversion algorithms are still experimental. I implemented them "quickly" in spare-time. So if you observe some issues or if you have some suggestions or better default parameter sets, please let me know :-). Also I need to perform some benchmarks in the future between the different algorithms.

According to your console output, the converter is indeed loaded properly. You can tune some of the parameters using rqt_reconfigure. By the way, better start with a convex hull or line algorithm since calculating concave hulls takes much more time and is often not required.

1 Hz is indeed slow. I recently added a FAQ section to the tutorials with some hints about parameters and their influences on computation time: Link

EDIT #1:

After trying to reproduce your parameter setting I recognized, that the planner wasn't loading the correct parameter namespace. Instead of namespace /.../TebLocalPlannerROS/costmap_converter/CostmapTo* it tried to access /.../TebLocalPlannerROS/CostmapTo*. Consequently, your parameters (even in rqt_reconfigure) did not changed anything. I fixed that in the source code and it is included in the new release I am going to push later today).

Regarding your rviz output: Each occupied cell (lethal obstacle, yellow cell) is transformed into a single point. No conversions are performed at all. The reason is that you are using a low costmap resolution (large resolution value >0.5?), but I configured the default values for resolution of 0.1 meters. Note, the default value of parameter cluster_max_distance is too low. Try to change all parameters with rqt_reconfigure first. Maybe I can change the default parameters such that they scale better with the resolution of the costmap in the future. However, in my opinion your resolution is way too high. The costmap raytrace algorithm marks a lot of free space in your map as obstacles. This could also make navigating through narrow doors nearly impossible. Additionally, after some tests with the converter and a resolution of 0.5, I am not really satisfied with the results of the current implementation.

I am really interested in your computer specs, if the planner runs with just 1 Hz for you?

Comments

Hi, thanks for your answer, tried everything you mentioned but it doesnt work as expected. Please see Edit #1 in my question :)

schultza ( 2016-05-20 02:51:02 -0600 )edit

Now its getting big, see edit #2 :) THANKS!!!!!!

schultza ( 2016-05-20 07:25:39 -0600 )edit

I think, that your computer specs are completely sufficient to run the planner smoothly without issues ;-) However, some small parameter change scan heavily affect the performance.

croesmann ( 2016-05-20 07:48:31 -0600 )edit

yeah, better reduce the resolution to 0.1. But I think, the size of your local costmap is too large. Of course, it depends on the size of your robot. But for common mobile robots (not cars) 5-6meters for width and height should be quite sufficient even without any costmap conversion.

croesmann ( 2016-05-20 07:51:35 -0600 )edit

Oh, ok, your wheelbase is approx. 1.8m: so probably a larger robot ;) do not forget to define a footprint model for optimization (e.g. Line model). And I often recommend to set the planner_frequency of the global plan to a small value in order to refine the global plan slowly.

croesmann ( 2016-05-20 10:37:15 -0600 )edit

Did everything and it works now pretty good :) Having some more questions but will I think I should open new threads for this :) THanks so far, sorry for the delayed answer, marked your answer. And yes the robot is a car ;)

schultza ( 2016-06-08 09:24:04 -0600 )edit

@schultza Hi, it seems you have a good understanding of the costmap_converter. I am having alot of trouble trying to figure out how it works. could you please spare the time to look at my question about it?

distro ( 2022-01-14 01:36:18 -0600 )edit

add a comment

Costmap converter visualization - teb_local_planner

1 Answer

Comments

Question Tools

Stats

Related questions

Costmap converter visualization - teb_local_planner edit

1 Answer

Comments

Question Tools

Stats

Related questions

Costmap converter visualization - teb_local_planner