Ubuntu 22.04 ROS2使用教程03-大陆548雷达ROS驱动包开发（开源）

前面已经熟悉了ros2基本的思想和开发流程，下面我结合实际项目需求进行ARS548毫米波雷达的协议适配，并转化成rviz能识别的检测点云目标及跟踪目标实时显示。ARS 548 RDI是一种77 GHz高性能优质长距离雷达传感器，带有数字波束的扫描天线，可与其他技术相结合实现高度自动驾驶。雷达通过SOMEIP协议进行协议交互或周期输出检测和跟踪目标。SOMEIP是在UDP之上实现的协议，本文先跳过SOMEIP直接通过UDP解析。旨在实战闭环验证ros2技术栈，记录备忘。文末附开源代码地址。

新建工作空间 ros_ws

$ mkdir -p ~/ros_ws/src

ars548_interface接口

接口文件创建

创建ars548_interface功能包

ROS2通过自带rosidl_default_generators包来为自定义的msg、srv和action文件生成各个语言的头文件！

ROS2通过在CMakeLists.txt中调用新增的宏rosidl_generate_interfaces来为msg、srv和action文件生成各个语言的头文件！而这个宏的实现定义在rosidl_default_generators包。在命名时踩了文件名和变量名规范的坑下面是具体要求。

文件名命名规范

文件名xxx.msg，必须以大写字母开头，并且不能有下划线

变量名命名规范

只能是小写+下划线，不能出现大写

It should have the pattern '^(?!.*__)(?!.*_$)[a-z][a-z0-9_]*$'

否则会报错：

NameError: 'ID_number' is an invalid field name. It should have the

pattern '^(?!.*__)(?!.*_$)[a-z][a-z0-9_]*$'

ROS2的基本原始数据类型

ROS2目前支持的内置类型：

每种内置类型都可以用来定义数组：

定义Num.msg

仅用作验证自定义msg流程用

int64 num

定义检测目标列表 DetectionList.msg

uint16 service_id
uint16 method_id
uint32 data_length
uint16 client_id
uint16 session_id
uint8 protocol_version
uint8 interface_version
uint8 message_type
uint8 return_code
uint64 crc
uint32 length
uint32 sqc
uint32 data_id
uint32 timestamp_nanoseconds
uint32 timestamp_seconds
uint8 timestamp_sync_status
uint32 event_data_qualifier
uint8 extended_qualifier
uint16 origin_invalid_flags
float32 origin_xpos
float32 origin_xstd
float32 origin_ypos
float32 origin_ystd
float32 origin_zpos
float32 origin_zstd
float32 origin_roll
float32 origin_rollstd
float32 origin_pitch
float32 origin_pitchstd
float32 origin_yaw
float32 origin_yawstd
uint8 list_invalid_flags
Detections[] detection_array
float32 list_rad_vel_domain_min
float32 list_rad_vel_domain_max
uint32 list_num_of_detections
float32 aln_azimuth_correction
float32 aln_elevation_correction
uint8 aln_status

定义检测目标Detection.msg

std_msgs/Header header
  
float32 f_x
float32 f_y
float32 f_z
uint8 u_invalid_flags
float32 f_range_rate
float32 f_range_rate_std
int8 s_rcs
uint16 u_measurement_id
uint8 u_positive_predictive_value
uint8 u_classification
uint8 u_multi_target_probability
uint16 u_object_id
uint8 u_ambiguity_flag
uint16 u_sort_index

修改后CMakeList.txt

添加了rosidl_default_generators std_msgs依赖和对应的msg

cmake_minimum_required(VERSION 3.8)
project(ars548_interface)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)

