0
$\begingroup$

Rosanswers logo

I've set up a system comprising of:

  • A laser scanner (Hokuyo URG-04LX)
  • Remote computer connected to laser scanner running roscore, hokuyo_node and laser_scan_matcher
  • A local computer running gmapping and rviz

and am trying to make move_base run on top of it all on the local machine. I went through Robot Setup, Navigation Tuning Guide, Using rviz with Navigation Stack and all related questions here. It seems my basic setup is already mostly correct, but there is still something missing as neither global nor local planners update calculated trajectory on robot pose change.

My setup is mostly as described in Robot Setup Tutorial, i.e.:

costmap_common_params.yaml:

obstacle_range: 2.5
raytrace_range: 3.0
robot_radius: 0.1
inflation_radius: 0.3

observation_sources: laser_scan_sensor

laser_scan_sensor: {sensor_frame: /laser, data_type: LaserScan, topic: /scan, marking: true, clearing: true}

global_costmap_params.yaml:

global_costmap:
  global_frame: odom
  robot_base_frame: base_link
  update_frequency: 5.0
  static_map: true

local_costmap_params.yaml:

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: 2.0
  height: 2.0
  resolution: 0.05

base_local_planner_params.yaml:

TrajectoryPlannerROS:

  max_vel_x: 0.1 #m/s
  min_vel_x: 0.0
  escape_vel: -0.1
  max_rotational_vel: 0.1 #rad/s
  min_in_place_rotational_vel: 0.0

  y_vels: [-0.1, -0.0, 0.0, 0.1]

  acc_lim_th: 1.0
  acc_lim_x: 1.0 #m/s2
  acc_lim_y: 1.0

  holonomic_robot: true

move_base.launch:

<launch>
  <master auto="start"/>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find my_robot)/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find my_robot)/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find my_robot)/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find my_robot)/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find my_robot)/base_local_planner_params.yaml" command="load" />
  </node>
</launch>

When I select a goal using rviz local and global trajectories get nicely drawn, but they don't change when I carry the "robot" around, only when new goal is being set. Besides, all I'm getting on /cmd_vel topic is the below output (so it would cause a real robot spin around endlessly).

linear: 
  x: 0.0
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: -0.05
---

Both localization and odometry work as desired. Is there anything missing?


Originally posted by tom on ROS Answers with karma: 1079 on 2011-12-28

Post score: 1


Original comments

Comment by Eric Perko on 2011-12-29:
You shouldn't need Gazebo to verify that your navigation configs are working - Stage will work just fine for that purpose and is a lot easier to setup than Gazebo (unless you already have models and such done). Stage won't be a completely accurate sim, but it's enough to verify your configs function

Comment by tom on 2011-12-29:
Hmm. it actually looks like min_vel_x is being used as "typical" output value on /cmd_vel. Changing min_vel_x = 0.1 causes move_base to publish linear: x: 0.1, instead of 0, hence working. Considering what it says here: http://www.ros.org/wiki/base_local_planner this doesn't seem correct, or is it?

Comment by tom on 2011-12-28:
I haven't tried setting Gazebo up. Odometry works fine, gets published in /odom frame (but not as topic, is it necessary?), SLAM works ok too. Can low (but positive) max. velocities affect the outcome? - I'll try out changing them.

Comment by Eric Perko on 2011-12-28:
Also, can you recreate this trouble in simulation?

Comment by Eric Perko on 2011-12-28:
Just curious, but are your max velocities really that low?

Comment by DimitriProsser on 2011-12-28:
Odometry calculates when you carry the robot around?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

You need to publish your odometry on the /odom topic (or whatever you remap it as) as well as via tf.

base_local_planner subscribes to /odom to get it's velocity information. Not publishing it will cause base_local_planner to always output something near the min velocities (which is why the output changed when you changed your min vels), since it never receives feedback that the base has started moving, so it can't accelerate past the min velocity.

I'm a bit surprised that it didn't output something at least slightly greater than the set min_vel, since your acceleration is pretty high (compared to your min and max vels). Then again, I'm not 100% how base_local_planner treats not receiving /odom updates at all, but I know for certain it won't work correctly without them. I'm a little surprised you don't get any kind of warning for something like "no odom updates in the last N secs" or something... if this does turn out to be the problem, you should file a bug report against base_local_planner to throw a warning if no odom is received for some amount of time.

EDIT: Here's the relevant callback from the base_local_planner source, trajectory_planner_ros.cpp

341   void TrajectoryPlannerROS::odomCallback(const nav_msgs::Odometry::ConstPtr& msg){
342     //we assume that the odometry is published in the frame of the base
343     boost::recursive_mutex::scoped_lock lock(odom_lock_);
344     base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
345     base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
346     base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
347     ROS_DEBUG("In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
348         base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
349   }

Originally posted by Eric Perko with karma: 8406 on 2011-12-29

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Eric Perko on 2011-12-30:
Just set the linear x and y twist and the rotational z twist. These should be in "base_link" frame - base_local_planner doesn't do anything to convert twists into the base frame that it needs them in. I've updated my answer to include the relevant callback from base_local_planner's C++.

Comment by tom on 2011-12-30:
Besides, do you know which exactly variables of the many provided in nav_msgs/Odometry does base_local_planner really need? Does it need both twist and pose, or just twist? Does it make any use of covariances, if given?

Comment by tom on 2011-12-30:
OK, thanks, I'll try it out as soon as I get a nav_msgs/Odometry message. At the moment all I have is a geometry_msgs/Pose2D from laser_scan_matcher. I'm not getting any warnings from move_base about missing subscriptions (nor from roswtf).

$\endgroup$

Your Answer

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