Oscillation when using orientation field of move_base with teb_local_planner.
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.
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 ...
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)
@pavel92 I am giving my goals using the actionlib interface for move_base:
This is the correct way to give movement goals right?
I am currently looking into changing the parameters, thanks for your response
You can give navigation goals directly in RViz using the 2D Nav Goal and allows you to easily set the desired orientation
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.
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?
can you please edit your question and add the teb_local_planner params?
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
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!