naturerobots / mbf_tutorials

Robot Navigation Tutorials for Move Base Flex (MBF)
13 stars 7 forks source link

Inquiry of efficacy of package for unexplored environments. #8

Open PhilipAmadasun opened 2 years ago

PhilipAmadasun commented 2 years ago

I wish to use your movebase actoin client and server to move the turtlebot3 robot around unexplored environments while peforming SLAM. Whta I'm msiing is how exactly to connect my mapserver to my movebase server, because it needs the map data to localize itself to start moving, How do I I feed it map information that is being generated by SLAM(turtlebot3_slam.launch) basically? What do I need to chnage withing your package to achieve this?

PhilipAmadasun commented 2 years ago

I made changes to the 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.pose.position.z = trans[2]
    robotpose.pose.orientation.x = rot[0]
    robotpose.pose.orientation.y = rot[1]
    robotpose.pose.orientation.z = rot[2]
    robotpose.pose.orientation.w = rot[3]
    print("hell")

    mb_as.publish_feedback(mb_msgs.MoveBaseFeedback(base_position=robotpose))

if __name__ == '__main__':
    rospy.init_node("move_base")

    # move_base_flex get_path and move_base action clients
    mbf_mb_ac = actionlib.SimpleActionClient("move_base_flex/move_base", mbf_msgs.MoveBaseAction)
    mbf_mb_ac.wait_for_server(rospy.Duration(10))

    mb_as = actionlib.SimpleActionServer('move_base', mb_msgs.MoveBaseAction, mb_execute_cb, auto_start=False)
    mb_as.start()

    rospy.spin()

But I get an error of Map data not being received. Would you happen to know what any of the possible issues could be?