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

机器人会自动运行到指定的目标点， 并能动态避开障碍。