fssim车子自动跑起来

fssim实现小车自动驾驶

启动fssim以后，通过

rqt_graph

查看节点和话题关系。取消所有隐藏，显示所有node和topic就是下面这个样子。其中节点有六个：

1. /automated_res
2. /fssim_robot_state_publisher
3. /fssim_rviz
4. /gazebo
5. /rosout
6. /rqt_gui_py_node_27715
   其中/rqt_gui_py_node_27715是我们为了查看节点与话题关系刚刚打开的节点，不算是fssim启动的。
   /rosout节点是用于查看各个节点的状态信息的。所有节点都会向/rosout同名话题发送信息,\rosou话题的订阅者只有\rosout节点。所以rosout相关的都是起到debug作用的。查看该话题的msg信息：

 rostopic info /rosout

得到以下信息

hqx@hqx-MacBookAir:~/demo$ rostopic info /rosout
Type: rosgraph_msgs/Log

Publishers: 
 * /fssim_rviz (http://hqx-MacBookAir:35693/)
 * /automated_res (http://hqx-MacBookAir:36691/)
 * /fssim_robot_state_publisher (http://hqx-MacBookAir:38007/)
 * /gazebo (http://hqx-MacBookAir:43237/)
 * /rqt_gui_py_node_27715 (http://hqx-MacBookAir:35233/)

Subscribers: 
 * /rosout (http://hqx-MacBookAir:33067/)

其msg为：rosgraph_msgs/Log
具体内容：

rosmsg show rosgraph_msgs/Log

byte DEBUG=1
byte INFO=2
byte WARN=4
byte ERROR=8
byte FATAL=16
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
byte level
string name
string msg
string file
string function
uint32 line
string[] topics

为了使关系网络简单化，对已经分析明白的rosout相关的debug内容做隐藏。之后得到下图：

此时只剩下了四个节点，已经清晰了不少。

观察过后大致看出来fssim是以gazebo进行仿真，用rviz进行可视化的操作。

控制

了解fssim仿真的控制接口，利用规划出的路线进行调用，可以实现fssim自动运行。
仿真环境对*/fssim/cmd*进行响应，它是一个话题，调用方法就是发送命令到这个话题中。
使用如下命令获得该话题的msg信息

rostopic info /fssim/cmd

得到该话题的msg为fssim_common/Cmd
使用如下命令获得的msg的详细信息

rosmsg show fssim_common/Cmd

结果如下：

hqx@hqx-Nitro-AN515-52:~$ rosmsg show fssim_common/Cmd
float64 dc
float64 delta

经过以下文章的测试fssim简单测试
得知dc是长度，单位千米，delta是转角，单位为弧度。
经过实际测试，小车并不是一步到位的，而是以一定的速度经过一段时间到达目的地。问题就是这个过程中车子的速度不清楚，还没有找到可以在哪里进行设置或者在哪里可以查看。毕竟控制接口只有这一个话题：/fssim/cmd
这个问题有待解决。

感知

想要使用仿真环境，感知的内容必不可少。那么怎么获得感知方面的信息呢？

雷达

从fssim用户界面左侧可以看到有lidar的选项，点击查看详细信息可以看见，所有雷达检测的锥桶的位置信息都在话题*/lidar/cones*
使用如下命令获得该话题的msg信息

rostopic info /lidar/cones

结果如下：

hqx@hqx-Nitro-AN515-52:~$ rostopic info /lidar/cones
Type: sensor_msgs/PointCloud2

Publishers: 
 * /gazebo (http://hqx-Nitro-AN515-52:44115/)

Subscribers: 
 * /fssim_rviz (http://hqx-Nitro-AN515-52:42079/)

得知话题的msg为sensor_msgs/PointCloud2，这是常用的点云msg。具体包含内容：

rosmsg show sensor_msgs/PointCloud2

结果如下：

hqx@hqx-Nitro-AN515-52:~$ rosmsg show sensor_msgs/PointCloud2
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height
uint32 width
sensor_msgs/PointField[] fields
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense

摄像头

所有摄像头检测的锥桶的位置信息都在话题/camera/cones
过程与雷达类似，而且topic对应的msg都是 sensor_msgs/PointCloud2
通过以上分析，发现感知部分想要能够工作，必须能够从点云文件sensor_msgs/PointCloud2 中获得锥桶的坐标。

规划

想要做规划，需要将雷达和摄像头的msg进行读取和处理。

仿真效果可视化（rviz界面中的各个选项）

fssim界面左侧还有几个选项，在打开时可以看到有没有他们都没有区别，这应该是以为我们没有调用话题进行设置。下面对rviz界面的几个选项进行分析：

Center Line

hqx@hqx-MacBookAir:~/demo$ rostopic info /control/pure_pursuit/center_line 
Type: geometry_msgs/PolygonStamped

Publishers: None

Subscribers: 
 * /fssim_rviz (http://hqx-MacBookAir:35693/)

这个在之前的rqt_graph也能看出来，这个话题没有人发布，只有rviz进行了订阅，所以如果要使用这个功能，就要写专门的发布者进行发布。功能按照字面意思理解应该是画中心线？
查看相应msg信息

hqx@hqx-MacBookAir:~/demo$ rosmsg show geometry_msgs/PolygonStamped
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Polygon polygon
  geometry_msgs/Point32[] points
    float32 x
    float32 y
    float32 z

Control Marker

与Center Line类似，直接看topic

hqx@hqx-MacBookAir:~/demo$ rostopic info /control/pure_pursuit/marker 
Type: visualization_msgs/MarkerArray

Publishers: None

Subscribers: 
 * /fssim_rviz (http://hqx-MacBookAir:35693/)

同样没有默认的发布者，具体功能不详。看msg

hqx@hqx-MacBookAir:~/demo$ rosmsg show visualization_msgs/MarkerArray
visualization_msgs/Marker[] markers
  uint8 ARROW=0
  uint8 CUBE=1
  uint8 SPHERE=2
  uint8 CYLINDER=3
  uint8 LINE_STRIP=4
  uint8 LINE_LIST=5
  uint8 CUBE_LIST=6
  uint8 SPHERE_LIST=7
  uint8 POINTS=8
  uint8 TEXT_VIEW_FACING=9
  uint8 MESH_RESOURCE=10
  uint8 TRIANGLE_LIST=11
  uint8 ADD=0
  uint8 MODIFY=0
  uint8 DELETE=2
  uint8 DELETEALL=3
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string ns
  int32 id
  int32 type
  int32 action
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  geometry_msgs/Vector3 scale
    float64 x
    float64 y
    float64 z
  std_msgs/ColorRGBA color
    float32 r
    float32 g
    float32 b
    float32 a
  duration lifetime
  bool frame_locked
  geometry_msgs/Point[] points
    float64 x
    float64 y
    float64 z
  std_msgs/ColorRGBA[] colors
    float32 r
    float32 g
    float32 b
    float32 a
  string text
  string mesh_resource
  bool mesh_use_embedded_materials

Map

与上面两个类似，先查话题，再看msg

hqx@hqx-MacBookAir:~/demo$ rostopic info /slam/marker_map 
Type: visualization_msgs/MarkerArray

Publishers: None

Subscribers: 
 * /fssim_rviz (http://hqx-MacBookAir:35397/)

只有接收者没有发布者，msg类型为 visualization_msgs/MarkerArray 和上一个就少的Control Marker是同一种msg。

Boundary Estimation

对应topic为：/boundary_estimation/visual

hqx@hqx-MacBookAir:~/demo$ rostopic info /boundary_estimation/visual
Type: visualization_msgs/MarkerArray

Publishers: None

Subscribers: 
 * /fssim_rviz (http://hqx-MacBookAir:35397/)

msg类型又是 visualization_msgs/MarkerArray

Lidar Observation & Camera Observations

这两个分别对应前面已经讲过的感知方面的内容。

FSSIM Track

hqx@hqx-MacBookAir:~/demo$ rostopic info /fssim/track/markers
Type: visualization_msgs/MarkerArray

Publishers: 
 * /gazebo (http://hqx-MacBookAir:46869/)

Subscribers: 
 - /fssim_rviz (http://hqx-MacBookAir:45147/)

根据实际测试，这一项对应rviz中的锥桶显示。
msg还是 visualization_msgs/MarkerArray

一些可能需要自定义的选项

通过rqt_graph得到的关系图可以轻易的看到有很多话题是只有发布者或者只用接收者。这些话题可能就需要使用者自己调用。这样的话题很多，前面也已经提到了一部分。例如有很多rviz订阅的用于可视化显示的话题没有默认发布者。经过整理，一共有以下这些话题，分为只有发布者和只有接收者两组。

只有发布者

话题名	发布者
/clicked_point	/fssim_rviz

实战演示

建立workspace

mkdir -p ~/demo/src
cd ~/demo/src
catkin_init_workspace
cd ..
catkin_make
source devel/setup.bash
echo "source /WORKSPACE/devel/setup.bash">>~/.bashrc
cd src
catkin_create_pkg fs_cmd roscpp std_msgs
cd ..
catkin_make
source devel/setup.bash

现在就算建立了一个功能包了，现在这个功能包没有代码，所以没有功能。需要在相应的地方编写代码才能编译出功能。使用roboware（专门为ros设计的ide）进行编程。roboware安装教程
使用roboware打开刚才创建的workspace

总结

1.文章很多地方没有进行实际测试，有的功能还不会用。
2.所有topic相关的用法必然是建立在搞懂msg的情况下的，但是文中很多地方msg我看不懂，所以使用也无从谈起。还需要和其他同学讨论具体的msg各个参数的含义。
3.文章持续更新中。