ARS_408毫米波雷达数据解析学习记录-利用RVIZ实现解析结果的可视化

前面已经基本完成了ARS_408毫米波雷达数据的获取与解析工作。虽然已经将从CAN口获取的数据解析处理成指定的消息类型并进行了发布，但是需要注意的是，它们只是处于文本数据形态，我们仍然无法得到直观的显示结果，如点的分布和目标相对传感器的位置分布等等。因此，这部分将会利用ROS中的 Rviz 三维可视化平台来对解析后的clusters / object数据进行三维可视化显示。

此外，考虑到之前部分是通过运行其它节点来启动的，因此为了操作方便，考虑利用launch文件一次性启动多个节点。

一、RVIZ 部分

• 定义 ContinentalRadarRViz 类

#ifndef ARS_40X_ARS_40X_RVIZ_HPP
#define ARS_40X_ARS_40X_RVIZ_HPP

#include 

namespace ars_40X {
     
enum {
     
  POINT,
  CAR,
  TRUCK,
  PEDESTRIAN,
  MOTORCYCLE,
  BICYCLE,
  WIDE,
  RESERVED
};	//定义object模式中class_type取值的枚举变量
enum {
     
  INVALID,
  PERCENT_25,
  PERCENT_50,
  PERCENT_75,
  PERCENT_90,
  PERCENT_99,
  PERCENT_99_9,
  PERCENT_100
};	//定义clusters/object中prob_of_exist取值的枚举变量
class ContinentalRadarRViz {
     
 public:
  ContinentalRadarRViz();
  ~ContinentalRadarRViz();
 private:
  void clusters_callback(ars_40X::ClusterList cluster_list);	//接收clusters数据的回调函数
  void objects_callback(ars_40X::ObjectList object_list);	//接收object数据的回调函数
  ros::Publisher clusters_pub_;		//创建可视化clusters数据的发布者
  ros::Publisher objects_pub_;		//创建可视化object数据的发布者
  ros::Publisher velocity_pub_;		//创建velocity数据的发布者
  ros::Subscriber clusters_sub_;	//创建clusters解析数据的订阅者
  ros::Subscriber objects_sub_;		//创建object解析数据的订阅者
};
}
#endif //ARS_40X_ARS_40X_RVIZ_HPP

• ContinentalRadarRViz类的实现

#include 
#include 
#include 

#include "ars_40X/ClusterList.h"
#include "ars_40X/ObjectList.h"
#include "ars_40X/ros/ars_40X_rviz.hpp"

