geometry_msgs 系列

• geometry_msgs/Pose

  1 2	geometry_msgs/Point position geometry_msgs/Quaternion orientation

• geometry_msgs::PoseStamped

  1 2	std_msgs/Header header geometry_msgs/Pose pose

• geometry_msgs::Point

  1 2 3

  float64 x float64 y float64 z

• geometry_msgs::Quaternion

  1 2 3 4

  float64 x float64 y float64 z float64 w

• geometry_msgs/PoseArray

  1 2	std_msgs/Header header geometry_msgs/Pose[] poses

• geometry_msgs/Pose2D

  1 2 3

  float64 x float64 y float64 theta

• geometry_msgs/Transform

  1 2	geometry_msgs/Vector3 translation geometry_msgs/Quaternion rotation

• geometry_msgs/PoseWithCovariance

  1 2	geometry_msgs/Pose pose float64[36] covariance

• geometry_msgs::Polygon

  1	geometry_msgs/Point32[] points

tf 系列

以下两个类都继承 tf::Transform

tf::Pose

tf::Stamped对数据类型做模板化（除了tf::Transform），并附带元素frameid和stamp、
setData成员函数

1
2
3
4

tf::Stamped<tf::Pose> pose;
getOrigin().getX();
getOrigin().getY();
tf::getYaw(pose.getRotation())

tf::StampedTransform

1
2
3
4
5

double x= transform.getOrigin().getX();
double y= transform.getOrigin().getY();
double z= transform.getOrigin().getZ();
double angle = transform.getRotation().getAngle();
ROS_INFO("x: %f, y: %f, z: %f, angle: %f",x,y,z,angle);

1
2
3
4
5
6
7
8
9
10
11

const geometry_msgs::PoseStamped& pose = ;
tf::StampedTransform  transform;
geometry_msgs::PoseStamped new_pose;

tf::Stamped<tf::Pose> tf_pose;
tf::poseStampedMsgToTF(pose, tf_pose);

tf_pose.setData(transform * tf_pose);
tf_pose.stamp_ = transform.stamp_;
tf_pose.frame_id_ = global_frame;
tf::poseStampedTFToMsg(tf_pose, new_pose);  //转为 geometry_msgs::PoseStamped类型

tf::poseStampedMsgToTF函数，把geometry_msgs::PoseStamped转化为Stamped<Pose>

PoseSE2

源码在pose_se2.h，有两个private成员

1
2

Eigen::Vector2d  _position; 
double _theta;

1
2
3
4
5

PoseSE2(double x, double y, double theta)
PoseSE2(const geometry_msgs::Pose& pose)
PoseSE2(const tf::Pose& pose)

friend std::ostream& operator<< (std::ostream& stream, const PoseSE2& pose)

Eigen::Vector2d orientationUnitVec() const: 获得当前朝向的单位向量

PoseSE2::average(start, goal);

costmap_converter/ObstacleArrayMsg

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

std_msgs/Header header    # frame_id 为map
costmap_converter/ObstacleMsg[] obstacles

# costmap_converter/ObstacleMsg 的成员如下
std_msgs/Header header    # frame_id 为map
# Obstacle footprint (polygon descriptions)
geometry_msgs/Polygon polygon
# radius for circular/point obstacles
float64 radius
# Obstacle ID
# Specify IDs in order to provide (temporal) relationships
# between obstacles among multiple messages.
int64 id
# Individual orientation (centroid)
geometry_msgs/Quaternion orientation
# Individual velocities (centroid)
geometry_msgs/TwistWithCovariance velocities

类Obstacle定义在obstacle.h，派生类有PointObstacle, CircularObstacle, LineObstacle,PolygonObstacle

1
2

typedef boost::shared_ptr<Obstacle> ObstaclePtr;
typedef std::vector<ObstaclePtr>  ObstContainer;

PoseSequence posevec; //!< Internal container storing the sequence of optimzable pose vertices

TimeDiffSequence timediffvec; //!< Internal container storing the sequence of optimzable timediff vertices

