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](http://wiki.ros.org/tf2/Tutorials/Quaternions) 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 "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.
↧
