From b0a5d34482350f81be3f63d005e000647d7eb63b Mon Sep 17 00:00:00 2001 From: smartSRA Date: Mon, 30 Jan 2023 17:01:27 +0100 Subject: [PATCH 1/5] Include new T169 uiface New nodes for the new T169 uiface and additional node service for saving mode changes --- CHANGELOG.md | 4 + Readme.md | 17 +- smart_extract.sh | 2 +- umrr_ros2_driver/CMakeLists.txt | 7 + umrr_ros2_driver/cmake/sensor_commands.hpp.in | 31 ++++ umrr_ros2_driver/config/sensor_commands.json | 17 ++ umrr_ros2_driver/config/sensor_params.json | 30 ++-- .../umrr_ros2_driver/sensor_commands.hpp | 31 ++++ .../smartmicro_radar_node.hpp | 42 ++++- umrr_ros2_driver/launch/radar.launch.py | 6 +- umrr_ros2_driver/package.xml | 2 +- umrr_ros2_driver/param/radar.template.yaml | 2 +- .../src/smartmicro_radar_node.cpp | 170 ++++++++++++++++-- .../test/radar_node_test.launch.py | 16 +- umrr_ros2_msgs/CMakeLists.txt | 1 + umrr_ros2_msgs/srv/SendCommand.srv | 4 + 16 files changed, 334 insertions(+), 48 deletions(-) create mode 100644 umrr_ros2_driver/cmake/sensor_commands.hpp.in create mode 100644 umrr_ros2_driver/config/sensor_commands.json create mode 100644 umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp create mode 100644 umrr_ros2_msgs/srv/SendCommand.srv diff --git a/CHANGELOG.md b/CHANGELOG.md index 048a45b..94ab165 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-01 + +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. \ No newline at end of file diff --git a/Readme.md b/Readme.md index 0d4fae8..5ff7006 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 01, 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 @@ -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..db2d661 --- /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)" + } + ] +} \ No newline at end of file diff --git a/umrr_ros2_driver/config/sensor_params.json b/umrr_ros2_driver/config/sensor_params.json index 4fc590c..f9096ec 100644 --- a/umrr_ros2_driver/config/sensor_params.json +++ b/umrr_ros2_driver/config/sensor_params.json @@ -17,7 +17,7 @@ "default": 0 }, { - "model": "umrr9f", + "model": "umrr9f_v1_1_1", "min": 0, "max": 2, "default": 0 @@ -41,7 +41,7 @@ "default": 0 }, { - "model": "umrr9f", + "model": "umrr9f_v1_1_1", "min": 1, "max": 2, "default": 1 @@ -72,7 +72,7 @@ "default": 2 }, { - "model": "umrr9f", + "model": "umrr9f_v1_1_1", "comment": "0=226MHz, 1=512MHz, 2=1536MHz, 3=3072MHz", "min": 0, "max": 3, @@ -123,7 +123,7 @@ "default": 0 }, { - "model": "umrr9f", + "model": "umrr9f_v1_1_1", "comment": " 0=off, 1=Short-Med, 2=Short-Long, 3=Med-Long, 4=Long-UltraShort, 5=Medium-UltraShort, 6=Short-UltraShort", "min": 0, "max": 6, @@ -147,6 +147,12 @@ "min": 0, "max": 2, "default": 1 + }, + { + "model": "umrr9f_v2_0_0", + "min": 0, + "max": 2, + "default": 1 } ] }, @@ -161,7 +167,7 @@ "default": -20 }, { - "model": "umrr9f", + "model": "umrr9f_v1_1_1", "min": -150.0, "max": 150.0, "default": -20 @@ -185,7 +191,7 @@ "default": -20 }, { - "model": "umrr9f", + "model": "umrr9f_v1_1_1", "min": -150.0, "max": 150.0, "default": -20 @@ -209,7 +215,7 @@ "default": -20 }, { - "model": "umrr9f", + "model": "umrr9f_v1_1_1", "min": -150.0, "max": 150.0, "default": -20 @@ -227,7 +233,7 @@ "comment": "Target Validation: minimum speed of target at fourth sweep", "sensors": [ { - "model": "umrr9f", + "model": "umrr9f_v1_1_1", "min": -150.0, "max": 150.0 }, @@ -250,7 +256,7 @@ "default": 20 }, { - "model": "umrr9f", + "model": "umrr9f_v1_1_1", "min": -150.0, "max": 150.0, "default": 20 @@ -274,7 +280,7 @@ "default": 20 }, { - "model": "umrr9f", + "model": "umrr9f_v1_1_1", "min": -150.0, "max": 150.0, "default": 20 @@ -298,7 +304,7 @@ "default": 20 }, { - "model": "umrr9f", + "model": "umrr9f_v1_1_1", "min": -150.0, "max": 150.0, "default": 20 @@ -316,7 +322,7 @@ "comment": "Target Validation: maximum speed of target at fourth sweep", "sensors": [ { - "model": "umrr9f", + "model": "umrr9f_v1_1_1", "min": -150.0, "max": 150.0 }, diff --git a/umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp b/umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp new file mode 100644 index 0000000..f0fdf50 --- /dev/null +++ b/umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp @@ -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 ="/code/umrr_ros2_driver/config/sensor_commands.json"; + +} // namespace radar +} // namespace drivers +} // namespace smartmicro + +#endif // UMRR_ROS2_DRIVER__CMAKE_SENSOR_COMMAND_PATH_H_ 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..e80298d 100644 --- a/umrr_ros2_driver/param/radar.template.yaml +++ b/umrr_ros2_driver/param/radar.template.yaml @@ -41,7 +41,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..dc5137a 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,79 @@ 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! "; + } + + if ( + com::types::ERROR_CODE_OK != inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::command_response, this, client_id, + std::placeholders::_2, command_name))) { + result->res = "Check command is listed!"; + 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 +477,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 +563,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 +619,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..4800cb1 --- /dev/null +++ b/umrr_ros2_msgs/srv/SendCommand.srv @@ -0,0 +1,4 @@ +string command +uint32 sensor_id +--- +string res \ No newline at end of file From 0c0a963a9ac1a103588a1f9c48db8551364a9650 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Mon, 30 Jan 2023 17:16:26 +0100 Subject: [PATCH 2/5] Common configurations for T169 v1 and v2 --- umrr_ros2_driver/config/sensor_params.json | 24 +++++++++++----------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/umrr_ros2_driver/config/sensor_params.json b/umrr_ros2_driver/config/sensor_params.json index f9096ec..c930190 100644 --- a/umrr_ros2_driver/config/sensor_params.json +++ b/umrr_ros2_driver/config/sensor_params.json @@ -17,7 +17,7 @@ "default": 0 }, { - "model": "umrr9f_v1_1_1", + "model": "umrr9f", "min": 0, "max": 2, "default": 0 @@ -41,7 +41,7 @@ "default": 0 }, { - "model": "umrr9f_v1_1_1", + "model": "umrr9f", "min": 1, "max": 2, "default": 1 @@ -72,7 +72,7 @@ "default": 2 }, { - "model": "umrr9f_v1_1_1", + "model": "umrr9f", "comment": "0=226MHz, 1=512MHz, 2=1536MHz, 3=3072MHz", "min": 0, "max": 3, @@ -123,7 +123,7 @@ "default": 0 }, { - "model": "umrr9f_v1_1_1", + "model": "umrr9f", "comment": " 0=off, 1=Short-Med, 2=Short-Long, 3=Med-Long, 4=Long-UltraShort, 5=Medium-UltraShort, 6=Short-UltraShort", "min": 0, "max": 6, @@ -167,7 +167,7 @@ "default": -20 }, { - "model": "umrr9f_v1_1_1", + "model": "umrr9f", "min": -150.0, "max": 150.0, "default": -20 @@ -191,7 +191,7 @@ "default": -20 }, { - "model": "umrr9f_v1_1_1", + "model": "umrr9f", "min": -150.0, "max": 150.0, "default": -20 @@ -215,7 +215,7 @@ "default": -20 }, { - "model": "umrr9f_v1_1_1", + "model": "umrr9f", "min": -150.0, "max": 150.0, "default": -20 @@ -233,7 +233,7 @@ "comment": "Target Validation: minimum speed of target at fourth sweep", "sensors": [ { - "model": "umrr9f_v1_1_1", + "model": "umrr9f", "min": -150.0, "max": 150.0 }, @@ -256,7 +256,7 @@ "default": 20 }, { - "model": "umrr9f_v1_1_1", + "model": "umrr9f", "min": -150.0, "max": 150.0, "default": 20 @@ -280,7 +280,7 @@ "default": 20 }, { - "model": "umrr9f_v1_1_1", + "model": "umrr9f", "min": -150.0, "max": 150.0, "default": 20 @@ -304,7 +304,7 @@ "default": 20 }, { - "model": "umrr9f_v1_1_1", + "model": "umrr9f", "min": -150.0, "max": 150.0, "default": 20 @@ -322,7 +322,7 @@ "comment": "Target Validation: maximum speed of target at fourth sweep", "sensors": [ { - "model": "umrr9f_v1_1_1", + "model": "umrr9f", "min": -150.0, "max": 150.0 }, From 28780cce55253e3be810df75d287287cf968b522 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Wed, 1 Feb 2023 11:45:23 +0100 Subject: [PATCH 3/5] Add missing return statement --- umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp | 2 +- umrr_ros2_driver/src/smartmicro_radar_node.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp b/umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp index f0fdf50..d09d538 100644 --- a/umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp +++ b/umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp @@ -22,7 +22,7 @@ namespace drivers namespace radar { -constexpr auto KSensorCommandFilePath ="/code/umrr_ros2_driver/config/sensor_commands.json"; +constexpr auto KSensorCommandFilePath ="/home/shah/ROS2_OFFIICAL_WS/src/smartmicro_ros2_radars/umrr_ros2_driver/config/sensor_commands.json"; } // namespace radar } // namespace drivers diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index dc5137a..ddc6fc6 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -428,6 +428,7 @@ void SmartmicroRadarNode::radar_command( if (!batch->AddRequest(radar_command)) { result->res = "Failed to add instruction to the batch! "; + return; } if ( @@ -435,7 +436,7 @@ void SmartmicroRadarNode::radar_command( batch, std::bind( &SmartmicroRadarNode::command_response, this, client_id, std::placeholders::_2, command_name))) { - result->res = "Check command is listed!"; + result->res = "Error in sending command to the sensor!"; return; } result->res = "Service conducted successfully"; From 9c86d69bd2b45f5253f25004f9fee7d2fd0f4417 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Mon, 6 Feb 2023 14:21:29 +0100 Subject: [PATCH 4/5] Add models in Readme and template Update gitognore list and remove configured file --- .gitignore | 1 + CHANGELOG.md | 2 +- Readme.md | 2 +- umrr_ros2_driver/config/sensor_commands.json | 2 +- .../umrr_ros2_driver/sensor_commands.hpp | 31 ------------------- umrr_ros2_driver/param/radar.template.yaml | 3 +- umrr_ros2_msgs/srv/SendCommand.srv | 2 +- 7 files changed, 7 insertions(+), 36 deletions(-) delete mode 100644 umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp 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 94ab165..41ade95 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -34,4 +34,4 @@ This release fixes the offset which causes anomaly in pointclouds from DRVEGRD 1 ## v4.0.0 - 2023-02-01 -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. \ No newline at end of file +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 5ff7006..c6716d0 100644 --- a/Readme.md +++ b/Readme.md @@ -74,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 diff --git a/umrr_ros2_driver/config/sensor_commands.json b/umrr_ros2_driver/config/sensor_commands.json index db2d661..f8a9919 100644 --- a/umrr_ros2_driver/config/sensor_commands.json +++ b/umrr_ros2_driver/config/sensor_commands.json @@ -14,4 +14,4 @@ "comment": "Restore default values in RAM and EEPROM. (3102.1)" } ] -} \ No newline at end of file +} diff --git a/umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp b/umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp deleted file mode 100644 index d09d538..0000000 --- a/umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp +++ /dev/null @@ -1,31 +0,0 @@ -// 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 ="/home/shah/ROS2_OFFIICAL_WS/src/smartmicro_ros2_radars/umrr_ros2_driver/config/sensor_commands.json"; - -} // namespace radar -} // namespace drivers -} // namespace smartmicro - -#endif // UMRR_ROS2_DRIVER__CMAKE_SENSOR_COMMAND_PATH_H_ diff --git a/umrr_ros2_driver/param/radar.template.yaml b/umrr_ros2_driver/param/radar.template.yaml index e80298d..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 diff --git a/umrr_ros2_msgs/srv/SendCommand.srv b/umrr_ros2_msgs/srv/SendCommand.srv index 4800cb1..f0446e0 100644 --- a/umrr_ros2_msgs/srv/SendCommand.srv +++ b/umrr_ros2_msgs/srv/SendCommand.srv @@ -1,4 +1,4 @@ string command uint32 sensor_id --- -string res \ No newline at end of file +string res From 38c821747b3b366da5d8716ae5fbaf2511733017 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Mon, 6 Feb 2023 15:10:17 +0100 Subject: [PATCH 5/5] Update release date --- CHANGELOG.md | 2 +- Readme.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 41ade95..36c78a0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -32,6 +32,6 @@ This minor release introduces signal-to-noise field in the pointclouds and also 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-01 +## 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 c6716d0..8976d81 100644 --- a/Readme.md +++ b/Readme.md @@ -26,7 +26,7 @@ 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: `February 01, 2023` +- 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`