0
$\begingroup$

Rosanswers logo

Hello,

I have been building a robot for a university project and working with ROS Navigation stack.

I have got the robot to work in Gazebo simulation using Gazebo plugins and managed to send goal poses through Rviz successfully. However now I am trying to reproduce the same on the real robot but the goal planning isn’t going so well. It seems that the localization and the odometry update isn’t working properly.

When I give the robot very close goal poses it moves with no problems, but when I give a goal which is slightly more far on the map (a couple of feet) it starts to go off track or in case of a 90 degree turn, the robot keeps going straight and then it takes a big turn or a detour in order to return to it’s goal and gives these warnings:

[ WARN] [1488103550.240345429]: Costmap2DROS transform timeout. Current time: 1488103550.2400, global_pose stamp: 1488103548.1647, tolerance: 2.0000
[ WARN] [1488103550.241602412]: Could not get robot pose, cancelling pose reconfiguration

Below is an image explaining a scenario with the desired path the robot should take and the path it takes with the ROS planner: image description

Here is my setup:

I am using real Kinect + fake laser depthimage_to_laserscan in combination with AMCL and Move Base.

I am publishing the odometry using this class since I am using roboclaw motor shield: https://github.com/sonyccd/roboclaw_ros/blob/master/roboclaw_node/nodes/roboclaw_node.py

I have based my move_base on this example and only changed the names of topics: https://github.com/turtlebot/turtlebot_apps/blob/indigo/turtlebot_navigation/param/global_costmap_params.yaml

Below is my TF graph: image description

My navigation launch file:

<arg name="scan_topic" default="kinect_scan"/> 
  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="base_link"/>
  <arg name="global_frame_id" default="map"/>

<!-- DG: Kinect cloud to laser scan -->
<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
    <remap from="image"     to="/camera/depth/image_raw"/>
    <remap from="camera_info" to="/camera/depth/camera_info"/>
    <remap from="scan" to="$(arg scan_topic)"/>
    <param name="range_max" type="double" value="4"/>
</node>

<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0 0 0 0 0 0 /base_link /camera_depth_frame 100"/>

  <!-- Map server -->
  <arg name="map_file" default="$(find rovytwo_navigation)/maps/empty/blank_map.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <arg name="use_map_topic"   default="false"/>
  <arg name="initial_pose_x"  default="0.0"/>
  <arg name="initial_pose_y"  default="0.0"/>
  <arg name="initial_pose_a"  default="0.0"/>
  <node pkg="amcl" type="amcl" name="amcl" output="screen">
    <param name="use_map_topic"             value="$(arg use_map_topic)"/>

    <param name="odom_model_type"           value="diff"/>
    <param name="odom_alpha5"               value="0.1"/>
    <param name="gui_publish_rate"          value="10.0"/> <!-- -->
    <param name="laser_max_beams"             value="60"/>
    <param name="laser_max_range"           value="12.0"/>
    <param name="min_particles"             value="500"/>
    <param name="max_particles"             value="2000"/>
    <param name="kld_err"                   value="0.05"/>
    <param name="kld_z"                     value="0.99"/>
    <param name="odom_alpha1"               value="0.2"/>
    <param name="odom_alpha2"               value="0.2"/>

    <param name="odom_alpha3"               value="0.2"/>
    <param name="odom_alpha4"               value="0.2"/>
    <param name="laser_z_hit"               value="0.5"/>
    <param name="laser_z_short"             value="0.05"/>
    <param name="laser_z_max"               value="0.05"/>
    <param name="laser_z_rand"              value="0.5"/>
    <param name="laser_sigma_hit"           value="0.2"/>
    <param name="laser_lambda_short"        value="0.1"/>
    <param name="laser_model_type"          value="likelihood_field"/>

    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d"              value="0.25"/>
    <param name="update_min_a"              value="0.2"/>
    <param name="odom_frame_id"             value="$(arg odom_frame_id)"/> 
    <param name="base_frame_id"             value="$(arg base_frame_id)"/> 
    <param name="global_frame_id"           value="$(arg global_frame_id)"/>
    <param name="resample_interval"         value="1"/>

    <param name="transform_tolerance"       value="1.0"/>
    <param name="recovery_alpha_slow"       value="0.0"/>
    <param name="recovery_alpha_fast"       value="0.0"/>
    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
    <remap from="scan"                      to="$(arg scan_topic)"/>
  </node>

  <!-- Move Base base_local_planner_params rename to dwa_-->
  <master auto="start"/>
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find rovytwo_navigation)/param/new/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find rovytwo_navigation)/param/new/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find rovytwo_navigation)/param/new/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find rovytwo_navigation)/param/new/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find rovytwo_navigation)/param/new/base_local_planner_params.yaml" command="load" />

    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    <remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/>
    <remap from="scan" to="$(arg scan_topic)"/>
  </node>

Common Costmap:

 obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[0.265, 0.278], [0.04, 0.193], [-0.04, 0.193], [-0.282, 0.178], [-0.282, -0.178], [-0.04, -0.193], [0.04, -0.193], [0.265, -0.278]]
inflation_radius: 0.4

observation_sources: laser_scan_sensor

