Sbpl_lattice_planner with teb_local_planner in Gazebo simulation.

asked 2016-08-29 08:55:10 -0600

Karthikeyan gravatar image

updated 2016-08-31 04:48:58 -0600


I'm using the sbpl_lattice as my global planner and teb_local_planner as my local planner in my navigation stack. I'm trying to run and visualize them in Gazebo Simulation environment and with Rviz as well. I created a launch file for that. Im posting it for reference.

<!--  -->
<include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>
<node ns="local_costmap" name="voxel_grid_throttle" pkg="topic_tools" type="throttle" args="messages voxel_grid 3.0 voxel_grid_throttled"/>
<node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">

<param name="footprint_padding" value="0.01" />
<param name="controller_frequency" value="10.0" />
<param name="controller_patience" value="100.0" />

<param name="base_global_planner" value="SBPLLatticePlanner" />
<param name="SBPLLatticePlanner/primitive_filename" value="$(find sbpl)/matlab/mprim/rover.mprim" />
<rosparam file="$(find sbpl_lattice_planner)/launch/sbpl_global_params.yaml" command="load" />

<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
<param name="controller_frequency" value="2.0" />
<param name="controller_patience" value="15.0" />
<rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/teb_local_planner_params.yaml" command="load" />

<rosparam file="$(find sbpl_lattice_planner)/launch/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find sbpl_lattice_planner)/launch/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find sbpl_lattice_planner)/launch/local_costmap_params_close.yaml" command="load" />
<rosparam file="$(find sbpl_lattice_planner)/launch/global_costmap_params.yaml" command="load" />
<rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/costmap_converter_params.yaml" command="load" />

<arg name="world_file"  default="$(env TURTLEBOT_GAZEBO_WORLD_FILE)"/>
<arg name="base"      value="$(optenv TURTLEBOT_BASE kobuki)"/>                     <!-- create, roomba -->
<arg name="battery"   value="$(optenv TURTLEBOT_BATTERY /proc/acpi/battery/BAT0)"/>  <!-- /proc/acpi/battery/BAT0 -->  
<arg name="gui" default="true"/>
<arg name="stacks"    value="$(optenv TURTLEBOT_STACKS hexagons)"/>  <!-- circles, hexagons --> 
<arg name="3d_sensor" value="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>  <!-- kinect, asus_xtion_pro --> 

<!-- Map server -->
<arg name="map_file" default="$(find  sbpl_lattice_planner)/launch/willow-full.pgm 0.1"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="use_sim_time" value="true"/>
<arg name="debug" value="false"/>
<arg name="gui" value="$(arg gui)" />
<arg name="world_name" value="$(arg world_file)"/>

<include file="$(find turtlebot_gazebo)/launch/includes/$(arg base).launch.xml">
<arg name="base" value="$(arg base)"/>
<arg name="stacks" value="$(arg stacks)"/>
<arg name="3d_sensor" value="$(arg 3d_sensor)"/>

<!-- Localization -->
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
<arg name="initial_pose_a" default="0.0"/>
<include file="$(find turtlebot_navigation)/launch/includes/amcl.launch.xml">
<arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
<arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
<arg name="initial_pose_a" value="$(arg initial_pose_a)"/>

<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="30.0" />

<!-- Launch Rviz -->
<include file="$(find turtlebot_rviz_launchers)/launch/view_navigation.launch"></include>

The problem i get in Rviz is I can't see the robot and and in the terminal I get something like this

[ WARN] [1472478371.694079060, 229.410000000]: The /hokuyo_scan observation buffer has not been updated for 223.84 seconds, and it should be updated every 5.00 seconds.
[ WARN] [1472478371.896180928, 229.610000000]: The /hokuyo_scan observation buffer has not been updated for 224.04 seconds, and it should be updated every 5.00 seconds.
[ WARN] [1472478372.093611715, 229.810000000]: The /hokuyo_scan observation buffer has ...
edit retag flag offensive close merge delete


The error seems to be about the laser scan, nothing about Sbpl_lattice_planner or teb_local_planner. Have you checked if gazebo is publishing /hokuyo_scan? Can you visualize /hokuyo_scan in rviz?

DavidN gravatar image DavidN  ( 2016-08-29 21:58:21 -0600 )edit

Can you please check the image which, I have linked with the answer, that's how my RViz looked. Is there any change to be done in the launch file?

Karthikeyan gravatar image Karthikeyan  ( 2016-08-30 03:08:32 -0600 )edit

In rviz, you can add laser scan topic visualizer. Then you can change the topic name of that visualizer to /hokuyo_scan. Also, you can run rostopic echo /hokuyo_scan to see if there is any data on the topic

DavidN gravatar image DavidN  ( 2016-08-30 04:10:04 -0600 )edit

no there isn't any data being published, I get this warning when i tried the rostopic command

WARNING: no messages received and simulated time is active.
Is /clock being published?
Karthikeyan gravatar image Karthikeyan  ( 2016-08-30 04:36:13 -0600 )edit

I have added config file costmap_common_params.yaml for the reference.

Karthikeyan gravatar image Karthikeyan  ( 2016-08-30 05:53:25 -0600 )edit

According to your config file, the costmap needs /hokuyo_scan. But there is no such topic available. Have you properly set up your robot in Gazebo? Your robot in gazebo should publish some laser topic, I guess. Please also post the output of "rosnode info /gazebo"

DavidN gravatar image DavidN  ( 2016-08-30 20:48:48 -0600 )edit

Hi, I have added my output for the command rosnode list /gazebo command.

Karthikeyan gravatar image Karthikeyan  ( 2016-08-31 04:50:19 -0600 )edit

There is no laser scan published from gazebo. Do you have a laser to your robot model in gazebo? Or do you plan to use different type of sensor for navigation?

DavidN gravatar image DavidN  ( 2016-09-01 00:24:02 -0600 )edit