Setting initial pose on AMCL

Software
1

Im literally trying to run AMCL on tutorial 7 by replacing the gmapping with AMCL on the launch file. Can I get any help on setting up the inital pose of the robot in the prebuilt map?

I am getting this warning
[ WARN] [1569331322.125531574]: Costmap2DROS transform timeout. Current time: 1569331322.1254, global_pose stamp: 1569331321.8709, tolerance: 0.2500
[ WARN] [1569331322.125701567]: Could not get robot pose, cancelling reconfiguration

After excuting roswtf

ERROR The following nodes should be connected but aren’t:

  • /move_base->/move_base (/move_base/global_costmap/footprint)
  • /move_base->/move_base (/move_base/local_costmap/footprint)

ERROR TF multiple authority contention:

  • node [/serial_node] publishing transform [base_link] with parent [odom] already published by node [/drive_controller]
  • node [/drive_controller] publishing transform [base_link] with parent [odom] already published by node [/serial_node]

My launch file -

<arg name="use_rosbot" default="true"/>
<arg name="use_gazebo" default="false"/>

<include if="$(arg use_gazebo)" file="$(find rosbot_gazebo)/launch/maze_world.launch"/>
<include if="$(arg use_gazebo)" file="$(find rosbot_gazebo)/launch/rosbot.launch"/>

<param if="$(arg use_gazebo)" name="use_sim_time" value="true"/>

<node if="$(arg use_rosbot)" pkg="rplidar_ros" type="rplidarNode" name="rplidar">
    <param name="angle_compensate" type="bool" value="true"/>
    <param name="serial_baudrate" type="int" value="115200"/><!--model A2 (ROSbot 2.0) -->
    <!--<param name="serial_baudrate" type="int" value="256000"/>--><!-- model A3 (ROSbot 2.0 PRO) -->
</node>

<node if="$(arg use_rosbot)" pkg="tutorial_pkg" type="drive_controller_node" name="drive_controller"/>

<node if="$(arg use_rosbot)" pkg="tf" type="static_transform_publisher" name="laser_broadcaster" args="0 0 0 3.14 0 0 base_link laser_frame 100" />

<node pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" name="teleop_twist_keyboard" output="screen"/>

<node pkg="rviz" type="rviz" name="rviz"/>

<arg name="map_file" default="/home/husarion/ros_workspace/src/rosbot_description/src/rosbot_navigation/maps/test_map.yaml"/> 
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" /> 

<node pkg="amcl" type="amcl" name="amcl" output="screen">
<remap from="scan" to="/scan"/>
<param name="odom_frame_id" value="odom"/>
    <param name="odom_model_type" value="diff-corrected"/>
    <param name="base_frame_id" value="base_link"/>
    <param name="update_min_d" value="0.5"/>
    <param name="update_min_a" value="1.0"/>
</node>

<node pkg="move_base" type="move_base" name="move_base" output="screen">
    <param name="controller_frequency" value="10.0"/>
    <rosparam file="$(find tutorial_pkg)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find tutorial_pkg)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find tutorial_pkg)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find tutorial_pkg)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find tutorial_pkg)/config/trajectory_planner.yaml" command="load" />
</node>
2

Hello thiaga,

Which device are you using, ROSbot 2.0 or ROSbot 2.0 PRO?
Warnings and errors that you posted are from move_base, it will not start working until robot position is provided by amcl.
Are you getting any other warnings or errors? Preferably, could you post here full output that you get in terminal?
Also, could you post here screenshot of rqt_graph and rqt_tf_tree?

Regards,
Łukasz

3

Hi Lukasz,
Yeah I am using Rosbot 2.0. No I am just getting the above mentioned warnings
WARN] [1569421675.022472375]: Costmap2DROS transform timeout. Current time: 1569421675.0223, global_pose stamp: 1569421674.7661, tolerance: 0.2500
[ WARN] [1569421675.022617042]: Could not get robot pose, cancelling reconfiguration

Here is the rqt_graph and rqt_tf_tree

ppamcl
ppamcl3257×804 238 KB
rqttftree
rqttftree765×940 63.6 KB

And also for the costmap warning I tried setting the tolerance value of local costmap from 0.25 to 0.5 matching the tolerance of global map (0.5). It worked, I was not getting the warnings anymore but the result(path the robot takes to the goal point) was not good enough. What should I do to improve it?

