0
$\begingroup$

Rosanswers logo

Hi everyone!

I am having an issue with the orientation fields of the move_base package and the teb_local_planner.

At first I would just set the orientation.w field to 1.0 and leave the xyz components set to 0. However, now I want to make the robot have a specific orientation when it reaches its goal location.

My train of thought was that if I wanted to rotate the robot around its base that I would have to rotate 0.5 * PI around its yaw axis. If I change the yaw component in my gazebo simulation then I can indeed rotate the robot around its y-axis.

I found this tutorial on the ROS wiki which states that you can convert a roll, pitch, yaw rotation to a quaternion.

Suppose my RPY respectively are [0, 0, 0.5 * PI] then the Quaternion is (w,x,y,z) (0.707107, 0, 0, 0.707107) move_base s canaccepts this as a valid quaternion and thus the goal is active. However, the robot starts to oscillate due to its local planner. I added an image of the RVIZ view where I visualized both the local and global plan. The robot stays on the first pose in the red poseArray and moves back and forward.

RVIZ view of the global and local plan.

Is my quaternion input incorrect or am I using the orientation field incorrectly? My goal is to have the robot arrive at its goal location with a specific orientation.

Here is the output of my navigation nodes:

SUMMARY
========

PARAMETERS
 * /move_base/base_local_planner: teb_local_planner...
 * /move_base/global_costmap/footprint: [[0.25, 0.154], [...
 * /move_base/global_costmap/global_frame: map
 * /move_base/global_costmap/inflation_radius: 0.55
 * /move_base/global_costmap/obstacle_range: 2.5
 * /move_base/global_costmap/raytrace_range: 3.0
 * /move_base/global_costmap/robot_base_frame: base_link
 * /move_base/global_costmap/static_map: True
 * /move_base/global_costmap/update_frequency: 5.0
 * /move_base/local_costmap/footprint: [[0.25, 0.154], [...
 * /move_base/local_costmap/global_frame: map
 * /move_base/local_costmap/height: 10.0
 * /move_base/local_costmap/inflation_radius: 0.55
 * /move_base/local_costmap/obstacle_range: 2.5
 * /move_base/local_costmap/publish_frequency: 40.0
 * /move_base/local_costmap/raytrace_range: 3.0
 * /move_base/local_costmap/resolution: 1
 * /move_base/local_costmap/robot_base_frame: base_link
 * /move_base/local_costmap/rolling_window: False
 * /move_base/local_costmap/static_map: True
 * /move_base/local_costmap/update_frequency: 60.0
 * /move_base/local_costmap/width: 10.0
 * /robot_pose_ekf/debug: True
 * /robot_pose_ekf/freq: 50.0
 * /robot_pose_ekf/imu_used: True
 * /robot_pose_ekf/odom_used: True
 * /robot_pose_ekf/output_frame: map
 * /robot_pose_ekf/self_diagnose: False
 * /robot_pose_ekf/sensor_timeout: 1.0
 * /robot_pose_ekf/vo_used: False
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES
  /
    link1_broadcaster (tf2_ros/static_transform_publisher)
    link2_broadcaster (tf2_ros/static_transform_publisher)
    map_server (map_server/map_server)
    move_base (move_base/move_base)
    robot_pose_ekf (robot_pose_ekf/robot_pose_ekf)

ROS_MASTER_URI=http://localhost:11311

process[link1_broadcaster-1]: started with pid [19739]
process[link2_broadcaster-2]: started with pid [19740]
process[robot_pose_ekf-3]: started with pid [19758]
process[map_server-4]: started with pid [19776]
process[move_base-5]: started with pid [19841]
[ INFO] [1540834009.296069299, 13.907000000]: Loading from pre-hydro parameter style
[ INFO] [1540834009.310812145, 13.922000000]: Using plugin "static_layer"
[ INFO] [1540834009.317225170, 13.928000000]: Requesting the map...
[ INFO] [1540834009.520453286, 14.132000000]: Resizing costmap to 100 X 100 at 1.000000 m/pix
[ INFO] [1540834009.621709934, 14.232000000]: Received a 100 X 100 map at 1.000000 m/pix
[ INFO] [1540834009.626584742, 14.237000000]: Using plugin "obstacle_layer"
[ INFO] [1540834009.628411482, 14.239000000]:     Subscribed to Topics: 
[ INFO] [1540834009.638543716, 14.249000000]: Using plugin "inflation_layer"
[ INFO] [1540834009.685154386, 14.296000000]: Loading from pre-hydro parameter style
[ INFO] [1540834009.699557342, 14.311000000]: Using plugin "static_layer"
[ INFO] [1540834009.703080097, 14.314000000]: Requesting the map...
[ INFO] [1540834009.705428648, 14.317000000]: Resizing costmap to 100 X 100 at 1.000000 m/pix
[ INFO] [1540834009.806388473, 14.417000000]: Received a 100 X 100 map at 1.000000 m/pix
[ INFO] [1540834009.811111339, 14.422000000]: Using plugin "obstacle_layer"
[ INFO] [1540834009.814059193, 14.425000000]:     Subscribed to Topics: 
[ INFO] [1540834009.824545616, 14.435000000]: Using plugin "inflation_layer"
[ INFO] [1540834009.866551233, 14.477000000]: Created local_planner teb_local_planner/TebLocalPlannerROS
[ INFO] [1540834009.920261537, 14.531000000]: No robot footprint model specified for trajectory optimization. Using point-shaped model.
[ INFO] [1540834009.920337374, 14.531000000]: Parallel planning in distinctive topologies enabled.
[ INFO] [1540834009.920377444, 14.531000000]: No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles.
[ INFO] [1540834010.103009231, 14.714000000]: Recovery behavior will clear layer obstacles
[ INFO] [1540834010.108103576, 14.719000000]: Recovery behavior will clear layer obstacles
[ INFO] [1540834010.143028158, 14.754000000]: odom received!

Sometimes (not always) these warnings are shown:

[ WARN] [1540833817.329080357, 41.430000000]: TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).
[ INFO] [1540833840.461887304, 64.530000000]: TebLocalPlannerROS: oscillation recovery disabled/expired.
[ WARN] [1540833864.453728681, 88.480000000]: TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).

Excuse me if the answer is very obvious. I looked through all the related questions and tutorials on the ROS wiki and this website but I could not find a solution for my problem.

Thanks in advance! I hope I did all the formatting correctly.


Originally posted by mliefaard on ROS Answers with karma: 26 on 2018-10-29

Post score: 0


Original comments

Comment by pavel92 on 2018-10-30:
How are you giving the goals? The moving of the red poseArray is probably due to the planner re-planning the path. Teb local planner requires a lot of tuning and is not as intuitive as the other planners. You can try changing the goal tolerance parameters (yaw_goal_tolerance, xy_goal_tolerance)

Comment by mliefaard on 2018-10-30:
@pavel92 I am giving my goals using the actionlib interface for move_base:

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

This is the correct way to give movement goals right?

I am currently looking into changing the parameters, thanks for your response

Comment by pavel92 on 2018-10-30:
You can give navigation goals directly in RViz using the 2D Nav Goal and allows you to easily set the desired orientation

Comment by mliefaard on 2018-10-30:
Yes, the navigation goals set directly in the RViz interface do not cause the same issue. I did not know that that was using a 2D Nav Goal so I am going to try to use 2D Nav Goal from my C++ node and see if I encounter the same issue as with the actionlib interface.

Comment by mliefaard on 2018-10-30:
The issue still persists if I use 2D Nav Goals published to move_base_simple/goal. Sometimes the teb_local_planner just does not generate a valid PoseArray to follow. It seems like it is trying to strafe as seen in the image that I added. Could this be caused by my parameters for teb_local_planner?

Comment by croesmann on 2018-10-30:
can you please edit your question and add the teb_local_planner params?

Comment by pavel92 on 2018-10-31:
If the robot rotates to the desired orientation with the goals set in RViz that means that probably the problem is not coming from the goal tolerance parameters from teb_local_planner params but may be coming from different parameters so please post your teb_local_planner_params.yaml file

Comment by mliefaard on 2018-10-31:
Sorry for the late response. I spent a lot of time configuring teb_local_planner but the problem persisted so I eventually decided to try other planners as pavel92 suggested earlier. eband_local_planner is working perfectly for me. Thanks for pointing me in the right direction!

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

After a lot of debugging it turned out that my teb_local_planner configuration was the problem since the problem also persisted when I issued a goal in RViz instead of from code.

As @pavel92 noted, teb_local_planner requires a lot of fine tuning. So I eventually gave up and switched to eband_local_planner which works perfectly for my use case. In my experience eband_local_planner was way simpler to configure and it worked immediately for my diff_drive robot.

(Unfortunately I cannot mark this as answered because I don't have enough points)


Originally posted by mliefaard with karma: 26 on 2018-10-31

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.