laser_scan_sensor: {sensor_frame: openni_depth_frame, data_type: LaserScan, topic: kinect_scan, marking: true, clearing: true}

map_type: costmap
transform_tolerance: 0.4 # seconds

Local Costmap:

local_costmap:
  global_frame: /odom
  robot_base_frame: /base_link
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 4.0
  height: 4.0
  # origin_x: -0.115
  resolution: 0.05
  transform_tolerance: 1.5 # 0.5 seconds

Global Costmap:

global_costmap:
  global_frame: /map
  robot_base_frame: base_link
  update_frequency: 2.0
  static_map: true

  transform_tolerance: 1.5 # 0.5 seconds
  publish_frequency: 1.0

Base Local Planner:

TrajectoryPlannerROS:

  max_vel_x: 0.5
  min_vel_x: 0.02
  max_rotational_vel: 1.0  
  min_in_place_rotational_vel: 0.2

  acc_lim_th: 1.0
  acc_lim_x: 0.2
  acc_lim_y: 0.2

  holonomic_robot: false

  # goal tolerance parameters
  yaw_goal_tolerance: 0.1         
  xy_goal_tolerance: 0.2
  latch_xy_goal_tolerance: true
  1. Which components should I be focusing on in order to make the robot create a much more efficient path plans when navigating to a goal pose?

  2. Could you please guide me on how to make sure that all topics are being published at the same frequency? I suspect that the topics aren’t working in synch.

  3. Is there supposed to be another TF from base_link to laser or other TFs when using AMCL like my case?


Originally posted by dgrixti on ROS Answers with karma: 95 on 2017-02-26

Post score: 1


Original comments

Comment by gvdhoorn on 2017-02-26:
Please attach all images directly to your post. If you ever delete them from your google drive, this question becomes useless / unintelligible. I've given you enough karma.

Comment by gvdhoorn on 2017-02-26:
As to text files (ie: launch files), same rationale: please include them directly into your question. Use the Preformatted Text button (the one with 101010 on it) to make sure things are formatted properly.

Comment by dgrixti on 2017-02-26:
Thanks for adding the karma. I have now included all images and code snippets to the post.

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

It seems you are navigating in an empy map? Why is that? In that case, AMCL will not be able to properly localize the robot as it does not have any reference.

Also, your TF tree regarding the camera seems a bit off. I would expect to see something like: camera_link -> camera_rgb_frame -> camera_rgb_optical_frame (meaning camera_link should be the parent of camera_rgb_frame and camera_depth_frame). But I guess your setup would work anyways, although it seems weird IMO.


Originally posted by Procópio with karma: 4402 on 2017-02-27

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by dgrixti on 2017-02-28:
Thanks for your feedback Procopio. The reason for using an empty map in this case is that I want to try to navigate in an empty space without any obstacles to make sure all is working fine with the simplest scenario.

Comment by dgrixti on 2017-02-28:
I tested with various maps which I found on the internet but all give the same problem unfortunately. I will make sure to revise the URDF to get the TF tree in the order suggested by you, but I have a feeling I could be missing something else. Could it be the way I publish /odom from the code?

Comment by Procópio on 2017-03-06:
if you have a robot, create the map of your surrounding and then load it and try navigating there. what happens?

Comment by dgrixti on 2017-03-07:
I have created gmappinng with linear speed 1.0 and angular speed 1.0 settings. Then I sent from teleop_node (adapted node from turtlebot) commands to move the robot at same speeds 1.0. / 1.0. The map was created well and then I used it for navigation. It seems that ROS still has some issues

Comment by dgrixti on 2017-03-07:
While I give 2D pose navigation goals in a straight line it seems to behave well, but the minute it needs to go around a gap or turn slightly to left or right it goes into a "Unstuck robot behaviour" mode and it just keeps turning on itself until it fails.

Comment by dgrixti on 2017-03-07:
Any advice on how to calibrate angular velocities with ROS? Or how to test that real velocity/angular speeds match with the ones ROS thinks it is happening on the map?

Comment by Procópio on 2017-03-08:
I think we are getting out of your original question here. As far as I understood you had a problem with localization, trying to localize your robot in an empty map. Another issue is your navigation config. That will be better addressed with a new, more localized, question.

Comment by dgrixti on 2017-03-10:
You're right, I also posted an answer which explains how I managed to get better results recently by improving the cmd_vel and odom. Thanks for your help!

Comment by StewartHemm74 on 2019-02-06:
How did you correct you cmd_vel to odom?

$\endgroup$
0
$\begingroup$

Rosanswers logo

It turns out that the biggest issue was that the robot was not able to perform all the turns indicated by move_base cmd_vel. In fact it seems that the robot was only able to move in a straight line or take sharp left / right turns.

Not being able to turn slightly it cause the robot inabilty to steer smoothly and this was confusing the planner and thus having to first move into an area where it could potentially take a sharp turn. By refining the code which converts cmd_vel to odom and cmd_vel to motor signals and testing by comparing actual movements with the ones projected on RViz I was able to improve the navigation drastically and now the robot is able to perform better trajectories.


Originally posted by dgrixti with karma: 95 on 2017-03-10

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.