前言
前面一章已经完成了turtlebot3的仿真操作以及slam导航,这一节将实现脚本控制turtlebot3在多点之间来回导航。工程链接:
下载地址
实现过程
启动环境地图
roslaunch turtlebot3_gazebo turtlebot3_world.launch
利用上一章所讲的建好的地图导航并打开rviz可视化
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml
利用2D pose Estimate修改位置
鼠标点击地图上的位置就是你的初始位置,箭头指向就是你的机器人初始朝向。
修改成如下图所示即可:
准备工作
创建多点导航脚本
cd ~/wanderbot_ws/src/wanderbot/src
vi patrol.py
patrol.py:多点导航脚本
#!/usr/bin/env python
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
waypoints = [ # <1>
[(1.522, 0.444, 0.0), (0.0, 0.0, -0.519, 0.85)],
[(-2.0432, -0.439, 0.0), (0.0, 0.0, -0.559, 0.82902)]
]
def goal_pose(pose): # <2>
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.frame_id = 'map'
goal_pose.target_pose.pose.position.x = pose[0][0]
goal_pose.target_pose.pose.position.y = pose[0][1]
goal_pose.target_pose.pose.position.z = pose[0][2]
goal_pose.target_pose.pose.orientation.x = pose[1][0]
goal_pose.target_pose.pose.orientation.y = pose[1][1]
goal_pose.target_pose.pose.orientation.z = pose[1][2]
goal_pose.target_pose.pose.orientation.w = pose[1][3]
return goal_pose
if __name__ == '__main__':
rospy.init_node('patrol')
client = actionlib.SimpleActionClient('move_base', MoveBaseAction) # <3>
client.wait_for_server()
while True:
for pose in waypoints: # <4>
print("goal:x=%f y=%f"%(pose[0][0],pose[0][1]))
goal = goal_pose(pose)
client.send_goal(goal)
client.wait_for_result()
确定导航点位置
rostopic list
查看所有话题名称
显然导航坐标的话题是amcl_pose。
AMCL:在ros中,我们使用amcl包在地图中定位机器人。amcl节点来实现一系列的概率定位算法,总称为自适应蒙特卡洛定位算法(Adaptive Monte Carlo Locatization)。
具体的,他使用sample_motion_model_odometry、beam_range_finder_model、likelihood_filed_range_finder_model、Augmented_MCL和KLD_Sampling_MCL算法。
所以我们监听这个话题的内容,这里还是用到ros的调试工具:rostopic
rostopic echo amcl_pose -n 1
打印出当前坐标:
pose:
position:
x: -2.03239229723
y: -0.408803583203
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.529167851235
w: 0.848517168488
显然这就是我们所要的坐标了。
然后在rviz中用
随便点几个点导航
再次打印到达目标的位置:
rostopic echo amcl_pose -n 1
ok我们知道了初始坐标和目标坐标:
初始:
position:
x: -2.03239229723
y: -0.408803583203
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.529167851235
w: 0.848517168488
目标:
position:
x: 1.76888229632
y: 0.588761452838
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.459069601992
w: 0.888400304214
把他们写到我们写好的多点导航脚本里面。
修改这部分内容即可。
修改完后退出,设置权限
chmod 777 patrol.py
启动多点导航
rosrun wanderbot patrol.py
可以看见机器人在初始坐标和目标点间来回走动了。
总结
emmm挺好玩的吧,顺便复习下前面学的。。。enjoy it~
参考
《ros机器人编程实践》
评论(2)
您还未登录,请登录后发表或查看评论