yemai/mmwave_ti_ros
1
0
Fork 0
mirror of https://git.ti.com/git/mmwave_radar/mmwave_ti_ros.git synced 2026-05-21 01:12:08 +00:00
Code Issues Packages Projects Releases Wiki Activity

[ros2_driver/ti_mmwave_rospkg_msgs] Updated to use the standard header message, std_msgs/Header. Note that "seq" field in the header was deprecated in ROS 2.

Browse Source 
[ros2_driver/ti_mmwave_rospkg]
- Now using the standard header.
- <message>.header.stamp = nodeHandle->now(): it is set using the node clock (this is common practice) instead of the system clock.
- Cleaned up parameter declarations for each node.
- Added a parameter for 'frame_id'
This commit is contained in:
JuneChul Roh
2024-03-06 18:22:50 -06:00
parent 87b61bb259
commit db9c5384e2
12 changed files with 119 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
ros2_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h
View File

@@ -26,8 +26,8 @@
#define COUNT_SYNC_MAX 2
class DataUARTHandlerNode : public rclcpp::Node
{
class DataUARTHandlerNode : public rclcpp::Node
{
public:
explicit DataUARTHandlerNode();
@@ -39,40 +39,40 @@ private:
class DataUARTHandler
{
public:
//DataUARTHandler();
DataUARTHandler(std::shared_ptr<rclcpp::Node> nh);
void setFrameID(char const* myFrameID);
/*User callable function to set the UARTPort*/
void setUARTPort(char const* mySerialPort);
/*User callable function to set the BaudRate*/
void setBaudRate(int myBaudRate);
/*User callable function to set maxAllowedElevationAngleDeg*/
void setMaxAllowedElevationAngleDeg(int myMaxAllowedElevationAngleDeg);
/*User callable function to set maxAllowedElevationAngleDeg*/
void setMaxAllowedAzimuthAngleDeg(int myMaxAllowedAzimuthAngleDeg);
void setNodeHandle(std::shared_ptr<rclcpp::Node> nh);
// void setNodeHandle(std::shared_ptr<rclcpp::Node> nh);
/*User callable function to start the handler's internal threads*/
void start(void);
/*User callable function to stop the handler's internal threads*/
void stop();
/*Helper functions to allow pthread compatability*/
static void* readIncomingData_helper(void *context);
static void* sortIncomingData_helper(void *context);
static void* syncedBufferSwap_helper(void *context);
/* Function to handle signals such as SIGINT */
@@ -81,7 +81,7 @@ public:
/*Sorted mmwDemo Data structure*/
mmwDataPacket mmwData;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr DataUARTHandler_pub;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr radar_scan_pcl_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarScan>::SharedPtr radar_scan_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarOccupancy>::SharedPtr radar_occupancy_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarPointTrackID>::SharedPtr radar_trackid_pub;
@@ -112,64 +112,64 @@ private:
char const* frameID;
/*Contains the name of the serial port*/
char const* dataSerialPort;
/*Contains the baud Rate*/
int dataBaudRate;
/*Contains the max_allowed_elevation_angle_deg (points with elevation angles
/*Contains the max_allowed_elevation_angle_deg (points with elevation angles
outside +/- max_allowed_elevation_angle_deg will be removed)*/
int maxAllowedElevationAngleDeg;
/*Contains the max_allowed_azimuth_angle_deg (points with azimuth angles
/*Contains the max_allowed_azimuth_angle_deg (points with azimuth angles
outside +/- max_allowed_azimuth_angle_deg will be removed)*/
int maxAllowedAzimuthAngleDeg;
/*Mutex protected variable which synchronizes threads*/
int countSync;
/*Boolean used to notify threads to exit*/
bool stop_threads;
/*Read/Write Buffers*/
std::vector<uint8_t> pingPongBuffers[2];
/*Pointer to current data (sort)*/
std::vector<uint8_t>* currentBufp;
/*Pointer to new data (read)*/
std::vector<uint8_t>* nextBufp;
/*Mutex protecting the countSync variable */
pthread_mutex_t countSync_mutex;
/*Mutex protecting the nextBufp pointer*/
pthread_mutex_t nextBufp_mutex;
/*Mutex protecting the currentBufp pointer*/
pthread_mutex_t currentBufp_mutex;
/*Condition variable which blocks the Swap Thread until signaled*/
pthread_cond_t countSync_max_cv;
/*Condition variable which blocks the Read Thread until signaled*/
pthread_cond_t read_go_cv;
/*Condition variable which blocks the Sort Thread until signaled*/
pthread_cond_t sort_go_cv;
/*Swap Buffer Pointers Thread*/
void *syncedBufferSwap(void);
/*Checks if the magic word was found*/
int isMagicWord(uint8_t last8Bytes[8]);
/*Read incoming UART Data Thread*/
void *readIncomingData(void);
/*Sort incoming UART Data Thread*/
void *sortIncomingData(void);
rclcpp::Node::SharedPtr nodeHandle;
};
#endif
#endif

2
ros2_driver/src/ti_mmwave_rospkg/package.xml
View File

@@ -58,5 +58,5 @@
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

80
ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp
View File

@@ -8,24 +8,20 @@
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
using namespace std::chrono_literals;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr DataUARTHandler_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarScan>::SharedPtr radar_scan_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarOccupancy>::SharedPtr radar_occupancy_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarPointTrackID>::SharedPtr radar_trackid_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarTrackArray>::SharedPtr radar_trackarray_pub;
std::shared_ptr<sensor_msgs::msg::PointCloud2> pc2_msg_;
DataUARTHandler* gDataHandlerPtr;
rclcpp::Clock clocker;
DataUARTHandlerNode::DataUARTHandlerNode() : Node("DataUARTHandlerNode")
{
this->declare_parameter("mmwavecli_name", rclcpp::PARAMETER_STRING);
this->declare_parameter("mmwavecli_cfg", rclcpp::PARAMETER_STRING);
// this->declare_parameter("mmwavecli_name", rclcpp::PARAMETER_STRING);
// this->declare_parameter("mmwavecli_cfg", rclcpp::PARAMETER_STRING);
this->declare_parameter("data_port", rclcpp::PARAMETER_STRING);
this->declare_parameter("data_rate", rclcpp::PARAMETER_STRING);
this->declare_parameter("max_allowed_elevation_angle_deg", rclcpp::PARAMETER_INTEGER);
this->declare_parameter("max_allowed_azimuth_angle_deg", rclcpp::PARAMETER_INTEGER);
this->declare_parameter("frame_id", rclcpp::PARAMETER_STRING);
}
@@ -66,7 +62,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (mmWaveCloudType,
DataUARTHandler::DataUARTHandler(std::shared_ptr<rclcpp::Node> nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1])
{
DataUARTHandler_pub = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/ti_mmwave/radar_scan_pcl", 100);
radar_scan_pcl_pub = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/ti_mmwave/radar_scan_pcl", 100);
radar_scan_pub = nh->create_publisher<ti_mmwave_rospkg_msgs::msg::RadarScan>("/ti_mmwave/radar_scan", 100);
radar_occupancy_pub = nh->create_publisher<ti_mmwave_rospkg_msgs::msg::RadarOccupancy>("/ti_mmwave/radar_occupancy", 100);
radar_trackid_pub = nh->create_publisher<ti_mmwave_rospkg_msgs::msg::RadarPointTrackID>("/ti_mmwave/radar_point_track_id", 100);
@@ -75,6 +71,8 @@ DataUARTHandler::DataUARTHandler(std::shared_ptr<rclcpp::Node> nh) : currentBufp
maxAllowedAzimuthAngleDeg = 90; // Use max angle if none specified
gDataHandlerPtr = this;
stop_threads = false;
nodeHandle = nh;
}
void DataUARTHandler::setFrameID(char const* myFrameID)
@@ -418,7 +416,6 @@ void *DataUARTHandler::sortIncomingData(void)
mmwData.numObjOut = mmwData.header.numDetectedObj;
}
RScan->header.stamp = static_cast<long int>(clocker.now().seconds());
RScan->header.frame_id = frameID;
RScan->height = 1;
RScan->width = mmwData.numObjOut;
@@ -490,8 +487,8 @@ void *DataUARTHandler::sortIncomingData(void)
RScan->points[i].z = temp[2]; // ROS standard coordinate system Z-axis is up which is the same as mmWave sensor Z-axis
//RScan->points[i].intensity = temp[5];
radarscan.frame_id = frameID;
radarscan.stamp = static_cast<long int>(clocker.now().seconds());
radarscan.header.stamp = nodeHandle->now();
radarscan.header.frame_id = frameID;
radarscan.point_id = i;
radarscan.x = temp[1];
radarscan.y = -temp[0];
@@ -526,8 +523,8 @@ void *DataUARTHandler::sortIncomingData(void)
RScan->points[i].z = mmwData.newObjOut.z; // ROS standard coordinate system Z-axis is up which is the same as mmWave sensor Z-axis
RScan->points[i].velocity = mmwData.newObjOut.velocity;
radarscan.frame_id = frameID;
radarscan.stamp = static_cast<long int>(clocker.now().seconds());
radarscan.header.stamp = nodeHandle->now();
radarscan.header.frame_id = frameID;
radarscan.point_id = i;
radarscan.x = mmwData.newObjOut.y;
radarscan.y = -mmwData.newObjOut.x;
@@ -602,13 +599,14 @@ void *DataUARTHandler::sortIncomingData(void)
radaroccupancy.state = mmwData.occupancy.state;
if (radaroccupancy.state == 0){
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Area is clear!");
}
else{
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Area is OCCUPIED!");
}
// if (radaroccupancy.state == 0){
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Area is clear!");
// }
// else{
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Area is OCCUPIED!");
// }
radaroccupancy.header.stamp = nodeHandle->now();
radar_occupancy_pub->publish(radaroccupancy);
sorterState = CHECK_TLV_TYPE;
@@ -693,8 +691,8 @@ void *DataUARTHandler::sortIncomingData(void)
RScan->points[i].velocity = realDoppler;
RScan->points[i].intensity = realSNR;
radarscan.frame_id = frameID;
radarscan.stamp = clocker.now().seconds();
radarscan.header.stamp = nodeHandle->now();
radarscan.header.frame_id = frameID;
radarscan.point_id = i;
radarscan.x = realRange * cos(realAzimuth) * cos(realElevation);
@@ -729,8 +727,7 @@ void *DataUARTHandler::sortIncomingData(void)
i = 0;
offset = 0;
mmwData.numObjOut = mmwData.header.numDetectedObj;
radartrackarray.frame_id = frameID;
radartrackarray.stamp = clocker.now().seconds();
radartrackarray.num_tracks = (int) tlvLen / 112;
//RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Number of Tracks is: %d",(tlvLen / 112));
@@ -788,8 +785,8 @@ void *DataUARTHandler::sortIncomingData(void)
memcpy( &mmwData.newListOut.confidenceLevel, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.confidenceLevel));
currentDatap += ( sizeof(mmwData.newListOut.confidenceLevel) );
radartrackcontents.frame_id = frameID;
radartrackcontents.stamp = clocker.now().seconds();
radartrackcontents.header.stamp = nodeHandle->now();
radartrackcontents.header.frame_id = frameID;
radartrackcontents.tid = mmwData.newListOut.tid;
radartrackcontents.posx = mmwData.newListOut.posY; // ROS standard coordinate system X-axis is forward which is the mmWave sensor Y-axis
radartrackcontents.posy = -mmwData.newListOut.posX; // ROS standard coordinate system Y-axis is left which is the mmWave sensor -(X-axis)
@@ -802,8 +799,10 @@ void *DataUARTHandler::sortIncomingData(void)
radartrackcontents.accz = mmwData.newListOut.accZ;
radartrackarray.track.push_back(radartrackcontents);
i++;
}
radartrackarray.header.stamp = nodeHandle->now();
radartrackarray.header.frame_id = frameID;
radar_trackarray_pub->publish(radartrackarray);
radartrackarray.track.clear();
sorterState = CHECK_TLV_TYPE;
@@ -881,8 +880,9 @@ void *DataUARTHandler::sortIncomingData(void)
RScan->points[i].z = realZ;
RScan->points[i].velocity = realDoppler;
RScan->points[i].intensity = realSNR;
radarscan.frame_id = frameID;
radarscan.stamp = clocker.now().seconds();
radarscan.header.stamp = nodeHandle->now();
radarscan.header.frame_id = frameID;
radarscan.point_id = i;
radarscan.x = realY;
radarscan.y = -(realX);
@@ -932,12 +932,12 @@ void *DataUARTHandler::sortIncomingData(void)
// throw away first track in order to allign with PointCloud
// if (frameID == 0){
radartrackid.frame_id = frameID;
radartrackid.header.frame_id = frameID;
// }
// else{
// radartrackid.header.frame_id = frameID - 1;
// }
radartrackid.stamp = clocker.now().seconds();
radartrackid.header.stamp = nodeHandle->now();
radartrackid.tid = mmwData.newIndexOut.targetID;
radar_trackid_pub->publish(radartrackid);
@@ -1040,8 +1040,9 @@ void *DataUARTHandler::sortIncomingData(void)
}
pc2_msg_ = std::make_shared<sensor_msgs::msg::PointCloud2>();
pcl::toROSMsg(*RScan, *pc2_msg_);
pc2_msg_->header.frame_id = "map";
DataUARTHandler_pub->publish(*pc2_msg_);
pc2_msg_->header.stamp = nodeHandle->now();
pc2_msg_->header.frame_id = frameID;
radar_scan_pcl_pub->publish(*pc2_msg_);
sorterState = SWAP_BUFFERS;
}
else // More TLV sections to parse
@@ -1257,17 +1258,22 @@ void DataUARTHandler::stop()
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<DataUARTHandlerNode>();
std::string mySerialPort = node->get_parameter("data_port").as_string();
std::string myBaudRate = node->get_parameter("data_rate").as_string();
std::string myFrameID = "/ti_mmwave_0";
std::string myFrameID = node->get_parameter("frame_id").as_string();
int maxAllowedElevationAngleDeg = node->get_parameter("max_allowed_elevation_angle_deg").as_int();
int maxAllowedAzimuthAngleDeg = node->get_parameter("max_allowed_azimuth_angle_deg").as_int();
DataUARTHandler d(node);
d.setFrameID(myFrameID.c_str());
d.setUARTPort(mySerialPort.c_str());
d.setBaudRate(std::stoi(myBaudRate));
d.setMaxAllowedElevationAngleDeg(90);
d.setMaxAllowedAzimuthAngleDeg(90);
d.setMaxAllowedElevationAngleDeg(maxAllowedElevationAngleDeg);
d.setMaxAllowedAzimuthAngleDeg(maxAllowedAzimuthAngleDeg);
d.start();
rclcpp::spin(node);
rclcpp::shutdown();
}

