当前位置:   article > 正文

基于Pixhawk和ROS搭建自主无人车(五):SLAM导航篇_fence requires position

fence requires position

参考

1. 硬件平台

在这里插入图片描述

2. 环境搭建

2.1 创建工作空间

$ cd
$ mkdir -p mav_ws/src
$ cd mav_ws
$ catkin_init_workspace
  • 1
  • 2
  • 3
  • 4

ROS命令 catkin_init_workspace 分析

2.2 安装 RPLiDAR 包

$ cd ~/mav_ws/src
$ git clone https://github.com/Slamtec/rplidar_ros.git
  • 1
  • 2

2.3 安装 Cartographer

  • 先安装一些工具

    $ sudo apt-get update
    $ sudo apt-get install -y python-wstool python-rosdep ninja-build stow
    # noetic 则使用下行代码,上行默认为 melodic 版本
    $ sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow
    
    • 1
    • 2
    • 3
    • 4
  • 使用 wstool 重新初始化工作区,然后合并 cartographer_ros.rosinstall 文件并获取依赖项的代码

    $ cd ~/mav_ws
    $ wstool init src
    $ wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall
    $ wstool update -t src
    
    • 1
    • 2
    • 3
    • 4
  • 安装 proto3 和 abseil

    $ wget http://fishros.com/install -O fishros && bash fishros  # 根据提示选择 3 安装 rosdepc 
    $ src/cartographer/scripts/install_proto3.sh
    $ sudo rosdepc init
    $ rosdepc update
    # 进行下一步之前,先注释 cartographer 包下 package.xml 中该行代码:(<depend>libabsl-dev</depend>)  
    $ src/cartographer/scripts/install_abseil.sh
    $ rosdepc install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
  • 安装 robot_pose_publisher(使用 TF 发布机器人相对于地图位置的节点)

    $ cd ~/mav_ws/src
    $ git clone https://github.com/GT-RAIL/robot_pose_publisher.git
    
    • 1
    • 2
  • 创建 cartographer.launch 文件

    $ cd ~/mav_ws/src/cartographer_ros/cartographer_ros/launch
    $ gedit cartographer.launch
    
    • 1
    • 2
    <!-- cartographer.launch -->
    <launch>
       <param name="/use_sim_time" value="false" />
       <node name="cartographer_node"
             pkg="cartographer_ros"
             type="cartographer_node"
             args="-configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename cartographer.lua"
             output="screen">
             <remap from="odom" to="/mavros/local_position/odom" />
             <remap from="imu" to="/mavros/imu/data" />
       </node>
       <node name="cartographer_occupancy_grid_node"
             pkg="cartographer_ros"
             type="cartographer_occupancy_grid_node" />
       <node name="robot_pose_publisher"
             pkg="robot_pose_publisher"
             type="robot_pose_publisher"
             respawn="false"
             output="screen" >
             <param name="is_stamped" type="bool" value="true"/>
             <remap from="robot_pose" to="/mavros/vision_pose/pose" />
       </node>
       <node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link laser 100" />
    </launch>
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
  • 创建 cartographer.lua 脚本文件

    $ cd ~/mav_ws/src/cartographer_ros/cartographer_ros/configuration_files
    $ gedit cartographer.lua
    
    • 1
    • 2
    include "map_builder.lua"
    include "trajectory_builder.lua"
    
    options = {
      map_builder = MAP_BUILDER,
      trajectory_builder = TRAJECTORY_BUILDER,
      map_frame = "map",
      tracking_frame = "base_link",
      published_frame = "base_link",
      odom_frame = "odom",
      provide_odom_frame = true,
      publish_frame_projected_to_2d = false,
      use_odometry = false,
      use_nav_sat = false,
      use_landmarks = false,
      num_laser_scans = 1,
      num_multi_echo_laser_scans = 0,
      num_subdivisions_per_laser_scan = 1,
      num_point_clouds = 0,
      lookup_transform_timeout_sec = 0.2,
      submap_publish_period_sec = 0.3,
      pose_publish_period_sec = 5e-3,
      trajectory_publish_period_sec = 30e-3,
      rangefinder_sampling_ratio = 1.,
      odometry_sampling_ratio = 1.,
      fixed_frame_pose_sampling_ratio = 1.,
      imu_sampling_ratio = 1.,
      landmarks_sampling_ratio = 1.,
    }
    
    MAP_BUILDER.use_trajectory_builder_2d = true
    
    TRAJECTORY_BUILDER_2D.min_range = 0.05
    TRAJECTORY_BUILDER_2D.max_range = 30
    TRAJECTORY_BUILDER_2D.missing_data_ray_length = 8.5
    TRAJECTORY_BUILDER_2D.use_imu_data = false
    TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 0.2
    TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 5
    TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
    TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
    TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 1.
    TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 10
    TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.2)
    -- for current lidar only 1 is good value
    TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
    
    TRAJECTORY_BUILDER_2D.min_z = -0.5
    TRAJECTORY_BUILDER_2D.max_z = 0.5
    
    POSE_GRAPH.constraint_builder.min_score = 0.65
    POSE_GRAPH.constraint_builder.global_localization_min_score = 0.65
    POSE_GRAPH.optimization_problem.huber_scale = 1e2
    POSE_GRAPH.optimize_every_n_nodes = 30
    
    return options
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55

