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.
add a comment
