ROS小车，乐视深度相机+cartographer+move_base从零开始配置导航

制作教程链接

ROS小车搭建——2里程计
持续更新中…

小车制作

暴风雪代码参考
芯片：stm32F407ZG

stm32ide配置PWM

cubemx配置PWM输出
RT-Thread 之 PWM 设备驱动详细配置过程

小车蓝牙控制使用说明

串口转蓝牙模块：
波特率：9600
编码规则：
方向控制界面：ZK，K对应0x4B
重力ZI，0x49
摇杆ZJ，0x4A

ROS小车驱动板

如何从编码器得到转速？

电机4.66r/s
麦轮直径70mm
线速度v=2∗π∗R∗n(单位r/s)
=2x3.14x4.66x3.5cm
=102.4628cm/s
=1.024628m/s
小车线速度标定参考
编码器参数


编码器一圈输出11个脉冲，也即线数为11.
转了n圈时，编码器输出脉冲=n圈x编码器线数x减速比x4倍计数=11x21.3x4=937.2n

任务划分

小车与ROS的串口通信

Float32型数据收发

cartographer安装

安装参考
问题1

CMake Error at CMakeLists.txt:32 (find_package):
  By not providing "Findabsl.cmake" in CMAKE_MODULE_PATH this project has
  asked CMake to find a package configuration file provided by "absl", but
  CMake did not find one.

  Could not find a package configuration file provided by "absl" with any of
  the following names:

    abslConfig.cmake
    absl-config.cmake

  Add the installation prefix of "absl" to CMAKE_PREFIX_PATH or set
  "absl_DIR" to a directory containing one of the above files.  If "absl"
  provides a separate development package or SDK, be sure it has been
  installed.

解决1：
在下载回来的cartographer上有安装absl的脚本

$sudo apt-get install stow
 sudo chmod +x ~/cartographer_ws/src/cartographer/scriptsinstall_abseil.sh
 cd ~/cartographer_ws/src/cartographer/scripts
 ./install_abseil.sh

接着安装
install_ceres.sh
问题2：

Could NOT find Lua (missing:  LUA_LIBRARIES LUA_INCLUDE_DIR) 
CMake Error at cmake/modules/FindLuaGoogle.cmake:217 (MESSAGE):
  Did not find Lua >= 5.2

解决2：

sudo apt-get install liblua5.2-dev 

运行cartographer示例

roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=/home/tarmy/ros/fsb/catkin_cartographer/bag/cartographer_paper_deutsches_museum.bag

问题1：

[demo_backpack_2d.launch] is neither a launch file in package [cartographer_ros] nor is [cartographer_ros] a launch file name
The traceback for the exception was written to the log file

解决1：这是由于cartographer_ros没有编译导致的
在src的上一级目录下，执行

catkin build

运行效果

其中bag数据集的话题发布有：

/clock
/horizontal_laser_2d
/imu
/rosout
/rosout_agg
/vertical_laser_2d

rostopic info /horizontal_laser_2d
Type: sensor_msgs/MultiEchoLaserScan

建图

查看坐标系变换树

rosrun rqt_tf_tree rqt_tf_tree

roslaunch cartographer_ros demo_revo_lds_leshi.launch

<!--node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" /-->

在文件中

provide_odom_frame = true
改为false表示不需要cartongrapher提供里程计，应该改用外部里程计

仿照backpack_2d.launch建图

参考文章：cartographer跑自己的数据包-2d建图篇（laser+Imu建图篇）
坐标系详解

catkin_make_isolated --install --use-ninja
source devel_isolated/setup.bash
roslaunch cartographer_ros backpack_2d_leshi.launch

问题一：
话题不匹配

[ERROR] [1604035182.275338998]: Client [/cartographer_node] wants topic /scan to have datatype/md5sum [sensor_msgs/MultiEchoLaserScan/6fefb0c6da89d7c8abe4b339f5c2f8fb], but our version has [sensor_msgs/LaserScan/90c7ef2dc6895d81024acba2ac42f369]. Dropping connection.

解决1：更改雷达适配

num——laser_scan 1 
num_echo_laser 0
num_subdivision 1

小觅的imu话题：
camera_imu_frame
/mynteye/imu/data_raw

  frame_id: "camera_imu_frame"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 9.5033203125
  y: -0.0873291015625
  z: -2.01215820313
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

效果图

初次建图效果：
蓝色为轨迹
分析：轨迹基本正确，cartographer本身有回环检测，但轨迹有时候会突变（下图直线部分）


大致体现了实验室轮廓

Cartographer保存地图

rosservice call /write_state  /home/tarmy/ros/fsb/catkin_cartographer/bag/map.pbstream '1'

纯定位

roslaunch cartographer_ros demo_backpack_2d_localization.launch load_state_filename:=/home/tarmy/ros/fsb/catkin_cartographer/bag/b2-2016-04-05-14-44-52.bag.pbstream bag_filename:=/home/tarmy/ros/fsb/catkin_cartographer/bag/b2-2016-04-27-12-31-41.bag

此期间发布的话题

