# 项目场景:
Ubuntu20.04/Ubuntu18.04(mmwave_radar/mmwave_ti_ros):
该项目为本人记录项目,主要意义用于在博客记录关于TI_Ros上位机数据接收模块修改的问题,还有中间出现的一些问题。ROS的基础装载等内容我已经在之前的博客中发布了。
问题列表
在这个ROS_demo中使用TI的开发板会通过函数进行一些雷达配置等流程,但我个人的板卡是已经配置好cfg的直接输出的雷达设备,不需要TI提供的所有流程,所以我如何做才能避免这些不必要的流程呢?
首先定位到处理这个流程的所有代码在哪里,哪些文件是我们要用到的,以及如何去修改:
- mmwave_ti_ros/ros_driver/src/ti_mmwave_rospkg/src(不需修改的已打×)
// mmWaveQuickConfig.cpp
//首先要知道ti_mmwave_rospkg包,例如ti_mmwave_rospkg::mmWaveCLI表示
//mmWaveCLI服务类型来自于ti_mmwave_rospkg包
ros::init(argc, argv, "mmWaveQuickConfig"); //初始化ROS节点,节点名为"mmWaveQuickConfig"。
ros::NodeHandle nh; //创建一个ROS节点句柄。
ros::NodeHandle private_nh("~"); //创建一个私有的ROS节点句柄。
ti_mmwave_rospkg::mmWaveCLI srv; //创建一个mmWaveCLI类型的服务对象。
if (argc != 2) {
return 1;
}
//参数个数不等于2,说明用户没有按照预期的方式提供参数 说明程序是错误的
std::string mmWaveCLIName;
private_nh.getParam("mmWaveCLI_name", mmWaveCLIName);
//从私有节点句柄中获取参数"mmWaveCLI_name",该参数是mmWaveCLI服务的名称。
ros::ServiceClient client = nh.serviceClient<ti_mmwave_rospkg::mmWaveCLI>(mmWaveCLIName);
//创建一个ROS服务客户端,用于调用mmWaveCLI服务
myParams.open(argv[1]);//打开配置文件(用于与ROS服务进行交互)
/*如果服务调用成功,且mmWave设备响应中包含"Done",则调用ParameterParser的ParamsParser函数对响应进行解析和处理。
用于从响应中提取配置参数,并将这些参数设置为ROS节点的参数,以便后续的操作可以按照这些参数进行配置和控制,
就是把srv中的信息给到nh用于后续的操作
*/
if (std::regex_search(srv.response.resp, std::regex("Done"))) {
parser.ParamsParser(srv, nh);}
//调用ParameterParser的CalParams函数,对ROS节点的参数进行计算处理。
parser.CalParams(nh);
/*所以我们要修改流程,并还要ros显示,我们需要删除配置部分,并且保留最终返回配置完成的关键信息。
这里我就直接简单明了的提供修改方案,并且后续可能在用到这个节点所以我们保留节点,删除配置操作。
1.只保留*/
ros::init(argc, argv, "mmWaveQuickConfig");
return 0;
/*2.修改DataUARTHandler.cpp,搜索nh->getParam,看到
while(!nh->hasParam("/ti_mmwave/doppler_vel_resolution")){}
这句是最关键的如果没有注释的话他会阻塞显示的进程,进入死循环
注释该while语句和所有nh->getParam 还有下面的ROS_INFO打印信息*/
// mmWaveDataHdl.cpp
//这个cpp中主要做的是对串口的配置,修改内容要根据我们板卡的输出性质来修改,它的原始代码中都是数据口,而我的输出是command口输出所以要修改成对应的command_port,command_rate
private_nh.getParam("command_port", mySerialPort);
private_nh.getParam("command_rate", myBaudRate);
//这里我额外解释一下command_port和command_rate的用处在哪里,它的使用是在后面的launch文件中实现的,里面有对应的内容。
// mmWaveCommSrv.cpp
/*这个函数的作用实际上做了什么?(就是给雷达发数据)
在onInit函数中,从ROS参数服务器中获取了命令端口、波特率和mmWave CLI的名称,并且打印了这些信息。
在commSrv_cb函数中,首先尝试打开一个串行端口,然后等待并读取之前潜在的响应,发送从客户端接收到的命令,接收来自mmWave传感器的响应,并关闭串行端口。
如果一切顺利,函数返回true,表示通信操作成功完成。*/
//1.这是第一种修改方式
#include "mmWaveCommSrv.hpp"
namespace ti_mmwave_rospkg
{
PLUGINLIB_EXPORT_CLASS(ti_mmwave_rospkg::mmWaveCommSrv, nodelet::Nodelet);
mmWaveCommSrv::mmWaveCommSrv() {}
void mmWaveCommSrv::onInit()
{
ros::NodeHandle private_nh = getPrivateNodeHandle();
ros::NodeHandle private_nh2("~");
private_nh2.getParam("mmWaveCLI_name", mmWaveCLIName);
NODELET_DEBUG("mmWaveCommsrv: Finished onInit function");
}
}
//2.这是第二种修改方式
//上面是我的修改方式,同样的你也可以不进行修改直接去mmwaveloader.cpp将下面这行代码注释
manager.load("mmWaveCommSrv", "ti_mmwave_rospkg/mmWaveCommSrv", remap, nargv);
// DataHandlerClass.cpp
/*这个cpp中除了前面要屏蔽的while语句之外,
再做修改则是为了修改ros上位机的数据显示,具体是什么意思就是解析流程在这里面,
接收设备发送到计算机上的数据,随后将其转变成ros的点显示。*/
case CHECK_TLV_TYPE://这个case语句用于判断tlv是否读取完成
//如果读取完成进行具体的对点云显示进行处理和将结果提交给ros的操作
DataUARTHandler_pub = nh->advertise<sensor_msgs::PointCloud2>("/ti_mmwave/radar_scan_pcl", 100);//搜索后在前几行
DataUARTHandler_pub.publish(RScan);//这段的位置是在CHECK_TLV_TYPE确定读取完之后的操作
//这就是一个发布点云到ros界面的操作,我们如果想要发布新的tlv主题到ros界面,就要靠这种写法
//但是这里我首先要提到,为了添加新的主题除了修改这里之外,我们还要再launch文件中添加我们要修改的主题例如:
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
<remap from="/ti_mmwave/radar_scan_track" to="/ti_mmwave/radar_scan_pcl_1"/>
//下面是代码的区分理解,但是需要清楚的是,有的代码修改是没有任何改变的,因为我们要注意launch文件与DataHandlerClass.cpp要匹配,所以即使将某些代码修改也不会有变化。
10-111 基础配置与函数
112-225 读取正在传入的数据实现
226-247 帧头0201040306050807识别实现
248-292 线程函数,它用于在两个缓冲区之间进行同步交换操作,用互斥锁和条件变量来实现线程间的同步操作,确保了在多线程环境下对缓冲区的安全访问和交换
293-846 数据处理,不包括0201040306050807
1.READ_OBJ_STRUCT、2.READ_LOG_MAG_RANGE、3.READ_NOISE、4.READ_AZIMUTH、5.READ_DOPPLER、6.READ_STATS、
7.READ_SIDE_INFO、8.READ_HEADER、9.READ_OCCUPANCY
293-391 tlv处理8 //READ_HEADER 这是一开始默认设置的当前执行状态 (理解上可以理解为 020104...是这个readheader的TYPE) tlv=8
392-559 tlv处理tlv1 //READ_OBJ_STRUCT tlv=1
560-592 tlv处理tlv7 //READ_SIDE_INFO tlv=7
593-620 tlv处理9 //READ_OCCUPANCY tlv=1030
621-628 tlv处理tlv2 //READ_LOG_MAG_RANGE tlv=2
629-645 tlv处理tlv3 //READ_NOISE tlv=3
646-662 tlv处理tlv4 //READ_AZIMUTH tlv=4
663-679 tlv处理tlv5 //READ_DOPPLER tlv=5
680-696 tlv处理tlv6 //READ_STATS tlv=6
697-812 这部分第一个if为判断是否读完tlv的 每执行一个tlv 就会++ 还会在结束的时候更新一下当前点云显示,保留在显示范围(max角度)内的点云点
并且更新保留在图上的点云点数的值
如果触发的是else 即当前没有读完所有tlv,存储tlv , 解析tlv 并switch到具体的处理部分
包括所有tlv的处理
813-846 确保线程间的协调和安全
847-931 线程的创建、信号的处理以及线程间的同步操作,在 ROS 中处理雷达设备数据时进行多线程管理和同步控制
可视化 932- end
特别注意:
518-523 为显示在rviz上的代码
mmwData是一个结构体,里面存着每一帧的所有雷达的输出,方便这一组数据的使用
- mmwave_ti_ros/ros1_driver/src/ti_mmwave_rospkg/include
//mmwave.h
//修改例如:
typedef struct MmwDemo_detectedObjForTx_t
{
int16_t speed; /*!< @brief Doppler index */
uint16_t peakVal; /*!< @brief Peak value */
int16_t x; /*!< @brief x - coordinate in meters. Q format provides the bitwidth. */
int16_t y; /*!< @brief y - coordinate in meters. Q format provides the bitwidth. */
int16_t z; /*!< @brief z - coordinate in meters. Q format provides the bitwidth. */
#ifdef SEND_SNR_INFO
uint16_t rangeSNRdB; /*!< @brief Range SNR (dB) */
uint16_t dopplerSNRdB; /*!< @brief Doppler SNR (dB) */
#endif
} MmwDemo_detectedObjForTx;
typedef struct MmwDemo_output_message_dataObjDescr_t
{
/*! @brief Number of detected objects */
uint16_t numDetetedObj;
/*! @brief Q format of detected objects x/y/z coordinates */
uint16_t xyzQFormat;
} MmwDemo_output_message_dataObjDescr;
MmwDemo_detectedObjForTx mrr_newObjOut;//-readcode
MmwDemo_output_message_dataObjDescr mrr_Qformat;//-readcode
//他们的作用就是你要在cpp文件中要到的结构体 同样你要建立结构体对象
//DataHandlerClass.h
//在这里需要添加我们要在rviz中新增的显示主题对象 如:
ros::Publisher DataUARTHandler_track;
//随后我会在DataHandlerClass.cpp中对DataUARTHandler_track进行操作
DataUARTHandler_track = nh->advertise<sensor_msgs::PointCloud2>("/ti_mmwave/radar_scan_track", 100);
- mmwave_ti_ros/ros1_driver/src/ti_mmwave_rospkg/launch
我这里使用的是TI_IWR6843 我选择了3d带z
//由于一开始我们就提到了,我们是只需要一个command口自动启动,并直接在rviz上进行显示处理的
//所以launch我们也要进行一些修改,这里直接给大家我的带注释修改的launch
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="6843" doc="TI mmWave sensor device type [1443, 1642, 6843]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<arg name="max_allowed_elevation_angle_deg" default="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
<arg name="max_allowed_azimuth_angle_deg" default="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<!-- 这里根据我们的串口设备和雷达设备的波特率进行选择-->
<param name="command_port" value="/dev/ttyUSB0" />
<param name="command_rate" value="921600" />
<!-- 我们不对data数据口有任何操作,直接注释 -->
<!--
<param name="data_port" value="/dev/ttyUSB1" />
<param name="data_rate" value="921600" />
-->
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<!-- 这里在Ubuntu20.04中是这样写 如果是ubuntu18.04 value="/ti_mmwave_0"-->
<param name="frame_id" value="ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
<!-- 这里将代码中添加的跟踪主题进行提交,在rviz上进行接收和显示 -->
<remap from="/ti_mmwave/radar_scan_track" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI"/>
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_rospkg)/launch/rviz/ti_mmwave_multi.rviz"/>
</launch>
效果展示:
提示:我的效果图为TI_mrr工程,我将其显示设置点云和跟踪。