智能⼩车建图导航-在rviz中导航（代码解读）
养不教⼀、解读launch⽂件：fake_mrobot_with_lar.launch
<launch>
<param name="/u_sim_time" value="fal" />
<!-- 加载机器⼈URDF/Xacro模型 -->
<!-- 关于这个⽽⾔，我们不需要太多的深究，因为不是了解框架的重点 -->
<arg name="urdf_file" default="$(find xacro)/xacro --inorder '$(find mrobot_description)/urdf/mrobot_with_rplidar.urdf.xacro'" /> <param name="robot_description" command="$(arg urdf_file)" />
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen" clear_params="true">
<rosparam file="$(find mrobot_bringup)/config/fake_mrobot_arbotix.yaml" command="load" />
<param name="sim" value="true"/>
</node>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
<param name="publish_frequency" type="double" value="20.0" />
</node>
</launch>
⼆、解读launch⽂件：fake_nav_demo.launch
<launch>
<!-- 告诉ROS节点要求获取ROS话题/clock发布的时间信息（默认为true）-->
<param name="u_sim_time" value="fal" />
<!-- 这个参数当回放bag数据集是设置为true，此时说明系统使⽤的是仿真时间，如果设置为fal，则系统使⽤walltime -->
<!-- 设置地图的配置⽂件，当然我把这个地图换掉了，yaml⽂件⾥⾯包含了地图的各种信息 -->
<arg name="map" default="map.yaml" />
<!-- 运⾏地图服务器，并且加载设置的地图-->
<node name="map_rver" pkg="map_rver" type="map_rver" args="$(find mrobot_navigation)/maps/$(arg map)"/>
<!-- 运⾏move_ba节点 -->
<include file="$(find mrobot_navigation)/launch/fake_move_ba.launch" />
<!-- 运⾏虚拟定位，兼容AMCL输出 -->
<node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" />
<!-- 对于虚拟定位，需要设置⼀个/odom与/map之间的静态坐标变换 -->
<node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />
<!-- 运⾏rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find mrobot_navigation)/rviz/nav.rviz"/>
雷锋报</launch>
fake_localization包提供了⼀个单⼀ROS node,fake_localization, ⽤来替代定位系统，它也提供了amcl定位算法ROS API的⼦集。
该node在仿真中被频繁使⽤，是⼀种不需要⼤量计算资源就能进⾏定位的⽅式。具体来说，fake_localization是将odometry数据转换成位姿，粒⼦云以及amcl⽤的那种tf格式的tf数据。
三、解读launch⽂件：fake_move_ba.launch
<launch>
<!-- 启动move_ba节点，加载配置所有的配置⽂件，下⾯的这⼏个配置⽂件，都是我们在导航功能包⾥⾯讲解过了 -->
<node pkg="move_ba" type="move_ba" respawn="fal" name="move_ba" output="screen" clear_params="true">
<rosparam file="$(find mrobot_navigation)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find mrobot_navigation)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find mrobot_navigation)/config/fake/local_costmap_params.yaml" command="load" />
<rosparam file="$(find mrobot_navigation)/config/fake/global_costmap_params.yaml" command="load" />
<rosparam file="$(find mrobot_navigation)/config/fake/ba_local_planner_params.yaml" command="load" />
</node>
</launch>
四、解读python⽂件：random_navigation.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib;
#这个库和rospy差不多，都是提供API接⼝的
import rospy
import actionlib
#actionlib是ROS中⼀个很重要的功能包集合
#尽管在ROS中已经提供了srevice机制来满⾜请求—响应式的使⽤场景
#但是假如某个请求执⾏时间很长，在此期间⽤户想查看执⾏的进度或者取消这个请求的话，rvice机制就不能满⾜，但是actionlib可满⾜⽤户这种需求。from actionlib_msgs.msg import *
from geometry_msgs.msg import Po, PoWithCovarianceStamped, Point, Quaternion, Twist
from move_ba_msgs.msg import MoveBaAction, MoveBaGoal
from random import sample
from math import pow, sqrt
class NavTest():
def __init__(lf):
rospy.init_node('random_navigation', anonymous=True)
<_shutdown(lf.shutdown)
# 在每个⽬标位置暂停的时间
#这个函数的作⽤就是得到设置后⾯的参数的值
# 到达⽬标的状态
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# 设置⽬标点的位置
笔记的英文# 如果想要获得某⼀点的坐标，在rviz中点击 2D Nav Goal 按键，然后单机地图中⼀点（或者可以直接在rviz中⽤publish）
# 在终端中就会看到坐标信息
locations = dict()
#后⾯的四元数应该表⽰的到点之后的位姿
locations['p1'] = Po(Point(5.61, -4.4, -0.00143), Quaternion(0.000, 0.000, -0.013, 1.000))
locations['p2'] = Po(Point(-4.53, 5.04, -0.00143), Quaternion(0.000, 0.000, 0.063, 0.998))
locations['p3'] = Po(Point(2.98, 0.975, -0.00143), Quaternion(0.000, 0.000, 0.946, -0.324))
locations['p4'] = Po(Point(0.708, -5.75, -0.00143), Quaternion(0.000, 0.000, 0.946, -0.324))
locations['p5'] = Po(Point(6.142, -3.75, -0.00143), Quaternion(0.000, 0.000, 0.946, -0.324))
locations['p5'] = Po(Point(6.142, -3.75, -0.00143), Quaternion(0.000, 0.000, 0.946, -0.324))
# 发布控制机器⼈的消息
# 订阅move_ba服务器的消息，其实和subscriber差不多
rospy.loginfo("Waiting for move_ba ")
# 60s等待时间限制
粗准焦螺旋
rospy.loginfo("Connected to move ba rver")
# 保存机器⼈的在rviz中的初始位置
initial_po = PoWithCovarianceStamped()
# 保存成功率、运⾏时间、和距离的变量
n_locations = len(locations) #在这个点的条件下，为5，表⽰⼀共有5个位置
n_goals = 0
n_success = 0 #成功导航的次数，后⾯成功⼀次就加1
i = n_locations #表⽰第i个位置
distance_traveled = 0 #设置穿过的路程，计算所有导航次数以来的总路程
start_time = w() #设置开始运动的初试时间
running_time = 0 #设置每次开始到结束的时间差
location = "" #设置当前位置
last_location = "" #设置最后⼀个位置
# 确保有初始位置
while initial_po.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# 开始主循环，随机导航
while not rospy.is_shutdown():
# 如果已经⾛完了所有点，再重新开始排序，就是说如果现在是的位置i就是5
if i == n_locations:英国的英语缩写
i = 0 #这个时候就把i再变成0，重新开始
quence = sample(locations, n_locations) #这个是将五个位置随机排成好⼀个次序
# 如果最后⼀个点（上⼀个点）和第⼀个点相同，则跳过，从后⼀个点开始
if quence[0] == last_location:
i = 1
# 在当前的排序中获取下⼀个⽬标点
location = quence[i]
# 跟踪⾏驶距离
# 使⽤更新的初始位置
if initial_po.header.stamp == "":
#这⾥通过三⾓形来计算斜边的边长（路程距离）
distance = sqrt(pow(locations[location].position.x -
locations[last_location].position.x, 2) +
pow(locations[location].position.y -
locations[last_location].position.y, 2))
el:
情感对白
rospy.loginfo("Updating current po.")
distance = sqrt(pow(locations[location].position.x -
initial_po.po.po.position.x, 2) +
pow(locations[location].position.y -
initial_po.po.po.position.y, 2))
initial_po.header.stamp = ""
# 存储上⼀次的位置，计算距离
# 存储上⼀次的位置，计算距离
last_location = location
# 计数器加1
i += 1
n_goals += 1
# 设定下⼀个⽬标点
# 让⽤户知道下⼀个位置
rospy.loginfo("Going to: " + str(location))
# 向下⼀个位置进发
# 五分钟时间限制（就是说这个导航的过程有没有在5分钟内完成）
finished_within_time = lf.move_ba.wait_for_result(rospy.Duration(300)) #300秒
# 查看是否成功到达
if not finished_within_time: #如果5分钟内没有完成完成
rospy.loginfo("Timed out achieving goal") #发布超时的指令
el:
state = lf._state() #如果已经成功到达，状态就得到get_state()，这个是上⾯的状态
的队列 if state == GoalStatus.SUCCEEDED: #如果状态为“SUCCCEEDED”，就执⾏下⾯⼀系列操作
rospy.loginfo("Goal succeeded!")
n_success += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
el:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) #否则输出error
# 运⾏所⽤时间
running_time = w() - start_time
running_time = running_time.cs / 60.0
# 输出本次导航的所有信息
rospy.loginfo("Success so far: " + str(n_success) + "/" +
str(n_goals) + " = " +
str(100 * n_success/n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) + " m")
敖丙图片rospy.st_time)
def update_initial_po(lf, initial_po):
lf.initial_po = initial_po
def shutdown(lf):
rospy.loginfo("Stopping ")
堆雪人的英语
rospy.sleep(2)
rospy.sleep(1)
def trunc(f, n):
slen = len('%.*f' % (n, f))
return float(str(f)[:slen])
if __name__ == '__main__':
if __name__ == '__main__':
try:
NavTest()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("Random navigation finished.")
action API
Move_ba节点提供SimpleActionServer()的⼀个实现，需要含有geometry_msgs/PoStamped消息的⽬标。可以通过ROS直接与move_ba节点通信，但如果考虑使⽤SimpleActionClient跟踪他们的状态，推荐发送⽬标到move_ba的⽅法。详见actionlib documentation来获取更多信息。
Action Subscribe Topics action订阅话题
1. move_ba/goal (move_ba_msgs/MoveBaActionGoal)
2. move_ba在世界中的⽬标
3. move_ba/cancel (actionlib_msgs/GoalID)撤销指定⽬标的请求
Action Published Topics action发布话题
1. move_ba/feedback (move_ba_msgs/MoveBaActionFeedback)包含基座在世界中当前位置
的反馈
2. move_ba/status (actionlib_msgs/GoalStatusArray)提供有关（被发送到move_ba action）⽬标的状态信息
3. move_ba/result (move_ba_msgs/MoveBaActionResult)对move_ba action 结果是空
