ROS探索总结(十三)——导航与定位框架
导航与定位是机器人研究中的重要部分。
一般机器人在陌生的环境下需要使用激光传感器(或者深度传感器转换成激光数据),先进行地图建模,然后在根据建立的地图进行导航、定位。在ROS中也有很多完善的包可以直接使用。
在ROS中,进行导航需要使用到的三个包是:
(1) move_base:根据参照的消息进行路径规划,使移动机器人到达指定的位置;
(2) gmapping:根据激光数据(或者深度数据模拟的激光数据)建立地图;
(3) amcl:根据已经有的地图进行定位。
参考链接:http://www.ros.org/wiki/navigation/Tutorials/RobotSetup
http://www.ros.org/wiki/navigation/Tutorials整体导航包的格局如下图所示:
其中白色框内的是ROS已经为我们准备好的必须使用的组件,灰色框内的是ROS中可选的组件,蓝色的是用户需要提供的机器人平台上的组件。
1、sensor transforms
其中涉及到使用tf进行传感器坐标的转换,因为我们使用的机器人的控制中心不一定是在传感器上,所以要把传感器的数据转换成在控制中心上的坐标信息。如下图所示,传感器获取的数据是在base_laser的坐标系统中的,但是我们控制的时候是以base_link为中心,所以要根据两者的位置关心进行坐标的变换。
变换的过程不需要我们自己处理,只需要将base_laser和base_link两者之间的位置关系告诉tf,就可以自动转换了。具体的实现可以参见:http://blog.csdn.net/hcx25909/article/details/9255001。
2、sensor sources
这里是机器人导航传感器数据输入,一般只有两种:(1) 激光传感器数据
(2) 点云数据
具体实现见:http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Sensors
3、odometry source
机器人的导航需要输入里程计的数据(tf,nav_msgs/Odometry_message),具体实现见:http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom
4、base controller
在导航过程中,该部分负责将之前得出来的数据转封装成具体的线速度和转向角度信息(Twist),并且发布给硬件平台。5、map_server
在导航过程中,地图并不是必须的,此时相当于是在一个无限大的空地上进行导航,并没有任何障碍物。但是考虑到实际情况,在我们使用导航的过程中还是要一个地图的。具体实现见:http://www.ros.org/wiki/slam_gmapping/Tutorials/MappingFromLoggedData
在ROS的导航中,costmap_2d这个包主要负责根据传感器的信息建立和更新二维或三维的地图。ROS的地图(costmap)采用网格(grid)的形式,每个网格的值从0~255,分为三种状态:占用(有障碍物)、无用(空闲的)、未知。具体状态和值的对应关系如下:
上图共分为五个部分:(下面的红色框图是机器人的轮廓,旁边的黑框是上图的映射位置)
(1) Lethal(致命的):机器人的中心与该网格的中心重合,此时机器人必然与障碍物冲突。
(2) Inscribed(内切):网格的外切圆与机器人的轮廓内切,此时机器人也必然与障碍物冲突。
(3) Possibly circumscribed(外切):网格的外切圆与机器人的轮廓外切,此时机器人相当于靠在障碍物附近,所以不一定冲突。
(4) Freespace(自由空间):没有障碍物的空间。
(5) Unknown(未知):未知的空间。
具体可见:http://www.ros.org/wiki/costmap_2d
----------------------------------------------------------------
在上一篇的博客中,我们一起学习了ROS定位于导航的总体框架,这一篇我们主要研究其中最重要的move_base包。
在总体框架图中可以看到,move_base提供了ROS导航的配置、运行、交互接口,它主要包括两个部分:
(1) 全局路径规划(global planner):根据给定的目标位置进行总体路径的规划;
(2) 本地实时规划(local planner):根据附近的障碍物进行躲避路线规划。
一、数据结构
- rosmsg show MoveBaseActionGoal
- [move_base_msgs/MoveBaseActionGoal]:
- std_msgs/Header header
- uint32 seq
- time stamp
- string frame_id
- actionlib_msgs/GoalID goal_id
- time stamp
- string id
- move_base_msgs/MoveBaseGoal goal
- geometry_msgs/PoseStamped target_pose
- std_msgs/Header header
- uint32 seq
- time stamp
- string frame_id
- geometry_msgs/Pose pose
- geometry_msgs/Point position
- float64 x
- float64 y
- float64 z
- geometry_msgs/Quaternion orientation
- float64 x
- float64 y
- float64 z
- float64 w
• base_local_planner_params.yaml
• costmap_common_params.yaml
• global_costmap_params.yaml
• local_costmap_params.yaml
三、全局路径规划(global planner)
navfn通过Dijkstra最优路径的算法,计算costmap上的最小花费路径,作为机器人的全局路线。将来在算法上应该还会加入A*算法。
具体见:http://www.ros.org/wiki/navfn?distro=fuerte
四、本地实时规划(local planner)
其中,Trajectory Rollout 和Dynamic Window approaches算法的主要思路如下:
(1) 采样机器人当前的状态(dx,dy,dtheta);
(2) 针对每个采样的速度,计算机器人以该速度行驶一段时间后的状态,得出一条行驶的路线。
(3) 利用一些评价标准为多条路线打分。
(4) 根据打分,选择最优路径。
(5) 重复上面过程。
具体参见:http://www.ros.org/wiki/base_local_planner?distro=groovy
五、ArbotiX仿真——手动设定目标
首先运行ArbotiX节点,并且加载机器人的URDF文件。
- roslaunch rbx1_bringup fake_turtlebot.launch
- roslaunch rbx1_nav fake_move_base_blank_map.launch
- <launch>
- <!-- Run the map server with a blank map -->
- <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>
- <include file="$(find rbx1_nav)/launch/fake_move_base.launch" />
- <!-- Run a static transform between /odom and /map -->
- <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />
- </launch>
- rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.vcg
- rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \
- '{ header: { frame_id: "base_link" }, pose: { position: { x: 1.0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'
- rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \
- '{ header: { frame_id: "map" }, pose: { position: { x: 0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'
然后我们可以认为的确定目标位置,点击rviz上方的2D Nav Goal按键,然后左键选择目标位置,机器人就开始自动导航了。
六、ArbotiX仿真——带有障碍物的路径规划
- rosrun rbx1_nav move_base_square.py
- #!/usr/bin/env python
- import roslib; roslib.load_manifest('rbx1_nav')
- import rospy
- import actionlib
- from actionlib_msgs.msg import *
- from geometry_msgs.msg import Pose, Point, Quaternion, Twist
- from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
- from tf.transformations import quaternion_from_euler
- from visualization_msgs.msg import Marker
- from math import radians, pi
- class MoveBaseSquare():
- def __init__(self):
- rospy.init_node('nav_test', anonymous=False)
- rospy.on_shutdown(self.shutdown)
- # How big is the square we want the robot to navigate?
- # 设定正方形的尺寸,默认是一米
- square_size = rospy.get_param("~square_size", 1.0) # meters
- # Create a list to hold the target quaternions (orientations)
- # 创建一个列表,保存目标的角度数据
- quaternions = list()
- # First define the corner orientations as Euler angles
- # 定义四个顶角处机器人的方向角度(Euler angles:http://zh.wikipedia.org/wiki/%E6%AC%A7%E6%8B%89%E8%A7%92)
- euler_angles = (pi/2, pi, 3*pi/2, 0)
- # Then convert the angles to quaternions
- # 将上面的Euler angles转换成Quaternion的格式
- for angle in euler_angles:
- q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
- q = Quaternion(*q_angle)
- quaternions.append(q)
- # Create a list to hold the waypoint poses
- # 创建一个列表存储导航点的位置
- waypoints = list()
- # Append each of the four waypoints to the list. Each waypoint
- # is a pose consisting of a position and orientation in the map frame.
- # 创建四个导航点的位置(角度和坐标位置)
- waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0]))
- waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1]))
- waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2]))
- waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
- # Initialize the visualization markers for RViz
- # 初始化可视化标记
- self.init_markers()
- # Set a visualization marker at each waypoint
- # 给每个定点的导航点一个可视化标记(就是rviz中看到的粉色圆盘标记)
- for waypoint in waypoints:
- p = Point()
- p = waypoint.position
- self.markers.points.append(p)
- # Publisher to manually control the robot (e.g. to stop it)
- # 发布TWist消息控制机器人
- self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
- # Subscribe to the move_base action server
- # 订阅move_base服务器的消息
- self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
- rospy.loginfo("Waiting for move_base action server...")
- # Wait 60 seconds for the action server to become available
- # 等待move_base服务器建立
- self.move_base.wait_for_server(rospy.Duration(60))
- rospy.loginfo("Connected to move base server")
- rospy.loginfo("Starting navigation test")
- # Initialize a counter to track waypoints
- # 初始化一个计数器,记录到达的顶点号
- i = 0
- # Cycle through the four waypoints
- # 主循环,环绕通过四个定点
- while i < 4 and not rospy.is_shutdown():
- # Update the marker display
- # 发布标记指示四个目标的位置,每个周期发布一起,确保标记可见
- self.marker_pub.publish(self.markers)
- # Intialize the waypoint goal
- # 初始化goal为MoveBaseGoal类型
- goal = MoveBaseGoal()
- # Use the map frame to define goal poses
- # 使用map的frame定义goal的frame id
- goal.target_pose.header.frame_id = 'map'
- # Set the time stamp to "now"
- # 设置时间戳
- goal.target_pose.header.stamp = rospy.Time.now()
- # Set the goal pose to the i-th waypoint
- # 设置目标位置是当前第几个导航点
- goal.target_pose.pose = waypoints[i]
- # Start the robot moving toward the goal
- # 机器人移动
- self.move(goal)
- i += 1
- def move(self, goal):
- # Send the goal pose to the MoveBaseAction server
- # 把目标位置发送给MoveBaseAction的服务器
- self.move_base.send_goal(goal)
- # Allow 1 minute to get there
- # 设定1分钟的时间限制
- finished_within_time = self.move_base.wait_for_result(rospy.Duration(60))
- # If we don't get there in time, abort the goal
- # 如果一分钟之内没有到达,放弃目标
- if not finished_within_time:
- self.move_base.cancel_goal()
- rospy.loginfo("Timed out achieving goal")
- else:
- # We made it!
- state = self.move_base.get_state()
- if state == GoalStatus.SUCCEEDED:
- rospy.loginfo("Goal succeeded!")
- def init_markers(self):
- # Set up our waypoint markers
- # 设置标记的尺寸
- marker_scale = 0.2
- marker_lifetime = 0 # 0 is forever
- marker_ns = 'waypoints'
- marker_id = 0
- marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}
- # Define a marker publisher.
- # 定义一个标记的发布者
- self.marker_pub = rospy.Publisher('waypoint_markers', Marker)
- # Initialize the marker points list.
- # 初始化标记点的列表
- self.markers = Marker()
- self.markers.ns = marker_ns
- self.markers.id = marker_id
- self.markers.type = Marker.SPHERE_LIST
- self.markers.action = Marker.ADD
- self.markers.lifetime = rospy.Duration(marker_lifetime)
- self.markers.scale.x = marker_scale
- self.markers.scale.y = marker_scale
- self.markers.color.r = marker_color['r']
- self.markers.color.g = marker_color['g']
- self.markers.color.b = marker_color['b']
- self.markers.color.a = marker_color['a']
- self.markers.header.frame_id = 'map'
- self.markers.header.stamp = rospy.Time.now()
- self.markers.points = list()
- def shutdown(self):
- rospy.loginfo("Stopping the robot...")
- # Cancel any active goals
- self.move_base.cancel_goal()
- rospy.sleep(2)
- # Stop the robot
- self.cmd_vel_pub.publish(Twist())
- rospy.sleep(1)
- if __name__ == '__main__':
- try:
- MoveBaseSquare()
- except rospy.ROSInterruptException:
- rospy.loginfo("Navigation test finished.")
现在我们尝试在之前的正方形路径中加入障碍物。把之前运行fake_move_base_blank_map.launch的中断Ctrl-C掉,然后运行:
- roslaunch rbx1_nav fake_move_base_obstacle.launch
然后就会看到在rviz中出现了障碍物。然后在运行之前走正方形路线的代码:
- rosrun rbx1_nav move_base_square.py
这回我们可以看到,在全局路径规划的时候,机器人已经将障碍物绕过去了,下过如下图:
----------------------------------------------------------------
ROS探索总结(十五)——amcl(导航与定位)
在理解了move_base的基础上,我们开始机器人的定位与导航。gmaping包是用来生成地图的,需要使用实际的机器人获取激光或者深度数据,所以我们先在已有的地图上进行导航与定位的仿真。
amcl是移动机器人二维环境下的概率定位系统。它实现了自适应(或kld采样)的蒙特卡罗定位方法,其中针对已有的地图使用粒子滤波器跟踪一个机器人的姿态。
一、测试
- roslaunch rbx1_bringup fake_turtlebot.launch
- roslaunch rbx1_nav fake_amcl.launch map:=test_map.yaml
可以看一下fake_amcl.launch这个文件的内容:
- <launch>
- <!-- Set the name of the map yaml file: can be overridden on the command line. -->
- <arg name="map" default="test_map.yaml" />
- <!-- Run the map server with the desired map -->
- <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/$(arg map)"/>
- <!-- The move_base node -->
- <include file="$(find rbx1_nav)/launch/fake_move_base.launch" />
- <!-- Run fake localization compatible with AMCL output -->
- <node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" />
- <!-- For fake localization we need static transforms between /odom and /map and /map and /world -->
- <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster"
- args="0 0 0 0 0 0 /odom /map 100" />
- </launch>
然后运行rviz:
- rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.vcg
二、自主导航
在实际应用中,我们往往希望机器人能够自主进行定位和导航,不需要认为的干预,这样才更智能化。在这一节的测试中,我们让目标点在地图中随机生成,然后机器人自动导航到达目标。
这里运行的主要文件是:fake_nav_test.launch,让我们来看一下这个文件的内容:
- <launch>
- <param name="use_sim_time" value="false" />
- <!-- Start the ArbotiX controller -->
- <include file="$(find rbx1_bringup)/launch/fake_turtlebot.launch" />
- <!-- Run the map server with the desired map -->
- <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/test_map.yaml"/>
- <!-- The move_base node -->
- <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
- <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />
- <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />
- <rosparam file="$(find rbx1_nav)/config/fake/local_costmap_params.yaml" command="load" />
- <rosparam file="$(find rbx1_nav)/config/fake/global_costmap_params.yaml" command="load" />
- <rosparam file="$(find rbx1_nav)/config/fake/base_local_planner_params.yaml" command="load" />
- <rosparam file="$(find rbx1_nav)/config/nav_test_params.yaml" command="load" />
- </node>
- <!-- Run fake localization compatible with AMCL output -->
- <node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" />
- <!-- For fake localization we need static transform between /odom and /map -->
- <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />
- <!-- Start the navigation test -->
- <node pkg="rbx1_nav" type="nav_test.py" name="nav_test" output="screen">
- <param name="rest_time" value="1" />
- <param name="fake_test" value="true" />
- </node>
- </launch>
这个lanuch的功能比较多:
(1) 加载机器人驱动
(2) 加载地图
(3) 启动move_base节点,并且加载配置文件
(4) 运行amcl节点
(5) 然后加载nav_test.py执行文件,进行随机导航
相当于是把我们之前实验中的多个lanuch文件合成了一个文件。
现在开始进行测试,先运行ROS:
- roscore
- rxconsole
- roslaunch rbx1_nav fake_nav_test.launch
- (打开新终端)
- rosrun rviz rviz -d `rospack find rbx1_nav`/nav_test_fuerte.vcg
三、导航代码分析
- #!/usr/bin/env python
- import roslib; roslib.load_manifest('rbx1_nav')
- import rospy
- import actionlib
- from actionlib_msgs.msg import *
- from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
- from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
- from random import sample
- from math import pow, sqrt
- class NavTest():
- def __init__(self):
- rospy.init_node('nav_test', anonymous=True)
- rospy.on_shutdown(self.shutdown)
- # How long in seconds should the robot pause at each location?
- # 在每个目标位置暂停的时间
- self.rest_time = rospy.get_param("~rest_time", 10)
- # Are we running in the fake simulator?
- # 是否仿真?
- self.fake_test = rospy.get_param("~fake_test", False)
- # Goal state return values
- # 到达目标的状态
- goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
- 'SUCCEEDED', 'ABORTED', 'REJECTED',
- 'PREEMPTING', 'RECALLING', 'RECALLED',
- 'LOST']
- # Set up the goal locations. Poses are defined in the map frame.
- # An easy way to find the pose coordinates is to point-and-click
- # Nav Goals in RViz when running in the simulator.
- # Pose coordinates are then displayed in the terminal
- # that was used to launch RViz.
- # 设置目标点的位置
- # 如果想要获得某一点的坐标,在rviz中点击 2D Nav Goal 按键,然后单机地图中一点
- # 在终端中就会看到坐标信息
- locations = dict()
- locations['hall_foyer'] = Pose(Point(0.643, 4.720, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975))
- locations['hall_kitchen'] = Pose(Point(-1.994, 4.382, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))
- locations['hall_bedroom'] = Pose(Point(-3.719, 4.401, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680))
- locations['living_room_1'] = Pose(Point(0.720, 2.229, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618))
- locations['living_room_2'] = Pose(Point(1.471, 1.007, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877))
- locations['dining_room_1'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))
- # Publisher to manually control the robot (e.g. to stop it)
- # 发布控制机器人的消息
- self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
- # Subscribe to the move_base action server
- # 订阅move_base服务器的消息
- self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
- rospy.loginfo("Waiting for move_base action server...")
- # Wait 60 seconds for the action server to become available
- # 60s等待时间限制
- self.move_base.wait_for_server(rospy.Duration(60))
- rospy.loginfo("Connected to move base server")
- # A variable to hold the initial pose of the robot to be set by
- # the user in RViz
- # 保存机器人的在rviz中的初始位置
- initial_pose = PoseWithCovarianceStamped()
- # Variables to keep track of success rate, running time,
- # and distance traveled
- # 保存成功率、运行时间、和距离的变量
- n_locations = len(locations)
- n_goals = 0
- n_successes = 0
- i = n_locations
- distance_traveled = 0
- start_time = rospy.Time.now()
- running_time = 0
- location = ""
- last_location = ""
- # Get the initial pose from the user
- # 获取初始位置(仿真中可以不需要)
- rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
- rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
- self.last_location = Pose()
- rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
- # Make sure we have the initial pose
- # 确保有初始位置
- while initial_pose.header.stamp == "":
- rospy.sleep(1)
- rospy.loginfo("Starting navigation test")
- # Begin the main loop and run through a sequence of locations
- # 开始主循环,随机导航
- while not rospy.is_shutdown():
- # If we've gone through the current sequence,
- # start with a new random sequence
- # 如果已经走完了所有点,再重新开始排序
- if i == n_locations:
- i = 0
- sequence = sample(locations, n_locations)
- # Skip over first location if it is the same as
- # the last location
- # 如果最后一个点和第一个点相同,则跳过
- if sequence[0] == last_location:
- i = 1
- # Get the next location in the current sequence
- # 在当前的排序中获取下一个目标点
- location = sequence[i]
- # Keep track of the distance traveled.
- # Use updated initial pose if available.
- # 跟踪形式距离
- # 使用更新的初始位置
- if initial_pose.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))
- else:
- rospy.loginfo("Updating current pose.")
- distance = sqrt(pow(locations[location].position.x -
- initial_pose.pose.pose.position.x, 2) +
- pow(locations[location].position.y -
- initial_pose.pose.pose.position.y, 2))
- initial_pose.header.stamp = ""
- # Store the last location for distance calculations
- # 存储上一次的位置,计算距离
- last_location = location
- # Increment the counters
- # 计数器加1
- i += 1
- n_goals += 1
- # Set up the next goal location
- # 设定下一个目标点
- self.goal = MoveBaseGoal()
- self.goal.target_pose.pose = locations[location]
- self.goal.target_pose.header.frame_id = 'map'
- self.goal.target_pose.header.stamp = rospy.Time.now()
- # Let the user know where the robot is going next
- # 让用户知道下一个位置
- rospy.loginfo("Going to: " + str(location))
- # Start the robot toward the next location
- # 向下一个位置进发
- self.move_base.send_goal(self.goal)
- # Allow 5 minutes to get there
- # 五分钟时间限制
- finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
- # Check for success or failure
- # 查看是否成功到达
- if not finished_within_time:
- self.move_base.cancel_goal()
- rospy.loginfo("Timed out achieving goal")
- else:
- state = self.move_base.get_state()
- if state == GoalStatus.SUCCEEDED:
- rospy.loginfo("Goal succeeded!")
- n_successes += 1
- distance_traveled += distance
- rospy.loginfo("State:" + str(state))
- else:
- rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
- # How long have we been running?
- # 运行所用时间
- running_time = rospy.Time.now() - start_time
- running_time = running_time.secs / 60.0
- # Print a summary success/failure, distance traveled and time elapsed
- # 输出本次导航的所有信息
- rospy.loginfo("Success so far: " + str(n_successes) + "/" +
- str(n_goals) + " = " +
- str(100 * n_successes/n_goals) + "%")
- rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
- " min Distance: " + str(trunc(distance_traveled, 1)) + " m")
- rospy.sleep(self.rest_time)
- def update_initial_pose(self, initial_pose):
- self.initial_pose = initial_pose
- def shutdown(self):
- rospy.loginfo("Stopping the robot...")
- self.move_base.cancel_goal()
- rospy.sleep(2)
- self.cmd_vel_pub.publish(Twist())
- rospy.sleep(1)
- def trunc(f, n):
- # Truncates/pads a float f to n decimal places without rounding
- slen = len('%.*f' % (n, f))
- return float(str(f)[:slen])
- if __name__ == '__main__':
- try:
- NavTest()
- rospy.spin()
- except rospy.ROSInterruptException:
- rospy.loginfo("AMCL navigation test finished.")
----------------------------------------------------------------
欢迎大家转载我的文章。
转载请注明:转自古-月
欢迎继续关注我的博客