Sbpl_lattice_planner with teb_local_planner in Gazebo simulation.

edit

Hello,

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.

<!--  -->
<launch>
<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" />
</node>

<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>

<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)"/>
</include>

<!-- 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)"/>
</include>

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

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

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 ...