diff --git a/.gitignore b/.gitignore index 9cc52ac..69310c6 100644 --- a/.gitignore +++ b/.gitignore @@ -6,6 +6,7 @@ umrr_ros2_driver/__pycache__ simulator/simulation/build_dir simulator/simulation/out umrr_ros2_driver/include/umrr_ros2_driver/sensor_params.hpp +umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp umrr_ros2_driver/smartmicro/* !umrr_ros2_driver/smartmicro/include umrr_ros2_driver/smartmicro/include/* diff --git a/CHANGELOG.md b/CHANGELOG.md index 048a45b..36c78a0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -31,3 +31,7 @@ This minor release introduces signal-to-noise field in the pointclouds and also ## v3.2.1 - 2022-12-16 This release fixes the offset which causes anomaly in pointclouds from DRVEGRD 152 sensor. It also fixes the timestamp calculation bug which causes rviz to crash and updates the simulator source files. + +## v4.0.0 - 2023-02-06 + +This release includes the new DRVEGRD 169 user-interface. The release includes new modes for DRVEGRD 169 and also introduces an additional ros2 service to save configurations. Previously used model 'UMRR9F' in radar parameters has been divided into two versions, the 'UMRR9F_V1_1_1' and 'UMRR9F_V2_0_0'. \ No newline at end of file diff --git a/Readme.md b/Readme.md index 0d4fae8..8976d81 100644 --- a/Readme.md +++ b/Readme.md @@ -26,11 +26,12 @@ A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96, UMRR11, DRVE 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: `December 16, 2022` -- Smart Access Automotive version: `v2.2.3` +- Date of release: `February 06, 2023` +- Smart Access Automotive version: `v3.0.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: `UMRR9F Type 169 AUTOMOTIVE v2.0.0` - User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.0.2` ### Sensor Firmwares @@ -39,6 +40,7 @@ This ROS2 driver release is compatible with the following sensor firmwares: - UMRR96 Type 153: V5.2.4 - UMRR9D Type 152: V2.1.0 - UMRR9F Type 169: V1.3.0 +- UMRR9F Type 169: V2.0.2 ### Point cloud message wrapper library To add targets to the point cloud in a safe and quick fashion a @@ -72,7 +74,7 @@ For more details, see the [`radar.template.yaml`](umrr_ros2_driver/param/radar.t - `iface_name`: name of the used network interface - `frame_id`: name of the frame in which the messages will be published - `history_size`: size of history for the message publisher -- `model`: the model of the sensor being used +- `model`: the model('umrr11', 'umrr9d', 'umrr96', 'umrr9f_v1_1_1', 'umrr9f_v2_0_0') of the sensor being used ## Mode of operations of the sensors The smartmicro radars come equipped with numerous features and modes of operation. Using the ros2 services provided one @@ -84,7 +86,7 @@ A ros2 `SetMode` service should be called to implement these mode changes. There - `sensor_id`: the id of the sensor to which the service call should be sent. For instance, changing the `Index of Transmit Antenna (tx_antenna_idx)` of a UMRR-11 sensor to `AEB (2)` mode would require the following call: -`ros2 service call /smartmicro_radar_node/set_radar_mode umrr_ros2_msgs/srv/SetMode "{param: "tx_antenna_idx", value: 2, sensor_id: 100}"` +`ros2 service call /smart_radar/set_radar_mode umrr_ros2_msgs/srv/SetMode "{param: "tx_antenna_idx", value: 2, sensor_id: 100}"` ## Configuration of the sensors In order to use multiple sensors (maximum of up to eight sensors) with the node the sensors should be configured separately. @@ -101,11 +103,18 @@ value in decimal `3232238400` should be used. - `sensor_id`: the sensor whose ip address is to be changed. The call for such a service would be as follows: -`ros2 service call /smartmicro_radar_node/set_ip_address umrr_ros2_msgs/srv/SetIp "{value_ip: 3232238400, sensor_id: 100}"` +`ros2 service call /smart_radar/set_ip_address umrr_ros2_msgs/srv/SetIp "{value_ip: 3232238400, sensor_id: 100}"` Note: For successfull execution of this call it is important that the sensor is restarted, the ip address in the [`radar.template.yaml`](umrr_ros2_driver/param/radar.template.yaml) is updated and the driver is build again. +## Saving mode changes +In order to save the mode changes, an additional service if provided. This service offers different save options and also the possibility to +set the default values for the sensors. The list of all the options could be found in the [`sensor_commands.json`](umrr_ros2_driver/config/sensor_commands.json). + +The call for such a service would be as follows: +`ros2 service call /smart_radar/send_command umrr_ros2_msgs/srv/SendCommand "{command: "comp_eeprom_ctrl_default_param_sec", sensor_id: 100}"` + ## Sensor Service Responses The sensor services respond with certain value codes. The following is a lookup table for the possible responses: diff --git a/smart_extract.sh b/smart_extract.sh index 0048d89..6034e6c 100755 --- a/smart_extract.sh +++ b/smart_extract.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -smart_pack=SmartAccessAutomotive_2_2_3.tgz +smart_pack=SmartAccessAutomotive_3_0_0.tgz 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 b33bd5f..6820363 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/libumrr9f_t169_automotivev2.0.0_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr9d_t152_automotivev1.0.2_user_interface.so" DESTINATION lib) @@ -65,6 +66,7 @@ set(CONFIG_FILE_PATH "${CONFIG_FOLDER_PATH}/smart_access_config.json") set(HW_INVENTORY_FILE_PATH "${CONFIG_FOLDER_PATH}/hw_inventory.json") set(ROUTING_TABLE_FILE_PATH "${CONFIG_FOLDER_PATH}/routing_table.json") set(SENSOR_PARAM_FILE_PATH "${PROJECT_SOURCE_DIR}/config/sensor_params.json") +set(SENSOR_COMMAND_FILE_PATH "${PROJECT_SOURCE_DIR}/config/sensor_commands.json") configure_file( "${PROJECT_SOURCE_DIR}/cmake/smart_access_config.json.in" "${CONFIG_FILE_PATH}" @ONLY) configure_file( @@ -73,6 +75,9 @@ configure_file( configure_file( "${PROJECT_SOURCE_DIR}/cmake/sensor_params.hpp.in" "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/sensor_params.hpp" @ONLY) +configure_file( + "${PROJECT_SOURCE_DIR}/cmake/sensor_commands.hpp.in" + "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/sensor_commands.hpp" @ONLY) ament_auto_add_library(smartmicro_radar_node SHARED "src/smartmicro_radar_node.cpp") @@ -81,6 +86,7 @@ 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_v2_0_0 smartmicro/include/umrr9d_t152_automotive_v1_0_2) # link smart_access_lib-linux-x86_64-gcc_9 to the node @@ -92,6 +98,7 @@ target_link_libraries(smartmicro_radar_node umrr11_t132_automotivev1.1.1_user_interface umrr96_t153_automotivev1.2.1_user_interface umrr9f_t169_automotivev1.1.1_user_interface + umrr9f_t169_automotivev2.0.0_user_interface umrr9d_t152_automotivev1.0.2_user_interface) rclcpp_components_register_node(smartmicro_radar_node diff --git a/umrr_ros2_driver/cmake/sensor_commands.hpp.in b/umrr_ros2_driver/cmake/sensor_commands.hpp.in new file mode 100644 index 0000000..c62e8ad --- /dev/null +++ b/umrr_ros2_driver/cmake/sensor_commands.hpp.in @@ -0,0 +1,31 @@ +// Copyright 2021 Apex.AI, Inc. +// +// 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. + +#ifndef UMRR_ROS2_DRIVER__CMAKE_SENSOR_COMMAND_PATH_H_ +#define UMRR_ROS2_DRIVER__CMAKE_SENSOR_COMMAND_PATH_H_ + +namespace smartmicro +{ +namespace drivers +{ +namespace radar +{ + +constexpr auto KSensorCommandFilePath ="@SENSOR_COMMAND_FILE_PATH@"; + +} // namespace radar +} // namespace drivers +} // namespace smartmicro + +#endif // UMRR_ROS2_DRIVER__CMAKE_SENSOR_COMMAND_PATH_H_ diff --git a/umrr_ros2_driver/config/sensor_commands.json b/umrr_ros2_driver/config/sensor_commands.json new file mode 100644 index 0000000..f8a9919 --- /dev/null +++ b/umrr_ros2_driver/config/sensor_commands.json @@ -0,0 +1,17 @@ +{ + "commands": + [ + { + "name": "comp_eeprom_ctrl_save_param_sec", + "comment": "Save the parameter inside the EEPROM. (3102.3)" + }, + { + "name": "comp_eeprom_ctrl_reset_param_sec", + "comment": "Restore default values in RAM. EEPROM content is not changed. (3102.2)" + }, + { + "name": "comp_eeprom_ctrl_default_param_sec", + "comment": "Restore default values in RAM and EEPROM. (3102.1)" + } + ] +} diff --git a/umrr_ros2_driver/config/sensor_params.json b/umrr_ros2_driver/config/sensor_params.json index 4fc590c..c930190 100644 --- a/umrr_ros2_driver/config/sensor_params.json +++ b/umrr_ros2_driver/config/sensor_params.json @@ -147,6 +147,12 @@ "min": 0, "max": 2, "default": 1 + }, + { + "model": "umrr9f_v2_0_0", + "min": 0, + "max": 2, + "default": 1 } ] }, 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 2b4d920..dd5a7b2 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 @@ -36,6 +37,7 @@ #include "umrr_ros2_msgs/srv/set_ip.hpp" #include "umrr_ros2_msgs/srv/set_mode.hpp" +#include "umrr_ros2_msgs/srv/send_command.hpp" namespace smartmicro { @@ -117,12 +119,27 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// @param[in] client_id The client_id of the sensor /// - void targetlist_callback_umrr9f( + void targetlist_callback_umrr9f_v1_1_1( const std::uint32_t sensor_idx, const std::shared_ptr< 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 + /// umrr9f 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_umrr9f_v2_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_0_0::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9f_v2_0_0, + const com::types::ClientId client_id); /// /// @brief A callback that is called when a new target list port for @@ -147,13 +164,21 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node void update_config_files_from_params(); /// - /// @brief Callaback for getting the instruction response. + /// @brief Callaback for getting the parameter response. /// - void sensor_response( + void mode_response( const com::types::ClientId client_id, const std::shared_ptr & response, const std::string instruction_name); + /// + /// @brief Callaback for getting the command response. + /// + void command_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, + const std::string command_name); + /// /// @brief Callback for changing IP address. /// @@ -175,8 +200,16 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + /// + /// @brief Send command to the sensor. + /// + void radar_command( + const std::shared_ptr request, + std::shared_ptr response); + rclcpp::Service::SharedPtr mode_srv_; rclcpp::Service::SharedPtr ip_addr_srv_; + rclcpp::Service::SharedPtr command_srv_; std::array m_sensors{}; @@ -197,7 +230,8 @@ bool check_signal = false; std::shared_ptr m_services{}; std::shared_ptr data_umrr11{}; std::shared_ptr data_umrr96{}; -std::shared_ptr data_umrr9f{}; +std::shared_ptr data_umrr9f_v1_1_1{}; +std::shared_ptr data_umrr9f_v2_0_0{}; std::shared_ptr data_umrr9d{}; } // namespace radar diff --git a/umrr_ros2_driver/launch/radar.launch.py b/umrr_ros2_driver/launch/radar.launch.py index 39f01ff..afbcc45 100644 --- a/umrr_ros2_driver/launch/radar.launch.py +++ b/umrr_ros2_driver/launch/radar.launch.py @@ -39,11 +39,13 @@ def generate_launch_description(): radar_node = Node( package=PACKAGE_NAME, executable='smartmicro_radar_node_exe', - parameters=[LaunchConfiguration('radar_param_file')], - name='smartmicro_radar_node' + name='smart_radar', + parameters=[LaunchConfiguration('radar_param_file')] ) return LaunchDescription([ radar_params, radar_node ]) + + diff --git a/umrr_ros2_driver/package.xml b/umrr_ros2_driver/package.xml index df0bc97..b24ee2f 100644 --- a/umrr_ros2_driver/package.xml +++ b/umrr_ros2_driver/package.xml @@ -2,7 +2,7 @@ umrr_ros2_driver - 3.2.1 + 4.0.0 A node to publish data from a smartmicro radar. s.m.s, smart microwave sensors GmbH. Apache 2.0 License diff --git a/umrr_ros2_driver/param/radar.template.yaml b/umrr_ros2_driver/param/radar.template.yaml index 55a2fa3..c8f3107 100644 --- a/umrr_ros2_driver/param/radar.template.yaml +++ b/umrr_ros2_driver/param/radar.template.yaml @@ -14,7 +14,8 @@ # As many as 10 sensors all named as "sensor_" in increasing order of numbers, # e.g., sensor_0, sensor_1, etc. The list must start with sensor_0. sensor_0: - # The model of the connected sensor. + # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # umrr9f_v1_1_1, umrr9f_v2_0_0 model: "umrr11" # The client_id of the sensor/source, must be a unique integer. id: 100 @@ -41,7 +42,7 @@ history_size: 10 sensor_2: # The model of the connected sensor. - model: "umrr9f" + model: "umrr9f_v1_1_1" # The client_id of the sensor/source, must be a unique integer. id: 300 # The ip address of the sensor or of the source acting as a sensor. diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 425ec41..ddc6fc6 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -27,6 +27,8 @@ #include #include #include +#include +#include #include @@ -46,6 +48,7 @@ #include "umrr_ros2_driver/config_path.hpp" #include "umrr_ros2_driver/sensor_params.hpp" +#include "umrr_ros2_driver/sensor_commands.hpp" using com::common::Instruction; using com::master::CmdRequest; @@ -142,7 +145,8 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option // Getting the data stream service 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_umrr9f_v1_1_1 = com::master::umrr9f_t169_automotive_v1_1_1::DataStreamServiceIface::Get(); + data_umrr9f_v2_0_0 = com::master::umrr9f_t169_automotive_v2_0_0::DataStreamServiceIface::Get(); data_umrr9d = com::master::umrr9d_t152_automotive_v1_0_2::DataStreamServiceIface::Get(); RCLCPP_INFO(this->get_logger(), "Data stream services have been received!"); @@ -170,13 +174,22 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option std::cout << "Failed to register targetlist callback for sensor umrr96" << std::endl; } if ( - sensor.model == "umrr9f" && + sensor.model == "umrr9f_v1_1_1" && com::types::ERROR_CODE_OK != - data_umrr9f->RegisterComTargetListPortReceiveCallback( + data_umrr9f_v1_1_1->RegisterComTargetListPortReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr9f, this, i, + &SmartmicroRadarNode::targetlist_callback_umrr9f_v1_1_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f" << std::endl; + std::cout << "Failed to register targetlist callback for sensor umrr9f_v1_1_1" << std::endl; + } + if ( + sensor.model == "umrr9f_v2_0_0" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_0_0->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_0_0, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_0_0" << std::endl; } if ( sensor.model == "umrr9d" && @@ -189,21 +202,26 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option } m_publishers[i] = create_publisher( - "umrr/targets_" + std::to_string(i), sensor.history_size); + "smart_radar/targets_" + std::to_string(i), sensor.history_size); } // create a ros2 service to change the radar parameters mode_srv_ = create_service( - "smartmicro_radar_node/set_radar_mode", + "smart_radar/set_radar_mode", std::bind( &SmartmicroRadarNode::radar_mode, this, std::placeholders::_1, std::placeholders::_2)); // create a ros2 service to change the IP address ip_addr_srv_ = create_service( - "smartmicro_radar_node/set_ip_address", + "smart_radar/set_ip_address", std::bind( &SmartmicroRadarNode::ip_address, this, std::placeholders::_1, std::placeholders::_2)); + // create a ros2 service to send command to radar + command_srv_ = create_service( + "smart_radar/send_command", + std::bind( + &SmartmicroRadarNode::radar_command, this, std::placeholders::_1, std::placeholders::_2)); RCLCPP_INFO(this->get_logger(), "Radar services are ready."); std::signal(SIGINT, [](int signal) { return terminate_on_receive(signal); }); @@ -285,7 +303,7 @@ void SmartmicroRadarNode::radar_mode( if ( com::types::ERROR_CODE_OK != inst->SendInstructionBatch( batch, std::bind( - &SmartmicroRadarNode::sensor_response, this, client_id, + &SmartmicroRadarNode::mode_response, this, client_id, std::placeholders::_2, instruction_name))) { result->res = "Check param is listed for sensor type and the min/max values!"; return; @@ -351,7 +369,80 @@ void SmartmicroRadarNode::ip_address( } } -void SmartmicroRadarNode::sensor_response( +void SmartmicroRadarNode::radar_command( + const std::shared_ptr request, + std::shared_ptr result) +{ + std::string command_name{}; + bool check_flag_command = false; + bool check_flag_id = false; + + std::ifstream command_file(KSensorCommandFilePath); + if (!command_file) + { + std::cout << "File is not present" << std::endl; + } + auto command_table = nlohmann::json::parse(command_file); + + command_name = request->command; + client_id = request->sensor_id; + + for (const auto & item : command_table.items()) { + for (const auto & command : item.value().items()) { + if (command_name == command.value()["name"]) { + check_flag_command = true; + break; + } + } + } + + if (!check_flag_command) { + result->res = "Invalid command name! "; + return; + } + + for (auto & sensor : m_sensors) { + if (client_id == sensor.id) { + check_flag_id = true; + break; + } + } + if (!check_flag_id) { + result->res = "The sensor ID value entered is invalid! "; + return; + } + + std::shared_ptr inst{m_services->GetInstructionService()}; + timer = this->create_wall_timer( + std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); + + std::shared_ptr batch; + + if (!inst->AllocateInstructionBatch(client_id, batch)) { + result->res = "Failed to allocate instruction batch! "; + return; + } + + std::shared_ptr radar_command = std::make_shared( + "auto_interface_command", request->command, 2010); + + if (!batch->AddRequest(radar_command)) { + result->res = "Failed to add instruction to the batch! "; + return; + } + + if ( + com::types::ERROR_CODE_OK != inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::command_response, this, client_id, + std::placeholders::_2, command_name))) { + result->res = "Error in sending command to the sensor!"; + return; + } + result->res = "Service conducted successfully"; +} + +void SmartmicroRadarNode::mode_response( const com::types::ClientId client_id, const std::shared_ptr & response, const std::string instruction_name) { @@ -387,6 +478,20 @@ void SmartmicroRadarNode::sensor_response_ip( } } +void SmartmicroRadarNode::command_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, const std::string command_name) +{ + std::vector>> command_resp; + if (response->GetResponse("auto_interface_command", command_name.c_str(), command_resp)) + { + for (auto & resp : command_resp) { + response_type = resp->GetResponseType(); + RCLCPP_INFO(this->get_logger(), "Response from sensor to command: %i", response_type); + } + } +} + void SmartmicroRadarNode::targetlist_callback_umrr11( const std::uint32_t sensor_idx, const std::shared_ptr< @@ -459,19 +564,55 @@ void SmartmicroRadarNode::targetlist_callback_umrr96( } } -void SmartmicroRadarNode::targetlist_callback_umrr9f( +void SmartmicroRadarNode::targetlist_callback_umrr9f_v1_1_1( const std::uint32_t sensor_idx, const std::shared_ptr< com::master::umrr9f_t169_automotive_v1_1_1::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9f, + targetlist_port_umrr9f_v1_1_1, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9f" << std::endl; + std::cout << "Targetlist callback is being called for umrr9f_v1_1_1" << std::endl; if (!check_signal) { std::shared_ptr< com::master::umrr9f_t169_automotive_v1_1_1::comtargetlistport::GenericPortHeader> port_header; - port_header = targetlist_port_umrr9f->GetGenericPortHeader(); + port_header = targetlist_port_umrr9f_v1_1_1->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_umrr9f_v1_1_1->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(); + const auto snr = target->GetPower() - target->GetTgtNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRCS(), target->GetTgtNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_0_0::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9f_v2_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist callback is being called for umrr9f_v2_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_0_0::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrr9f_v2_0_0->GetGenericPortHeader(); sensor_msgs::msg::PointCloud2 msg; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; @@ -479,7 +620,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f( 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_umrr9f->GetTargetList()) { + for (const auto & target : targetlist_port_umrr9f_v2_0_0->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); diff --git a/umrr_ros2_driver/test/radar_node_test.launch.py b/umrr_ros2_driver/test/radar_node_test.launch.py index b863fd7..230a3b2 100644 --- a/umrr_ros2_driver/test/radar_node_test.launch.py +++ b/umrr_ros2_driver/test/radar_node_test.launch.py @@ -36,7 +36,7 @@ def generate_test_description(): radar_node = Node( package=PACKAGE_NAME, executable='smartmicro_radar_node_exe', - name='smartmicro_radar_node', + name='smart_radar', output='screen', parameters=[ os.path.join(get_package_share_directory(PACKAGE_NAME), @@ -47,7 +47,7 @@ def generate_test_description(): frequency_sweep_service = ExecuteProcess( cmd = [[ 'ros2 service call ', - '/smartmicro_radar_node/set_radar_mode ', + '/smart_radar/set_radar_mode ', 'umrr_ros2_msgs/srv/SetMode ', '"{param: "frequency_sweep_idx", value: 1, sensor_id: 200}"' ]], @@ -58,7 +58,7 @@ def generate_test_description(): angular_separation_service = ExecuteProcess( cmd = [[ 'ros2 service call ', - '/smartmicro_radar_node/set_radar_mode ', + '/smart_radar/set_radar_mode ', 'umrr_ros2_msgs/srv/SetMode ', '"{param: "angular_separation", value: 1, sensor_id: 100}"' ]], @@ -69,7 +69,7 @@ def generate_test_description(): range_toggle_mode_service = ExecuteProcess( cmd = [[ 'ros2 service call ', - '/smartmicro_radar_node/set_radar_mode ', + '/smart_radar/set_radar_mode ', 'umrr_ros2_msgs/srv/SetMode ', '"{param: "range_toggle_mode", value: 4, sensor_id: 300}"' ]], @@ -80,7 +80,7 @@ def generate_test_description(): center_frequency_idx_service = ExecuteProcess( cmd = [[ 'ros2 service call ', - '/smartmicro_radar_node/set_radar_mode ', + '/smart_radar/set_radar_mode ', 'umrr_ros2_msgs/srv/SetMode ', '"{param: "center_frequency_idx", value: 1, sensor_id: 400}"' ]], @@ -142,19 +142,19 @@ def data_rx_s3_callback(msg): sub_s1 = self.test_node.create_subscription( sensor_msgs.PointCloud2, - '/umrr/targets_0', + '/smart_radar/targets_0', data_rx_s1_callback, 10 ) sub_s2 = self.test_node.create_subscription( sensor_msgs.PointCloud2, - '/umrr/targets_1', + '/smart_radar/targets_1', data_rx_s2_callback, 10 ) sub_s3 = self.test_node.create_subscription( sensor_msgs.PointCloud2, - '/umrr/targets_2', + '/smart_radar/targets_2', data_rx_s3_callback, 10 ) diff --git a/umrr_ros2_msgs/CMakeLists.txt b/umrr_ros2_msgs/CMakeLists.txt index 753c363..23f66f7 100644 --- a/umrr_ros2_msgs/CMakeLists.txt +++ b/umrr_ros2_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetMode.srv" "srv/SetIp.srv" + "srv/SendCommand.srv" ) ament_export_dependencies(rosidl_default_runtime) diff --git a/umrr_ros2_msgs/srv/SendCommand.srv b/umrr_ros2_msgs/srv/SendCommand.srv new file mode 100644 index 0000000..f0446e0 --- /dev/null +++ b/umrr_ros2_msgs/srv/SendCommand.srv @@ -0,0 +1,4 @@ +string command +uint32 sensor_id +--- +string res