Turbot与python编程-实现带避障的直行导航

Turbot与python教程-实现带避障的直行导航

说明：

• 介绍如何实现通过python控制turbot实现带避障的直行导航

代码：

• 参考代码：github
• 实现代码：

#Code is inspired by http://wiki.ros.org/navigation/Tutorials/SendingSimpleGoals (written in C++).
#TurtleBot must have minimal.launch & amcl_demo.launch running prior to starting this script.
#goForwardWithAvoidObstacle.py

import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
from actionlib_msgs.msg import *

class GoForwardAvoid():
    def __init__(self):
        rospy.init_node('nav_test', anonymous=False)

    #what to do if shut down (e.g. ctrl + C or failure)
    rospy.on_shutdown(self.shutdown)

    
    #tell the action client that we want to spin a thread by default
    self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
    rospy.loginfo("wait for the action server to come up")
    #allow up to 5 seconds for the action server to come up
    self.move_base.wait_for_server(rospy.Duration(5))

    #we'll send a goal to the robot to move 3 meters forward
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = 'base_link'
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = 3.0 #3 meters
    goal.target_pose.pose.orientation.w = 1.0 #go forward

    #start moving
        self.move_base.send_goal(goal)

    #allow TurtleBot up to 60 seconds to complete task
    success = self.move_base.wait_for_result(rospy.Duration(60)) 


    if not success:
                self.move_base.cancel_goal()
                rospy.loginfo("The base failed to move forward 3 meters for some reason")
        else:
        # We made it!
        state = self.move_base.get_state()
        if state == GoalStatus.SUCCEEDED:
            rospy.loginfo("Hooray, the base moved 3 meters forward")



    def shutdown(self):
        rospy.loginfo("Stop")


if __name__ == '__main__':
    try:
        GoForwardAvoid()
    except rospy.ROSInterruptException:
        rospy.loginfo("Exception thrown")

演示：

• 主机，新终端，启动底盘

$ roslaunch turbot_bringup minimal.launch

• 主机，新终端，启动AMCL，kinect版

$ roslaunch turbot_slam amcl_demo.launch map_file:=/path_to_map/xxx.yaml

• 或者使用激光AMCL

$ roslaunch turbot_slam laser_amcl_demo.launch map_file:=/path_to_map/xxx.yaml

• path_to_map使用实际地图路径
• xxx为实际的地图名称

• 从机，新终端，启动脚本

$ rosrun turbot_code goForwardWithAvoidObstacle.py

• 机器人向前直行3米，并能动态避开障碍。