/clicked_point
/clock
/constraint_list
/horizontal_laser_2d
/imu
/initialpose
/joint_states
/landmark_poses_list
/map
/map_updates
/move_base_simple/goal
/rosout
/rosout_agg
/scan_matched_points2
/submap_list
/tf
/tf_static
/trajectory_node_list
/vertical_laser_2d

11/4更改记录
gedit backpack_2d_tarmy.urdf

  <joint name="imu2odom" type="fixed">
    <parent link="camera_link" />
    <child link="camera_imu_frame" />
    <origin xyz="0 0 0" />
  </joint>

注意：demo_backpack_2d_localization.launch

  <param name="/use_sim_time" value="true" />

要改为false，推测当此为false时，urdf中的坐标转换才被使用！！！！

<param name="/use_sim_time" value="false" />

纯定位时的坐标树

常用命令集合区

停止轨迹接收

rosservice call /finish_trajectory 0

保存地图

rosservice call /write_state  /home/tarmy/ros/fsb/catkin_cartographer/bag/map
.pbstream '1'

Move_base轨迹规划

在轨迹规划前，需要知道小车在先前建立好的地图中的位置，这通过RVIZ中的2D_pose_estimate估计实现
查找move_base是否是源码安装

rospack find move_base
/opt/ros/kinetic/share/move_base

例子：在launch文件中静态坐标变换
实现odom到map之间做了一个静态的坐标变换

 <!-- 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 0 0 /map /odom 100" />

参考：Move_base各个Yaml文件配置及注释
问题1：找不到Move_base这个节点

ERROR: cannot launch node of type [move_base/move_base]: can't locate node [move_base] in package [move_base]

解决1：找到cartographer_ros/package.xml,添加
<exec_depend>move_base</exec_depend>
从字面上就可以看出来，这是添加执行依赖
然后在启动launch，查看结果

source devel/setup.bash
roslaunch cartographer_ros leshi_move_base.launch 

SUMMARY
========
PARAMETERS
 * /move_base/DWAPlannerROS/acc_lim_theta: 3.2
 * /move_base/DWAPlannerROS/acc_lim_x: 2.5
 * /move_base/DWAPlannerROS/acc_lim_y: 0.0
 * /move_base/DWAPlannerROS/controller_frequency: 10.0
 * /move_base/DWAPlannerROS/forward_point_distance: 0.325
 * /move_base/DWAPlannerROS/global_frame_id: /odom
 * /move_base/DWAPlannerROS/goal_distance_bias: 20.0
 * /move_base/DWAPlannerROS/latch_xy_goal_tolerance: False
 * /move_base/DWAPlannerROS/max_scaling_factor: 0.2
 * /move_base/DWAPlannerROS/max_vel_theta: 2.75
 * /move_base/DWAPlannerROS/max_vel_trans: 0.22
 * /move_base/DWAPlannerROS/max_vel_x: 0.22

Move_base节点成功启动，可以加载参数。

Move_base配置文件的必要解释

base_local_planner
Given a plan to follow and a costmap, the controller produces velocity commands to send to a mobile base.
base_local_planner_params.yaml文件参数解释

TrajectoryPlannerROS:
  max_vel_x: 0.45
  min_vel_x: 0.1
  max_vel_theta: 1.0
  min_in_place_vel_theta: 0.4

  acc_lim_theta: 3.2
  acc_lim_x: 2.5
  acc_lim_y: 2.5

  holonomic_robot: true  #true表示为全向机器人
  dwa: true        #使用dwa规划路径则为真

他的功能是给一个global plan和local costmap，局部路径规划器计算出可行的速度发送给机器人

统筹深度相机、cartographer、move_base实时建图导航

问题1：超时？

[ WARN] [1604581656.731997127]: Costmap2DROS transform timeout. Current time: 1604581656.7320, global_pose stamp: 0.0000, tolerance: 5.0000

参考1
解决1：

ag transform_tolerance

transform_tolerance修改为10


全局路径有了，局部路径没有

局部路径规划问题

dwa为true后，可以启用dwa，但出现如下问题
问题1：

None of the points of the global plan were in the local costmap, global plan points too far from robot

解决1：在dwa的yaml配置中

forward_point_distance改为0.9

问题2：

[ WARN] [1604630562.396520507]: Rotate recovery behavior started

分析2：RecoveryBehavior用来应对导航过程中各模块的故障，当全局规划故障、局部规划故障、震荡时都会进入到恢复行为中，它先清理周围一定范围以外的costmap（障碍层），接下来重新执行规划，若不奏效，则旋转180度，再执行规划。交替两次后，已转过360度，若还是没能排除故障，则恢复行为失败，关闭Movebase规划器
保持小车摄像头静止时，规划成功，可以看到cmd_vel话题，Got the plan提示规划成功，绿色小短线是局部路径规划，其实局部路径规划应该可以理解为由短程离散的全局路径点生成小车坐标系下的速度行进点。

问题3：

Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors

参考DWA参数调节

半成品图

行人轨迹

rostopic echo /md_centerPeople | grep frame_id
 frame_id: "camera_rgb_optical_frame"
rostopic info /md_centerPeople
Type: sensor_msgs/PointCloud2

Publishers: 
 * /firefly_sbx/vio (http://ubuntu:39379/)

  geometry_msgs::PoseStamped 中的point类型为float64