提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

一、启动仿真

roslaunch mbot_gazebo view_mbot_gazebo.launch
sudo apt install ros-noetic-teleop-twist-keyboard
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
sudo apt-get install ros-noetic-map-server

二、建图

1创建工作空间

mkdir -p xjq_ws/src 
cd xjq_ws/src 
catkin_create_pkg xjq_navigation actionlib geometry_msgs move_base_msgs rospy
catkin_make
echo "source ~/PATH/xjq_ws/devel/setup.bash">> ~/.bashrc
cd xjq_navigation
mkdir launch map

2.创建LAUNCH文件

launch/gmapping.launch：

<launch>
    /
    <arg name="scan_topic" default="scan" />
   
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
        // mbot_gazebo 会通过发/odom topic，传出里程计数据
        <param name="odom_frame" value="odom"/>
        <param name="map_update_interval" value="5.0"/>
        <!-- Set maxUrange < actual maximum range of the Laser -->
        <param name="maxRange" value="5.0"/>
        <param name="maxUrange" value="4.5"/>
        <param name="sigma" value="0.05"/>
        <param name="kernelSize" value="1"/>
        <param name="lstep" value="0.05"/>
        <param name="astep" value="0.05"/>
        <param name="iterations" value="5"/>
        <param name="lsigma" value="0.075"/>
        <param name="ogain" value="3.0"/>
        <param name="lskip" value="0"/>
        <param name="srr" value="0.01"/>
        <param name="srt" value="0.02"/>
        <param name="str" value="0.01"/>
        <param name="stt" value="0.02"/>
        <param name="linearUpdate" value="0.5"/>
        <param name="angularUpdate" value="0.436"/>
        <param name="temporalUpdate" value="-1.0"/>
        <param name="resampleThreshold" value="0.5"/>
        <param name="particles" value="80"/>
        <param name="xmin" value="-1.0"/>
        <param name="ymin" value="-1.0"/>
        <param name="xmax" value="1.0"/>
        <param name="ymax" value="1.0"/>
        <param name="delta" value="0.05"/>
        <param name="llsamplerange" value="0.01"/>
        <param name="llsamplestep" value="0.01"/>
        <param name="lasamplerange" value="0.005"/>
        <param name="lasamplestep" value="0.005"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>
    
</launch>

手动建图

rosrun teleop_twist_keyboard teleop_twist_keyboard.py
roslaunch xjq_navigation gmapping.launch 

保存地图

cd ~/catkin_ws/src/xjq_navigation/maps
rosrun map_server map_saver -f map

三、导航

1.创建LAUNCH文件

launch/navigation_with_map.launch

<launch>

    <!-- 设置地图的配置文件 -->
    <arg name="map" default="map.yaml" />

    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mbot_navigation)/maps/$(arg map)"/>

    <!-- 运行move_base节点 -->
    <include file="$(find mbot_navigation)/launch/move_base.launch"/>

    <!-- 启动AMCL节点 -->
    <include file="$(find mbot_navigation)/launch/amcl.launch" />


</launch>

launch/amcl.launch

 <launch>
    <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"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
        <param name="min_particles"             value="500"/>
        <param name="max_particles"             value="3000"/>
        <param name="kld_err"                   value="0.02"/>
        <param name="update_min_d"              value="0.20"/>
        <param name="update_min_a"              value="0.20"/>
        <param name="resample_interval"         value="1"/>
        <param name="transform_tolerance"       value="0.5"/>
        <param name="recovery_alpha_slow"       value="0.00"/>
        <param name="recovery_alpha_fast"       value="0.00"/>
        <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)"/>
        <param name="gui_publish_rate"          value="50.0"/>

        <remap from="scan"                      to="$(arg scan_topic)"/>
        <param name="laser_max_range"           value="3.5"/>
        <param name="laser_max_beams"           value="180"/>
        <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_likelihood_max_dist" value="2.0"/>
        <param name="laser_model_type"          value="likelihood_field"/>

        <param name="odom_model_type"           value="diff"/>
        <param name="odom_alpha1"               value="0.1"/>
        <param name="odom_alpha2"               value="0.1"/>
        <param name="odom_alpha3"               value="0.1"/>
        <param name="odom_alpha4"               value="0.1"/>
        <param name="odom_frame_id"             value="odom"/>
        <param name="base_frame_id"             value="base_footprint"/>
    </node>
</launch>

launch/move_base.launch

 <launch>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />

    <rosparam file="$(find mbot_navigation)/config/move_base/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find mbot_navigation)/config/move_base/costmap_common_params.yaml" command="load" ns="local_costmap" /> 
    <rosparam file="$(find mbot_navigation)/config/move_base/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/move_base/global_costmap_params.yaml" command="load" />

    <rosparam file="$(find mbot_navigation)/config/move_base/move_base_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/move_base/dwa_local_planner_params.yaml" command="load" />
  </node>
  
</launch>

2.导航

roslaunch xjq_navigation navigation_with_map.launch
mkdir scripts/test.py
vim CMakeLists.txt

mkdir config

waypoints.yaml

waypoints:
  - x: 1.0
    y: 0.0
    theta: 0.0
  - x: 1.0
    y: 0.0
    theta: 0.0
  - x: 1.0
    y: 0.0
    theta: 0.0

rosparam load target_points.yaml

 catkin_install_python(PROGRAMS
   scripts/test.py
   scripts/get_current_pos.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

#!/usr/bin/env python3
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 tf.transformations import quaternion_from_euler

def create_pose(x, y, theta):
    pose = Pose()
    pose.position.x = x
    pose.position.y = y
    pose.position.z = 0.0

    quaternion = quaternion_from_euler(0, 0, theta)
    pose.orientation = Quaternion(*quaternion)

    return pose
    
def main():
  rospy.init_node("move_test", anonymous=True)
  move_base_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
  rospy.loginfo("Waiting for move_base action server...")  
  while move_base_client.wait_for_server(rospy.Duration(5.0)) == 0:
    rospy.loginfo("connected to move base server")
  	
  target_list = []
  # 从参数服务器读取目标点数据
  waypoints = rospy.get_param('waypoints')
  rospy.loginfo("Loaded waypoints: %s", waypoints)
  # 处理目标点
  for wp in waypoints:
      pose = create_pose(wp['x'], wp['y'], wp['theta'])
      rospy.loginfo("Waypoint Pose: %s", pose)
      target_list.append(pose)
        

  
  for i, target in enumerate(target_list):
    start_time = rospy.Time.now()  
    
    goal = MoveBaseGoal()
    
    goal.target_pose.header.frame_id = 'base_link'
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose = target
    
    #goal.target_pose.pose.position.x = 1.0
    #goal.target_pose.pose.orientation.w = 1.0
    
    rospy.loginfo("going to {0} goal".format(i))
    move_base_client.send_goal(goal)
    finished_within_time = move_base_client.wait_for_result(rospy.Duration(300))
    
    if not finished_within_time:
      move_base_client.cancel_goal()
      rospy.loginfo("time out, failed to goal")
    else:
      running_time = (rospy.Time.now() - start_time).to_sec()
      if move_base_client.get_state() == GoalStatus.SUCCEEDED:
        rospy.loginfo("go to {0} goal succeeded, run time: {1} sec".format(i, running_time))
      else:
        rospy.loginfo("goal failed")
    
if __name__ == "__main__":
  main()
  

rosrun xjq_navigation test.py