前面已经基本完成了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 <ros/ros.h>
namespace ars_40X {
enum {
POINT,
CAR,
TRUCK,
PEDESTRIAN,
MOTORCYCLE,
BICYCLE,
WIDE,
RESERVED
};
enum {
INVALID,
PERCENT_25,
PERCENT_50,
PERCENT_75,
PERCENT_90,
PERCENT_99,
PERCENT_99_9,
PERCENT_100
};
class ContinentalRadarRViz {
public:
ContinentalRadarRViz();
~ContinentalRadarRViz();
private:
void clusters_callback(ars_40X::ClusterList cluster_list);
void objects_callback(ars_40X::ObjectList object_list);
ros::Publisher clusters_pub_;
ros::Publisher objects_pub_;
ros::Publisher velocity_pub_;
ros::Subscriber clusters_sub_;
ros::Subscriber objects_sub_;
};
}
#endif
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#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);
objects_pub_ = nh.advertise<visualization_msgs::MarkerArray>("visualize_objects", 50);
velocity_pub_ = nh.advertise<visualization_msgs::MarkerArray>("visualize_velocity", 50);
clusters_sub_ =
nh.subscribe("ars_40X/clusters", 50, &ContinentalRadarRViz::clusters_callback, this);
objects_sub_ =
nh.subscribe("ars_40X/objects", 50, &ContinentalRadarRViz::objects_callback, this);
}
ContinentalRadarRViz::~ContinentalRadarRViz() {
}
void ContinentalRadarRViz::clusters_callback(ars_40X::ClusterList cluster_list) {
visualization_msgs::MarkerArray marker_array;
for (auto cluster : cluster_list.clusters) {
visualization_msgs::Marker marker;
marker.type = visualization_msgs::Marker::POINTS;
marker.header.frame_id = cluster_list.header.frame_id;
marker.action = visualization_msgs::Marker::ADD;
marker.header.stamp = cluster_list.header.stamp;
marker.id = cluster.id;
marker.points.push_back(cluster.position.pose.position);
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";
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.color.a = 1.0;
marker.lifetime.fromSec(0.1);
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.type = visualization_msgs::Marker::LINE_STRIP;
marker.header.frame_id = object_list.header.frame_id;
marker.action = visualization_msgs::Marker::ADD;
marker.header.stamp = object_list.header.stamp;
marker.id = object.id;
geometry_msgs::Point pos1, pos2, pos3, pos4;
tf2::Quaternion q;
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)) {
continue;
}
marker.pose = object.position.pose;
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.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";
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.lifetime.fromSec(0.1);
marker_array.markers.push_back(marker);
visualization_msgs::Marker velocity;
velocity.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
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));
velocity.pose = object.position.pose;
velocity.scale.z = 1;
velocity.color.a = 1;
velocity_array.markers.push_back(velocity);
}
objects_pub_.publish(marker_array);
velocity_pub_.publish(velocity_array);
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "ars_40X_rviz");
ars_40X::ContinentalRadarRViz ars_40X_rviz;
ros::spin();
}
<build_depend>message_generation</build_depend> <build_export_depend>message_runtime</build_export_depend>
...
<depend>geometry_msgs</depend>
<depend>visualization_msgs</depend>
...
<exec_depend>message_runtime</exec_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 文件
<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
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)