ROS小车搭建——2里程计
持续更新中…
暴风雪代码参考
芯片：stm32F407ZG
cubemx配置PWM输出
RT-Thread 之 PWM 设备驱动详细配置过程
串口转蓝牙模块：
波特率：9600
编码规则：
方向控制界面：ZK，K对应0x4B
重力ZI，0x49
摇杆ZJ，0x4A
电机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
Float32型数据收发
安装参考
问题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
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提供里程计，应该改用外部里程计
参考文章：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本身有回环检测，但轨迹有时候会突变（下图直线部分）
大致体现了实验室轮廓
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'
在轨迹规划前，需要知道小车在先前建立好的地图中的位置，这通过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节点成功启动，可以加载参数。
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，局部路径规划器计算出可行的速度发送给机器人
问题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