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.")
----------------------------------------------------------------
欢迎大家转载我的文章。
转载请注明:转自古-月
欢迎继续关注我的博客
ROS探索总结(十五)——amcl(导航与定位)的更多相关文章
- ROS探索总结(十三)——导航与定位框架
导航与定位是机器人研究中的重要部分. 一般机器人在陌生的环境下需要使用激光传感器(或者深度传感器转换成激光数据),先进行地图建模,然后在根据建立的地图进行导航.定位.在ROS中也有很多 ...
- ROS探索总结(五)——创建简单的机器人模型smartcar
前面我们使用的是已有的机器人模型进行仿真,这一节我们将建立一个简单的智能车机器人smartcar,为后面建立复杂机器人打下基础. 一.创建硬件描述包 roscreat-pkg smartcar_de ...
- ROS探索总结(十八)——重读tf
在之前的博客中,有讲解tf的相关内容,本篇博客重新整理了tf的介绍和学习内容,对tf的认识会更加系统. 1 tf简介 1.1 什么是tf tf是一个让用户随时间跟踪多个参考系的功能包,它使用一种树型数 ...
- 第十五篇 Integration Services:SSIS参数
本篇文章是Integration Services系列的第十五篇,详细内容请参考原文. 简介在前一篇,我们使用SSDT-BI将第一个SSIS项目My_First_SSIS_Project升级/转换到S ...
- 【译】第十五篇 Integration Services:SSIS参数
本篇文章是Integration Services系列的第十五篇,详细内容请参考原文. 简介在前一篇,我们使用SSDT-BI将第一个SSIS项目My_First_SSIS_Project升级/转换到S ...
- Bootstrap <基础二十五>警告(Alerts)
警告(Alerts)以及 Bootstrap 所提供的用于警告的 class.警告(Alerts)向用户提供了一种定义消息样式的方式.它们为典型的用户操作提供了上下文信息反馈. 您可以为警告框添加一个 ...
- Bootstrap<基础十五> 输入框组
Bootstrap 支持的另一个特性,输入框组.输入框组扩展自 表单控件.使用输入框组,可以很容易地向基于文本的输入框添加作为前缀和后缀的文本或按钮. 通过向输入域添加前缀和后缀的内容,您可以向用户输 ...
- 谈谈一些有趣的CSS题目(十五)-- 奇妙的 background-clip: text
开本系列,谈谈一些有趣的 CSS 题目,题目类型天马行空,想到什么说什么,不仅为了拓宽一下解决问题的思路,更涉及一些容易忽视的 CSS 细节. 解题不考虑兼容性,题目天马行空,想到什么说什么,如果解题 ...
- TIAGo ROS模拟教程2 - 自主机器人导航
TIAGo ROS Simulation Tutorial 2 – Autonomous robot navigation TIAGo ROS模拟教程2 - 自主机器人导航 发表于 12月 23,20 ...
随机推荐
- CSS简单使用
CSS简单使用 标签 : 前端技术 CSS(Cascading Style Sheet : 层叠样式表单)用来定义网页显示效果. 可以解决HTML代码对样式定义的重复,提高后期样式代码的可维护性,并增 ...
- SpriteKit:在场景过渡中暂停动画
Pausing Scenes During a Transition 你应该意识到两个重要的SKTrnsition属性在场景之间的过渡中. 它们是pausesIncomingScene和pausesO ...
- Android4.3 屏蔽HOME按键返回桌面详解(源码环境下)
点击打开链接 首先声明我是做系统开发的(高通平台),所以下面介绍的方法并不适合应用开发者. 最经有个需求要屏蔽HOME按键返回桌面并且实现自己的功能,发现以前的方式报错用不了,上网搜索了一下,发现都是 ...
- 【移动开发】一张图搞定Activity和Fragment的生命周期
- iOS的settings bundle中开关按钮(Toggle Switch)取不到值的问题
大熊猫猪·侯佩原创或翻译作品.欢迎转载,转载请注明出处. 如果觉得写的不好请多提意见,如果觉得不错请多多支持点赞.谢谢! hopy ;) 在Xcode7.2中设置App的settings bundle ...
- android orm持久层框架
; ; i < 2; i++) { )); ); h1.setWord("这是修改过的数据"); tv.setText(tv.getText() + "\n&quo ...
- android横竖屏切换activity生命周期变化
1.新建一个Activity,并把各个生命周期打印出来 2.运行Activity,得到如下信息 onCreate--> onStart--> onResume--> 3.按crtl+ ...
- java中hashCode()与equals()详解
首先之所以会将hashCode()与equals()放到一起是因为它们具备一个相同的作用:用来比较某个东西.其中hashCode()主要是用在hash表中提高 查找效率,而equals()则相对而言使 ...
- markdown语法(看这张图就够了)
这是维基百科的一张图,基本就够用了 https://en.wikipedia.org/wiki/Markdown#Example
- ledisdb:支持类redis接口的嵌入式nosql
ledisdb现在可以支持嵌入式使用.你可以将其作为一个独立的lib(类似leveldb)直接嵌入到你自己的应用中去,而无需在启动单独的服务. ledisdb提供的API仍然类似redis接口.首先, ...