Hi! I'm writing a node, that should allow my turtlebot3_waffle to move to ar_tag, sending tf between base_link and ar_Tag as a goal to move_base.
But, after launching this node I have this:
[ WARN] [1553587624.636453683, 2739.978000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1553587637.825312516, 2745.080000000]: Rotate recovery behavior started.
[ WARN] [1553587662.356952081, 2756.631000000]: Clearing costmap to unstuck robot (1.840000m).
[ WARN] [1553587673.253428778, 2761.732000000]: Rotate recovery behavior started.
[ERROR] [1553587684.935379862, 2766.931000000]: Aborting because a valid plan could not be found. Even after executing all recovery behaviors
this is my code:
#!/usr/bin/env python
import rospy
import math
import tf2_ros
import geometry_msgs.msg
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
def move_base(bar_x, bar_y):
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "base_link"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = bar_x
goal.target_pose.pose.position.y = bar_y
goal.target_pose.pose.position.z = 0
goal.target_pose.pose.orientation.w = 1
client.send_goal(goal)
wait = client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
return client.get_result()
if __name__ == '__main__':
try:
rospy.init_node('ar_tag_navigation')
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
trans = tfBuffer.lookup_transform('base_link', 'ar_marker_0', rospy.Time())
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rate.sleep()
continue
BAr_X = trans.transform.translation.x
BAr_Y = trans.transform.translation.y
result = move_base(BAr_X, BAr_Y)
if result:
rospy.loginfo("Goal execution done!")
except rospy.ROSInterruptException:
rospy.loginfo("Navigation test finished.")
I realy don't know what is wrong. I saw tf tree and it's ok
↧