2.4 编译工作空间

$ cd ~/mav_ws
$ catkin build
$ source devel/setup.bash
  • 1
  • 2
  • 3

catkin_make 和catkin build这两个命令的区别

2.5 启动

  • 启动 roscore

    $ roscore
    
    • 1
  • 启动激光雷达节点

    # 本文使用 RPLiDAR A1 型号激光雷达,根据不同型号替换 rplidar_a1.launch
    $ roslaunch rplidar_ros rplidar_a1.launch
    
    • 1
    • 2
    <!-- rplidar_a1.launch -->
    <!-- 注意查看激光雷达连接机载电脑的端口号 /dev/ttyUSB0 -->
    <launch>
      <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
      <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>
      <param name="serial_baudrate"     type="int"    value="115200"/>
      <param name="frame_id"            type="string" value="laser"/>
      <param name="inverted"            type="bool"   value="false"/>
      <param name="angle_compensate"    type="bool"   value="true"/>
      </node>
    </launch>
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
  • 启动 cartographer

    $ roslaunch cartographer_ros cartographer.launch
    
    • 1
  • 启动 MAVROS 通信

    $ roslaunch mavros apm.launch
    
    • 1

    基于Pixhawk和ROS搭建自主无人车(三):ROS通信篇

3. 配置 APM 固件参数

  • 打开 Mission Planner 地面站,在全部参数表中配置以下参数(记得写入参数并重启地面站

    AHRS_EKF_TYPE = 3
    EK2_ENABLE = 0
    EK3_ENABLE = 1
    EK3_SRC1_POSXY = 6
    EK3_SRC1_POSZ = 1
    EK3_SRC1_VELXY = 6
    EK3_SRC1_VELZ = 6
    EK3_SRC1_YAW = 6
    GPS_TYPE = 0
    VISO_TYPE = 1
    ARMING_CHECK = 388598
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
  • 在飞行数据界面地图上右键,然后选择 “设置家在此” >> “Set Home Here” >> “Set EKF Origin Here”,车辆应立即出现在地图上
    在这里插入图片描述

4. 测试

  • 要确认 ROS 端正常工作,请输入以下命令,并且应显示 cartographer 位置估计的实时更新
    $ rostopic echo /mavros/vision_pose/pose
    $ rostopic info /mavros/vision_pose/pose
    
    • 1
    • 2

在这里插入图片描述

在这里插入图片描述

  • 打开 Mission Planner 地面站,在飞行数据界面按 Ctrl+F,然后点击 MAVLink Inspector,出现以下界面证明 VISION_POSITION_ESTIMATE 消息已成功发送到飞控
    在这里插入图片描述

5. 报错解决

  • PreArm:Fence requires position:将参数 FENCE_ENABLE 设置为 0
    在这里插入图片描述

  • FreArm:servo function 33 unassigned:将 33-36 对应的 Motor 都设置好(可以不使用,但必须设置
    在这里插入图片描述

在这里插入图片描述

6. 从 rviz 发布指令

  • 通过远程桌面(如 Todesk)或 ssh 在机载电脑启动 rviz
    • Add --> LaserScan --> /scan
    • Add --> Map --> /map(本文 cartographer 建图节点 died 导致无法显示 map,暂未解决
    • Add --> Pose --> /mavros/vision_pose/pose
    $ rosrun rviz rviz
    
    • 1

在这里插入图片描述

  • 修改 mavros 的 node.launch 文件

    ...
    <rosparam command="load" file="$(arg config_yaml)" />
    <!-- 在上行代码后添加下行代码 -->
    <remap from="/mavros/setpoint_position/local" to="/move_base_simple/goal" />
    ...
    
    • 1
    • 2
    • 3
    • 4
    • 5
  • Hold 模式下解锁(Arm)车辆,并切换到 Guided 模式,在 rviz 中设置 2D Nav Goal

    • 此时 Mission Planner 地面站应显示车辆正在驶向的目标位置(Mission Planner 地面站在目标位置放置一个绿色标记,并从车辆向目标绘制一条橙色线)
  • 通信测试,显示下图所示内容表示通过 rviz 发布指令通信成功

    # 应将发布者显示为 “rviz”,订阅者显示为 “mavros”
    $ rostopic info /move_base_simple/goal
    
    # 在 rviz 中设置 “2D NavGoal” 后应立即显示 “position” 和 “orientation”
    $ rostopic echo /move_base_simple/goal
    
    # 显示 ROS 节点及其连接的图表
    $ rosrun rqt_graph rqt_graph
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8

在这里插入图片描述

在这里插入图片描述

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/IT小白/article/detail/619249
推荐阅读
相关标签
  

闽ICP备14008679号