VertexPose 继承g2o::BaseVertex<3, PoseSE2 >。
This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized via g2o

VertexTimeDiff继承g2o::BaseVertex<1, double>。This class stores and wraps a time difference \f$ \Delta T \f$ into a vertex that can be optimized via g2o

判断初始化：bool isInit() const {return !timediff_vec_.empty() && !pose_vec_.empty(); }

TimedElasticBand::clearTimedElasticBand(): 清空 timediffvec posevec

addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf”;

所有约束的信息矩阵都是对角矩阵

• 速度

1
2
3
4
5

Eigen::Matrix<double,2,2> information;
information(0,0) = cfg_->optim.weight_max_vel_x;
information(1,1) = cfg_->optim.weight_max_vel_theta;
information(0,1) = 0.0;
information(1,0) = 0.0;

• 加速度

  1 2 3 4

  Eigen::Matrix<double,2,2> information; information.fill(0); information(0,0) = cfg_->optim.weight_acc_lim_x; information(1,1) = cfg_->optim.weight_acc_lim_theta;

• 时间

  1 2	Eigen::Matrix<double,1,1> information; information.fill(cfg_->optim.weight_optimaltime);

• 最短距离

1
2

Eigen::Matrix<double,1,1> information;
information.fill(cfg_->optim.weight_shortest_path);

• 优先转弯方向，二元边

  1 2 3

  // create edge for satisfiying kinematic constraints Eigen::Matrix<double,1,1> information_rotdir; information_rotdir.fill(cfg_->optim.weight_prefer_rotdir);

• 障碍

1
2

Eigen::Matrix<double,1,1> information;
information.fill(cfg_->optim.weight_obstacle * weight_multiplier);

• 动态障碍 AddEdgesDynamicObstacles

1
2
3
4

Eigen::Matrix<double,2,2> information;
information(0,0) = cfg_->optim.weight_dynamic_obstacle * weight_multiplier;
information(1,1) = cfg_->optim.weight_dynamic_obstacle_inflation;
information(0,1) = information(1,0) = 0;

贝叶斯法则： $P(x|z) = \frac{P(z|x)P(x)}{P(z)}$

$P(x|z)$是后验， $P(z|x)$ 是似然， $P(x)$是先验

• 先验：指根据以往经验得到事件发生的概率

最大后验： 使$P(x|z)$最大的x估计，比如分布是高斯分布，那么求出的是均值，比估计整个分布简单的多。

有时求最大后验时，$P(x)$也不知道，所以只把似然最大化。

最大似然： 在什么状态下，最可能产生当前的观测。对于高斯分布，就是什么情况下最可能取得均值，让概率密度函数最大

在状态估计中，可以这么理解，先验就是没有得到观测值时的概率分布，似然就是观测的概率分布，后验就是在得到观测值后对先验校正后的概率分布。

机器人导航时偶然出现倒退的小速度-0.02，但周围又没有障碍物，其实就是参数max_vel_x_backwards=0.02

1. 从OpenCV的github release下载，然后解压

   1 2 3 4 5 6

   cd opencv-4.2.0 mkdir build cd build cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local .. make sudo make install

   添加路径库 sudo vim /etc/ld.so.conf.d/opencv.conf，打开了一个新文档，在里面写入/usr/local/lib

2. 配置环境变量sudo vim /etc/profile，在后面添加以下内容

   1 2	PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/lib/pkgconfig export PKG_CONFIG_PATH

3.测试

1
2
3
4
5
6

cd ~
# opencv的源码目录
cd opencv/samples/cpp/example_cmake
cmake .
make
./opencv_example

1. 检查是否有多个OpenCV版本

   1 2 3 4 5 6 7 8 9

   locate libopencv_video.so /usr/lib/x86_64-linux-gnu/libopencv_video.so /usr/lib/x86_64-linux-gnu/libopencv_video.so.3.2 /usr/lib/x86_64-linux-gnu/libopencv_video.so.3.2.0 /usr/local/lib/libopencv_video.so /usr/local/lib/libopencv_video.so.4.2 /usr/local/lib/libopencv_video.so.4.2.0

