diff --git a/ros2_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h b/ros2_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h index 9b378ec..a3cfdc8 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h +++ b/ros2_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h @@ -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 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 nh); - + // void setNodeHandle(std::shared_ptr 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::SharedPtr DataUARTHandler_pub; + rclcpp::Publisher::SharedPtr radar_scan_pcl_pub; rclcpp::Publisher::SharedPtr radar_scan_pub; rclcpp::Publisher::SharedPtr radar_occupancy_pub; rclcpp::Publisher::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 pingPongBuffers[2]; - + /*Pointer to current data (sort)*/ std::vector* currentBufp; - + /*Pointer to new data (read)*/ std::vector* 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 diff --git a/ros2_driver/src/ti_mmwave_rospkg/package.xml b/ros2_driver/src/ti_mmwave_rospkg/package.xml index 3ca72ba..190feed 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/package.xml +++ b/ros2_driver/src/ti_mmwave_rospkg/package.xml @@ -58,5 +58,5 @@ ament_cmake - + \ No newline at end of file diff --git a/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp b/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp index fab762c..24cede6 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp +++ b/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp @@ -8,24 +8,20 @@ #include #include #include -using namespace std::chrono_literals; -rclcpp::Publisher::SharedPtr DataUARTHandler_pub; -rclcpp::Publisher::SharedPtr radar_scan_pub; -rclcpp::Publisher::SharedPtr radar_occupancy_pub; -rclcpp::Publisher::SharedPtr radar_trackid_pub; -rclcpp::Publisher::SharedPtr radar_trackarray_pub; std::shared_ptr 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 nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1]) { - DataUARTHandler_pub = nh->create_publisher("/ti_mmwave/radar_scan_pcl", 100); + radar_scan_pcl_pub = nh->create_publisher("/ti_mmwave/radar_scan_pcl", 100); radar_scan_pub = nh->create_publisher("/ti_mmwave/radar_scan", 100); radar_occupancy_pub = nh->create_publisher("/ti_mmwave/radar_occupancy", 100); radar_trackid_pub = nh->create_publisher("/ti_mmwave/radar_point_track_id", 100); @@ -75,6 +71,8 @@ DataUARTHandler::DataUARTHandler(std::shared_ptr 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(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(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(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, ¤tBufp->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(); 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(); 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(); } \ No newline at end of file diff --git a/ros2_driver/src/ti_mmwave_rospkg/src/ParameterParser.cpp b/ros2_driver/src/ti_mmwave_rospkg/src/ParameterParser.cpp index b6e37ec..90b2f18 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/src/ParameterParser.cpp +++ b/ros2_driver/src/ti_mmwave_rospkg/src/ParameterParser.cpp @@ -15,17 +15,17 @@ std::shared_ptr 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(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(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; diff --git a/ros2_driver/src/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp b/ros2_driver/src/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp index ccb3a2c..42cd911 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp +++ b/ros2_driver/src/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp @@ -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 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 request, +void handle_service_request(const std::shared_ptr request, const std::shared_ptr 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 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_ptrcomm)) { rclcpp::shutdown(); - exit(0); + exit(0); } } diff --git a/ros2_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp b/ros2_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp index 2f76fad..abf176f 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp +++ b/ros2_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp @@ -38,7 +38,7 @@ int main(int argc, char **argv) { rclcpp::Client::SharedPtr client = nodeptr->create_client("/ti_mmwave_rospkg_msgs/mmwave_cli"); auto request = std::make_shared(); auto response = std::make_shared(); - 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"; diff --git a/ros2_driver/src/ti_mmwave_rospkg_msgs/CMakeLists.txt b/ros2_driver/src/ti_mmwave_rospkg_msgs/CMakeLists.txt index c7df19e..02baa45 100644 --- a/ros2_driver/src/ti_mmwave_rospkg_msgs/CMakeLists.txt +++ b/ros2_driver/src/ti_mmwave_rospkg_msgs/CMakeLists.txt @@ -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() \ No newline at end of file diff --git a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarOccupancy.msg b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarOccupancy.msg index bcd710d..51548a8 100644 --- a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarOccupancy.msg +++ b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarOccupancy.msg @@ -1,4 +1,2 @@ -uint32 seq -uint32 stamp -string frame_id +std_msgs/Header header uint32 state \ No newline at end of file diff --git a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarPointTrackID.msg b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarPointTrackID.msg index c8ad51c..68e99ff 100644 --- a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarPointTrackID.msg +++ b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarPointTrackID.msg @@ -1,3 +1,2 @@ -string frame_id -uint32 stamp -uint8 tid \ No newline at end of file +std_msgs/Header header +uint8 tid diff --git a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarScan.msg b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarScan.msg index 8dbc477..29c8f09 100644 --- a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarScan.msg +++ b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarScan.msg @@ -1,6 +1,4 @@ -uint32 seq -uint32 stamp -string frame_id +std_msgs/Header header uint16 point_id float32 x float32 y diff --git a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarTrackArray.msg b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarTrackArray.msg index 989425d..e18954f 100644 --- a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarTrackArray.msg +++ b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarTrackArray.msg @@ -1,4 +1,3 @@ -string frame_id -uint32 stamp +std_msgs/Header header uint32 num_tracks RadarTrackContents[] track diff --git a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarTrackContents.msg b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarTrackContents.msg index 1e109a8..2cc5e18 100644 --- a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarTrackContents.msg +++ b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarTrackContents.msg @@ -1,5 +1,4 @@ -string frame_id -uint32 stamp +std_msgs/Header header uint32 tid float32 posx float32 posy