namespace ars_40X {
     
ContinentalRadarRViz::ContinentalRadarRViz() {
     
  ros::NodeHandle nh;
  clusters_pub_ = nh.advertise<visualization_msgs::MarkerArray>("visualize_clusters", 50);	//发布名为visualize_clusters的topic，消息类型为visualization_msgs::MarkerArray，发布序列的大小为50。
  objects_pub_ = nh.advertise<visualization_msgs::MarkerArray>("visualize_objects", 50);	//发布名为visualize_objects的topic，消息类型为visualization_msgs::MarkerArray，发布序列的大小为50。
  velocity_pub_ = nh.advertise<visualization_msgs::MarkerArray>("visualize_velocity", 50);	//发布名为visualize_velocity的topic，消息类型为visualization_msgs::MarkerArray，发布序列的大小为50。
  clusters_sub_ =
      nh.subscribe("ars_40X/clusters", 50, &ContinentalRadarRViz::clusters_callback, this);	//订阅ars_40X/clusters话题，并将获取的数据交由回调函数clusters_callback处理，消息队列上限为50。
  objects_sub_ =
      nh.subscribe("ars_40X/objects", 50, &ContinentalRadarRViz::objects_callback, this);	//订阅ars_40X/objects话题，并将获取的数据交由回调函数objects_callback处理，消息队列上限为50。
}
ContinentalRadarRViz::~ContinentalRadarRViz() {
     
}
void ContinentalRadarRViz::clusters_callback(ars_40X::ClusterList cluster_list) {
     
  visualization_msgs::MarkerArray marker_array;			//创建要发布的消息队列对象
  for (auto cluster : cluster_list.clusters) {
     		//遍历从话题中获取的cluster数据
    visualization_msgs::Marker marker;	//创建单个的Marker对象
    marker.type = visualization_msgs::Marker::POINTS;	//指定Marker的类型为POINTS类型
    marker.header.frame_id = cluster_list.header.frame_id;	
    marker.action = visualization_msgs::Marker::ADD;	//action域指定marker的行为，包括ADD(增加)和DELETE(删除)操作
    marker.header.stamp = cluster_list.header.stamp;
    marker.id = cluster.id;		//为这个marker设置一个唯一的ID，一个marker接收到相同ns和id就会用新的信息代替旧的
    marker.points.push_back(cluster.position.pose.position);	//将点坐标加入到marker内的点队列内
    switch (cluster.prob_of_exist) {
     
      case INVALID: {
     
        marker.color.r = 0.0f;
        marker.color.g = 0.0f;
        marker.color.b = 0.0f;	//黑色
        marker.ns = "Invalid";	//命名空间（ns）和id是用来给这个marker创建一个唯一的名字的
        break;
      }
      case PERCENT_25: {
     
        marker.color.r = 0.0f;
        marker.color.g = 0.0f;
        marker.color.b = 1.0f;	//蓝色
        marker.ns = "25%";
        break;
      }
      case PERCENT_50: {
     
        marker.color.r = 0.0f;
        marker.color.g = 1.0f;
        marker.color.b = 0.0f;	//绿色
        marker.ns = "50%";
        break;
      }
      case PERCENT_75: {
     
        marker.color.r = 0.0f;
        marker.color.g = 1.0f;
        marker.color.b = 1.0f;	//青色
        marker.ns = "75%";
        break;
      }
      case PERCENT_90: {
     
        marker.color.r = 1.0f;
        marker.color.g = 0.0f;
        marker.color.b = 0.0f;	//红色
        marker.ns = "90%";
        break;
      }
      case PERCENT_99: {
     
        marker.color.r = 1.0f;
        marker.color.g = 0.0f;
        marker.color.b = 1.0f;	//亮紫色（洋红色）
        marker.ns = "99%";
        break;
      }
      case PERCENT_99_9: {
     
        marker.color.r = 1.0f;
        marker.color.g = 1.0f;
        marker.color.b = 0.0f;	//黄色
        marker.ns = "99.9%";
        break;
      }
      case PERCENT_100: {
     
        marker.color.r = 1.0f;
        marker.color.g = 1.0f;
        marker.color.b = 1.0f;	//白色
        marker.ns = "100%";
        break;
      }
    }
    marker.scale.x = 0.1;
    marker.scale.y = 0.1;	//指定了Marker的规模，对于基本形状，1表示在这边长度是1米。
    marker.color.a = 1.0;	//alpha值为0意味着完全透明, 1意味着完全不透明
    marker.lifetime.fromSec(0.1);	//lifetime域指定marker在被自动删除之前会持续多久
    marker_array.markers.push_back(marker);
  }
  clusters_pub_.publish(marker_array);
}
void ContinentalRadarRViz::objects_callback(ars_40X::ObjectList object_list) {
     
  visualization_msgs::MarkerArray marker_array, velocity_array;	//创建要发布的消息队列对象
  for (auto object : object_list.objects) {
     
    visualization_msgs::Marker marker;	//创建单个的Marker对象
    marker.type = visualization_msgs::Marker::LINE_STRIP;	//指定Marker的类型是线。LINE_STRIP类型将每个点用作一组连接的线中的顶点,其中点0连接到点1, 点1连接到点2,点2连接到点3等
    marker.header.frame_id = object_list.header.frame_id;
    marker.action = visualization_msgs::Marker::ADD;	//指定我们要对marker做什么的, 有ADD和DELETE两个选项, ADD是创建或者修改
    marker.header.stamp = object_list.header.stamp;
    marker.id = object.id;	//为这个marker设置一个唯一的ID，一个marker接收到相同ns和id就会用新的信息代替旧的
    geometry_msgs::Point pos1, pos2, pos3, pos4;	//创建Point对象
    tf2::Quaternion q;	//创建tf2类型的四元数
    q.setValue(
        object.position.pose.orientation.x,
        object.position.pose.orientation.y,
        object.position.pose.orientation.z,
        object.position.pose.orientation.w);	//设置四元数的值
    tf2::Matrix3x3 m(q);	//四元数转换为旋转矩阵
    double roll, pitch, yaw;	//定义欧拉角
    m.getRPY(roll, pitch, yaw);		//从旋转矩阵中获取欧拉角的值
    if (isnan(yaw)) {
     	//如果给定的值能被转换为数字，则返回false；如果不能转换为数字，则返回true
      continue;
    }
    marker.pose = object.position.pose;		//给marker的位姿赋值，（在线中的点通过位姿变换）
    pos1.x = object.length / 2;
    pos1.y = object.width / 2;
    pos2.x = object.length / 2;
    pos2.y = -object.width / 2;
    pos3.x = -object.length / 2;
    pos3.y = -object.width / 2;
    pos4.x = -object.length / 2;
    pos4.y = object.width / 2;		//获取矩形四个角点坐标
    marker.points.push_back(pos1);
    marker.points.push_back(pos2);
    marker.points.push_back(pos3);
    marker.points.push_back(pos4);
    marker.points.push_back(pos1);	//将角点坐标存入到marker中
    marker.scale.x = 0.1;	//设置线段宽度

    switch (object.class_type) {
     
      case POINT: {
     				//点
        marker.color.r = 0.0f;
        marker.color.g = 0.0f;
        marker.color.b = 0.0f;	//黑色
        break;
      }
      case CAR: {
     				//车
        marker.color.r = 0.0f;
        marker.color.g = 0.0f;
        marker.color.b = 1.0f;	//蓝色
        break;
      }
      case TRUCK: {
     				//卡车
        marker.color.r = 0.0f;
        marker.color.g = 1.0f;
        marker.color.b = 0.0f;	//绿色
        break;
      }
      case PEDESTRIAN: {
     		//行人
        marker.color.r = 0.0f;
        marker.color.g = 1.0f;
        marker.color.b = 1.0f;	//青色
        break;
      }
      case MOTORCYCLE: {
     		//摩托车
        marker.color.r = 1.0f;
        marker.color.g = 0.0f;
        marker.color.b = 0.0f;	//红色
        break;
      }
      case BICYCLE: {
     			//自行车
        marker.color.r = 1.0f;
        marker.color.g = 0.0f;
        marker.color.b = 1.0f;	//亮紫色（洋红色）
        break;
      }
      case WIDE: {
     				//其它类型
        marker.color.r = 1.0f;
        marker.color.g = 1.0f;
        marker.color.b = 0.0f;	//黄色
        break;
      }
      case RESERVED: {
     			//保留
        marker.color.r = 1.0f;
        marker.color.g = 1.0f;
        marker.color.b = 1.0f;	//白色
        break;
      }
    }
    switch (object.prob_of_exist) {
     
      case INVALID: {
     
        marker.ns = "Invalid";	//根据目标的prob_of_exist为maker指定命名空间(ns)
        break;
      }
      case PERCENT_25: {
     
        marker.ns = "25%";
        break;
      }
      case PERCENT_50: {
     
        marker.ns = "50%";
        break;
      }
      case PERCENT_75: {
     
        marker.ns = "75%";
        break;
      }
      case PERCENT_90: {
     
        marker.ns = "90%";
        break;
      }
      case PERCENT_99: {
     
        marker.ns = "99%";
        break;
      }
      case PERCENT_99_9: {
     
        marker.ns = "99.9%";
        break;
      }
      case PERCENT_100: {
     
        marker.ns = "100%";
        break;
      }
    }
    marker.color.a = 1.0;	//设置marker为完全不透明
    marker.lifetime.fromSec(0.1);	//lifetime域指定marker在被自动删除之前会坚持多久
    marker_array.markers.push_back(marker);		//将初始话后的marker加入到marker_array中

    visualization_msgs::Marker velocity;	//创建用来存放velocity的Marker对象
    velocity.type = visualization_msgs::Marker::TEXT_VIEW_FACING;	//设置Marker类型为有方向的3D的文字
    velocity.header.frame_id = object_list.header.frame_id;
    velocity.action = visualization_msgs::Marker::ADD;
    velocity.header.stamp = object_list.header.stamp;
    velocity.id = object.id;
    velocity.text = std::to_string(hypot(object.relative_velocity.twist.linear.x,
                                         object.relative_velocity.twist.linear.y));	//hypot() 返回欧几里德范数 sqrt(x*x + y*y), 这里是计算目标速度并将其赋值给velocity的text部分
    velocity.pose = object.position.pose;	//将object位姿赋值给velocity，即指定文字的位置和方向
    velocity.scale.z = 1;	//TEXT_VIEW_FACING类型仅使用scale.z用于指定文本字体的高度
    velocity.color.a = 1;	//内容设置为完全不透明

    velocity_array.markers.push_back(velocity);
  }
  objects_pub_.publish(marker_array);	
  velocity_pub_.publish(velocity_array);	//发布object位置及其速度信息
}
}

