Map data not received by MoveBaseActionServer

asked 2022-07-24 05:27:33 -0600

distro gravatar image

This is my move base action_server action client that was inspired by [this].(https://uos.github.io/mbf_docs/tutorials/beginner/basic_navigation/)

I want to test to see if I can autonomously move my turtlebot3(melodic) around while it's performig SLAM. I'm running my server through a launch file. I get an error:Request for map failed; trying again... I don't know how to connect to the map data which SLAM is getting. Please help. I have no clue what I'm doing wrong. Here is my client:

#! /usr/bin/env python

import rospy
import actionlib
import mbf_msgs.msg as mbf_msgs
import move_base_msgs.msg as mb_msgs
import tf
from actionlib_msgs.msg import GoalStatus

def create_goal(x, y, z, xx, yy, zz, ww):
    goal = mb_msgs.MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = x
    goal.target_pose.pose.position.y = y
    goal.target_pose.pose.position.z = z
    goal.target_pose.pose.orientation.x = xx
    goal.target_pose.pose.orientation.y = yy
    goal.target_pose.pose.orientation.z = zz
    goal.target_pose.pose.orientation.w = ww
    return goal

def move(goal):
    client.send_goal(goal)
    client.wait_for_result()
    return client.get_state() == GoalStatus.SUCCEEDED


def drive_circle():
    goals = [   create_goal(-1.75, 0.74, 0, 0, 0, 0.539, 0.843),
                create_goal(-0.36, 1.92, 0, 0, 0, -0.020, 0.999),
                create_goal(0.957, 1.60, 0, 0, 0, -0.163, 0.987),
                create_goal(1.8741, 0.3830, 0, 0, 0, -0.70, 0.711),
                create_goal(1.752, -0.928, 0, 0, 0, -0.856, 0.517),
                create_goal(0.418, -2.116, 0, 0, 0, 0.998, 0.0619),
                create_goal(-0.775, -1.80, 0, 0, 0, 0.954, 0.300),
                create_goal(-1.990, -0.508, 0, 0, 0, -0.112, 0.999)
    ]

    for goal in goals:
        rospy.loginfo("Attempting to drive to %s %s", goal.target_pose.pose.position.x, goal.target_pose.pose.position.y)
        if not move(goal):
            return False

    return True

if __name__ == '__main__':
    try:
        rospy.init_node('mb_relay_client')

        client = actionlib.SimpleActionClient('move_base', mb_msgs.MoveBaseAction)
        client.wait_for_server(rospy.Duration(10))
        rospy.loginfo("Connected to SimpleActionServer 'move_base'")

        result = drive_circle()
        rospy.loginfo("Drove circle with result: %s", result)

    except rospy.ROSInterruptException:
        rospy.logerror("program interrupted before completion")

Here is my Server:

import actionlib
import rospy
import mbf_msgs.msg as mbf_msgs
import move_base_msgs.msg as mb_msgs
from geometry_msgs.msg import PoseStamped
import tf

robotpose = PoseStamped()

def mb_execute_cb(msg):
    mbf_mb_ac.send_goal(mbf_msgs.MoveBaseGoal(target_pose=msg.target_pose),
                        feedback_cb=mbf_feedback_cb)

    rospy.logdebug("Relaying move_base goal to mbf")
    mbf_mb_ac.wait_for_result()

    status = mbf_mb_ac.get_state()
    result = mbf_mb_ac.get_result()

    rospy.logdebug("MBF execution completed with result [%d]: %s", result.outcome, result.message)
    if result.outcome == mbf_msgs.MoveBaseResult.SUCCESS:
        mb_as.set_succeeded(mb_msgs.MoveBaseResult(), "Goal reached.")
    else:
        mb_as.set_aborted(mb_msgs.MoveBaseResult(), result.message)

def mbf_feedback_cb():
    listener = tf.TransformListener()
    listener.waitForTransform('map', 'base_footprint', rospy.Time(0), rospy.Duration(50))
    (trans, rot) = listener.lookupTransform('map', 'base_footprint', rospy.Time(0))
    robotpose.header.stamp = rospy.get_rostime()
    robotpose.header.frame_id = 'map'
    robotpose.pose.position.x = trans[0]
    robotpose.pose.position.y = trans[1]
    robotpose ...
(more)
edit retag flag offensive close merge delete