Sbpl_lattice_planner with teb_local_planner in Gazebo simulation.
Hello,
I'm using the sbpllattice as my global planner and teblocal_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 not been updated for 224.24 seconds, and it should be updated every 5.00 seconds.
[ WARN] [1472478427.884417626, 285.510000000]: Timed out waiting for transform from base_footprint to map to become available before running costmap, tf error: . canTransform returned after 0.1 timeout was 0.1.
Can somebody point me out where have I gone wrong ?
EDIT #1: I'm adding the config file costmapcommonparams.yaml
#START VOXEL STUFF
map_type: voxel
origin_z: 0.0
z_resolution: 0.2
z_voxels: 10
unknown_threshold: 9
mark_threshold: 0
#END VOXEL STUFF
transform_tolerance: 0.3
obstacle_range: 2.5
max_obstacle_height: 2.0
raytrace_range: 3.0
footprint: [[-0.13, -0.20], [-0.13, 0.20], [0.20, 0.20], [0.20, -0.2]]
#robot_radius: 0.46
footprint_padding: 0.01
inflation_radius: 0.55
cost_scaling_factor: 10.0
lethal_cost_threshold: 100
observation_sources: base_scan
base_scan: {sensor_frame: hokuyo_link,
data_type: LaserScan,
topic: /hokuyo_scan,
marking: true,
clearing: true,
expected_update_rate: 5.0,
observation_persistence: 0.0,
min_obstacle_height: -0.10,
max_obstacle_height: 2.0}
#observation_sources: hokuyo_scan
#hokuyo_scan: {data_type: LaserScan, expected_update_rate: 0.4,
# observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}
EDIT #2,
I had run the command rosnode info /gazebo and the output was
Node [/gazebo]
Publications:
* /camera/rgb/camera_info [sensor_msgs/CameraInfo]
* /camera/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
* /camera/rgb/image_raw/compressed/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
* /camera/rgb/image_raw/compressedDepth [sensor_msgs/CompressedImage]
* /camera/rgb/image_raw/compressedDepth/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
* /gazebo/model_states [gazebo_msgs/ModelStates]
* /camera/depth/points [sensor_msgs/PointCloud2]
* /camera/rgb/image_raw/compressedDepth/parameter_updates [dynamic_reconfigure/Config]
* /camera/rgb/image_raw/compressed [sensor_msgs/CompressedImage]
* /gazebo/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
* /mobile_base/events/bumper [kobuki_msgs/BumperEvent]
* /mobile_base/sensors/imu_data [sensor_msgs/Imu]
* /camera/rgb/image_raw [sensor_msgs/Image]
* /tf [tf2_msgs/TFMessage]
* /camera/rgb/image_raw/compressed/parameter_updates [dynamic_reconfigure/Config]
* /camera/parameter_updates [dynamic_reconfigure/Config]
* /joint_states [sensor_msgs/JointState]
* /rosout [rosgraph_msgs/Log]
* /camera/depth/image_raw [sensor_msgs/Image]
* /mobile_base/events/cliff [kobuki_msgs/CliffEvent]
* /camera/rgb/image_raw/theora [theora_image_transport/Packet]
* /camera/rgb/image_raw/theora/parameter_updates [dynamic_reconfigure/Config]
* /gazebo/parameter_updates [dynamic_reconfigure/Config]
* /odom [nav_msgs/Odometry]
* /camera/depth/camera_info [sensor_msgs/CameraInfo]
* /gazebo/link_states [gazebo_msgs/LinkStates]
* /clock [rosgraph_msgs/Clock]
* /camera/rgb/image_raw/theora/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
Subscriptions:
* /gazebo/set_link_state [unknown type]
* /mobile_base/commands/reset_odometry [unknown type]
* /mobile_base/commands/velocity [geometry_msgs/Twist]
* /gazebo/set_model_state [unknown type]
* /mobile_base/commands/motor_power [unknown type]
* /clock [rosgraph_msgs/Clock]
Services:
* /gazebo/set_joint_properties
* /gazebo/set_log
ger_level
* /gazebo/get_physics_properties
* /gazebo/reset_world
* /camera/rgb/image_raw/compressed/set_parameters
* /gazebo/set_model_configuration
* /camera/rgb/image_raw/theora/set_parameters
* /gazebo/pause_physics
* /gazebo/set_link_state
* /gazebo/spawn_sdf_model
* /gazebo/unpause_physics
* /gazebo/spawn_urdf_model
* /camera/set_parameters
* /gazebo/get_world_properties
* /gazebo/get_loggers
* /gazebo/apply_joint_effort
* /gazebo/set_parameters
* /gazebo/set_physics_properties
* /gazebo/set_model_state
* /gazebo/reset_simulation
* /gazebo/delete_model
* /gazebo/get_link_properties
* /camera/rgb/image_raw/compressedDepth/set_parameters
* /gazebo/set_link_properties
* /gazebo/get_joint_properties
* /gazebo/apply_body_wrench
* /gazebo/get_link_state
* /gazebo/clear_body_wrenches
* /camera/set_camera_info
* /gazebo/spawn_gazebo_model
* /gazebo/get_model_state
* /gazebo/clear_joint_forces
* /gazebo/get_model_properties
contacting node http://ubuntu:34402/ ...
Pid: 3934
Connections:
* topic: /rosout
* to: /rosout
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /gazebo
* direction: outbound
* transport: INTRAPROCESS
* topic: /clock
* to: /move_base
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /kobuki_safety_controller
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /bumper2pointcloud
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /cmd_vel_mux
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /local_costmap/voxel_grid_throttle
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /navigation_velocity_smoother
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /move_base_node
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /map_server
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /rosout
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /mobile_base_nodelet_manager
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /rviz
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /amcl
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /robot_state_publisher
* direction: outbound
* transport: TCPROS
* topic: /tf
* to: /move_base
* direction: outbound
* transport: TCPROS
* topic: /tf
* to: /move_base_node
* direction: outbound
* transport: TCPROS
* topic: /tf
* to: /amcl
* direction: outbound
* transport: TCPROS
* topic: /tf
* to: /rviz
* direction: outbound
* transport: TCPROS
* topic: /joint_states
* to: /robot_state_publisher
* direction: outbound
* transport: TCPROS
* topic: /odom
* to: /mobile_base_nodelet_manager
* direction: outbound
* transport: TCPROS
* topic: /odom
* to: /move_base_node
* direction: outbound
* transport: TCPROS
* topic: /mobile_base/events/cliff
* to: /mobile_base_nodelet_manager
* direction: outbound
* transport: TCPROS
* topic: /mobile_base/events/bumper
* to: /mobile_base_nodelet_manager
* direction: outbound
* transport: TCPROS
* topic: /clock
* to: /gazebo (http://ubuntu:34402/)
* direction: inbound
* transport: INTRAPROCESS
* topic: /mobile_base/commands/velocity
* to: /mobile_base_nodelet_manager (http://ubuntu:47967/)
* direction: inbound
* transport: TCPROS
Thanks, Karthik.
Asked by Karthikeyan on 2016-08-29 08:55:10 UTC
Comments
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?
Asked by DavidN on 2016-08-29 21:58:21 UTC
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?
Asked by Karthikeyan on 2016-08-30 03:08:32 UTC
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 topicAsked by DavidN on 2016-08-30 04:10:04 UTC
no there isn't any data being published, I get this warning when i tried the rostopic command
Asked by Karthikeyan on 2016-08-30 04:36:13 UTC
I have added config file costmap_common_params.yaml for the reference.
Asked by Karthikeyan on 2016-08-30 05:53:25 UTC
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"
Asked by DavidN on 2016-08-30 20:48:48 UTC
Hi, I have added my output for the command
rosnode list /gazebo
command.Asked by Karthikeyan on 2016-08-31 04:50:19 UTC
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?
Asked by DavidN on 2016-09-01 00:24:02 UTC
Hello. Can you please share the teb_local_planner_params.yaml and costmap_converter_params.yaml files? I am working on a similar project and they would help me so much as a reference since I can't find an example in the package or others online! Thank you.
Asked by Mayar on 2021-10-22 16:34:18 UTC