diff --git a/CHANGELOG.md b/CHANGELOG.md index bf8f542..c88caee 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,3 +19,7 @@ This release also offers the possibilty of configuring the ip addresses of the s ## v3.0.0 - 2022-09-23 Major release includes the new smartmicro sensor DRVEGRD 169. The driver offers mode changes and configuration of the DRVEGRD 169 along with publishing the radar targets as pointcloud data. The callbacks for datastream now require a clientID. + +## v3.1.0 - 2022-10-19 + +This release includes the new smartmicro sensor DRVEGRD 152. The driver offers mode changes and configuration of the DRVEGRD 152 along with publishing the radar targets as pointcloud data. The callbacks for datastream now require a clientID. \ No newline at end of file diff --git a/CONTRIBUTION.md b/CONTRIBUTING.md similarity index 100% rename from CONTRIBUTION.md rename to CONTRIBUTING.md diff --git a/Readme.md b/Readme.md index 218cd02..e17d4ae 100644 --- a/Readme.md +++ b/Readme.md @@ -22,20 +22,23 @@ ros2 launch umrr_ros2_driver radar.launch.py - ROS2 foxy ### UMRR radars and Smart Access API version -A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96 radar, UMRR11 radar or UMRR9F radar are +A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96, UMRR11, DRVEGRD 152 or DRVEGRD 169 radar are required to run this node. This code is bundled with a version of Smart Access API. Please make sure the version used to publish the data is compatible with this version: -- Date of release: `September 22, 2022` -- Smart Access Automotive version: `v2.0.0` +- Date of release: `October 19, 2022` +- Smart Access Automotive version: `v2.1.0` - User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.1` - User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.1` - User interface version: `UMRR9F Type 169 AUTOMOTIVE v1.1.1` +- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.0.1` ### Sensor Firmwares This ROS2 driver release is compatible with the following sensor firmwares: - UMRR11 Type 132: V5.1.4 - UMRR96 Type 153: V5.2.4 +- UMRR9D Type 152: V1.0.0 +- UMRR9F Type 169: V1.3.0 ### Point cloud message wrapper library To add targets to the point cloud in a safe and quick fashion a diff --git a/docker-compose.yml b/docker-compose.yml index 64e2208..f3c20a2 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -64,12 +64,28 @@ services: networks: device_network: ipv4_address: 172.22.10.103 + sensor_3: + depends_on: + build_simulator: + condition: service_completed_successfully + image: umrr-ros:latest + volumes: + - .:/code + environment: + - SMART_ACCESS_CFG_FILE_PATH=/code/simulator/config_umrr9d/com_lib_config.json + - LD_LIBRARY_PATH=/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9 + entrypoint: ["/code/simulator/simulation/out/bin/simulator"] + command: ["11", "1", "0", "C"] + networks: + device_network: + ipv4_address: 172.22.10.104 ros_node: image: umrr-ros:latest depends_on: - "sensor_0" - "sensor_1" - "sensor_2" + - "sensor_3" volumes: - .:/code command: ["colcon", "test", "--packages-above", "umrr_ros2_driver"] diff --git a/simulator/config_umrr9d/com_lib_config.json b/simulator/config_umrr9d/com_lib_config.json new file mode 100644 index 0000000..069ef60 --- /dev/null +++ b/simulator/config_umrr9d/com_lib_config.json @@ -0,0 +1,16 @@ +{ + "name": "Com Lib Config", + "version": "1.0.0", + "client_id": 400, + "role": "slave", + "shared_lib_path": "/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9", + "config_path": "/code/simulator/config_umrr9d", + "user_interface_name": "umrr9d_t152_automotive", + "user_interface_major_v": 1, + "user_interface_minor_v": 0, + "user_interface_patch_v": 1, + "download_path": "", + "instruction_serialization_type": "port_based", + "data_serialization_type": "port_based", + "alive": false +} diff --git a/simulator/config_umrr9d/hw_inventory.json b/simulator/config_umrr9d/hw_inventory.json new file mode 100644 index 0000000..94f370a --- /dev/null +++ b/simulator/config_umrr9d/hw_inventory.json @@ -0,0 +1,12 @@ +{ + "hwItems": [ + { + "type": "eth", + "dev_id": 1, + "iface_name": "eth0", + "port": 55555 + } + ], + "name": "HW inventory List", + "version": "1.1.0" +} diff --git a/simulator/config_umrr9d/routing_table.json b/simulator/config_umrr9d/routing_table.json new file mode 100644 index 0000000..1eaffd3 --- /dev/null +++ b/simulator/config_umrr9d/routing_table.json @@ -0,0 +1,14 @@ +{ + "clients": [ + { + "client_id": 1, + "data_serialization_type": "port_based", + "instruction_serialization_type": "port_based", + "ip": "172.22.10.100", + "link_type": "eth", + "port": 55555 + } + ], + "name": "Client Routing Table", + "version": "1.0.0" +} diff --git a/simulator/simulation/src/simulator.cpp b/simulator/simulation/src/simulator.cpp index 386cdf9..39003fd 100644 --- a/simulator/simulation/src/simulator.cpp +++ b/simulator/simulation/src/simulator.cpp @@ -1,3 +1,17 @@ +// Copyright (c) 2021, s.m.s, smart microwave sensors GmbH, Brunswick, Germany. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include #include @@ -5,13 +19,15 @@ #include #include #include +#include +#include +#include + #include #include #include #include -#include -#include -#include +#include #include #include @@ -19,19 +35,21 @@ using namespace com::common; using namespace com::master; using namespace com::types; +namespace +{ uint64_t identifier; uint64_t majorVersion; uint64_t minorVersion; std::string port; std::string portFile; - +} // namespace std::shared_ptr dataServices = - com::common::DataServicesIface::Get(); + com::common::DataServicesIface::Get(); -void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) { - InstructionBuffer *receive = - reinterpret_cast(buffer.GetBufferPtr()); +void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) +{ + InstructionBuffer * receive = reinterpret_cast(buffer.GetBufferPtr()); int sizeIncomingBuf = buffer.GetSize(); uint32_t instnumber = receive->GetNumOfInstructions(); auto instructions = receive->GetInstructions(); @@ -41,40 +59,33 @@ void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) { std::cout << "Base user interface major version set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); instruction->SetValue(1); - } else if (instruction->GetSectionId() == 3042 && - instruction->GetId() == 21) { + } else if (instruction->GetSectionId() == 3042 && instruction->GetId() == 21) { std::cout << "Base user interface minor version set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); instruction->SetValue(0); - } else if (instruction->GetSectionId() == 3042 && - instruction->GetId() == 22) { + } else if (instruction->GetSectionId() == 3042 && instruction->GetId() == 22) { std::cout << "User interface identifier set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); instruction->SetValue(identifier); - } else if (instruction->GetSectionId() == 3042 && - instruction->GetId() == 23) { + } else if (instruction->GetSectionId() == 3042 && instruction->GetId() == 23) { std::cout << "User interface major version set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); instruction->SetValue(majorVersion); - } else if (instruction->GetSectionId() == 3042 && - instruction->GetId() == 24) { + } else if (instruction->GetSectionId() == 3042 && instruction->GetId() == 24) { std::cout << "User interface minor version set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); instruction->SetValue(minorVersion); - } else if (instruction->GetSectionId() == 2010 && - instruction->GetId() == 2) { - std::cout << "UMRR96 mode frequency_sweep set!" - << std::endl; + } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 2) { + std::cout << "UMRR96 mode frequency_sweep set!" << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 4) { + std::cout << "UMRR11 mode angular_separation set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); - } else if (instruction->GetSectionId() == 2010 && - instruction->GetId() == 4) { - std::cout << "UMRR11 mode angular_separation set!" - << std::endl; + } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 5) { + std::cout << "UMRR9F mode range_toggle_mode set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); - } else if (instruction->GetSectionId() == 2010 && - instruction->GetId() == 5) { - std::cout << "UMRR9F mode range_toggle_mode set!" - << std::endl; + } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 1) { + std::cout << "UMRR9D mode center_frequency_idx set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); } else { std::cout << "Unknown instruction received from ROS driver!" << std::endl; @@ -84,17 +95,17 @@ void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) { dataServices->SetInstructionBuffer(clientId, *receive, nullptr); } -void stream_port(std::string portFile) { - +void stream_port(std::string portFile) +{ dataServices->RegisterInstRecvCallback(slave_callback); ClientId masterId = 1; PortId portTargetListId = 66; std::ifstream ifs(portFile, std::ifstream::binary | std::ios::binary); - std::filebuf *pbuf = ifs.rdbuf(); + std::filebuf * pbuf = ifs.rdbuf(); int size = pbuf->pubseekoff(0, ifs.end, ifs.in); pbuf->pubseekpos(0, ifs.in); - char *filebuffer = new (std::nothrow) char[size]; + char * filebuffer = new (std::nothrow) char[size]; if (filebuffer == nullptr) { std::cout << "error assigning memory!" << std::endl; @@ -104,18 +115,17 @@ void stream_port(std::string portFile) { BufferDescriptor bufferdesc((uint8_t *)filebuffer, size); std::this_thread::sleep_for(std::chrono::seconds(1)); - if (ERROR_CODE_OK != - dataServices->StreamDataPort(masterId, portTargetListId, bufferdesc)) { + if (ERROR_CODE_OK != dataServices->StreamDataPort(masterId, portTargetListId, bufferdesc)) { return; } std::cout << "sensor is transmitting data! " << std::endl; ifs.close(); delete[] filebuffer; - } - -int main(int argc, char *argv[]) { + +int main(int argc, char * argv[]) +{ if (argc != 5) { std::cout << "Specifiy User Interface for the sensor" << std::endl; return 1; @@ -125,28 +135,29 @@ int main(int argc, char *argv[]) { majorVersion = strtoll(argv[2], nullptr, 10); minorVersion = strtoll(argv[3], nullptr, 10); port = argv[4]; - + if (!dataServices->Init()) { throw std::runtime_error("Data services have not been initialized!"); } - + auto Start = std::chrono::steady_clock::now(); while (1) { - - if(port == "A") { + if (port == "A") { std::string portFile = "/code/simulator/targetlist_port_v2_1_0.bin"; stream_port(portFile); - } - else if(port == "B") { + } else if (port == "B") { std::string portFile = "/code/simulator/targetlist_port_v3_0_0.bin"; stream_port(portFile); - } - else { + } else if (port == "C") { + std::string portFile = "/code/simulator/targetlist_port_v4_0_0.bin"; + stream_port(portFile); + } else { std::cout << "Invalid input!" << std::endl; } - if (std::chrono::steady_clock::now() - Start > std::chrono::seconds(30)) + if (std::chrono::steady_clock::now() - Start > std::chrono::seconds(30)) { break; + } } return 0; } diff --git a/simulator/targetlist_port_v4_0_0.bin b/simulator/targetlist_port_v4_0_0.bin new file mode 100644 index 0000000..c23d2f2 Binary files /dev/null and b/simulator/targetlist_port_v4_0_0.bin differ diff --git a/smart_extract.sh b/smart_extract.sh index 24fba3e..04bb9f5 100755 --- a/smart_extract.sh +++ b/smart_extract.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -smart_pack=SmartAccessAutomotive_2_0_0.tar.gz +smart_pack=SmartAccessAutomotive_2_1_0.tar.gz URL_smartbinaries=https://www.smartmicro.com/fileadmin/media/Downloads/Automotive_Radar/Software/${smart_pack} cat << EOF diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index cb47350..5398087 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -56,6 +56,7 @@ install(FILES "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr11_t132_automotivev1.1.1_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr96_t153_automotivev1.2.1_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr9f_t169_automotivev1.1.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr9d_t152_automotivev1.0.1_user_interface.so" DESTINATION lib) set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") @@ -79,7 +80,8 @@ target_include_directories(smartmicro_radar_node PUBLIC ${CMAKE_INSTALL_PREFIX}/ smartmicro/include/ smartmicro/include/umrr11_t132_automotive_v1_1_1 smartmicro/include/umrr96_t153_automotive_v1_2_1 -smartmicro/include/umrr9f_t169_automotive_v1_1_1) +smartmicro/include/umrr9f_t169_automotive_v1_1_1 +smartmicro/include/umrr9d_t152_automotive_v1_0_1) # link smart_access_lib-linux-x86_64-gcc_9 to the node target_link_libraries(smartmicro_radar_node @@ -89,7 +91,8 @@ target_link_libraries(smartmicro_radar_node osal umrr11_t132_automotivev1.1.1_user_interface umrr96_t153_automotivev1.2.1_user_interface - umrr9f_t169_automotivev1.1.1_user_interface) + umrr9f_t169_automotivev1.1.1_user_interface + umrr9d_t152_automotivev1.0.1_user_interface) rclcpp_components_register_node(smartmicro_radar_node PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" diff --git a/umrr_ros2_driver/config/sensor_params.json b/umrr_ros2_driver/config/sensor_params.json index ea5c9b8..4fc590c 100644 --- a/umrr_ros2_driver/config/sensor_params.json +++ b/umrr_ros2_driver/config/sensor_params.json @@ -45,6 +45,12 @@ "min": 1, "max": 2, "default": 1 + }, + { + "model": "umrr9d", + "min": 0, + "max": 3, + "default": 1 } ] }, @@ -71,6 +77,13 @@ "min": 0, "max": 3, "default": 1 + }, + { + "model": "umrr9d", + "comment": "0=medium-range, 1=long-range", + "min": 0, + "max": 1, + "default": 0 } ] }, @@ -88,7 +101,7 @@ }, { "name": "angular_separation", - "comment": "0 = Disable Angular Separation, 1 = Enable Angular Separation", + "comment": "0=Disable Angular Separation, 1=Enable Angular Separation", "sensors": [ { "model": "umrr11", @@ -115,6 +128,25 @@ "min": 0, "max": 6, "default": 0 + }, + { + "model": "umrr9d", + "comment": " 0=off, 1=Med-Long", + "min": 0, + "max": 1, + "default": 0 + } + ] + }, + { + "name": "detection_sensitivity", + "comment": "Detection sensitivity: 0=low, 1=normal, 2=high", + "sensors": [ + { + "model": "umrr9d", + "min": 0, + "max": 2, + "default": 1 } ] }, @@ -133,6 +165,12 @@ "min": -150.0, "max": 150.0, "default": -20 + }, + { + "model": "umrr9d", + "min": -112.0, + "max": 56.0, + "default": -112.0 } ] }, @@ -151,6 +189,12 @@ "min": -150.0, "max": 150.0, "default": -20 + }, + { + "model": "umrr9d", + "min": -112.0, + "max": 56.0, + "default": -112.0 } ] }, @@ -169,6 +213,12 @@ "min": -150.0, "max": 150.0, "default": -20 + }, + { + "model": "umrr9d", + "min": -112.0, + "max": 56.0, + "default": -112.0 } ] }, @@ -180,6 +230,12 @@ "model": "umrr9f", "min": -150.0, "max": 150.0 + }, + { + "model": "umrr9d", + "min": -112.0, + "max": 56.0, + "default": -112.0 } ] }, @@ -198,6 +254,12 @@ "min": -150.0, "max": 150.0, "default": 20 + }, + { + "model": "umrr9d", + "min": -112.0, + "max": 56.0, + "default": 56.0 } ] }, @@ -216,6 +278,12 @@ "min": -150.0, "max": 150.0, "default": 20 + }, + { + "model": "umrr9d", + "min": -112.0, + "max": 56.0, + "default": 56.0 } ] }, @@ -234,6 +302,12 @@ "min": -150.0, "max": 150.0, "default": 20 + }, + { + "model": "umrr9d", + "min": -112.0, + "max": 56.0, + "default": 56.0 } ] }, @@ -245,6 +319,12 @@ "model": "umrr9f", "min": -150.0, "max": 150.0 + }, + { + "model": "umrr9d", + "min": -112.0, + "max": 56.0, + "default": 56.0 } ] }, diff --git a/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp b/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp index 51c7e42..242a15a 100644 --- a/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp +++ b/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -122,6 +123,22 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node com::master::umrr9f_t169_automotive_v1_1_1::comtargetlistport::ComTargetListPort> & targetlist_port_umrr9f, const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9d arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] target_list_port The target list port + /// @param[in] client_id The client_id of the sensor + /// + + void targetlist_callback_umrr9d( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_0_1::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9d, + const com::types::ClientId client_id); /// /// @brief Read parameters and update the json config files required by @@ -181,6 +198,7 @@ std::shared_ptr m_services{}; std::shared_ptr data_umrr11{}; std::shared_ptr data_umrr96{}; std::shared_ptr data_umrr9f{}; +std::shared_ptr data_umrr9d{}; } // namespace radar } // namespace drivers diff --git a/umrr_ros2_driver/param/radar.template.yaml b/umrr_ros2_driver/param/radar.template.yaml index 6182304..55a2fa3 100644 --- a/umrr_ros2_driver/param/radar.template.yaml +++ b/umrr_ros2_driver/param/radar.template.yaml @@ -52,3 +52,16 @@ frame_id: "umrr" # Specify the history size. history_size: 10 + sensor_3: + # The model of the connected sensor. + model: "umrr9d" + # The client_id of the sensor/source, must be a unique integer. + id: 400 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.104" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 40ed817..b92b490 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include @@ -138,6 +140,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option data_umrr11 = com::master::umrr11_t132_automotive_v1_1_1::DataStreamServiceIface::Get(); data_umrr96 = com::master::umrr96_t153_automotive_v1_2_1::DataStreamServiceIface::Get(); data_umrr9f = com::master::umrr9f_t169_automotive_v1_1_1::DataStreamServiceIface::Get(); + data_umrr9d = com::master::umrr9d_t152_automotive_v1_0_1::DataStreamServiceIface::Get(); RCLCPP_INFO(this->get_logger(), "Data stream services have been received!"); // Wait init time @@ -172,6 +175,15 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; } + if ( + sensor.model == "umrr9d" && + com::types::ERROR_CODE_OK != + data_umrr9d->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9d, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout <<"Failed to register targetlist callback for sensor umrr9d"<< std::endl; + } m_publishers[i] = create_publisher( "umrr/targets_" + std::to_string(i), sensor.history_size); @@ -468,15 +480,65 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f( const auto range_2d = range * std::cos(elevation_angle); const auto azimuth_angle = target->GetAzimuthAngle(); modifier.push_back( - {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), - range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetRCS(), - target->GetTgtNoise(), target->GetPower()}); + { + range_2d * std::cos(azimuth_angle), + range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), + target->GetSpeedRadial(), + target->GetRCS(), + target->GetTgtNoise(), + target->GetPower() + } + ); } m_publishers[sensor_idx]->publish(msg); } } +void SmartmicroRadarNode::targetlist_callback_umrr9d( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_0_1::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9d, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist callback is being called for umrr9d" << std::endl; + if(!check_signal) { + std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_0_1::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrr9d->GetGenericPortHeader(); + sensor_msgs::msg::PointCloud2 msg; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; + const auto sec = std::chrono::duration_cast(timestamp); + const auto nanosec = std::chrono::duration_cast(timestamp - sec); + msg.header.stamp.sec = sec.count(); + msg.header.stamp.nanosec = nanosec.count(); + for (const auto & target : targetlist_port_umrr9d->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + modifier.push_back( + { + range_2d * std::cos(azimuth_angle), + range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), + target->GetSpeedRadial(), + target->GetRCS(), + target->GetTgtNoise(), + target->GetPower() + } + ); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + + void SmartmicroRadarNode::update_config_files_from_params() { const auto dev_id = declare_parameter(kDevIdTag, 0); diff --git a/umrr_ros2_driver/test/radar_node_test.launch.py b/umrr_ros2_driver/test/radar_node_test.launch.py index 4bbc74d..b863fd7 100644 --- a/umrr_ros2_driver/test/radar_node_test.launch.py +++ b/umrr_ros2_driver/test/radar_node_test.launch.py @@ -77,12 +77,24 @@ def generate_test_description(): shell = True ) + center_frequency_idx_service = ExecuteProcess( + cmd = [[ + 'ros2 service call ', + '/smartmicro_radar_node/set_radar_mode ', + 'umrr_ros2_msgs/srv/SetMode ', + '"{param: "center_frequency_idx", value: 1, sensor_id: 400}"' + ]], + output='screen', + shell = True + ) + return ( launch.LaunchDescription([ radar_node, frequency_sweep_service, angular_separation_service, range_toggle_mode_service, + center_frequency_idx_service, launch_testing.actions.ReadyToTest(), ]), {