< >
Home » Turbot与Python教程 » Turbot与python编程-实现带避障的定点导航

Turbot与python编程-实现带避障的定点导航

Turbot与python编程-实现带避障的定点导航

说明:

  • 介绍如何利用turbot机器人利用雷达或kinect进行定点导航,同时能动态避开障碍

代码:

  • 实现代码:
# TurtleBot must have minimal.launch & amcl_demo.launch
# running prior to starting this script
# For simulation: launch gazebo world & amcl_demo prior to run this script
# goToSpecificPointOnMap.py

import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion
form sys import argv

class GoToPose():
    def __init__(self):

        self.goal_sent = 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))

    def goto(self, pos, quat):

        # Send a goal
        self.goal_sent = True
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = 'map'
    goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.pose = Pose(Point(pos['x'], pos['y'], 0.000),
                                     Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4']))

    # 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)) 

        state = self.move_base.get_state()
        result = False

        if success and state == GoalStatus.SUCCEEDED:
            # We made it!
            result = True
        else:
            self.move_base.cancel_goal()

        self.goal_sent = False
        return result

    def shutdown(self):
        if self.goal_sent:
            self.move_base.cancel_goal()
        rospy.loginfo("Stop")
        rospy.sleep(1)

if __name__ == '__main__':
    try:
        rospy.init_node('nav_test', anonymous=False)
        navigator = GoToPose()

        # Customize the following values so they are appropriate for your location
        px = float(argv[1])
        py = float(argv[2])
        position = {'x': px, 'y' : py}
        quaternion = {'r1' : 0.000, 'r2' : 0.000, 'r3' : 0.000, 'r4' : 1.000}

        rospy.loginfo("Go to (%s, %s) pose", position['x'], position['y'])
        success = navigator.goto(position, quaternion)

        if success:
            rospy.loginfo("Hooray, reached the desired pose")
        else:
            rospy.loginfo("The base failed to reach the desired pose")

        # Sleep to give the last log messages time to be sent
        rospy.sleep(1)

    except rospy.ROSInterruptException:
        rospy.loginfo("Ctrl-C caught. Quitting")

步骤:

  • 主机,新终端,启动底盘
$ roslaunch turbot_bringup minimal.launch
  • 主机上,新终端,启动雷达amcl, 并指定地图
$ roslaunch turbot_slam laser_amcl_demo.launch map_file:=/path_to_map/xxx.yaml
  • path_to_map为你地图的目录
  • xxx为你地图名称
  • 从机,新终端,启动rviz
$ roslaunch turbot_rviz nav.launch 
  • 获取地图数据方法
    • 从rviz界面上,点击Publish point,鼠标移动到地图
    • 查看rviz的做下动态状态条,会出现相应的坐标信息
    • 记录合适的坐标信息
  • 效果图:

请输入图片描述

  • 指定格式为:
$ rosrun turbot_code goToSpecificPointOnMap.py  x  y 
  • 实际如:
$ rosrun turbot_code goToSpecificPointOnMap.py  -1.14 -1.44
  • 机器人会自动运行到指定的目标点, 并能动态避开障碍。

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: turbot与python编程