# set(msg_files
#   "msg/Objects.msg"
#   "msg/ObjectList.msg"
#   "msg/Detections.msg"
#   "msg/DetectionList.msg"
#   "msg/RadarBasicStatus.msg"
#   "msg/AccelerationLateralCog.msg"
#   "msg/AccelerationLongitudinalCog.msg"
#   "msg/CharacteristicSpeed.msg"
#   "msg/DrivingDirection.msg"
#   "msg/SteeringAngleFrontAxle.msg"
#   "msg/VelocityVehicle.msg"
#   "msg/YawRate.msg"
# )

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Num.msg"
  "msg/Objects.msg"
  "msg/ObjectList.msg"
  "msg/Detections.msg"
  "msg/DetectionList.msg"
  "msg/RadarBasicStatus.msg"
  "msg/AccelerationLateralCog.msg"
  "msg/AccelerationLongitudinalCog.msg"
  "msg/CharacteristicSpeed.msg"
  "msg/DrivingDirection.msg"
  "msg/SteeringAngleFrontAxle.msg"
  "msg/VelocityVehicle.msg"
  "msg/YawRate.msg"
  # "msg/ObjectPosition.msg"
  # "srv/AddTwoInts.srv"
  # "srv/GetObjectPosition.srv"
  # "action/MoveCircle.action"
  DEPENDENCIES std_msgs
 )
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

修改package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>ars548_interface</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="neal@todo.todo">neal</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <depend>std_msgs</depend>
  
  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

构建（生成头文件）

colcon build

编译完成生成目录,生成目录有其它msg生成的内容，忽略即可

接口文件验证

以使用Num.msg为例，使用pub/sub节点测试

Publisher代码

#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "ars548_interface/msg/num.hpp"     // CHANGE

using namespace std::chrono_literals;