4

The full output
husarion@test-612836-2307:~$ roslaunch tutorial_pkg pathplana.launch
… logging to /home/husarion/.ros/log/87718bd8-dfa0-11e9-83c8-409f384f007f/roslaunch-test-612836-2307-13945.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://test-612836-2307:34425/

SUMMARY

PARAMETERS

  • /amcl/base_frame_id: base_link
  • /amcl/min_particles: 500
  • /amcl/odom_frame_id: odom
  • /amcl/odom_model_type: diff
  • /amcl/update_min_a: 0.1
  • /amcl/update_min_d: 0.05
  • /move_base/TrajectoryPlannerROS/acc_lim_Y: 2.5
  • /move_base/TrajectoryPlannerROS/acc_lim_theta: 0.25
  • /move_base/TrajectoryPlannerROS/acc_lim_x: 2.5
  • /move_base/TrajectoryPlannerROS/holonomic_robot: False
  • /move_base/TrajectoryPlannerROS/max_vel_theta: 0.35
  • /move_base/TrajectoryPlannerROS/max_vel_x: 0.2
  • /move_base/TrajectoryPlannerROS/meter_scoring: True
  • /move_base/TrajectoryPlannerROS/min_in_place_vel_theta: 0.25
  • /move_base/TrajectoryPlannerROS/min_vel_theta: -0.35
  • /move_base/TrajectoryPlannerROS/min_vel_x: 0.1
  • /move_base/TrajectoryPlannerROS/xy_goal_tolerance: 0.15
  • /move_base/TrajectoryPlannerROS/yaw_goal_tolerance: 0.25
  • /move_base/controller_frequency: 10.0
  • /move_base/global_costmap/always_send_full_costmap: True
  • /move_base/global_costmap/footprint: [[0.12, 0.14], [0…
  • /move_base/global_costmap/global_frame: map
  • /move_base/global_costmap/height: 15
  • /move_base/global_costmap/inflation_radius: 2.5
  • /move_base/global_costmap/laser_scan_sensor/clearing: True
  • /move_base/global_costmap/laser_scan_sensor/data_type: LaserScan
  • /move_base/global_costmap/laser_scan_sensor/marking: True
  • /move_base/global_costmap/laser_scan_sensor/sensor_frame: laser_frame
  • /move_base/global_costmap/laser_scan_sensor/topic: scan
  • /move_base/global_costmap/map_topic: /map
  • /move_base/global_costmap/observation_sources: laser_scan_sensor
  • /move_base/global_costmap/obstacle_range: 6.0
  • /move_base/global_costmap/origin_x: -7.5
  • /move_base/global_costmap/origin_y: -7.5
  • /move_base/global_costmap/publish_frequency: 2.5
  • /move_base/global_costmap/raytrace_range: 8.5
  • /move_base/global_costmap/resolution: 0.1
  • /move_base/global_costmap/robot_base_frame: base_link
  • /move_base/global_costmap/rolling_window: True
  • /move_base/global_costmap/static_map: True
  • /move_base/global_costmap/subscribe_to_updates: True
  • /move_base/global_costmap/transform_tolerance: 0.5
  • /move_base/global_costmap/update_frequency: 2.5
  • /move_base/global_costmap/width: 15
  • /move_base/local_costmap/always_send_full_costmap: True
  • /move_base/local_costmap/footprint: [[0.12, 0.14], [0…
  • /move_base/local_costmap/global_frame: map
  • /move_base/local_costmap/height: 3
  • /move_base/local_costmap/inflation_radius: 0.6
  • /move_base/local_costmap/laser_scan_sensor/clearing: True
  • /move_base/local_costmap/laser_scan_sensor/data_type: LaserScan
  • /move_base/local_costmap/laser_scan_sensor/marking: True
  • /move_base/local_costmap/laser_scan_sensor/sensor_frame: laser_frame
  • /move_base/local_costmap/laser_scan_sensor/topic: scan
  • /move_base/local_costmap/map_topic: /map
  • /move_base/local_costmap/observation_sources: laser_scan_sensor
  • /move_base/local_costmap/obstacle_range: 6.0
  • /move_base/local_costmap/origin_x: -1.5
  • /move_base/local_costmap/origin_y: -1.5
  • /move_base/local_costmap/publish_frequency: 5
  • /move_base/local_costmap/raytrace_range: 8.5
  • /move_base/local_costmap/resolution: 0.1
  • /move_base/local_costmap/robot_base_frame: base_link
  • /move_base/local_costmap/rolling_window: True
  • /move_base/local_costmap/static_map: False
  • /move_base/local_costmap/subscribe_to_updates: True
  • /move_base/local_costmap/transform_tolerance: 0.25
  • /move_base/local_costmap/update_frequency: 5
  • /move_base/local_costmap/width: 3
  • /rosdistro: kinetic
  • /rosversion: 1.12.13
  • /rplidar/angle_compensate: True
  • /rplidar/serial_baudrate: 115200

NODES
/
amcl (amcl/amcl)
drive_controller (tutorial_pkg/drive_controller_node)
laser_broadcaster (tf/static_transform_publisher)
map_server (map_server/map_server)
move_base (move_base/move_base)
rplidar (rplidar_ros/rplidarNode)
rviz (rviz/rviz)
teleop_twist_keyboard (teleop_twist_keyboard/teleop_twist_keyboard.py)

ROS_MASTER_URI=http://master:11311

process[rplidar-1]: started with pid [14008]
process[drive_controller-2]: started with pid [14009]
process[laser_broadcaster-3]: started with pid [14011]
process[teleop_twist_keyboard-4]: started with pid [14018]
process[rviz-5]: started with pid [14031]
process[map_server-6]: started with pid [14039]
process[amcl-7]: started with pid [14043]
process[move_base-8]: started with pid [14048]
libEGL warning: DRI2: failed to authenticate
[ INFO] [1569423894.607858684]: Requesting the map…
[ INFO] [1569423894.671302017]: Received a 1984 X 1984 map @ 0.100 m/pix

[ INFO] [1569423895.321322601]: Initializing likelihood field model; this can take some time on large maps…

Reading from the keyboard and Publishing to Twist!

Moving around:
u i o
j k l
m , .

For Holonomic mode (strafing), hold down the shift key:

U I O
J K L
M < >

t : up (+z)
b : down (-z)

anything else : stop

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%

CTRL-C to quit

currently: speed 0.5 turn 1.0
[ INFO] [1569423895.952725226]: Done initializing likelihood field model.
[ INFO] [1569423896.389048934]: Using plugin “static_layer”
[ INFO] [1569423896.434599684]: Requesting the map…
[ INFO] [1569423896.661932268]: Resizing static layer to 1984 X 1984 at 0.100000 m/pix
[ INFO] [1569423896.755629601]: Received a 1984 X 1984 map at 0.100000 m/pix
[ INFO] [1569423896.770436351]: Using plugin “obstacle_layer”
[ INFO] [1569423896.780435268]: Subscribed to Topics: laser_scan_sensor
[ INFO] [1569423896.880254976]: Using plugin “inflation_layer”
[ INFO] [1569423897.141398435]: Using plugin “obstacle_layer”
[ INFO] [1569423897.153027768]: Subscribed to Topics: laser_scan_sensor
[ INFO] [1569423897.261651435]: Using plugin “inflation_layer”
[ INFO] [1569423897.515612893]: Created local_planner base_local_planner/TrajectoryPlannerROS
[ INFO] [1569423897.567237602]: Sim period is set to 0.10
[ INFO] [1569423898.282345352]: Recovery behavior will clear layer obstacles
[ INFO] [1569423898.302989810]: Recovery behavior will clear layer obstacles
[ WARN] [1569423900.634068853]: Costmap2DROS transform timeout. Current time: 1569423900.6340, global_pose stamp: 1569423900.3816, tolerance: 0.2500
[ WARN] [1569423900.634182311]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423901.662777770]: Costmap2DROS transform timeout. Current time: 1569423901.6627, global_pose stamp: 1569423901.4115, tolerance: 0.2500
[ WARN] [1569423903.462789271]: Costmap2DROS transform timeout. Current time: 1569423903.4627, global_pose stamp: 1569423903.2071, tolerance: 0.2500
[ WARN] [1569423904.233718521]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423910.634609816]: Costmap2DROS transform timeout. Current time: 1569423910.6343, global_pose stamp: 1569423910.3815, tolerance: 0.2500
[ WARN] [1569423910.634802316]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423912.934297067]: Costmap2DROS transform timeout. Current time: 1569423912.9342, global_pose stamp: 1569423912.6815, tolerance: 0.2500
[ WARN] [1569423912.934418401]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423914.462819985]: Costmap2DROS transform timeout. Current time: 1569423914.4627, global_pose stamp: 1569423914.2116, tolerance: 0.2500
[ WARN] [1569423919.062811362]: Costmap2DROS transform timeout. Current time: 1569423919.0627, global_pose stamp: 1569423918.8116, tolerance: 0.2500
[ WARN] [1569423923.934224823]: Costmap2DROS transform timeout. Current time: 1569423923.9341, global_pose stamp: 1569423923.6816, tolerance: 0.2500
[ WARN] [1569423923.934354031]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423927.262824282]: Costmap2DROS transform timeout. Current time: 1569423927.2627, global_pose stamp: 1569423927.0116, tolerance: 0.2500
[ WARN] [1569423928.033917450]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423930.333657784]: Costmap2DROS transform timeout. Current time: 1569423930.3336, global_pose stamp: 1569423930.0815, tolerance: 0.2500
[ WARN] [1569423930.333771242]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423934.933939661]: Costmap2DROS transform timeout. Current time: 1569423934.9338, global_pose stamp: 1569423934.6816, tolerance: 0.2500
[ WARN] [1569423934.934187869]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423936.462785745]: Costmap2DROS transform timeout. Current time: 1569423936.4627, global_pose stamp: 1569423936.2115, tolerance: 0.2500
[ WARN] [1569423936.735306537]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423938.262797246]: Costmap2DROS transform timeout. Current time: 1569423938.2627, global_pose stamp: 1569423938.0116, tolerance: 0.2500
[ WARN] [1569423940.062790955]: Costmap2DROS transform timeout. Current time: 1569423940.0627, global_pose stamp: 1569423939.8115, tolerance: 0.2500
[ WARN] [1569423940.833843289]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423941.862804206]: Costmap2DROS transform timeout. Current time: 1569423941.8627, global_pose stamp: 1569423941.6116, tolerance: 0.2500
[ WARN] [1569423943.934175665]: Costmap2DROS transform timeout. Current time: 1569423943.9341, global_pose stamp: 1569423943.6815, tolerance: 0.2500
[ WARN] [1569423943.934309832]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423948.033883167]: Costmap2DROS transform timeout. Current time: 1569423948.0338, global_pose stamp: 1569423947.7815, tolerance: 0.2500
[ WARN] [1569423948.034006251]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423949.062811418]: Costmap2DROS transform timeout. Current time: 1569423949.0627, global_pose stamp: 1569423948.8116, tolerance: 0.2500
[ WARN] [1569423949.334420168]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1569423952.862838420]: Costmap2DROS transform timeout. Current time: 1569423952.8627, global_pose stamp: 1569423952.6116, tolerance: 0.2500
[ WARN] [1569423960.062838631]: Costmap2DROS transform timeout. Current time: 1569423960.0627, global_pose stamp: 1569423959.8115, tolerance: 0.2500
[ WARN] [1569423963.134424050]: Costmap2DROS transform timeout. Current time: 1569423963.1343, global_pose stamp: 1569423962.8815, tolerance: 0.2500
[ WARN] [1569423963.134552675]: Could not get robot pose, cancelling reconfiguration

5

Hello thiaga,

AMCL has plenty of parameters for tuning, I would advise you to start with below parameters:

<param name="update_min_d" value="0.1"/>
<param name="update_min_a" value="0.1"/>
<param name="min_particles" value="2500"/>
<param name="max_particles" value="10000"/>

It will force to update AMCL more often and with better approximation.

What do you mean by “not good enough”? Is ROSbot hitting obstacles, unable to reach destination, taking non optimal path or something else?

Regards,
Łukasz

6

Ok thanks I will try tuning the AMCL parameters. No the robot is not hitting obstacles but its moving slowly and at times it gets lost of track and keeps spinning in one point and then eventually comes to the goal point. So when compared with the tutorial pathplanning this result is not great.

The fix made on costmap tolerance is that a right one? Or just a temporary fix?

7

Yes, the costmap tolerance is right fix.