32
ros2_driver/src/ti_mmwave_rospkg/src/ParameterParser.cpp
View File

@@ -15,17 +15,17 @@
std::shared_ptr<rclcpp::Node> nodeptr3 = nullptr;
class ParameterParser : public rclcpp::Node
{
class ParameterParser : public rclcpp::Node
{
public:
ParameterParser() : Node("ParameterParser")
{
this->declare_parameter("device_name", rclcpp::PARAMETER_STRING);
this->declare_parameter("device_name", rclcpp::PARAMETER_STRING);
this->declare_parameter("mmwavecli_name", rclcpp::PARAMETER_STRING);
this->declare_parameter("mmwavecli_cfg", rclcpp::PARAMETER_STRING);
parameters_client_test = std::make_shared<rclcpp::AsyncParametersClient>(this, "/ConfigParameterServer");
parameters_client_test->wait_for_service();
auto parameters_future = parameters_client_test->get_parameters({
auto parameters_future = parameters_client_test->get_parameters({
"/ti_mmwave/startFreq",
"/ti_mmwave/idleTime",
"/ti_mmwave/adcStartTime",
@@ -44,7 +44,7 @@ public:
"/ti_mmwave/zoneMinY",
"/ti_mmwave/zoneMaxY",
"/ti_mmwave/zoneMinZ",
"/ti_mmwave/zoneMaxZ"
"/ti_mmwave/zoneMaxZ"
},
std::bind(&ParameterParser::callbackGlobalParam, this, std::placeholders::_1));
}
@@ -89,19 +89,19 @@ int main(int argc, char **argv) {
std::ifstream myCfgParam;
std::string str_param;
std::string deviceName = nodeptr3->get_parameter("device_name").as_string();
std::string mmWaveCLIname = nodeptr3->get_parameter("mmwavecli_name").as_string();
// std::string mmWaveCLIname = nodeptr3->get_parameter("mmwavecli_name").as_string();
std::string mmWaveCLIcfg = nodeptr3->get_parameter("mmwavecli_cfg").as_string();
auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(nodeptr3, "/ConfigParameterServer");
myCfgParam.open(mmWaveCLIcfg);
if (deviceName.compare("6432") != 0)
{
if (myCfgParam.is_open())
if (myCfgParam.is_open())
{
while( std::getline(myCfgParam, str_param))
while( std::getline(myCfgParam, str_param))
{
str_param.erase(std::remove(str_param.begin(), str_param.end(), '\r'), str_param.end());
if (!(std::regex_match(str_param, std::regex("^\\s*%.*")) || std::regex_match(str_param, std::regex("^\\s*"))))
if (!(std::regex_match(str_param, std::regex("^\\s*%.*")) || std::regex_match(str_param, std::regex("^\\s*"))))
{
//RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"myParams equals %s\n", str_param.c_str() );
std::istringstream ss(str_param);
@@ -111,7 +111,7 @@ int main(int argc, char **argv) {
v.push_back(token);
}
if (!v[0].compare("profileCfg"))
if (!v[0].compare("profileCfg"))
{
//RCLCPP_INFO(this->get_logger(), "ProfileCfg");
parameters_client->set_parameters(
@@ -132,8 +132,8 @@ int main(int argc, char **argv) {
freqSlopeConst = std::stof(v[8]);
numAdcSamples = std::stof(v[10]);
digOutSampleRate = std::stof(v[11]);
}
else if (!v[0].compare("frameCfg"))
}
else if (!v[0].compare("frameCfg"))
{
parameters_client->set_parameters(
{
@@ -149,8 +149,8 @@ int main(int argc, char **argv) {
numLoops = std::stoi(v[3]);
numFrames = std::stoi(v[4]);
framePeriodicity = std::stof(v[5]);
}
else if (!v[0].compare("zoneDef"))
}
else if (!v[0].compare("zoneDef"))
{
parameters_client->set_parameters(
{
@@ -169,7 +169,7 @@ int main(int argc, char **argv) {
zoneMinZ = std::stoi(v[6]);
zoneMaxZ = std::stoi(v[7]);
}
}
}
}
}
@@ -182,7 +182,7 @@ int main(int argc, char **argv) {
float adc_duration = nr / fs;
float BW = adc_duration * kf;
float PRI = (idleTime + rampEndTime) * 1e-6;
float fc = startFreq * 1e9 + kf * (adcStartTime * 1e-6 + adc_duration / 2);
float fc = startFreq * 1e9 + kf * (adcStartTime * 1e-6 + adc_duration / 2);
float vrange = c0 / (2 * BW);
float max_range = nr * vrange;
float max_vel = c0 / (2 * fc * PRI) / ntx;

32
ros2_driver/src/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp
View File

@@ -14,7 +14,7 @@
/*Include ROS specific headers*/
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "serial/serial.h"
#include "serial/serial.h"
#include "pluginlib/class_list_macros.hpp"
/*mmWave Driver Headers*/
@@ -26,43 +26,43 @@ using namespace std::chrono_literals;
std::shared_ptr<rclcpp::Node> nodeptr2 = nullptr;
class mmWaveCommSrv : public rclcpp::Node
{
{
public:
mmWaveCommSrv() : Node("mmWaveCommSrv")
{
this->declare_parameter("command_port", rclcpp::PARAMETER_STRING);
this->declare_parameter("command_rate", rclcpp::PARAMETER_STRING);
this->declare_parameter("data_port", rclcpp::PARAMETER_STRING);
this->declare_parameter("data_rate", rclcpp::PARAMETER_STRING);
this->declare_parameter("max_allowed_elevation_angle_deg", rclcpp::PARAMETER_STRING);
this->declare_parameter("max_allowed_azimuth_angle_deg", rclcpp::PARAMETER_STRING);
this->declare_parameter("frame_id", rclcpp::PARAMETER_STRING);
this->declare_parameter("mmwavecli_name", rclcpp::PARAMETER_STRING);
this->declare_parameter("/ti_mmwave/radar_scan_pcl", rclcpp::PARAMETER_STRING);
// this->declare_parameter("data_port", rclcpp::PARAMETER_STRING);
// this->declare_parameter("data_rate", rclcpp::PARAMETER_STRING);
// this->declare_parameter("max_allowed_elevation_angle_deg", rclcpp::PARAMETER_STRING);
// this->declare_parameter("max_allowed_azimuth_angle_deg", rclcpp::PARAMETER_STRING);
// this->declare_parameter("frame_id", rclcpp::PARAMETER_STRING);
// this->declare_parameter("mmwavecli_name", rclcpp::PARAMETER_STRING);
// this->declare_parameter("/ti_mmwave/radar_scan_pcl", rclcpp::PARAMETER_STRING);
}
private:
};
void handle_service_request(const std::shared_ptr<ti_mmwave_rospkg_msgs::srv::MmwaveCli::Request> request,
void handle_service_request(const std::shared_ptr<ti_mmwave_rospkg_msgs::srv::MmwaveCli::Request> request,
const std::shared_ptr<ti_mmwave_rospkg_msgs::srv::MmwaveCli::Response> response)
{
std::string mySerialPort = nodeptr2->get_parameter("command_port").as_string();
std::string myBaudRate = nodeptr2->get_parameter("command_rate").as_string();
std::string mmWaveCLIName = nodeptr2->get_parameter("mmwavecli_name").as_string();
// std::string mmWaveCLIName = nodeptr2->get_parameter("mmwavecli_name").as_string();
std::string sensorStart = "sensorStart\n";
/*Open Serial port and error check*/
serial::Serial mySerialObject("", std::stoi(myBaudRate), serial::Timeout::simpleTimeout(1000));
mySerialObject.setPort(mySerialPort.c_str());
try
try
{
mySerialObject.open();
}
catch (std::exception &e1)
}
catch (std::exception &e1)
{
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"\n\n\nmmWaveCommSrv: Failed to open User serial port with error: %s\n\n", e1.what());
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"\n\n\nmmWaveCommSrv: Power cycle the mmWave Sensor with the reset button while keeping the USB connected. Ensure the correct launch and configuration files are being used. Close all nodes, wait 10 Seconds, then relaunch the driver\n\n");
@@ -72,7 +72,7 @@ void handle_service_request(const std::shared_ptr<ti_mmwave_rospkg_msgs::srv::Mm
/*Read any previous pending response(s)*/
while (mySerialObject.available() > 0)
{
{
mySerialObject.readline(response->resp, 1024, ":/>");
//RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"mmWaveCommSrv: Received (previous) response from sensor: '%s'", response->resp.c_str());
response->resp = "";
@@ -92,7 +92,7 @@ void handle_service_request(const std::shared_ptr<ti_mmwave_rospkg_msgs::srv::Mm
if(!sensorStart.compare(request->comm))
{
rclcpp::shutdown();
exit(0);
exit(0);
}
}

2
ros2_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp
View File

@@ -38,7 +38,7 @@ int main(int argc, char **argv) {
rclcpp::Client<ti_mmwave_rospkg_msgs::srv::MmwaveCli>::SharedPtr client = nodeptr->create_client<ti_mmwave_rospkg_msgs::srv::MmwaveCli>("/ti_mmwave_rospkg_msgs/mmwave_cli");
auto request = std::make_shared<ti_mmwave_rospkg_msgs::srv::MmwaveCli::Request>();
auto response = std::make_shared<ti_mmwave_rospkg_msgs::srv::MmwaveCli::Response>();
std::string mmWaveCLIname = nodeptr->get_parameter("mmwavecli_name").as_string();
// std::string mmWaveCLIname = nodeptr->get_parameter("mmwavecli_name").as_string();
std::string mmWaveCLIcfg = nodeptr->get_parameter("mmwavecli_cfg").as_string();
std::string sensorStart = "sensorStart";

13
ros2_driver/src/ti_mmwave_rospkg_msgs/CMakeLists.txt
View File

@@ -9,14 +9,10 @@ project(ti_mmwave_rospkg_msgs)
## dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
## Generate messages in the 'msg' folder
set(msg_files
"msg/RadarScan.msg"
@@ -33,14 +29,9 @@ set(srv_files
rosidl_generate_interfaces(ti_mmwave_rospkg_msgs
${msg_files}
${srv_files}
DEPENDENCIES std_msgs
)
## ament_target_dependencies(std_msgs rclcpp)
ament_export_dependencies(
rosidl_default_runtime
std_msgs
rclcpp
)
ament_export_dependencies(rosidl_default_runtime)
ament_package()

4
ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarOccupancy.msg
View File

@@ -1,4 +1,2 @@
uint32 seq
uint32 stamp
string frame_id
std_msgs/Header header
uint32 state

5
ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarPointTrackID.msg
View File

@@ -1,3 +1,2 @@
string frame_id
uint32 stamp
uint8 tid
std_msgs/Header header
uint8 tid

4
ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarScan.msg
View File

@@ -1,6 +1,4 @@
uint32 seq
uint32 stamp
string frame_id
std_msgs/Header header
uint16 point_id
float32 x
float32 y

3
ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarTrackArray.msg
View File

@@ -1,4 +1,3 @@
string frame_id
uint32 stamp
std_msgs/Header header
uint32 num_tracks
RadarTrackContents[] track

3
ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarTrackContents.msg
View File

@@ -1,5 +1,4 @@
string frame_id
uint32 stamp
std_msgs/Header header
uint32 tid
float32 posx
float32 posy