class MinimalPublisher : public rclcpp::Node
{
public:
  MinimalPublisher()
  : Node("minimal_publisher"), count_(0)
  {
    publisher_ = this->create_publisher<ars548_interface::msg::Num>("topic", 10);    // CHANGE
    timer_ = this->create_wall_timer(
      500ms, std::bind(&MinimalPublisher::timer_callback, this));
  }

private:
  void timer_callback()
  {
    auto message = ars548_interface::msg::Num();                               // CHANGE
    message.num = this->count_++;                                        // CHANGE
    RCLCPP_INFO(this->get_logger(), "Publishing: '%d'", message.num);    // CHANGE
    publisher_->publish(message);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<ars548_interface::msg::Num>::SharedPtr publisher_;         // CHANGE
  size_t count_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}

Subscriber代码

#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "ars548_interface/msg/num.hpp"     // CHANGE
using std::placeholders::_1;

class MinimalSubscriber : public rclcpp::Node
{
public:
  MinimalSubscriber()
  : Node("minimal_subscriber")
  {
    subscription_ = this->create_subscription<ars548_interface::msg::Num>(          // CHANGE
      "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
  }

private:
  void topic_callback(const ars548_interface::msg::Num::SharedPtr msg) const       // CHANGE
  {
    RCLCPP_INFO(this->get_logger(), "I heard: '%d'", msg->num);              // CHANGE
  }
  rclcpp::Subscription<ars548_interface::msg::Num>::SharedPtr subscription_;       // CHANGE
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalSubscriber>());
  rclcpp::shutdown();
  return 0;
}

CMakeList.txt修改

Package.xml添加depend

编译

colcon build

运行

打开新终端，source 环境，并运行:

ros2 run ars548_process talkerros2 run ars548_process listener

ars548_process功能包

创建功能包

$ cd ~/ros_ws/src
$ ros2 pkg create --build-type ament_cmake ars548_process

添加测试节点

CMakeList.txt如下

cmake_minimum_required(VERSION 3.8)
project(ars548_process)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(rclcpp REQUIRED)
add_executable(node_helloworld_class src/node_helloworld_class.cpp)
ament_target_dependencies(node_helloworld_class rclcpp)

install(TARGETS
  node_helloworld_class
  DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()
#node_helloworld_class.cpp
#include <unistd.h>
#include "rclcpp/rclcpp.hpp"


/***
创建一个HelloWorld节点, 初始化时输出“hello world”日志
***/
class HelloWorldNode : public rclcpp::Node
{
    public:
        HelloWorldNode()
        : Node("node_helloworld_class")                          // ROS2节点父类初始化
        {
            while(rclcpp::ok())                                  // ROS2系统是否正常运行
            {
                RCLCPP_INFO(this->get_logger(), "Hello World");  // ROS2日志输出
                sleep(1);                                        // 休眠控制循环时间
            }
        }
};

// ROS2节点主入口main函数
int main(int argc, char * argv[])                               
{

    // ROS2 C++接口初始化
    rclcpp::init(argc, argv);        
    
    // 创建ROS2节点对象并进行初始化                 
    rclcpp::spin(std::make_shared<HelloWorldNode>()); 
    
    // 关闭ROS2 C++接口
    rclcpp::shutdown();                               
    
    return 0;
}
source install/local_setup.sh # 仅在当前终端生效
echo " source ~/dev_ws/install/local_setup.sh" >> ~/.bashrc # 所有终端均生效

添加ars548_process_node节点

关键源码

#include "ars548_interface/msg/objects.hpp"
#include <ars548_interface/msg/object_list.hpp>
#include <ars548_interface/msg/detections.hpp>
#include <ars548_interface/msg/detection_list.hpp>
#include <ars548_interface/msg/radar_basic_status.hpp>
#include <ars548_interface/msg/acceleration_lateral_cog.hpp>
#include <ars548_interface/msg/acceleration_longitudinal_cog.hpp>
#include <ars548_interface/msg/characteristic_speed.hpp>
#include <ars548_interface/msg/driving_direction.hpp>
#include <ars548_interface/msg/steering_angle_front_axle.hpp>
#include <ars548_interface/msg/velocity_vehicle.hpp>
#include <ars548_interface/msg/yaw_rate.hpp>
#include "rclcpp/rclcpp.hpp"
#include <thread>

#include "udp_interface.h"
#include "data_struct.h"
#include "data_process.h"
#include "converttype.h"

UdpInterface udp_io;
DataProcess data_process;
ConvertType cvt;

char rec_data[40000];
char send_data[1024];
int rec_length;
struct sockaddr_in source_address;

// RadarObjectList object_list;
RadarDetectionList detection_list;
// RadarBasicStatus basic_status;


// ros::Publisher objects_pub;
rclcpp::Publisher<ars548_interface::msg::DetectionList>::SharedPtr detections_pub ;
rclcpp::Logger global_logger = rclcpp::get_logger("global_logger");
void publishDetectionList(RadarDetectionList list)
{
    ars548_interface::msg::Detections det;
    ars548_interface::msg::DetectionList det_list;
    int i;
    det_list.detection_array.clear();

    det_list.service_id = list.serviceID;
    det_list.method_id = list.MethodID;
    det_list.data_length = list.data_length;
    det_list.client_id = list.clientID;
    det_list.session_id = list.sessionID;
    det_list.protocol_version = list.protocol_version;
    det_list.interface_version = list.interface_version;
    det_list.message_type = list.message_type;
    det_list.return_code = list.return_code;
    det_list.crc = list.CRC;
    det_list.length = list.Length;
    det_list.sqc = list.SQC;
    det_list.data_id = list.DataID;
    det_list.timestamp_nanoseconds = list.Timestamp_Nanoseconds;
    det_list.timestamp_seconds = list.Timestamp_Seconds;
    det_list.timestamp_sync_status = list.Timestamp_SyncStatus;
    det_list.event_data_qualifier = list.EventDataQualifier;
    det_list.extended_qualifier = list.ExtendedQualifier;
    det_list.origin_invalid_flags = list.Origin_InvalidFlags;
    det_list.origin_xpos = list.Origin_Xpos;
    det_list.origin_xstd = list.Origin_Xstd;
    det_list.origin_ypos = list.Origin_Ypos;
    det_list.origin_ystd = list.Origin_Ystd;
    det_list.origin_zpos = list.Origin_Zpos;
    det_list.origin_zstd = list.Origin_Zstd;
    det_list.origin_roll = list.Origin_Roll;
    det_list.origin_rollstd = list.Origin_Rollstd;
    det_list.origin_pitch = list.Origin_Pitch;
    det_list.origin_pitchstd = list.Origin_Pitchstd;
    det_list.origin_yaw = list.Origin_Yaw;
    det_list.origin_yawstd = list.Origin_Yawstd;
    det_list.list_invalid_flags = list.List_InvalidFlags;
    det_list.list_rad_vel_domain_min = list.List_RadVelDomain_Min;
    det_list.list_rad_vel_domain_max = list.List_RadVelDomain_Max;
    det_list.list_num_of_detections = list.List_NumOfDetections;
    det_list.aln_azimuth_correction = list.Aln_AzimuthCorrection;
    det_list.aln_elevation_correction = list.Aln_ElevationCorrection;
    det_list.aln_status = list.Aln_Status;

    for(i=0;i<list.List_NumOfDetections;i++)
    {
        det.header.frame_id = "/world";
        det.header.stamp = rclcpp::Clock().now();

        det.f_x = list.detection_array[i].f_x;
        det.f_y = list.detection_array[i].f_y;
        det.f_z = list.detection_array[i].f_z;
        det.u_invalid_flags = list.detection_array[i].u_InvalidFlags;
        det.f_range_rate = list.detection_array[i].f_RangeRate;
        det.f_range_rate_std = list.detection_array[i].f_RangeRateSTD;
        det.s_rcs = list.detection_array[i].s_RCS;
        det.u_measurement_id = list.detection_array[i].u_MeasurementID;
        det.u_positive_predictive_value = list.detection_array[i].u_PositivePredictiveValue;
        det.u_classification = list.detection_array[i].u_Classification;
        det.u_multi_target_probability = list.detection_array[i].u_MultiTargetProbability;
        det.u_object_id = list.detection_array[i].u_ObjectID;
        det.u_ambiguity_flag = list.detection_array[i].u_AmbiguityFlag;
        det.u_sort_index = list.detection_array[i].u_SortIndex;

        det_list.detection_array.push_back(det);
    }

    detections_pub->publish(det_list);  
    RCLCPP_INFO(global_logger, "Published: '%d'", det_list.timestamp_seconds);  // 打印日志  
}

void ProcessRadarData(char *data, int len)
{
    switch(len)
    {
        // case 9401:
        //     if(data_process.processObjectListMessage(data,&object_list))
        //     {
        //         if(object_list.ObjectList_NumOfObjects>0)
        //             publishObjectList(object_list);    
        //     }
        // break;

        case 35336:
            if(data_process.processDetectionListMessage(data,&detection_list))
            {
                if(detection_list.List_NumOfDetections)
                    publishDetectionList(detection_list);
            }
        break;

        // case 84:
        //     if(data_process.processBasicStatusMessage(data,&basic_status))
        //     {
        //         publishBasicStatus(basic_status);
        //     }
        // break;

        default:
        break;
    }
}

//thread for receive radar data
void receiveThread()
{
    cout<< "receiveThread()"<<endl;
    // while(ros::ok())
    while(rclcpp::ok())
    {
        cout<< "rclcpp::ok()"<<endl;
        udp_io.receiveFromRadar((struct sockaddr *)&source_address,rec_data,rec_length);
        //打印雷达地址，用于连接多个雷达时区分哪一个雷达
        cout<< source_address.sin_addr.s_addr<<endl;
        if(rec_length>0)
        {
            ProcessRadarData(rec_data,rec_length);    
        }
    }

}


int main(int argc, char **argv)
{
    //ros::init(argc, argv, "ars548_process_node");
    // ros::NodeHandle nh;
    // ros::NodeHandle private_nh("~");
    // ROS2 C++接口初始化
    rclcpp::init(argc, argv);
    // 创建ROS2节点对象并进行初始化                                                                   
    std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("ars548_process_node");  
    detections_pub = node->create_publisher<ars548_interface::msg::DetectionList>("/ars548_process/detection_list", 10); 
    

    int ret=udp_io.initUdpMulticastServer("224.0.2.2",42102);
    if(ret<0)
    {
        cout<<"initUdpMulticastServer error!"<<endl;
        return 0;
    }
    cout<<"initUdpMulticastServer ok!"<<ret<<endl;
    // udp_io.initUdpUnicastClient("10.13.1.113",42101,42401);

    std::thread rec_thread = std::thread(receiveThread);  

    // ros::spin();
    // 循环等待ROS2退出
    rclcpp::spin(node);    
    // 关闭ROS2 C++接口                                                                     
    rclcpp::shutdown();  

    return 0;
}

CMakeList.txt如下

cmake_minimum_required(VERSION 3.8)
project(ars548_process)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(ars548_interface REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
#测试节点
add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp ars548_interface)         # CHANGE
#测试节点
add_executable(listener src/subscriber_member_function.cpp)
ament_target_dependencies(listener rclcpp ars548_interface)     # CHANGE
#ars548_process_node节点，接收udp数据，解析数据，发布ros消息
add_executable(ars548_process_node src/ars548_process_node.cpp src/udp_interface.cpp src/data_process.cpp src/converttype.cpp)
ament_target_dependencies(ars548_process_node rclcpp std_msgs ars548_interface sensor_msgs geometry_msgs) 
#测试节点
add_executable(node_helloworld_class src/node_helloworld_class.cpp)
ament_target_dependencies(node_helloworld_class rclcpp)

install(TARGETS
  talker
  listener
  node_helloworld_class
  ars548_process_node
  DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

package.xml修改添加内容如下

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>ars548_process</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="neal@todo.todo">neal</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>


  <depend>ars548_interface</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

编译运行

修改完成colcon 编译运行即可，ros2和ros编程接口差别还是比较大的。代码使用了udp组播功能，代码编译正常可正常启动，但是未接收到组播数据。修改配置方法见文末。

添加info_convert_node节点

info_convert_node节点用来将object和detection列表的数据转换为rviz可以显示的数据格式。

这个节点接收ars548_process_node节点发布/ars548_process/object_list话题/ars548_process/detection_list话题

经过格式转换之后，发布成rviz可以展示的格式。发布的话题包括：

/ars548_process/object_marker

用来显示object的话题,object先暂时屏蔽，快速验证点云

/ars548_process/detection_point_cloud

用来显示detection的话题

部分关键代码及配置

CMakeList.txt

cmake_minimum_required(VERSION 3.8)
project(ars548_process)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(ars548_interface REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(PCL REQUIRED) 
find_package(pcl_conversions REQUIRED)  
#测试节点
add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp ars548_interface)         # CHANGE
#测试节点
add_executable(listener src/subscriber_member_function.cpp)
ament_target_dependencies(listener rclcpp ars548_interface)     # CHANGE
#ars548_process_node节点，接收udp数据，解析数据，发布ros消息
add_executable(ars548_process_node src/ars548_process_node.cpp src/udp_interface.cpp src/data_process.cpp src/converttype.cpp)
ament_target_dependencies(ars548_process_node rclcpp std_msgs ars548_interface sensor_msgs geometry_msgs) 
#info_convert_node节点，接收ros消息,处理成rviz可用消息发布
add_executable(info_convert_node src/info_convert_node.cpp)
ament_target_dependencies(info_convert_node rclcpp std_msgs ars548_interface sensor_msgs geometry_msgs PCL pcl_conversions) 
#测试节点
add_executable(node_helloworld_class src/node_helloworld_class.cpp)
ament_target_dependencies(node_helloworld_class rclcpp)
#pcl 测试节点
add_executable(pcl_test src/pcl_test.cpp)
ament_target_dependencies(pcl_test rclcpp std_msgs ars548_interface sensor_msgs geometry_msgs PCL pcl_conversions)
#install 必须加，否则source后ros2 run无法识别指令
install(TARGETS
  talker
  listener
  node_helloworld_class
  ars548_process_node
  info_convert_node
  pcl_test
  DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

info_convert_node.cpp

#include "rclcpp/rclcpp.hpp"

#include <ars548_interface/msg/objects.hpp>
#include <ars548_interface/msg/object_list.hpp>
#include <ars548_interface/msg/detections.hpp>
#include <ars548_interface/msg/detection_list.hpp>
// #include <visualization_msgs/Marker.h>
// #include <visualization_msgs/MarkerArray.h>
// #include <sensor_msgs/PointCloud.h>
// #include <geometry_msgs/Point32.h>
// #include <sensor_msgs/ChannelFloat32.h>
#include "sensor_msgs/msg/point_cloud2.hpp"  
#include "std_msgs/msg/header.hpp"  
#include <pcl/conversions.h>  // 确保包含此头文件  
#include <pcl/point_cloud.h>  
#include <pcl/point_types.h>  
#include <pcl_conversions/pcl_conversions.h>  
// ros::Publisher objects_marker_pub;
// ros::Publisher detections_cloud_pub;
rclcpp::Subscription<ars548_interface::msg::DetectionList>::SharedPtr det_list_sub ;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr detections_cloud_pub ;
rclcpp::Logger global_logger = rclcpp::get_logger("global_logger");
// void objectReceive(const ars548_msg::ObjectList& msg)
// {
//     uint size = msg.object_array.size();
//     visualization_msgs::Marker my_marker; 
//     visualization_msgs::MarkerArray marker_array;
//     marker_array.markers.clear();

//     if(size>0)
//     {
//         for(uint i=0; i<size; i++)
//         {
//             my_marker.header.frame_id = "/world";
//             my_marker.header.stamp = msg.object_array[i].header.stamp;
//             my_marker.ns = "object_shapes";
            
//             my_marker.id = msg.object_array[i].u_ID; 
//             my_marker.type = visualization_msgs::Marker::CUBE;
//             my_marker.action = visualization_msgs::Marker::ADD; 
//             my_marker.pose.position.x = msg.object_array[i].u_Position_X;
//             my_marker.pose.position.y = msg.object_array[i].u_Position_Y;
//             my_marker.pose.position.z = msg.object_array[i].u_Position_Z;

//             my_marker.pose.orientation.x = 0.0;
//             my_marker.pose.orientation.y = 0.0;
//             my_marker.pose.orientation.z = sin(msg.object_array[i].u_Position_Orientation/2);
//             my_marker.pose.orientation.w = cos(msg.object_array[i].u_Position_Orientation/2);

//             if((msg.object_array[i].u_Shape_Length_Edge_Mean>0.2)||(msg.object_array[i].u_Shape_Width_Edge_Mean>0.2))
//             {
//                 my_marker.scale.x = msg.object_array[i].u_Shape_Length_Edge_Mean;
//                 my_marker.scale.y = msg.object_array[i].u_Shape_Width_Edge_Mean;
//                 my_marker.scale.z = (msg.object_array[i].u_Shape_Length_Edge_Mean+msg.object_array[i].u_Shape_Width_Edge_Mean)/2;
//             }
//             else
//             {
//                 my_marker.scale.x = 0.2;
//                 my_marker.scale.y = 0.2;
//                 my_marker.scale.z = 0.2;
//             }

//             my_marker.color.r = 0.0f;
//             my_marker.color.g = 1.0f;
//             my_marker.color.b = 0.0f;
//             my_marker.color.a = 1.0;

//             my_marker.lifetime = ros::Duration(0.5);

//             marker_array.markers.push_back(my_marker);
//         }

//         objects_marker_pub.publish(marker_array);
//     }
// }

void detectionReceive(const ars548_interface::msg::DetectionList& msg)
{
    uint size = msg.detection_array.size();
    // 创建点云数据，可以根据需要自定义点云  
    pcl::PointCloud<pcl::PointXYZ> cloud;  
    cloud.width = 5;  
    cloud.height = 1;  
    cloud.is_dense = false;  
    cloud.points.resize(cloud.width * cloud.height);
    if(size>0)
    {
        cloud.header.frame_id = "/world";
        // cloud.header.stamp = msg.detection_array[0].header.stamp;
        cloud.points.clear();
        for(uint i=0;i<size;i++) 
        {
            cloud.points[i].x = msg.detection_array[i].f_x;
            cloud.points[i].y = msg.detection_array[i].f_y; 
            cloud.points[i].z = msg.detection_array[i].f_z; 

        }
    }
    
    // 转换为 PointCloud2 消息  
    sensor_msgs::msg::PointCloud2 msg2;  
    pcl::toROSMsg(cloud, msg2);  
    msg2.header.frame_id = "map";  // 设置坐标系  
    // msg2.header.stamp = now();  
    detections_cloud_pub->publish(msg2);  
    RCLCPP_INFO(global_logger, "Publishing PointCloud with %lu points", cloud.points.size()); 
    

}

int main(int argc, char **argv)
{
    // ros::init(argc, argv, "info_convert_node");
    // ros::NodeHandle nh;
    // ros::NodeHandle private_nh("~");

    // ros::Subscriber obj_list_sub = nh.subscribe("/ars548_process/object_list", 10, &objectReceive);
    // ros::Subscriber det_list_sub = nh.subscribe("/ars548_process/detection_list", 10, &detectionReceive);
    // objects_marker_pub = nh.advertise<visualization_msgs::MarkerArray>("/ars548_process/object_marker", 10);
    // detections_cloud_pub = nh.advertise<sensor_msgs::PointCloud>("/ars548_process/detection_point_cloud", 10);

    // ros::spin();
    // ROS2 C++接口初始化
    rclcpp::init(argc, argv);
    // 创建ROS2节点对象并进行初始化                                                                   
    std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("info_convert_node");  
    det_list_sub = node->create_subscription<ars548_interface::msg::DetectionList>("/ars548_process/detection_list", 10, detectionReceive);
    detections_cloud_pub = node->create_publisher<sensor_msgs::msg::PointCloud2>("/ars548_process/detection_point_cloud", 10); 
    // ros::spin();
    // 循环等待ROS2退出
    rclcpp::spin(node);    
    // 关闭ROS2 C++接口                                                                     
    rclcpp::shutdown();  
}

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>ars548_process</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="neal@todo.todo">neal</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>


  <depend>ars548_interface</depend>
  <depend>pcl</depend>  <!-- 添加这行 -->  
  <exec_depend>pcl_conversions</exec_depend>  <!-- 添加这行 -->
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

节点运行

ros2 run ars548_process ars548_process_node
ros2 run ars548_process info_convert_node

启动RVIZ查看

Launch 功能包

创建功能包ars548_launch,启动三个节点ars548_process_node、info_convert_node、usbcam、rviz2

import os  
from launch import LaunchDescription  # launch文件的描述类
from launch_ros.actions import Node   # 节点启动的描述类
from ament_index_python.packages import get_package_share_directory # 查询功能包路径的方法

def generate_launch_description():  
    rviz_config = os.path.join(          # 找到配置文件的完整路径
      get_package_share_directory('ars548_launch'),
      'rviz',
      'ros2_rviz.rviz'
      )
    return LaunchDescription([ 
        Node(  
            package='ars548_process',  # 替换为您的包名  
            executable='info_convert_node',  # 替换为您的节点可执行文件  
            name='info_convert_node',  # 可选的节点名称  
            output='screen',  # 输出到终端  
            parameters=[{'param_name': 'param_value'}]  # 可选的参数  
        ), 
        Node(  
            package='ars548_process',  # 替换为您的包名  
            executable='ars548_process_node',  # 替换为您的节点可执行文件  
            name='ars548_process_node',  # 可选的节点名称  
            output='screen',  # 输出到终端  
            parameters=[{'param_name': 'param_value'}]  # 可选的参数  
        ), 
        #ros2 run usb_cam usb_cam_node_exe
        Node(  
            package='usb_cam',  # 替换为您的包名  
            executable='usb_cam_node_exe',  # 替换为您的节点可执行文件  
            name='usb_cam_node_exe',  # 可选的节点名称  
            output='screen',  # 输出到终端  
            parameters=[{'param_name': 'param_value'}]  # 可选的参数  
        ), 
        Node(                             # 配置一个节点的启动
            package='rviz2',               # 节点所在的功能包
            executable='rviz2',            # 节点的可执行文件名
            name='rviz2',                  # 对节点重新命名
            arguments=['-d', rviz_config]  # 加载命令行参数
        ),
        # 添加更多节点，如果需要的话  
    ])

RVIZ2显示USB摄像头

上面雷达检测数据可以正常显示，无真值对比很难看出效果，现在RVIZ2添加USB摄像头显示功能。

先使用usb-cam模块验证

sudo apt install ros-humble-usb-cam
sudo apt install ros-humble-image*
ros2 run usb_cam usb_cam_node_exe

组播配置

开启组播

使用以下命令检查是否已启用组播功能。

sysctl net.ipv4.ip_forward

如果返回值为1，则表示组播功能已启用。如果返回值为0，则需要手动启用组播功能。

编辑 /etc/sysctl.conf 文件

net.ipv4.ip_forward = 1

使用下面命令生效配置

sysctl -p

配置防火墙

查看防火墙规则

sudo iptables -L

如果发现有防火墙规则阻止组播消息，你可以使用以下命令允许组播流量通过防火墙：

sudo iptables -I INPUT -d 224.0.0.0/4 -j ACCEPT
sudo iptables -I OUTPUT -d 224.0.0.0/4 -j ACCEPT

  • 修改完成后重新ros2 run效果如下，可以正常接收处理数据。

Wireshark运行截图，必须sudo启动wireshark

开源地址

https://github.com/nealwang123/ros_ws.git

参考连接

ROS2如何创建自定义msg、srv和action文件

//www.greatytc.com/p/c39b3762d301

ROS1迁移ROS2经验总结（针对点云神经网络）

https://blog.csdn.net/weixin_44589524/article/details/135497875

ROS1到ROS2迁移教程（以rslidar_to_velodyne功能包为例）+ ROS2版本LIO-SAM试跑

https://blog.csdn.net/lixushi/article/details/131628941


©著作权归作者所有,转载或内容合作请联系作者
  • 人面猴
    序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
    沈念sama阅读 211,194评论 6赞 490
  • 死咒
    序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
    沈念sama阅读 90,058评论 2赞 385
  • 救了他两次的神仙让他今天三更去死
    文/潘晓璐 我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
    开封第一讲书人阅读 156,780评论 0赞 346
  • 道士缉凶录：失踪的卖姜人
    文/不坏的土叔 我叫张陵，是天一观的道长。 经常有香客问我，道长，这世上最难降的妖魔是什么？ 我笑而不...
    开封第一讲书人阅读 56,388评论 1赞 283
  • 港岛之恋（遗憾婚礼）
    正文 为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
    茶点故事阅读 65,430评论 5赞 384
  • 恶毒庶女顶嫁案：这布局不是一般人想出来的
    文/花漫 我一把揭开白布。 她就那样静静地躺着，像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上，一...
    开封第一讲书人阅读 49,764评论 1赞 290
  • 城市分裂传说
    那天，我揣着相机与录音，去河边找鬼。 笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。 我是一名探鬼主播，决...
    沈念sama阅读 38,907评论 3赞 406
  • 双鸳鸯连环套：你想象不到人心有多黑
    文/苍兰香墨 我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
    开封第一讲书人阅读 37,679评论 0赞 266
  • 万荣杀人案实录
    序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
    沈念sama阅读 44,122评论 1赞 303
  • 护林员之死
    正文 独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 36,459评论 2赞 325
  • 白月光启示录
    正文 我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 38,605评论 1赞 340
  • 活死人
    序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
    沈念sama阅读 34,270评论 4赞 329
  • 日本核电站爆炸内幕
    正文 年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
    茶点故事阅读 39,867评论 3赞 312
  • 男人毒药：我在死后第九天来索命
    文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 30,734评论 0赞 21
  • 一桩弑父案，背后竟有这般阴谋
    文/苍兰香墨 我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 31,961评论 1赞 265
  • 情欲美人皮
    我被黑心中介骗来泰国打工， 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。 一个月前我还...
    沈念sama阅读 46,297评论 2赞 360
  • 代替公主和亲
    正文 我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
    茶点故事阅读 43,472评论 2赞 348

推荐阅读更多精彩内容