int main(int argc, char **argv) {
     
  ros::init(argc, argv, "ars_40X_rviz");	//ros初始化，节点名称为ars_40X_rviz
  ars_40X::ContinentalRadarRViz ars_40X_rviz;	//创建ContinentalRadarRViz类对象
  ros::spin();
}

• 修改 package.xml 和 CMakeLists.txt 文件

  ①package.xml文件(增加部分)

<build_depend>message_generationbuild_depend> <build_export_depend>message_runtimebuild_export_depend>
...

<depend>geometry_msgsdepend>
<depend>visualization_msgsdepend>
...
<exec_depend>message_runtimeexec_depend>

​ ②CMakeLists.txt文件(增加部分)

find_package(geometry_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)

add_executable(ars_40X_rviz
    src/ros/ars_40X_rviz.cpp
    )
add_dependencies(ars_40X_rviz ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(ars_40X_rviz ${catkin_LIBRARIES})

install(
    TARGETS
	...
    ars_40X_rviz
	...
    ARCHIVE DESTINATION lib/${PROJECT_NAME}
    LIBRARY DESTINATION lib/${PROJECT_NAME}
    RUNTIME DESTINATION lib/${PROJECT_NAME}
)

二、添加 launch 文件

• 考虑到直接无参数启动rviz时使用的是默认配置文件～/.rviz/default.rviz，为方便节点启动时直接使用已经设置好的rviz配置，可以将之前正常运行时的rviz配置保存为配置文件，并在rviz启动时作为参数传入。

• 必备的节点属性：

  • pkg： 表示该节点的package，相当于 rosrun 命令后面的第一个参数；
  • type： 可执行文件的名字，rosrun 命令的第二个参数；是否可以理解为要执行的节点
  • name： 该节点的名字，相当于代码中 ros::init() 中设置的信息，有了它代码中的名称会被覆盖（因此一般会与代码中的命名保持一致）。

• 其它属性

  • output： 将标准输出显示在屏幕上而不是记录在日志中；

  • 包含其他文件include file=“path to launch file”： 在启动文件中包含其他启动文件的内容（包括所有的节点和参数），可使用如下命令使路径更为简单include file="($find package-name)/launch-file-name"

  • 启动参数（launch arguments）： 为了使启动文件便于配置，roslaunch 还支持启动参数,有时也简称为参数甚至 args，其功能有点像可执行程序中的局部变量。 声明参数：

    获取参数：一旦参数值被声明并且被赋值,可以用下面的 arg 替换(arg substitution)语法来使用该参数值了：$(arg arg-name) 每个该替换出现的地方，roslaunch 都将它替换成参数值。

<launch>
  <arg name="visualize" default="true"/>
  <arg name="obstacle_array" default="false"/>		

  <node pkg="ars_40X" type="ars_40X_ros" name="ars_40X_ros" output="screen">
  node>			

  <group if="$(arg visualize)">		
    <node pkg="ars_40X" type="ars_40X_rviz" name="ars_40X_rviz"/>
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find ars_40X)/rviz_cfg/ars_40X.rviz"/>	
  group>

  <group if="$(arg obstacle_array)">	
    <node pkg="ars_40X" type="ars_40X_obstacle_array" name="ars_40X_obstacle_array">
    node>
  group>
launch>

• 使用方法

roslaunch package-name launch-file-name arg-name:=arg-value