1. 如果你需要在Python3环境下使用OpenCV，那么sudo pip3 install opencv-python，python后不用加3。对于在Python环境中使用,比如说查看版本

   1 2 3 4 5 6 7

   cyp@cyp:~$ python Python 3.6.7 (default, Oct 22 2018, 11:32:17) [GCC 8.2.0] on linux Type "help", "copyright", "credits" or "license" for more information. >>> import cv2 as cv >>> cv.__version__ '4.1.0'

在使用g++编译使用opencv的C++程序时，使用 g++ <cpp_code> pkg-config opencv --libs --cflags opencv 也可以使用cmake编译

1. 使用指定的版本

在opencv编译好后，所在目录中一般会有一个叫OpenCVConfig.cmake的文件，这个文件中指定了CMake要去哪里找OpenCV，其.h文件在哪里等，比如其中一行：

1
2

# Provide the include directories to the caller 
set(OpenCV_INCLUDE_DIRS "/home/ubuntu/src/opencv-3.1.0/build" "/home/ubuntu/src/opencv-3.1.0/include" "/home/ubuntu/src/opencv-3.1.0/include/opencv") 

1

set(OpenCV_DIR "/home/ubuntu/src/opencv-3.1.0/build")

Gazebo对电脑显卡有一定要求，对Nvida显卡支持较好，对AMD的显卡支持较差，如果是AMD的显卡，复杂的世界模型一般加载不出来

利用spawn_model脚本向gazebo_ros节点（在主题中，空间名为 gazebo）发出服务请求，进而添加 URDF 到 Gazebo 中

1

rosrun gazebo_ros spawn_model -file `rospack find MYROBOT_description`/urdf/MYROBOT.urdf -urdf -x 0 -y 0 -z 1 -model MYROBOT

xacro转urdf

如果是kinetic，执行

1

rosrun xacro xacro.py robot1.xacro > robot1_processed.urdf

1
2
3

xacro: Traditional processing is deprecated. Switch to --inorder processing!
To check for compatibility of your document, use option --check-order.
For more infos, see http://wiki.ros.org/xacro#Processing_Order xacro.py is deprecated; please use xacro instead

如果是在Melodic，执行rosrun xacro xacro --inorder $(locate robot.urdf.xacro) > robot_inorder.urdf，结果得到

1

xacro: in-order processing became default in ROS Melodic. You can drop the option.

查看关节图

先执行sudo apt install -y liburdfdom-tools

1
2
3

check_urdf pr2.urdf
# 再打开转换的pdf
urdf_to_graphiz pr2.urdf

查看spawn_model的所有参数，可以运行：

1

rosrun gazebo_ros spawn_model -h

在 launch文件中，对于urdf可以这样编写：

1
2

<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find baxter_description)/urdf/baxter.urdf -urdf -z 1 -model baxter" />

对于xacro，这样编写:

1
2
3
4
5

<!-- Convert an xacro and put on parameter server -->
<param name="robot_description" command="$(find xacro)/xacro.py $(find pr2_description)/robots/pr2.urdf.xacro" />

<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model pr2" />

URDF的报错原因：加了等标签，但没加具体内容。

rosrun gazebo_ros spawn_model -file robot.urdf -urdf -model robot

sdf: standard definition of world for Gazebo

1
2
3
4
5
6
7
8
9
10

<launch>
  <include file="$(find gazebo_ros)/launch/empty_world.launch" >
  </include>

  <arg name='robot_urdf' default="$(find xacro)/xacro '$(find vehicle_simulator)/urdf/robot.urdf.xacro'" />
  <param name="robot_description" command="$(arg robot_urdf)" />
  <node pkg="gazebo_ros" type="spawn_model" name="spawn_robot" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0.5"/>

  <node pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" name="teleop" />
</launch>

Gazebo的gui参数经常是false，这是因为显示界面会显著加大资源占用

world文件可以直接改名称，不影响使用

