understanding tf with move_base

asked 2019-03-26 03:23:40 -0500

EGOR gravatar image

updated 2019-03-26 03:42:45 -0500

mgruhler gravatar image

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

edit retag flag offensive close merge delete

Comments

This has nothing to do with how you create or send the goal to move_base This is all about the data in your costmap. Please show the configuration files of your costmap and maybe add a screenshot of rviz which shows what the costmap looks like when you get this problem...

mgruhler gravatar image mgruhler  ( 2019-03-26 03:44:29 -0500 )edit

Here is costmap_comon_params:

obstacle_range: 3.0 raytrace_range: 3.5

footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]] #robot_radius: 0.17

inflation_radius: 1.0 cost_scaling_factor: 3.0

map_type: costmap observation_sources: scan scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

here is global_costmap params

global_costmap: global_frame: /map robot_base_frame: /base_footprint

update_frequency: 10.0 publish_frequency: 10.0 transform_tolerance: 0.5

static_map: true

And also local_costmap_params

local_costmap: global_frame: /odom robot_base_frame: /base_footprint

update_frequency: 10.0 publish_frequency: 10.0 transform_tolerance: 0.5

static_map: false
rolling_window: true width: 3 height: 3 resolution: 0.05

EGOR gravatar image EGOR  ( 2019-03-26 04:32:48 -0500 )edit