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（路徑規劃）
在上一篇的博客中，我們一起學習了ROS定位于導航的總體框架，這一篇我們主要研究其中最重要的move_base包。
? ? ? ? ? ?在總體框架圖中可以看到，move_base提供了ROS導航的配置、運行、交互接口，它主要包括兩個部分：
? ? ? （1） 全局路徑規劃（global planner）：根據給定的目標位置進行總體路徑的規劃；
? ? ? （2） 本地實時規劃（local planner）：根據附近的障礙物進行躲避路線規劃。
一、數據結構
? ? ? ??ROS中定義了MoveBaseActionGoal數據結構來存儲導航的目標位置數據，其中最重要的就是位置坐標（position）和方向（orientation）。 [plain]?view plaincopy
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??
二、配置文件
? ? ? ??move_base使用前需要配置一些參數：運行成本、機器人半徑、到達目標位置的距離，機器人移動的速度，這些參數都在rbx1_nav包的以下幾個配置文件中：
? ? ? ? ? base_local_planner_params.yaml
? ? ? ? ? costmap_common_params.yaml
? ? ? ? ? global_costmap_params.yaml
? ? ? ? ? local_costmap_params.yaml
三、全局路徑規劃（global planner）
? ? ? ??在ROS的導航中，首先會通過全局路徑規劃，計算出機器人到目標位置的全局路線。這一功能是navfn這個包實現的。
? ? ? ? navfn通過Dijkstra最優路徑的算法，計算costmap上的最小花費路徑，作為機器人的全局路線。將來在算法上應該還會加入A*算法。
? ? ? ? 具體見：http://www.ros.org/wiki/navfn?distro=fuerte
四、本地實時規劃（local planner）
? ? ? ??本地的實時規劃是利用base_local_planner包實現的。該包使用Trajectory Rollout 和Dynamic Window approaches算法計算機器人每個周期內應該行駛的速度和角度（dx，dy，dtheta velocities）。
? ? ? ??base_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仿真——手動設定目標
? ? ? ? 在這一步，我們暫時使用空白地圖（blank_map.pgm），就在空地上進行無障礙仿真。
? ? ? ? 首先運行ArbotiX節點，并且加載機器人的URDF文件。 [plain]?view plaincopy
roslaunch?rbx1_bringup?fake_turtlebot.launch??
? ? ? ? ?然后運行move_base和加載空白地圖的launch文件（fake_move_base_blank_map.launch）： [plain]?view plaincopy
roslaunch?rbx1_nav?fake_move_base_blank_map.launch??
? ? ? ??該文件的具體內容如下： [plain]?view plaincopy
<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>??
? ? ? ??其中調用了fake_move_base.launch文件，是運行move_base節點并進行參數配置。
? ? ? ? 然后調用rviz就可以看到機器人了。 [plain]?view plaincopy
rosrun?rviz?rviz?-d?`rospack?find?rbx1_nav`/nav_fuerte.vcg??
? ? ? ?? ? ? ? ? 我們先以1m的速度進行一下測試： ? ? ? ? 讓機器人前進一米： [plain]?view plaincopy
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?}?}?}'?? ? ? ? ??讓機器人后退一米，回到原來的位置： [plain]?view plaincopy
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中的軌跡圖如下：
? ? ? ??在機器人移動過程中，有一條藍色的線（被黃線擋住了）就是機器人的全局規劃的路徑；紅色的箭頭是實施規劃的路線，會不斷更新，有的時候會呈現很大的弧線，那是因為機器人在轉向的過程中盡量希望保持平穩的角度。如果覺得路徑規劃的精度不夠，可以修改配置文件中的pdist_scale參數進行修正。
? ? ? ? 然后我們可以認為的確定目標位置，點擊rviz上方的2D Nav Goal按鍵，然后左鍵選擇目標位置，機器人就開始自動導航了。
六、ArbotiX仿真——帶有障礙物的路徑規劃
? ? ? ??首先我們讓機器人走一個正方形的路線。先通過上面的命令，讓機器人回到原始位置（0，0，0），然后按reset按鍵，把所有的箭頭清除。接著運行走正方形路徑的代碼： [plain]?view plaincopy
rosrun?rbx1_nav?move_base_square.py?? ? ? ? ??在rviz中可以看到：
? ? ? ??四個頂角的粉色圓盤就是我們設定的位置，正方形比較規則，可見定位還是比較準確的。然我們先來分析一下走正方形路線的代碼： [python]?view plaincopy
?? 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)?? ?????????? ?????????? ?????????? ????????square_size?=?rospy.get_param("~square_size",?1.0)??? ?????????? ?????????? ?????????? ????????quaternions?=?list()?? ?????????? ?????????? ?????????? ????????euler_angles?=?(pi/2,?pi,?3*pi/2,?0)?? ?????????? ?????????? ?????????? ????????for?angle?in?euler_angles:?? ????????????q_angle?=?quaternion_from_euler(0,?0,?angle,?axes='sxyz')?? ????????????q?=?Quaternion(*q_angle)?? ????????????quaternions.append(q)?? ?????????? ?????????? ?????????? ????????waypoints?=?list()?? ?????????? ?????????? ?????????? ?????????? ????????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]))?? ?????????? ?????????? ?????????? ????????self.init_markers()?? ?????????? ?????????? ?????????? ????????for?waypoint?in?waypoints:????????????? ????????????p?=?Point()?? ????????????p?=?waypoint.position?? ????????????self.markers.points.append(p)?? ?????????????? ?????????? ?????????? ????????self.cmd_vel_pub?=?rospy.Publisher('cmd_vel',?Twist)?? ?????????? ?????????? ?????????? ????????self.move_base?=?actionlib.SimpleActionClient("move_base",?MoveBaseAction)?? ?????????? ????????rospy.loginfo("Waiting?for?move_base?action?server...")?? ?????????? ?????????? ?????????? ????????self.move_base.wait_for_server(rospy.Duration(60))?? ?????????? ????????rospy.loginfo("Connected?to?move?base?server")?? ????????rospy.loginfo("Starting?navigation?test")?? ?????????? ?????????? ?????????? ????????i?=?0?? ?????????? ?????????? ?????????? ????????while?i?<?4?and?not?rospy.is_shutdown():?? ?????????????? ?????????????? ????????????self.marker_pub.publish(self.markers)?? ?????????????? ?????????????? ?????????????? ????????????goal?=?MoveBaseGoal()?? ?????????????? ?????????????? ?????????????? ????????????goal.target_pose.header.frame_id?=?'map'?? ?????????????? ?????????????? ?????????????? ????????????goal.target_pose.header.stamp?=?rospy.Time.now()?? ?????????????? ?????????????? ?????????????? ????????????goal.target_pose.pose?=?waypoints[i]?? ?????????????? ?????????????? ?????????????? ????????????self.move(goal)?? ?????????????? ????????????i?+=?1?? ?????????? ????def?move(self,?goal):?? ?????????????? ?????????????? ????????????self.move_base.send_goal(goal)?? ?????????????? ?????????????? ?????????????? ????????????finished_within_time?=?self.move_base.wait_for_result(rospy.Duration(60))??? ?????????????? ?????????????? ?????????????? ????????????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!")?? ?????????????????????? ????def?init_markers(self):?? ?????????? ?????????? ????????marker_scale?=?0.2?? ????????marker_lifetime?=?0??? ????????marker_ns?=?'waypoints'?? ????????marker_id?=?0?? ????????marker_color?=?{'r':?1.0,?'g':?0.7,?'b':?1.0,?'a':?1.0}?? ?????????? ?????????? ?????????? ????????self.marker_pub?=?rospy.Publisher('waypoint_markers',?Marker)?? ?????????? ?????????? ?????????? ????????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...")?? ?????????? ????????self.move_base.cancel_goal()?? ????????rospy.sleep(2)?? ?????????? ????????self.cmd_vel_pub.publish(Twist())?? ????????rospy.sleep(1)?? ?? if?__name__?==?'__main__':?? ????try:?? ????????MoveBaseSquare()?? ????except?rospy.ROSInterruptException:?? ????????rospy.loginfo("Navigation?test?finished.")?? ? ? ? ??但是，在實際情況中，往往需要讓機器人自動躲避障礙物。move_base包的一個強大的功能就是可以在全局規劃的過程中自動躲避障礙物，而不影響全局路徑。障礙物可以是靜態的（比如墻、桌子等），也可以是動態的（比如人走過）。
? ? ? ? 現在我們嘗試在之前的正方形路徑中加入障礙物。把之前運行fake_move_base_blank_map.launch的中斷Ctrl-C掉，然后運行： [plain]?view plaincopy
roslaunch?rbx1_nav?fake_move_base_obstacle.launch?? ? ? ? ??然后就會看到在rviz中出現了障礙物。然后在運行之前走正方形路線的代碼： [plain]?view plaincopy
rosrun?rbx1_nav?move_base_square.py??? ? ? ? ??這回我們可以看到，在全局路徑規劃的時候，機器人已經將障礙物繞過去了，下過如下圖： ? ? ? ??在上圖中，黑色的線是障礙物，周圍淺色的橢圓形是根據配置文件中的inflation_radius參數計算出來的安全緩沖區。全局規劃的路徑基本已經是最短路徑了。在仿真的過程中也可以動態重配置那四個配置文件，修改仿真參數。 ----------------------------------------------------------------
ROS探索總結（十五）——amcl（導航與定位）
在理解了move_base的基礎上，我們開始機器人的定位與導航。gmaping包是用來生成地圖的，需要使用實際的機器人獲取激光或者深度數據，所以我們先在已有的地圖上進行導航與定位的仿真。
? ? ? ? amcl是移動機器人二維環境下的概率定位系統。它實現了自適應（或kld采樣）的蒙特卡羅定位方法，其中針對已有的地圖使用粒子濾波器跟蹤一個機器人的姿態。
一、測試
? ? ? ? 首先運行機器人節點： [plain]?view plaincopy
roslaunch?rbx1_bringup?fake_turtlebot.launch??
? ? ? ? 然后運行amcl節點，使用測試地圖： ? ? ? ? [plain]?view plaincopy
roslaunch?rbx1_nav?fake_amcl.launch?map:=test_map.yaml?? ? ? ? ??可以看一下fake_amcl.launch這個文件的內容： [plain]?view plaincopy
<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>??
? ? ? ? ?這個lanuch文件作用是加載地圖，并且調用fake_move_base.launch文件打開move_base節點并加載配置文件，最后運行amcl。
? ? ? ? ?然后運行rviz： [plain]?view plaincopy
rosrun?rviz?rviz?-d?`rospack?find?rbx1_nav`/nav_fuerte.vcg??
? ? ? ? ?這時在rvoiz中就應該顯示出了地圖和機器人：
? ? ? ??現在就可以通過rviz在地圖上選擇目標位置了，然后就會看到機器人自動規劃出一條全局路徑，并且導航前進：
二、自主導航?
? ? ? ? 在實際應用中，我們往往希望機器人能夠自主進行定位和導航，不需要認為的干預，這樣才更智能化。在這一節的測試中，我們讓目標點在地圖中隨機生成，然后機器人自動導航到達目標。
? ? ? ? 這里運行的主要文件是：fake_nav_test.launch，讓我們來看一下這個文件的內容： ??
[plain]?view plaincopy
<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： [plain]?view plaincopy
roscore??
? ? ? ??然后我們運行一個監控的窗口，可以實時看到機器人發送的數據： [plain]?view plaincopy
rxconsole??
? ? ? ??接著運行lanuch文件，并且在一個新的終端中打開rviz： [plain]?view plaincopy
roslaunch?rbx1_nav?fake_nav_test.launch?? （打開新終端）?? rosrun?rviz?rviz?-d?`rospack?find?rbx1_nav`/nav_test_fuerte.vcg??
? ? ? ??好了，此時就看到了機器人已經放在地圖當中了。然后我們點擊rviz上的“2D Pose Estimate”按鍵，然后左鍵在機器人上單擊，讓綠色的箭頭和黃色的箭頭重合，機器人就開始隨機選擇目標導航了：
? ? ? ?? ? ? ? ? ?在監控窗口中，我們可以看到機器人發送的狀態信息：
? ? ? ??其中包括距離信息、狀態信息、目標的編號、成功率和速度等信息。
三、導航代碼分析
[plain]?view plaincopy
#!/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.")?? ----------------------------------------------------------------
歡迎大家轉載我的文章。
轉載請注明：轉自古-月
http://blog.csdn.net/hcx25909
歡迎繼續關注我的博客
總結
以上是生活随笔為你收集整理的ROS探索总结（十三）（十四）（十五）——导航与定位框架 move_base（路径规划） amcl（导航与定位）的全部內容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。