兼容问题

加入camera仿真

Gazebo只给出了水平FOV, 竖直FOV会根据图片的宽和高自动计算。 Gazebo有函数double VerticalFOV() const，解释: The vertical FOV is calculated from width, height, hfov。有bool SetHorizontalFOV (double _hfov)函数，但是没有函数bool SetVerticalFOV (double _hfov)

差速模块

出现下面日志，说明差速控制模块正常加载。

1
2
3
4
5
6

[ INFO] [1720590402.172629168, 68.803000000]: Starting plugin DiffDrive(ns = //)
[ INFO] [1720590402.172729086, 68.803000000]: DiffDrive(ns = //): <rosDebugLevel> = na
[ INFO] [1720590402.173268546, 68.803000000]: DiffDrive(ns = //): <tf_prefix> = 
[ INFO] [1720590402.173831755, 68.803000000]: DiffDrive(ns = //): Try to subscribe to cmd_vel
[ INFO] [1720590402.175204165, 68.803000000]: DiffDrive(ns = //): Subscribe to cmd_vel
[ INFO] [1720590402.175652313, 68.803000000]: DiffDrive(ns = //): Advertise odom on odom

libgazebo_ros_gps_sensor.so

libgazebo_ros_bumper.so

libgazebo_ros_ray_sensor.so

视频
这个问题的本质是ROS的全局路径算法不考虑车体的轮廓，跟轮廓有关的避障交给了局部路径，DWA和TEB算法里都有footprintCost之类的函数，可以判断是否撞了障碍，但全局路径算法没有。 全局路径是把车当成了点，从所在栅格到目标栅格进行规划。这里开始规划出的路线就是这样，在走了一段时间被TEB认为不可行后，全局路径换成另一条，但是马上被认为不如之前的全局路径更优，又换回去了。如此循环，所以走不出去了。

但是要让全局路径考虑轮廓，会改动很大(即使不考虑动态障碍)，成本太高了。所以有必要考虑其他导航框架了。

暂时的解决方法只有设置中间点或者加虚拟墙

在ROS当中，原作者是不推荐用多线程的，他建议用多进程，变成一个个节点的形式进行通信。多线程分为两种模式：同步和异步。

• 同步：MultiThreadSpinner s(4)，一共5个线程。包括了主线程。

• 异步：AsyncSpinner s(4)， 一共5个线程。包括了主线程。

对于多话题的订阅，我们先看传统的方法：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

void cb1(const geometry_msgs::PoseStamped::ConstPtr& msg) 
{
    ROS_INFO("uwb_pose x: %f", msg->pose.position.x);
}

void cb2(const geometry_msgs::PoseStamped::ConstPtr& msg) 
{
    sleep(2);
    ROS_INFO("yolo_pose x: %f", msg->pose.position.x);
}

int main(int argc, char **argv) 
{
    ros::init(argc, argv, "node");
    ros::NodeHandle nh;
    setlocale(LC_ALL, "");

    ros::Subscriber uwbSub = nh.subscribe<geometry_msgs::PoseStamped>("uwb_pose", 1, cb1);
    ros::Subscriber yoloSub = nh.subscribe<geometry_msgs::PoseStamped>("yolo_pose", 1, cb2);
    ros::spin();
    return 1;
}

把代码加上ros::MultiThreadedSpinner s(2); ( 无需加入头文件 ), ros::spin();改为ros::spin(s);, 再运行会发现cb1里没有缺少一个消息。

这里多线程的目的是保证线程cb1不丢失消息，而不是cb2，它丢失消息是必然的。

对于ros::AsyncSpinner，代码在ros::Subscriber定义之后这样写:

1
2
3

ros::AsyncSpinner spinner(2);
spinner.start();
ros::waitForShutdown();

当程序当中有数据处理线程的时候，建议开辟异步多线程订阅，算法写在订阅函数里面。 当然，目前的处理当中，我更倾向于重新开辟一个线程，然后通过循环数组来进行数据交互。

参考：ROS多线程订阅消息