实验四 定位与导航算法
【體驗內容】
一、全局規劃：Astar算法
【檢查點1】在上次gmapping實驗課中構建的地圖上，使用astar規劃一條路徑。
在工作空間的src目錄下，下載astar算法功能包并編譯
git clone https://gitee.com/massif_li/astar
cd ..
catkin_make
修改launch文件中的yaml地圖文件地址，運行launch文件
roslaunch astar astar.launch
在顯示出來的rviz界面中可以看到設置的地圖文件，分辨點擊2D Pose Estimate和2D Nav Goal，即可在地圖中設置起始位姿和目標位姿，可以看到astar程序為我們規劃一條路徑。
使用rqt_graph查看此時運行的節點，將左上角的選項從Nodes only改為Nodes/Topics (all)，可以看到astar節點接受起始點位姿和地圖，生成導航路徑和覆蓋在原地圖上的路徑圖像蒙版。
其中這幾個話題的消息類型：
/initialpose —— geometry_msgs/PoseWithCovarianceStamped（帶協方差標志的位姿）
/move_base_simple/goal —— geometry_msgs/PoseStamped（位姿）
/map —— nav_msgs/OccupancyGrid（占用柵格）
/nav_path —— nav_msgs/Path（路徑，由一個標準頭和一組位姿組成）
/mask —— nav_msgs/OccupancyGrid（占用柵格）
二、Turtlebot定位與導航
【檢查點2】完成蒙特卡洛定位（即使得位姿粒子群在運動控制下獲得收斂的效果）。
在工作空間的src目錄下，下載以下功能包并編譯（這是用戶與turtlebot進行交互的功能包，后面會使用其中的turtlebot_rviz_launchers顯示蒙特卡洛定位過程與效果）
git clone https://gitee.com/massif_li/turtlebot_interactions.git
cd ..
catkin_make
在turtlebot_ws/ src / turtlebot_apps / turtlebot_navigation / launch / includes / amcl目錄下創建rplidar_amcl.launch.xml文件，代碼如下：
rplidar_amcl.launch.xml
<launch><arg name="use_map_topic" default="false"/><arg name="scan_topic" default="scan"/> <arg name="initial_pose_x" default="0.0"/><arg name="initial_pose_y" default="0.0"/><arg name="initial_pose_a" default="0.0"/><arg name="odom_frame_id" default="odom"/><arg name="base_frame_id" default="base_footprint"/><arg name="global_frame_id" default="map"/><node pkg="amcl" type="amcl" name="amcl"><param name="use_map_topic" value="$(arg use_map_topic)"/><!-- Publish scans from best pose at a max of 10 Hz --><param name="odom_model_type" value="diff"/><param name="odom_alpha5" value="0.1"/><param name="gui_publish_rate" value="10.0"/><param name="laser_max_beams" value="60"/><param name="laser_max_range" value="12.0"/><param name="min_particles" value="500"/><param name="max_particles" value="2000"/><param name="kld_err" value="0.05"/><param name="kld_z" value="0.99"/><param name="odom_alpha1" value="0.2"/><param name="odom_alpha2" value="0.2"/><!-- translation std dev, m --><param name="odom_alpha3" value="0.2"/><param name="odom_alpha4" value="0.2"/><param name="laser_z_hit" value="0.5"/><param name="laser_z_short" value="0.05"/><param name="laser_z_max" value="0.05"/><param name="laser_z_rand" value="0.5"/><param name="laser_sigma_hit" value="0.2"/><param name="laser_lambda_short" value="0.1"/><param name="laser_model_type" value="likelihood_field"/><!-- <param name="laser_model_type" value="beam"/> --><param name="laser_likelihood_max_dist" value="2.0"/><param name="update_min_d" value="0.25"/><param name="update_min_a" value="0.2"/><param name="odom_frame_id" value="$(arg odom_frame_id)"/> <param name="base_frame_id" value="$(arg base_frame_id)"/> <param name="global_frame_id" value="$(arg global_frame_id)"/><param name="resample_interval" value="1"/><!-- Increase tolerance because the computer can get quite busy --><param name="transform_tolerance" value="1.0"/><param name="recovery_alpha_slow" value="0.0"/><param name="recovery_alpha_fast" value="0.0"/><param name="initial_pose_x" value="$(arg initial_pose_x)"/><param name="initial_pose_y" value="$(arg initial_pose_y)"/><param name="initial_pose_a" value="$(arg initial_pose_a)"/><remap from="scan" to="$(arg scan_topic)"/></node>
</launch>
在turtlebot_ws/src/turtlebot_app/turtlebot_navigation/param目錄新建rplidar_costmap_params.yaml文件，
rplidar_costmap_params.yaml
global_costmap:robot_radius: 0.20obstacle_layer:scan:data_type: LaserScantopic: scanmarking: trueclearing: truemin_obstacle_height: 0.05max_obstacle_height: 0.35local_costmap:robot_radius: 0.18obstacle_layer:scan:data_type: LaserScantopic: scanmarking: trueclearing: truemin_obstacle_height: 0.05max_obstacle_height: 0.35
在turtlebot_ws/src/turtlebot_app/turtlebot_navigation/launch目錄下創建rplidar_amcl_demo.launch文件，代碼如下：
rplidar_amcl_demo.launch
<launch><!-- 2D sensor --><arg name="2D_laser" default="rplidar"/><include file="$(find rplidar_ros)/launch/rplidar.launch"></include><!-- Map server --><arg name="map_file" default="/home/lijian/Documents/turtlebot_ws/map/rplidar_gmapping.yaml"/><node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" /><!-- AMCL --><arg name="custom_amcl_launch_file" default="$(find turtlebot_navigation)/launch/includes/amcl/$(arg 2D_laser)_amcl.launch.xml"/><arg name="initial_pose_x" default="0.0"/> <!-- Use 17.0 for willow's map in simulation --><arg name="initial_pose_y" default="0.0"/> <!-- Use 17.0 for willow's map in simulation --><arg name="initial_pose_a" default="0.0"/><include file="$(arg custom_amcl_launch_file)"><arg name="initial_pose_x" value="$(arg initial_pose_x)"/><arg name="initial_pose_y" value="$(arg initial_pose_y)"/><arg name="initial_pose_a" value="$(arg initial_pose_a)"/></include><!-- Move base --><arg name="custom_param_file" default="$(find turtlebot_navigation)/param/$(arg 2D_laser)_costmap_params.yaml"/><include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"><arg name="custom_param_file" value="$(arg custom_param_file)"/></include></launch>
編譯
catkin_make
替換好rplidar_amcl_demo.launch中的地圖信息
roslaunch turtlebot_navigation rplidar_amcl_demo.launch
roslaunch turtlebot_rviz_launchers view_navigation.launch
指定
三、全局與局部規劃理解
【不檢查】點擊“2D Nav Goal”，指定目標點，對完成定位的turtlebot進行運動規劃，自行了解全局規劃和局部規劃的區別。
【作業內容】
四、C++編寫柵格地圖（可課后完成）
用C++編寫一個 m×nm\times nm×n 個格子的柵格地圖，并繪制圖案，如棋盤格、文字等。復雜圖案、彩圖、動態圖可加分。
實現思路：了解 nav_msgs/OccupancyGrid 消息的數據結構，編寫話題發布程序持續發出該類型話題，可以使用 rviz 查看發出的柵格圖樣。
范例：
#include "ros/ros.h"
#include <iostream>
#include "std_msgs/Float32.h"
#include "nav_msgs/OccupancyGrid.h"/* /map話題的消息類型為nav_msgs/OccupancyGrid 具體的數據結構為std_msgs/Header headeruint32 seqtime stampstring frame_id
nav_msgs/MapMetaData infotime map_load_timefloat32 resolutionuint32 widthuint32 heightgeometry_msgs/Pose origingeometry_msgs/Point positionfloat64 xfloat64 yfloat64 zgeometry_msgs/Quaternion orientationfloat64 xfloat64 yfloat64 zfloat64 w
int8[] data*/using namespace std;int main(int argc, char **argv)
{// 初始化節點ros::init(argc,argv,"mapnode");ros::NodeHandle n;ros::Publisher mappub = n.advertise<nav_msgs::OccupancyGrid>("map",1,true);ros::Publisher intpub = n.advertise<std_msgs::Float32>("number",1,true);nav_msgs::OccupancyGrid rosMap;rosMap.info.resolution = 1.0;rosMap.info.width = 20;rosMap.info.height = 10;rosMap.info.origin.position.x = 0.0;rosMap.info.origin.position.y = 0.0;rosMap.info.origin.position.z = 0.0;rosMap.info.origin.orientation.x = 0.0;rosMap.info.origin.orientation.y = 0.0;rosMap.info.origin.orientation.z = 0.0;rosMap.info.origin.orientation.w = 1.0;rosMap.data.resize(rosMap.info.width * rosMap.info.height);int count = 0;for(int i = 0;i < rosMap.info.width * rosMap.info.height; i++){rosMap.data[i] = count++;}rosMap.header.stamp = ros::Time::now();rosMap.header.frame_id = "map";mappub.publish(rosMap);std_msgs::Float32 mynumber;mynumber.data = 2.3;while(1){// intpub.publish(mynumber);mappub.publish(rosMap);cout << "Hello" << endl;}
}
總結
以上是生活随笔為你收集整理的实验四 定位与导航算法的全部內容，希望文章能夠幫你解決所遇到的問題。
