Inquiry on how to use MoveBaseActionGoal with Turtlebot2
Im trying to use the code below to move my turtlebot to goals inside of my map as a test.
#!/usr/bin/python3
import rospy
import actionlib # Use the actionlib package for client and server
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
GoalPoints = [ [(-1.06042258242,-0.438168283892, 0.0), (0.0, 0.0, 0.940821633704, 0.338902129758)],
[(-0.360606235518,0.964747030027, 0.0), (0.0, 0.0, 0.494397065335, 0.869236182972)]]
def assign_goal(pose):
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.frame_id = 'map'
goal_pose.target_pose.pose.position.x = pose[0][0]
goal_pose.target_pose.pose.position.y = pose[0][1]
goal_pose.target_pose.pose.position.z = pose[0][2]
goal_pose.target_pose.pose.orientation.x = pose[1][0]
goal_pose.target_pose.pose.orientation.y = pose[1][1]
goal_pose.target_pose.pose.orientation.z = pose[1][2]
goal_pose.target_pose.pose.orientation.w = pose[1][3]
return goal_pose
if __name__ == '__main__':
rospy.init_node('MoveTBtoGoalPoints')
# Create a SimpleActionClient of a move_base action type and wait for server.
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
#
for TBpose in GoalPoints:
TBgoal = assign_goal(TBpose) # For each goal point assign pose
client.send_goal(TBgoal)
success = client.wait_for_result(rospy.Duration(900))
# client.wait_for_result()
if success:
# if (client.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo("success")
else:
rospy.loginfo("failed")
This is what I get in my terminal where I launched my roslaunch turtlebot_navigation amcl_demo.launch map_file:=/home/turtlebot/mymap2.yaml.
[ WARN] [1643772990.930868465]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1643772996.130840366]: Rotate recovery behavior started.
[ WARN] [1643773009.481429243]: Clearing costmap to unstuck robot (1.840000m).
[ WARN] [1643773014.681199480]: Rotate recovery behavior started.
[ERROR] [1643773028.031572793]: Aborting because a valid plan could not be found. Even after executing all recovery behaviors
[ WARN] [1643773033.265505642]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1643773038.465451459]: Rotate recovery behavior started.
[ WARN] [1643773051.815918846]: Clearing costmap to unstuck robot (1.840000m).
[ WARN] [1643773057.015864710]: Rotate recovery behavior started.
[ERROR] [1643773070.316353760]: Aborting because a valid plan could not be found. Even after executing all recovery behaviors
The turtlebot simply rotates when the code is run and I get a "suceeded" message as well when clearly the robot ahs completely failed to move anywhere. I need help figuring out what's going on.
Asked by distro on 2022-02-01 22:50:14 UTC
Answers
Read carefully this guide to ROS Navigation tuning:
https://kaiyuzheng.me/documents/navguide.pdf
It will explain you a lot of parameters of ROS Navigation stack.
I think that the rotations you have observed are a part of 6 Recovery behaviors:
6 Recovery Behaviors
An annoying thing about robot navigation is that the robot may get stuck. Fortunately, the navigation stack has recovery behaviors built-in. Even so, sometimes the robot will exhaust all available recovery behaviors and stay still. Therefore, we may need to figure out a more robust solution.
Types of recovery behaviors ROS navigation has two recovery behaviors. They are clear costmap recovery and rotate recovery. Clear costmap recovery is basically reverting the local costmap to have the same state as the global costmap. Rotate recovery is to recover by rotating 360 degrees in place.
Asked by ljaniec on 2022-02-02 11:15:20 UTC
Comments
@ljaniec Thanks I'll study it as it's just good to know regardless and will definitely help my general understanding. However, when I use 2D Nav Goal provided by RVIZ to set goals for the robot, it doesn't get stuck or need to recover. With my code I assumed I was using the same topics that 2D Nav Goal publishes to. It just isnt clear to me why my code gives me this issue but using 2D Nav Goal doesnt.
Asked by distro on 2022-02-02 18:33:55 UTC
I would check through ROS CLI what (and where) messagges are sent with the RViz and compare it to your code. Are you sure that your messages are built correctly? Is initial pose correct? Is real robot in correct place at the start of your tests? Did your code for assigning goals come from some checked sources? The ROS Wiki suggest this tutorial for ROS Navigation with TurtleBot, can you read it and compare to your code?
Asked by ljaniec on 2022-02-06 10:34:56 UTC
@ljaniec I have messed around with parameters and nothing has helped.if you perhaps have a turtlebot2 could you please try these parameters yourself? I have them here: @ljaniec I have messed around with parameters and nothing has helped.if you perhaps have a turtlebot2 could you please try these parameters yourself? I have them here:
DWAPlannerROS:
# Robot Configuration Parameters - Kobuki
max_vel_x: 0.22 # 0.55
min_vel_x: 0.0
max_vel_y: 0.0 # diff drive robot
min_vel_y: 0.0 # diff drive robot
max_trans_vel: 0.15 # choose slightly less than the base's capability,0.5
min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity
trans_stopped_vel: 0.1
# Warning!
# do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
# are non-negligible and small in place rotational velocities will be created.
max_rot_vel: 2.0 # choose slightly less than the base's capability,5.0
min_rot_vel: 0.4 # this is the min angular velocity when there is negligible translational velocity
rot_stopped_vel: 0.4
acc_lim_x: 1.0 # maximum is theoretically 2.0, but we
acc_lim_theta: 2.0
acc_lim_y: 0.0 # diff drive robot
# Goal Tolerance Parameters
yaw_goal_tolerance: 0.4 # 0.05,0.3
xy_goal_tolerance: 0.3 # 0.10,0.15
# latch_xy_goal_tolerance: false
# Forward Simulation Parameters
sim_time: 1.0 # 1.7,1.0
vx_samples: 20 # 3
vy_samples: 1 # diff drive robot, there is only one sample
vtheta_samples: 40 # 20
# Trajectory Scoring Parameters
path_distance_bias: 34.0 # 32.0,64.0 - weighting for how much it should stick to the global path plan
goal_distance_bias: 64.0 # 24.0,34.0 - wighting for how much it should attempt to reach its goal
occdist_scale: 0.01 # 0.01 - weighting for how much the controller should avoid obstacles,0.5
forward_point_distance: 0.325 # 0.325 - how far along to place an additional scoring point
stop_time_buffer: 0.2 # 0.2 - amount of time a robot must stop in before colliding for a valid traj.
scaling_speed: 0.25 # 0.25 - absolute velocity at which to start scaling the robot's footprint
max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed.
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags
# Debugging
publish_traj_pc : true
publish_cost_grid_pc: true
global_frame_id: odom
# Differential-drive robot configuration - necessary?
# holonomic_robot: false
costmap_common_params.yaml:
max_obstacle_height: 0.60 # assume something like an arm is mounted on top of the robot
# Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation)
robot_radius: 0.20 # distance a circular robot should be clear of the obstacle (kobuki 0.18)
# footprint: [[x0, y0], [x1, y1], ... [xn, yn]] # if the robot is not circular
map_type: voxel
obstacle_layer:
enabled: true
max_obstacle_height: 0.6
origin_z: 0.0
z_resolution: 0.2
z_voxels: 2
unknown_threshold: 15
mark_threshold: 0
combination_method: 1
track_unknown_space: true #true needed for disabling global path planning through unknown space
obstacle_range: 2.5
raytrace_range: 3.0
origin_z: 0.0
z_resolution: 0.2
z_voxels: 2
publish_voxel_map: false
observation_sources: scan bump
scan:
data_type: LaserScan
topic: scan
marking: true
clearing: true
min_obstacle_height: 0.25
max_obstacle_height: 0.35
bump:
data_type: PointCloud2
topic: mobile_base/sensors/bumper_pointcloud
marking: true
clearing: false
min_obstacle_height: 0.0
max_obstacle_height: 0.15
# for debugging only, let's you see the entire voxel grid
#cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
inflation_layer:
enabled: true
cost_scaling_factor: 5.0 # exponential rate at which the obstacle cost drops off(default is 10) 2.5, 5.0
inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths.
static_layer:
enabled: true
Asked by distro on 2022-02-03 18:33:58 UTC
Comments