From d468a28438ec19ea302a9fc05b59a8b89714217d Mon Sep 17 00:00:00 2001 From: smartSRA Date: Mon, 19 Jun 2023 10:16:53 +0200 Subject: [PATCH 01/15] Driver adopted to include CAN support Separate param files for adapters and sensors. Callbacks for CAN interface included and older port callbacks adopted to new port interfaces. Launch file adopted to launch node with multiple parameter files. Smart access configs adapoted to offer CAN and port based communication. New params included for the node to be declared. --- umrr_ros2_driver/CMakeLists.txt | 37 +- .../cmake/smart_access_config.json.in | 4 +- umrr_ros2_driver/config/hw_inventory.json | 13 +- umrr_ros2_driver/config/routing_table.json | 15 +- .../smartmicro_radar_node.hpp | 209 ++-- umrr_ros2_driver/launch/radar.launch.py | 21 +- .../param/radar.adapter.template.yaml | 31 + .../param/radar.sensor.template.yaml | 122 +++ umrr_ros2_driver/param/radar.template.yaml | 127 ++- .../src/smartmicro_radar_node.cpp | 922 +++++++++--------- 10 files changed, 890 insertions(+), 611 deletions(-) create mode 100644 umrr_ros2_driver/param/radar.adapter.template.yaml create mode 100644 umrr_ros2_driver/param/radar.sensor.template.yaml diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index 6820363..87a0450 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -44,20 +44,21 @@ if(NOT json_POPULATED) endif() ament_auto_find_build_dependencies() +set(SMARTMICRO_LIB_DIR "lib-linux-x86_64-gcc_7") # specify the path to search for the libraries -link_directories(${CMAKE_CURRENT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9) +link_directories(${CMAKE_CURRENT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}) install(FILES - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libsmart_access.so" - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libosal.so" - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libcom_lib.so" - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libbasev1.0.2_user_interface.so" - "${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" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libsmart_access.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libosal.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libcom_lib.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libbasev1.0.2_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr11_t132_automotivev1.1.2_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr96_t153_automotivev1.2.2_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev1.1.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.1.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.0.3_user_interface.so" DESTINATION lib) set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") @@ -83,11 +84,11 @@ ament_auto_add_library(smartmicro_radar_node SHARED "src/smartmicro_radar_node.c target_include_directories(smartmicro_radar_node PUBLIC ${CMAKE_INSTALL_PREFIX}/include/ smartmicro/include/ -smartmicro/include/umrr11_t132_automotive_v1_1_1 -smartmicro/include/umrr96_t153_automotive_v1_2_1 +smartmicro/include/umrr11_t132_automotive_v1_1_2 +smartmicro/include/umrr96_t153_automotive_v1_2_2 smartmicro/include/umrr9f_t169_automotive_v1_1_1 -smartmicro/include/umrr9f_t169_automotive_v2_0_0 -smartmicro/include/umrr9d_t152_automotive_v1_0_2) +smartmicro/include/umrr9f_t169_automotive_v2_1_1 +smartmicro/include/umrr9d_t152_automotive_v1_0_3) # link smart_access_lib-linux-x86_64-gcc_9 to the node target_link_libraries(smartmicro_radar_node @@ -95,11 +96,11 @@ target_link_libraries(smartmicro_radar_node nlohmann_json::nlohmann_json com_lib osal - umrr11_t132_automotivev1.1.1_user_interface - umrr96_t153_automotivev1.2.1_user_interface + umrr11_t132_automotivev1.1.2_user_interface + umrr96_t153_automotivev1.2.2_user_interface umrr9f_t169_automotivev1.1.1_user_interface - umrr9f_t169_automotivev2.0.0_user_interface - umrr9d_t152_automotivev1.0.2_user_interface) + umrr9f_t169_automotivev2.1.1_user_interface + umrr9d_t152_automotivev1.0.3_user_interface) rclcpp_components_register_node(smartmicro_radar_node PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" diff --git a/umrr_ros2_driver/cmake/smart_access_config.json.in b/umrr_ros2_driver/cmake/smart_access_config.json.in index 2b566d0..644827d 100644 --- a/umrr_ros2_driver/cmake/smart_access_config.json.in +++ b/umrr_ros2_driver/cmake/smart_access_config.json.in @@ -10,7 +10,7 @@ "user_interface_minor_v": 0, "user_interface_patch_v": 2, "download_path": "", - "instruction_serialization_type": "port_based", - "data_serialization_type": "port_based", + "instruction_serialization_type": "can_based", + "data_serialization_type": "can_based", "alive": false } diff --git a/umrr_ros2_driver/config/hw_inventory.json b/umrr_ros2_driver/config/hw_inventory.json index cae6c8f..8e6627c 100644 --- a/umrr_ros2_driver/config/hw_inventory.json +++ b/umrr_ros2_driver/config/hw_inventory.json @@ -1,12 +1,9 @@ { - "name": "HW Inventory List", - "version": "1.1.0", "hwItems": [ { - "type": "eth", - "dev_id": 2, - "iface_name": "eth0", - "port": 55555 + } - ] -} + ], + "name": "HW Inventory List", + "version": "1.1.0" +} \ No newline at end of file diff --git a/umrr_ros2_driver/config/routing_table.json b/umrr_ros2_driver/config/routing_table.json index 28d5870..96643cc 100644 --- a/umrr_ros2_driver/config/routing_table.json +++ b/umrr_ros2_driver/config/routing_table.json @@ -1,14 +1,9 @@ { - "name": "Client Routing Table", - "version": "1.0.0", "clients": [ { - "client_id": 111, - "link_type": "eth", - "ip": "172.22.10.101", - "port": 55555, - "instruction_serialization_type": "port_based", - "data_serialization_type": "port_based" + "can_network_id": 0 } - ] -} + ], + "name": "Client Routing Table", + "version": "1.0.0" +} \ No newline at end of file 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 dd5a7b2..d6f69d6 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 @@ -17,7 +17,6 @@ #ifndef UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ #define UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ - #include #include #include @@ -25,59 +24,71 @@ #include #include -#include -#include +#include +#include +#include #include -#include -#include +#include #include #include #include +#include "umrr_ros2_msgs/srv/send_command.hpp" #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 -{ -namespace drivers -{ -namespace radar -{ -namespace detail -{ +namespace smartmicro { +namespace drivers { +namespace radar { +namespace detail { constexpr auto kMaxSensorCount = 10UL; +constexpr auto kMaxHwCount = 6UL; -struct SensorConfig -{ +struct SensorConfig { std::uint32_t id{}; - std::string ip{}; - std::uint32_t port{}; + std::uint32_t dev_id{}; std::string frame_id{}; std::uint32_t history_size{}; + std::string ip{}; + std::string link_type{}; std::string model{}; + std::uint32_t port{}; + std::string inst_type{}; + std::string data_type{}; + std::string uifname{}; + std::uint32_t uifmajorv{}; + std::uint32_t uifminorv{}; + std::uint32_t uifpatchv{}; +}; + +struct HWConfig { + std::uint32_t hw_dev_id{}; + std::string hw_iface_name{}; + std::string hw_type{}; + std::uint32_t baudrate{}; + std::uint32_t port{}; }; -} // namespace detail +} // namespace detail /// /// @brief The class for the Smartmicro radar node. /// -class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node -{ +class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { public: /// /// ROS 2 parameter constructor. /// /// @param[in] node_options Node options for this node. /// - explicit SmartmicroRadarNode(const rclcpp::NodeOptions & node_options); - + explicit SmartmicroRadarNode(const rclcpp::NodeOptions &node_options); + protected: /// /// @brief A timer to handle the services. /// void my_timer_callback() { timer->cancel(); } + void on_shutdown_callback(); private: /// @@ -89,11 +100,11 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// @param[in] client_id The client_id of the sensor /// void targetlist_callback_umrr11( - const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr11_t132_automotive_v1_1_1::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr11, - const com::types::ClientId client_id); + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_port_umrr11, + const com::types::ClientId client_id); /// /// @brief A callback that is called when a new target list port for @@ -104,11 +115,33 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// @param[in] client_id The client_id of the sensor /// void targetlist_callback_umrr96( - const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr96_t153_automotive_v1_2_1::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr96, - const com::types::ClientId client_id); + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_port_umrr96, + const com::types::ClientId client_id); + + void CAN_targetlist_callback_umrr96( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_can_umrr96, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr96 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 CAN_targetlist_callback_umrr11( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_can_umrr11, + const com::types::ClientId client_id); /// /// @brief A callback that is called when a new target list port for @@ -120,11 +153,11 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// 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); + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_port_umrr9f, + const com::types::ClientId client_id); /// /// @brief A callback that is called when a new target list port for /// umrr9f arrives. @@ -134,13 +167,13 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// @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); - + void targetlist_callback_umrr9f_v2_1_1( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_port_umrr9f_v2_1_1, + const com::types::ClientId client_id); + /// /// @brief A callback that is called when a new target list port for /// umrr9d arrives. @@ -150,13 +183,28 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// @param[in] client_id The client_id of the sensor /// + void CAN_targetlist_callback_umrr9d( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_can_umrr9d, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr96 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_2::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9d, - const com::types::ClientId client_id); - + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_port_umrr9d, + const com::types::ClientId client_id); + /// /// @brief Read parameters and update the json config files required by /// Smart Access C++ API. @@ -166,56 +214,59 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// /// @brief Callaback for getting the parameter response. /// - void mode_response( - const com::types::ClientId client_id, - const std::shared_ptr & response, - const std::string instruction_name); + 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); + 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. /// void sensor_response_ip( - const com::types::ClientId client_id, - const std::shared_ptr & response); + const com::types::ClientId client_id, + const std::shared_ptr &response); /// /// @brief Send instructions to the sensor. /// void radar_mode( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); /// /// @brief Configure the sensor ip address. /// - void ip_address( - const std::shared_ptr request, - std::shared_ptr response); + void + ip_address(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); - + 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{}; + std::array m_adapters{}; - std::array::SharedPtr, detail::kMaxSensorCount> - m_publishers{}; + std::array::SharedPtr, + detail::kMaxSensorCount> + m_publishers{}; std::size_t m_number_of_sensors{}; + std::size_t m_number_of_adapters{}; rclcpp::TimerBase::SharedPtr timer; com::types::ClientId client_id; std::uint64_t response_type{}; @@ -228,14 +279,14 @@ void terminate_on_receive(int signal); bool check_signal = false; std::shared_ptr m_services{}; -std::shared_ptr data_umrr11{}; -std::shared_ptr data_umrr96{}; +std::shared_ptr data_umrr11{}; +std::shared_ptr data_umrr96{}; std::shared_ptr data_umrr9f_v1_1_1{}; -std::shared_ptr data_umrr9f_v2_0_0{}; -std::shared_ptr data_umrr9d{}; +std::shared_ptr data_umrr9f_v2_1_1{}; +std::shared_ptr data_umrr9d{}; -} // namespace radar -} // namespace drivers -} // namespace smartmicro +} // namespace radar +} // namespace drivers +} // namespace smartmicro -#endif // UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ +#endif // UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ diff --git a/umrr_ros2_driver/launch/radar.launch.py b/umrr_ros2_driver/launch/radar.launch.py index afbcc45..92a6471 100644 --- a/umrr_ros2_driver/launch/radar.launch.py +++ b/umrr_ros2_driver/launch/radar.launch.py @@ -27,24 +27,21 @@ def generate_launch_description(): """Generate the launch description.""" - radar_param_file = os.path.join( - get_package_share_directory(PACKAGE_NAME), 'param/radar.template.yaml') - - radar_params = DeclareLaunchArgument( - 'radar_param_file', - default_value=radar_param_file, - description='Path to param file for the radar node.' - ) + + radar_sensor_params = os.path.join( + get_package_share_directory(PACKAGE_NAME), 'param/radar.sensor.template.yaml') + + radar_adapter_params = os.path.join( + get_package_share_directory(PACKAGE_NAME), 'param/radar.adapter.template.yaml') radar_node = Node( package=PACKAGE_NAME, executable='smartmicro_radar_node_exe', - name='smart_radar', - parameters=[LaunchConfiguration('radar_param_file')] + name='smarty_pants', + parameters=[radar_sensor_params, radar_adapter_params] ) - + return LaunchDescription([ - radar_params, radar_node ]) diff --git a/umrr_ros2_driver/param/radar.adapter.template.yaml b/umrr_ros2_driver/param/radar.adapter.template.yaml new file mode 100644 index 0000000..ea0a624 --- /dev/null +++ b/umrr_ros2_driver/param/radar.adapter.template.yaml @@ -0,0 +1,31 @@ +--- +/**: + ros__parameters: + # This sets the iface_name in the hw_inventory.json + # sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx + # sudo ip link set up slcan0 + # One sensor per interface + master_inst_serial_type: "can_based" + master_data_serial_type: "can_based" + adapters: + adapter_0: + hw_type: "can" + hw_dev_id: 2 + hw_iface_name: "slcan0" + baudrate: 500000 + adapter_1: + hw_type: "eth" + hw_dev_id: 3 + hw_iface_name: "enx002432163c8d" + port: 55555 + adapter_2: + hw_type: "can" + hw_dev_id: 4 + hw_iface_name: "slcan1" + baudrate: 500000 + #adapter_3: + # hw_type: "can" + # hw_dev_id: 5 + # hw_iface_name: "can0" + # baudrate: 500000 + diff --git a/umrr_ros2_driver/param/radar.sensor.template.yaml b/umrr_ros2_driver/param/radar.sensor.template.yaml new file mode 100644 index 0000000..0e618a3 --- /dev/null +++ b/umrr_ros2_driver/param/radar.sensor.template.yaml @@ -0,0 +1,122 @@ +--- +/**: + ros__parameters: +# An array of sensors to subscribe to. + sensors: + # 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. Available models umrr11, umrr9d, umrr96, + # # umrr9f_v1_1_1, umrr9f_v2_0_0 + # link_type: "can" + # model: "umrr96_can" + # # The client_id of the sensor/source, must be a unique integer. + # id: 500 + # # Adapter id to which sensor is connected + # dev_id: 2 + # # The interface name of the sensor + # uifname: "umrr96_t153_automotive" + # # The major version of the interface + # uifmajorv: 1 + # # The minor version of the interface + # uifminorv: 2 + # # The pathc version of the interface + # uifpatchv: 2 + # # The frame_id to be set to the published messages. + # frame_id: "umrr" + # # Specify the history size. + # history_size: 10 + sensor_0: + # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # umrr9f_v1_1_1, umrr9f_v2_0_0 + link_type: "can" + model: "umrr11_can" + # The client_id of the sensor/source, must be a unique integer. + id: 600 + # Adapter id to which sensor is connected + dev_id: 4 + # The interface name of the sensor + uifname: "umrr11_t132_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 2 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + sensor_1: + link_type: "eth" + # The model of the connected sensor. + model: "umrr9f_v2_1_1" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 700 + # The ip address of the sensor or of the source acting as a sensor. + ip: "192.168.11.11" + # 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 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr9f_t169_automotive" + # The major version of the interface + uifmajorv: 2 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 1 + sensor_2: + link_type: "eth" + # The model of the connected sensor. + model: "umrr11" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 800 + # The ip address of the sensor or of the source acting as a sensor. + ip: "192.168.11.153" + # 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 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr11_t132_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 2 + # sensor_3: + # # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # # umrr9f_v1_1_1, umrr9f_v2_0_0 + # link_type: "can" + # model: "umrr9d_can" + # # The client_id of the sensor/source, must be a unique integer. + # id: 900 + # # Adapter id to which sensor is connected + # dev_id: 5 + # # The interface name of the sensor + # uifname: "umrr9d_t152_automotive" + # # The major version of the interface + # uifmajorv: 1 + # # The minor version of the interface + # uifminorv: 0 + # # The pathc version of the interface + # uifpatchv: 3 + # # The frame_id to be set to the published messages. + # frame_id: "umrr" + # # Specify the history size. + # history_size: 10 \ No newline at end of file diff --git a/umrr_ros2_driver/param/radar.template.yaml b/umrr_ros2_driver/param/radar.template.yaml index c8f3107..3faeaf5 100644 --- a/umrr_ros2_driver/param/radar.template.yaml +++ b/umrr_ros2_driver/param/radar.template.yaml @@ -1,14 +1,33 @@ --- /**: ros__parameters: - # The client_id to be set in smart_access_config.json, must be a unique integer. - master_client_id: 1000 - # This sets the dev_id in the hw_inventory.json - hw_dev_id: 2 - # This sets the port in the hw_inventory.json - hw_port: 55555 # This sets the iface_name in the hw_inventory.json - hw_iface_name: "eth0" + # sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx + # sudo ip link set up slcan0 + # One sensor per interface + master_inst_serial_type: "can_based" + master_data_serial_type: "can_based" + adapters: + adapter_0: + hw_type: "can" + hw_dev_id: 2 + hw_iface_name: "slcan0" + baudrate: 500000 + adapter_1: + hw_type: "eth" + hw_dev_id: 3 + hw_iface_name: "enx002432163c8d" + port: 55555 + adapter_2: + hw_type: "can" + hw_dev_id: 4 + hw_iface_name: "slcan1" + baudrate: 500000 + #adapter_3: + # hw_type: "can" + # hw_dev_id: 5 + # hw_iface_name: "slcan2" + # baudrate: 2500000 # An array of sensors to subscribe to. sensors: # As many as 10 sensors all named as "sensor_" in increasing order of numbers, @@ -16,53 +35,89 @@ sensor_0: # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, # umrr9f_v1_1_1, umrr9f_v2_0_0 - model: "umrr11" + link_type: "can" + model: "umrr96_can" # The client_id of the sensor/source, must be a unique integer. - id: 100 - # The ip address of the sensor or of the source acting as a sensor. - ip: "172.22.10.101" - # The port to be used. - port: 55555 + id: 200 + # Adapter id to which sensor is connected + dev_id: 2 + # The interface name of the sensor + uifname: "umrr96_t153_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 2 + # The pathc version of the interface + uifpatchv: 2 # The frame_id to be set to the published messages. frame_id: "umrr" # Specify the history size. history_size: 10 sensor_1: - # The model of the connected sensor. - model: "umrr96" + # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # umrr9f_v1_1_1, umrr9f_v2_0_0 + link_type: "can" + model: "umrr11_can" # The client_id of the sensor/source, must be a unique integer. - id: 200 - # The ip address of the sensor or of the source acting as a sensor. - ip: "172.22.10.102" - # The port to be used. - port: 55555 + id: 500 + # Adapter id to which sensor is connected + dev_id: 4 + # The interface name of the sensor + uifname: "umrr11_t132_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 2 # The frame_id to be set to the published messages. frame_id: "umrr" # Specify the history size. history_size: 10 sensor_2: + link_type: "eth" # The model of the connected sensor. - 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. - ip: "172.22.10.103" - # 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 - sensor_3: - # The model of the connected sensor. - model: "umrr9d" + model: "umrr96" + # Adapter id to which sensor is connected + dev_id: 3 # The client_id of the sensor/source, must be a unique integer. - id: 400 + id: 100 # The ip address of the sensor or of the source acting as a sensor. - ip: "172.22.10.104" + ip: "192.168.11.11" # 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 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr96_t153_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 2 + # The pathc version of the interface + uifpatchv: 2 + # sensor_3: + # # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # # umrr9f_v1_1_1, umrr9f_v2_0_0 + # link_type: "eth" + # model: "umrr11_can" + # # The client_id of the sensor/source, must be a unique integer. + # id: 800 + # # Adapter id to which sensor is connected + # dev_id: 5 + # # The interface name of the sensor + # uifname: "umrr11_t132_automotive" + # # The major version of the interface + # uifmajorv: 1 + # # The minor version of the interface + # uifminorv: 1 + # # The pathc version of the interface + # uifpatchv: 2 + # # 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 ddc6fc6..53b3033 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -19,16 +19,16 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include -#include -#include +#include +#include #include @@ -47,8 +47,8 @@ #include #include "umrr_ros2_driver/config_path.hpp" -#include "umrr_ros2_driver/sensor_params.hpp" #include "umrr_ros2_driver/sensor_commands.hpp" +#include "umrr_ros2_driver/sensor_params.hpp" using com::common::Instruction; using com::master::CmdRequest; @@ -62,8 +62,7 @@ using com::master::ResponseBatch; using com::master::SetParamRequest; using point_cloud_msg_wrapper::PointCloud2Modifier; -namespace -{ +namespace { constexpr auto kDefaultClientId = 0; constexpr auto kDefaultInterfaceName = "lo"; constexpr auto kDefaultIp = "127.0.0.1"; @@ -71,66 +70,83 @@ constexpr auto kDefaultPort = 55555; constexpr auto kDefaultHistorySize = 10; constexpr auto kDefaultFrameId = "umrr"; constexpr auto kDefaultSensorType = "umrr11"; +constexpr auto kDefaultInstType = "port_based"; +constexpr auto kDefaultDataType = "port_based"; + +constexpr auto kDefaultHwDevId = 1; +constexpr auto kDefaultHwDevIface = "slcan"; +constexpr auto kDefaultHwLinkType = "can"; + +constexpr auto kHwDevLinkTag = "type"; +constexpr auto kClientLinkTag = "link_type"; +constexpr auto kHwDevIdTag = "dev_id"; +constexpr auto kHwDevIfaceNameTag = "iface_name"; + +constexpr auto kHwDevPortTag = "hw_port"; constexpr auto kClientIdTag = "client_id"; constexpr auto kPortTag = "port"; +constexpr auto kBaudRateTag = "baudrate"; constexpr auto kIpTag = "ip"; -constexpr auto kIfaceNameTag = "iface_name"; -constexpr auto kMasterClientIdTag = "master_client_id"; -constexpr auto kDevIdTag = "hw_dev_id"; -constexpr auto kDevIfaceNameTag = "hw_iface_name"; -constexpr auto kDevPortTag = "hw_port"; -constexpr auto kDevIdJsonTag = "dev_id"; +constexpr auto kInstSerialTypeTag = "master_inst_serial_type"; +constexpr auto kDataSerialTypeTag = "master_data_serial_type"; + +constexpr auto kInstSerialTypeJsonTag = "instruction_serialization_type"; +constexpr auto kDataSerialTypeJsonTag = "data_serialization_type"; + constexpr auto kClientsJsonTag = "clients"; constexpr auto kHwItemsJsonTag = "hwItems"; +constexpr auto kUINameTag = "user_interface_name"; +constexpr auto kUIMajorVTag = "user_interface_major_v"; +constexpr auto kUIMinorVTag = "user_interface_minor_v"; +constexpr auto kUIPatchVTag = "user_interface_patch_v"; -constexpr bool float_eq(const float a, const float b) noexcept -{ +constexpr bool float_eq(const float a, const float b) noexcept { const auto maximum = std::max(std::fabs(a), std::fabs(b)); return std::fabs(a - b) <= maximum * std::numeric_limits::epsilon(); } -struct RadarPoint -{ +struct RadarPoint { float x{}; float y{}; float z{}; float radial_speed{}; float power{}; - float RCS{}; - float Noise{}; + float rcs{}; + float noise{}; float snr{}; - constexpr friend bool operator==(const RadarPoint & p1, const RadarPoint & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && float_eq(p1.z, p2.z) && - float_eq(p1.radial_speed, p2.radial_speed) && float_eq(p1.power, p2.power) && - float_eq(p1.RCS, p2.RCS) && float_eq(p1.Noise, p2.Noise) && float_eq(p1.snr, p2.snr); + constexpr friend bool operator==(const RadarPoint &p1, + const RadarPoint &p2) noexcept { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && float_eq(p1.radial_speed, p2.radial_speed) && + float_eq(p1.power, p2.power) && float_eq(p1.rcs, p2.rcs) && + float_eq(p1.noise, p2.noise) && float_eq(p1.snr, p2.snr); } }; LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(radial_speed); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(power); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(RCS); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(Noise); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(rcs); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(noise); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(snr); -using Generators = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, field_radial_speed_generator, field_RCS_generator, - field_Noise_generator, field_power_generator, field_snr_generator>; +using Generators = std::tuple; using RadarCloudModifier = PointCloud2Modifier; -} // namespace - -namespace smartmicro -{ -namespace drivers -{ -namespace radar -{ -SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_options) -: rclcpp::Node{"smartmicro_radar_node", node_options} -{ +} // namespace + +namespace smartmicro { +namespace drivers { +namespace radar { +SmartmicroRadarNode::SmartmicroRadarNode( + const rclcpp::NodeOptions &node_options) + : rclcpp::Node{"smartmicro_radar_node", node_options} { + // auto options = rclcpp::NodeOptions().allow_undeclared_parameters(true); update_config_files_from_params(); const auto override = false; @@ -140,106 +156,123 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option m_services = CommunicationServicesIface::Get(); if (!m_services->Init()) { throw std::runtime_error("Initialization failed"); + // return 1; } // 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_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!"); - // Wait init time + data_umrr11 = + com::master::umrr11_t132_automotive_v1_1_2::DataStreamServiceIface::Get(); + data_umrr96 = + com::master::umrr96_t153_automotive_v1_2_2::DataStreamServiceIface::Get(); + data_umrr9d = + com::master::umrr9d_t152_automotive_v1_0_3::DataStreamServiceIface::Get(); + data_umrr9f_v2_1_1 = + com::master::umrr9f_t169_automotive_v2_1_1::DataStreamServiceIface::Get(); + + // Wait for initailization std::this_thread::sleep_for(std::chrono::seconds(2)); + RCLCPP_INFO(this->get_logger(), "Data stream services have been received!"); for (auto i = 0UL; i < m_number_of_sensors; ++i) { - const auto & sensor = m_sensors[i]; - if ( - sensor.model == "umrr11" && - com::types::ERROR_CODE_OK != - data_umrr11->RegisterComTargetListPortReceiveCallback( - sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr11, this, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; + const auto &sensor = m_sensors[i]; + + m_publishers[i] = create_publisher( + "smart_radar/targets_" + std::to_string(i), sensor.history_size); + + if (sensor.model == "umrr96_can" && + com::types::ERROR_CODE_OK != + data_umrr96->RegisterComTargetBaseListReceiveCallback( + sensor.id, + std::bind(&SmartmicroRadarNode::CAN_targetlist_callback_umrr96, + this, i, std::placeholders::_1, + std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr96" + << std::endl; } - if ( - sensor.model == "umrr96" && - com::types::ERROR_CODE_OK != - data_umrr96->RegisterComTargetListPortReceiveCallback( - sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr96, this, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr96" << std::endl; + if (sensor.model == "umrr11_can" && + com::types::ERROR_CODE_OK != + data_umrr11->RegisterComTargetBaseListReceiveCallback( + sensor.id, + std::bind(&SmartmicroRadarNode::CAN_targetlist_callback_umrr11, + this, i, std::placeholders::_1, + std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr11" + << std::endl; } - if ( - sensor.model == "umrr9f_v1_1_1" && - com::types::ERROR_CODE_OK != - data_umrr9f_v1_1_1->RegisterComTargetListPortReceiveCallback( - sensor.id, std::bind( - &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_v1_1_1" << std::endl; + if (sensor.model == "umrr96" && + com::types::ERROR_CODE_OK != + data_umrr96->RegisterComTargetListReceiveCallback( + sensor.id, + std::bind(&SmartmicroRadarNode::targetlist_callback_umrr96, + this, i, std::placeholders::_1, + std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr96" + << 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 == "umrr11" && + com::types::ERROR_CODE_OK != + data_umrr11->RegisterComTargetListReceiveCallback( + sensor.id, + std::bind(&SmartmicroRadarNode::targetlist_callback_umrr11, + this, i, 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; + if (sensor.model == "umrr9f_v2_1_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_1_1->RegisterComTargetListReceiveCallback( + sensor.id, + std::bind(&SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1, + this, i, std::placeholders::_1, + std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr11" + << std::endl; + } + if (sensor.model == "umrr9d_can" && + com::types::ERROR_CODE_OK != + data_umrr9d->RegisterComTargetBaseListReceiveCallback( + sensor.id, + std::bind(&SmartmicroRadarNode::CAN_targetlist_callback_umrr9d, + this, i, std::placeholders::_1, + std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9d" + << std::endl; + } + if (sensor.model == "umrr9d" && + com::types::ERROR_CODE_OK != + data_umrr9d->RegisterComTargetListReceiveCallback( + 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( - "smart_radar/targets_" + std::to_string(i), sensor.history_size); } // create a ros2 service to change the radar parameters mode_srv_ = create_service( - "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( - "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."); + "smart_radar/set_radar_mode", + std::bind(&SmartmicroRadarNode::radar_mode, this, std::placeholders::_1, + std::placeholders::_2)); - std::signal(SIGINT, [](int signal) { return terminate_on_receive(signal); }); + RCLCPP_INFO(this->get_logger(), "Radar services are ready."); + rclcpp::on_shutdown( + std::bind(&SmartmicroRadarNode::on_shutdown_callback, this)); } -void terminate_on_receive(int signal) -{ - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "SIGINT has been received"); +void SmartmicroRadarNode::on_shutdown_callback() { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "SHUTDOWN CALLED!!!"); check_signal = true; rclcpp::Rate sleepRate(std::chrono::milliseconds(100)); sleepRate.sleep(); - rclcpp::shutdown(); + m_services.reset(); } void SmartmicroRadarNode::radar_mode( - const std::shared_ptr request, - std::shared_ptr result) -{ + const std::shared_ptr request, + std::shared_ptr result) { std::string instruction_name{}; bool check_flag_param = false; bool check_flag_id = false; @@ -250,8 +283,8 @@ void SmartmicroRadarNode::radar_mode( instruction_name = request->param; client_id = request->sensor_id; - for (const auto & item : param_table.items()) { - for (const auto & param : item.value().items()) { + for (const auto &item : param_table.items()) { + for (const auto ¶m : item.value().items()) { if (instruction_name == param.value()["name"]) { check_flag_param = true; break; @@ -264,7 +297,7 @@ void SmartmicroRadarNode::radar_mode( return; } - for (auto & sensor : m_sensors) { + for (auto &sensor : m_sensors) { if (client_id == sensor.id) { check_flag_id = true; break; @@ -275,9 +308,11 @@ void SmartmicroRadarNode::radar_mode( return; } - std::shared_ptr inst{m_services->GetInstructionService()}; + std::shared_ptr inst{ + m_services->GetInstructionService()}; timer = this->create_wall_timer( - std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); + std::chrono::seconds(2), + std::bind(&SmartmicroRadarNode::my_timer_callback, this)); std::shared_ptr batch; @@ -287,11 +322,12 @@ void SmartmicroRadarNode::radar_mode( } std::shared_ptr> radar_mode_01 = - std::make_shared>( - "auto_interface_0dim", request->param, request->value); + std::make_shared>( + "auto_interface_0dim", request->param, request->value); std::shared_ptr> radar_mode_02 = - std::make_shared>("auto_interface_0dim", request->param, request->value); + std::make_shared>("auto_interface_0dim", + request->param, request->value); if (!batch->AddRequest(radar_mode_01)) { result->res = "Failed to add instruction to the batch! "; @@ -300,461 +336,455 @@ void SmartmicroRadarNode::radar_mode( result->res = "Failed to add instruction to the batch! "; } - if ( - com::types::ERROR_CODE_OK != inst->SendInstructionBatch( - batch, std::bind( - &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; - } - result->res = "Service conducted successfully"; -} - -void SmartmicroRadarNode::ip_address( - const std::shared_ptr request, - std::shared_ptr result) -{ - std::shared_ptr inst{m_services->GetInstructionService()}; - timer = this->create_wall_timer( - std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); - bool check_flag = false; - client_id = request->sensor_id; - for (auto & sensor : m_sensors) { - if (client_id == sensor.id) { - check_flag = true; - break; - } - } - if (!check_flag) { - result->res_ip = "Sensor ID entered is not listed in the param file! "; - return; - } - - std::shared_ptr batch; - if (!inst->AllocateInstructionBatch(client_id, batch)) { - result->res_ip = "Failed to allocate instruction batch! "; - return; - } - - std::shared_ptr> ip_address = - std::make_shared>( - "auto_interface_0dim", "ip_source_address", request->value_ip); - - std::shared_ptr cmd = - std::make_shared("auto_interface_command", "comp_eeprom_ctrl_save_param_sec", 2010); - - if (!batch->AddRequest(ip_address)) { - result->res_ip = "Failed to add instruction to batch! "; - return; - } - if (!batch->AddRequest(cmd)) { - result->res_ip = "Failed to add instruction to batch! "; - return; - } - // send instruction batch to the device - if ( - com::types::ERROR_CODE_OK != - inst->SendInstructionBatch( - batch, std::bind( - &SmartmicroRadarNode::sensor_response_ip, this, client_id, std::placeholders::_2))) { - result->res_ip = "Service not conducted"; - return; - } else { - RCLCPP_INFO( - this->get_logger(), - "Radar must be restarted and the parameters in the param file " - "must be updated !!."); - result->res_ip = "Service conducted successfully"; - } -} - -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!"; + if (com::types::ERROR_CODE_OK != + inst->SendInstructionBatch( + batch, std::bind(&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; } 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) -{ + const com::types::ClientId client_id, + const std::shared_ptr &response, + const std::string instruction_name) { std::vector>> myResp_1; std::vector>> myResp_2; - if (response->GetResponse("auto_interface_0dim", instruction_name.c_str(), myResp_1)) { - for (auto & resp : myResp_1) { + if (response->GetResponse("auto_interface_0dim", + instruction_name.c_str(), myResp_1)) { + for (auto &resp : myResp_1) { response_type = resp->GetResponseType(); - RCLCPP_INFO( - this->get_logger(), "Response from '%s' : %i", instruction_name.c_str(), response_type); + RCLCPP_INFO(this->get_logger(), "Response from '%s' : %i", + instruction_name.c_str(), response_type); } } - if (response->GetResponse("auto_interface_0dim", instruction_name.c_str(), myResp_2)) { - for (auto & resp : myResp_2) { + if (response->GetResponse("auto_interface_0dim", + instruction_name.c_str(), myResp_2)) { + for (auto &resp : myResp_2) { response_type = resp->GetResponseType(); - RCLCPP_INFO( - this->get_logger(), "Response from '%s' : %i", instruction_name.c_str(), response_type); + RCLCPP_INFO(this->get_logger(), "Response from '%s' : %i", + instruction_name.c_str(), response_type); } } } -void SmartmicroRadarNode::sensor_response_ip( - const com::types::ClientId client_id, - const std::shared_ptr & response) -{ - std::vector>> myResp_2; - if (response->GetResponse("auto_interface_0dim", "ip_source_address", myResp_2)) { - for (auto & resp : myResp_2) { - response_type = resp->GetResponseType(); - RCLCPP_INFO(this->get_logger(), "Response from sensor for ip change: %i", response_type); +void SmartmicroRadarNode::targetlist_callback_umrr96( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_port_umrr96, + const com::types::ClientId client_id) { + std::cout << "Targetlist for umrr96" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrr96_t153_automotive_v1_2_2::comtargetlist::PortHeader> + port_header; + port_header = targetlist_port_umrr96->GetPortHeader(); + 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_umrr96->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->GetNoise(); + 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->GetNoise(), snr}); } + + m_publishers[sensor_idx]->publish(msg); } } -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 + &targetlist_port_umrr11, + const com::types::ClientId client_id) { + std::cout << "Targetlist for umrr11" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrr11_t132_automotive_v1_1_2::comtargetlist::PortHeader> + port_header; + port_header = targetlist_port_umrr11->GetPortHeader(); + 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_umrr11->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->GetNoise(); + 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->GetNoise(), snr}); } + + m_publishers[sensor_idx]->publish(msg); } } -void SmartmicroRadarNode::targetlist_callback_umrr11( - const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr11_t132_automotive_v1_1_1::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr11, - const com::types::ClientId client_id) -{ - std::cout << "Targetlist callback is being called for umrr11" << std::endl; +void SmartmicroRadarNode::targetlist_callback_umrr9d( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_port_umrr9d, + const com::types::ClientId client_id) { + std::cout << "Targetlist for umrr9d" << std::endl; if (!check_signal) { std::shared_ptr< - com::master::umrr11_t132_automotive_v1_1_1::comtargetlistport::GenericPortHeader> - port_header; - port_header = targetlist_port_umrr11->GetGenericPortHeader(); + com::master::umrr9d_t152_automotive_v1_0_3::comtargetlist::PortHeader> + port_header; + port_header = targetlist_port_umrr9d->GetPortHeader(); 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); + 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_umrr11->GetTargetList()) { + 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(); - 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}); + const auto snr = target->GetPower() - target->GetNoise(); + 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->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); } } -void SmartmicroRadarNode::targetlist_callback_umrr96( - const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr96_t153_automotive_v1_2_1::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr96, - const com::types::ClientId client_id) -{ - std::cout << "Targetlist callback is being called for umrr96" << std::endl; +void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_port_umrr9f_v2_1_1, + const com::types::ClientId client_id) { + std::cout << "Targetlist for umrr9f" << std::endl; if (!check_signal) { std::shared_ptr< - com::master::umrr96_t153_automotive_v1_2_1::comtargetlistport::GenericPortHeader> - port_header; - port_header = targetlist_port_umrr96->GetGenericPortHeader(); + com::master::umrr9f_t169_automotive_v2_1_1::comtargetlist::PortHeader> + port_header; + port_header = targetlist_port_umrr9f_v2_1_1->GetPortHeader(); 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); + 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_umrr96->GetTargetList()) { + for (const auto &target : targetlist_port_umrr9f_v2_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}); + const auto snr = target->GetPower() - target->GetNoise(); + 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->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); } } -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_v1_1_1, - const com::types::ClientId client_id) -{ - std::cout << "Targetlist callback is being called for umrr9f_v1_1_1" << std::endl; +void SmartmicroRadarNode::CAN_targetlist_callback_umrr96( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_can_umrr96, + const com::types::ClientId client_id) { + std::cout << "CAN Targetlist for umrr96" << 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_v1_1_1->GetGenericPortHeader(); + std::shared_ptr + port_header; + port_header = targetlist_can_umrr96->GetPortHeader(); 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); + 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()) { + for (const auto &target : targetlist_can_umrr96->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}); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back({range_2d * std::cos(azimuth_angle), + range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), + target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), 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; +void SmartmicroRadarNode::CAN_targetlist_callback_umrr11( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_can_umrr11, + const com::types::ClientId client_id) { + std::cout << "CAN Targetlist for umrr11" << 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(); + std::shared_ptr + port_header; + port_header = targetlist_can_umrr11->GetPortHeader(); 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); + 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_v2_0_0->GetTargetList()) { + for (const auto &target : targetlist_can_umrr11->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}); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back({range_2d * std::cos(azimuth_angle), + range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), + target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); } 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_2::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9d, - const com::types::ClientId client_id) -{ +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_can_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_2::comtargetlistport::GenericPortHeader> - port_header; - port_header = targetlist_port_umrr9d->GetGenericPortHeader(); + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9d->GetPortHeader(); 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); + 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()) { + for (const auto &target : targetlist_can_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(); - 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}); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back({range_2d * std::cos(azimuth_angle), + range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), + target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); } } -void SmartmicroRadarNode::update_config_files_from_params() -{ - const auto dev_id = declare_parameter(kDevIdTag, 0); - if (!dev_id) { - throw std::runtime_error("Parameter dev_id not set."); - } - const auto dev_port = declare_parameter(kDevPortTag, 0); - if (!dev_port) { - throw std::runtime_error("Parameter dev_port not set."); - } - const auto dev_iface_name = declare_parameter(kDevIfaceNameTag, std::string{}); - if (dev_iface_name.empty()) { - throw std::runtime_error("Parameter dev_iface_name not set."); - } - const auto master_client_id = declare_parameter(kMasterClientIdTag, 0); - if (!master_client_id) { - throw std::runtime_error("Parameter master_client_id not set."); - } +void SmartmicroRadarNode::update_config_files_from_params() { + RCLCPP_INFO(this->get_logger(), "Updating configs again."); + const auto master_inst_serial_type = + declare_parameter(kInstSerialTypeTag, std::string{}); + const auto master_data_serial_type = + declare_parameter(kDataSerialTypeTag, std::string{}); + + auto read_adapter_params_if_possible = [&](const std::uint32_t index) { + RCLCPP_INFO(this->get_logger(), "Updating ADAPTER params."); + auto ¤t_adapter = m_adapters[index]; + const auto prefix_2 = "adapters.adapter_" + std::to_string(index); + current_adapter.hw_dev_id = + this->declare_parameter(prefix_2 + ".hw_dev_id", kDefaultHwDevId); + if (current_adapter.hw_dev_id == kDefaultHwDevId) { + // The id was not set, so the adapter with this index was not defined. + // Stop here. + return false; + } + current_adapter.hw_iface_name = this->declare_parameter( + prefix_2 + ".hw_iface_name", kDefaultHwDevIface); + current_adapter.hw_type = + this->declare_parameter(prefix_2 + ".hw_type", kDefaultHwLinkType); + current_adapter.baudrate = + this->declare_parameter(prefix_2 + ".baudrate", 500000); + current_adapter.port = + this->declare_parameter(prefix_2 + ".port", kDefaultPort); + return true; + }; + // [] start the lambda function + // & for the capturing value by reference + // const std::uint32_t index specifies parameter list of lambda auto read_sensor_params_if_possible = [&](const std::uint32_t index) { - auto & current_sensor = m_sensors[index]; - const auto prefix = "sensors.sensor_" + std::to_string(index); - current_sensor.model = this->declare_parameter(prefix + ".model", kDefaultSensorType); - current_sensor.id = this->declare_parameter(prefix + ".id", kDefaultClientId); - if (current_sensor.id == kDefaultClientId) { + RCLCPP_INFO(this->get_logger(), "Updating SENSOR params."); + auto &sensor = m_sensors[index]; + const auto prefix_3 = "sensors.sensor_" + std::to_string(index); + sensor.dev_id = + this->declare_parameter(prefix_3 + ".dev_id", kDefaultHwDevId); + sensor.uifname = this->declare_parameter(prefix_3 + ".uifname", ""); + sensor.uifmajorv = this->declare_parameter(prefix_3 + ".uifmajorv", 0); + sensor.uifminorv = this->declare_parameter(prefix_3 + ".uifminorv", 0); + sensor.uifpatchv = this->declare_parameter(prefix_3 + ".uifpatchv", 0); + sensor.model = + this->declare_parameter(prefix_3 + ".model", kDefaultSensorType); + sensor.id = this->declare_parameter(prefix_3 + ".id", kDefaultClientId); + if (sensor.id == kDefaultClientId) { // The id was not set, so the sensor with this index was not defined. Stop // here. return false; } - current_sensor.ip = this->declare_parameter(prefix + ".ip", kDefaultIp); - current_sensor.port = this->declare_parameter(prefix + ".port", kDefaultPort); - current_sensor.frame_id = this->declare_parameter(prefix + ".frame_id", kDefaultFrameId); - current_sensor.history_size = - this->declare_parameter(prefix + ".history_size", kDefaultHistorySize); - + sensor.ip = this->declare_parameter(prefix_3 + ".ip", ""); + sensor.port = this->declare_parameter(prefix_3 + ".port", 0); + sensor.frame_id = + this->declare_parameter(prefix_3 + ".frame_id", kDefaultFrameId); + sensor.history_size = this->declare_parameter(prefix_3 + ".history_size", + kDefaultHistorySize); + sensor.inst_type = this->declare_parameter(prefix_3 + ".inst_type", ""); + sensor.data_type = this->declare_parameter(prefix_3 + ".data_type", ""); + sensor.link_type = + this->declare_parameter(prefix_3 + ".link_type", kDefaultHwLinkType); return true; }; + for (auto j = 0UL; j < m_adapters.size(); ++j) { + if (!read_adapter_params_if_possible(j)) { + m_number_of_adapters = j; + break; + } + } + + if (!m_number_of_adapters) { + throw std::runtime_error("At least one adapter must be configured."); + } + for (auto i = 0UL; i < m_sensors.size(); ++i) { if (!read_sensor_params_if_possible(i)) { m_number_of_sensors = i; break; } } - if (!m_number_of_sensors) { throw std::runtime_error("At least one sensor must be configured."); } auto config = nlohmann::json::parse(std::ifstream{kConfigFilePath}); - config[kClientIdTag] = master_client_id; + config[kDataSerialTypeJsonTag] = master_data_serial_type; + config[kInstSerialTypeJsonTag] = master_inst_serial_type; std::ofstream{kConfigFilePath, std::ios::trunc} << config; - auto hw_inventory = nlohmann::json::parse(std::ifstream{kHwInventoryFilePath}); - auto & hw_items = hw_inventory[kHwItemsJsonTag]; + auto hw_inventory = + nlohmann::json::parse(std::ifstream{kHwInventoryFilePath}); + auto &hw_items = hw_inventory[kHwItemsJsonTag]; if (hw_items.empty()) { - throw std::runtime_error("There are no 'hwItems' defined in the hw_inventory.json file."); + throw std::runtime_error( + "There are no 'hwItems' defined in the hw_inventory.json file."); + } + auto hw_item = hw_items.front(); + hw_items.clear(); + for (auto j = 0UL; j < m_number_of_adapters; ++j) { + const auto &adapter = m_adapters[j]; + hw_item[kPortTag] = adapter.port; + hw_item[kHwDevLinkTag] = adapter.hw_type; + hw_item[kHwDevIdTag] = adapter.hw_dev_id; + hw_item[kHwDevIfaceNameTag] = adapter.hw_iface_name; + hw_item[kBaudRateTag] = adapter.baudrate; + hw_items.push_back(hw_item); } - auto & hw_item = hw_items.front(); - hw_item[kDevIdJsonTag] = dev_id; - hw_item[kPortTag] = dev_port; - hw_item[kIfaceNameTag] = dev_iface_name; std::ofstream{kHwInventoryFilePath, std::ios::trunc} << hw_inventory; - auto routing_table = nlohmann::json::parse(std::ifstream{kRoutingTableFilePath}); - auto & clients = routing_table[kClientsJsonTag]; + auto routing_table = + nlohmann::json::parse(std::ifstream{kRoutingTableFilePath}); + auto &clients = routing_table[kClientsJsonTag]; if (clients.empty()) { - throw std::runtime_error("There are no 'clients' defined in the routing_table.json file."); + throw std::runtime_error( + "There are no 'clients' defined in the routing_table.json file."); } - auto client = clients.front(); // Make a copy of the first client. + auto client = clients.front(); // Make a copy of the first client. clients.clear(); for (auto i = 0UL; i < m_number_of_sensors; ++i) { - const auto & sensor = m_sensors[i]; - client[kClientIdTag] = sensor.id; + const auto &sensor = m_sensors[i]; + client[kClientLinkTag] = sensor.link_type; + client[kClientIdTag] = sensor.id; // + client[kHwDevIdTag] = sensor.dev_id; client[kPortTag] = sensor.port; client[kIpTag] = sensor.ip; + client[kInstSerialTypeJsonTag] = sensor.inst_type; + client[kDataSerialTypeJsonTag] = sensor.data_type; + client[kUINameTag] = sensor.uifname; + client[kUIMajorVTag] = sensor.uifmajorv; + client[kUIMinorVTag] = sensor.uifminorv; + client[kUIPatchVTag] = sensor.uifpatchv; clients.push_back(client); } - std::ofstream{kRoutingTableFilePath, std::ios::trunc} << routing_table; + + std::ofstream{kRoutingTableFilePath, std::ios::trunc} << std::setw(4) + << routing_table; } -} // namespace radar -} // namespace drivers -} // namespace smartmicro +} // namespace radar +} // namespace drivers +} // namespace smartmicro RCLCPP_COMPONENTS_REGISTER_NODE(smartmicro::drivers::radar::SmartmicroRadarNode) From d31adff646fcf55f5a897aaa21d8184d7d33083d Mon Sep 17 00:00:00 2001 From: smartSRA Date: Tue, 4 Jul 2023 10:54:46 +0200 Subject: [PATCH 02/15] Removed the old para template file --- .../smartmicro_radar_node.hpp | 91 ++++++++----- umrr_ros2_driver/param/radar.template.yaml | 123 ------------------ .../src/smartmicro_radar_node.cpp | 72 ++++++++-- 3 files changed, 116 insertions(+), 170 deletions(-) delete mode 100644 umrr_ros2_driver/param/radar.template.yaml 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 d6f69d6..37878eb 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 @@ -120,27 +120,20 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { comtargetlist::ComTargetList> &targetlist_port_umrr96, const com::types::ClientId client_id); - - void CAN_targetlist_callback_umrr96( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_can_umrr96, - const com::types::ClientId client_id); - + /// /// @brief A callback that is called when a new target list port for - /// umrr96 arrives. + /// 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 CAN_targetlist_callback_umrr11( + void targetlist_callback_umrr9f_v1_1_1( const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_can_umrr11, + const std::shared_ptr + &targetlist_port_umrr9f, const com::types::ClientId client_id); /// @@ -151,60 +144,88 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { /// @param[in] target_list_port The target list port /// @param[in] client_id The client_id of the sensor /// - - void targetlist_callback_umrr9f_v1_1_1( + void targetlist_callback_umrr9f_v2_1_1( const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_port_umrr9f, + const std::shared_ptr + &targetlist_port_umrr9f_v2_1_1, const com::types::ClientId client_id); + /// /// @brief A callback that is called when a new target list port for - /// umrr9f arrives. + /// umrr96 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_1_1( + void targetlist_callback_umrr9d( const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_port_umrr9f_v2_1_1, + &targetlist_port_umrr9d, const com::types::ClientId client_id); /// - /// @brief A callback that is called when a new target list port for - /// umrr9d arrives. + /// @brief A callback that is called when a new CAN target list for + /// umrr96 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 CAN_targetlist_callback_umrr96( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_can_umrr96, + const com::types::ClientId client_id); - void CAN_targetlist_callback_umrr9d( + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr11 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 CAN_targetlist_callback_umrr11( const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_can_umrr9d, + &targetlist_can_umrr11, const com::types::ClientId client_id); - + /// - /// @brief A callback that is called when a new target list port for - /// umrr96 arrives. + /// @brief A callback that is called when a new CAN target list 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_umrr9d( + void CAN_targetlist_callback_umrr9f_v2_1_1( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_can_umrr9f_v2_1_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list 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 CAN_targetlist_callback_umrr9d( const std::uint32_t sensor_idx, const std::shared_ptr - &targetlist_port_umrr9d, + comtargetbaselist::ComTargetBaseList> + &targetlist_can_umrr9d, const com::types::ClientId client_id); - + /// /// @brief Read parameters and update the json config files required by /// Smart Access C++ API. diff --git a/umrr_ros2_driver/param/radar.template.yaml b/umrr_ros2_driver/param/radar.template.yaml deleted file mode 100644 index 3faeaf5..0000000 --- a/umrr_ros2_driver/param/radar.template.yaml +++ /dev/null @@ -1,123 +0,0 @@ ---- -/**: - ros__parameters: - # This sets the iface_name in the hw_inventory.json - # sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx - # sudo ip link set up slcan0 - # One sensor per interface - master_inst_serial_type: "can_based" - master_data_serial_type: "can_based" - adapters: - adapter_0: - hw_type: "can" - hw_dev_id: 2 - hw_iface_name: "slcan0" - baudrate: 500000 - adapter_1: - hw_type: "eth" - hw_dev_id: 3 - hw_iface_name: "enx002432163c8d" - port: 55555 - adapter_2: - hw_type: "can" - hw_dev_id: 4 - hw_iface_name: "slcan1" - baudrate: 500000 - #adapter_3: - # hw_type: "can" - # hw_dev_id: 5 - # hw_iface_name: "slcan2" - # baudrate: 2500000 - # An array of sensors to subscribe to. - sensors: - # 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. Available models umrr11, umrr9d, umrr96, - # umrr9f_v1_1_1, umrr9f_v2_0_0 - link_type: "can" - model: "umrr96_can" - # The client_id of the sensor/source, must be a unique integer. - id: 200 - # Adapter id to which sensor is connected - dev_id: 2 - # The interface name of the sensor - uifname: "umrr96_t153_automotive" - # The major version of the interface - uifmajorv: 1 - # The minor version of the interface - uifminorv: 2 - # The pathc version of the interface - uifpatchv: 2 - # The frame_id to be set to the published messages. - frame_id: "umrr" - # Specify the history size. - history_size: 10 - sensor_1: - # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, - # umrr9f_v1_1_1, umrr9f_v2_0_0 - link_type: "can" - model: "umrr11_can" - # The client_id of the sensor/source, must be a unique integer. - id: 500 - # Adapter id to which sensor is connected - dev_id: 4 - # The interface name of the sensor - uifname: "umrr11_t132_automotive" - # The major version of the interface - uifmajorv: 1 - # The minor version of the interface - uifminorv: 1 - # The pathc version of the interface - uifpatchv: 2 - # The frame_id to be set to the published messages. - frame_id: "umrr" - # Specify the history size. - history_size: 10 - sensor_2: - link_type: "eth" - # The model of the connected sensor. - model: "umrr96" - # Adapter id to which sensor is connected - dev_id: 3 - # The client_id of the sensor/source, must be a unique integer. - id: 100 - # The ip address of the sensor or of the source acting as a sensor. - ip: "192.168.11.11" - # 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 - inst_type: "port_based" - data_type: "port_based" - # The interface name of the sensor - uifname: "umrr96_t153_automotive" - # The major version of the interface - uifmajorv: 1 - # The minor version of the interface - uifminorv: 2 - # The pathc version of the interface - uifpatchv: 2 - # sensor_3: - # # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, - # # umrr9f_v1_1_1, umrr9f_v2_0_0 - # link_type: "eth" - # model: "umrr11_can" - # # The client_id of the sensor/source, must be a unique integer. - # id: 800 - # # Adapter id to which sensor is connected - # dev_id: 5 - # # The interface name of the sensor - # uifname: "umrr11_t132_automotive" - # # The major version of the interface - # uifmajorv: 1 - # # The minor version of the interface - # uifminorv: 1 - # # The pathc version of the interface - # uifpatchv: 2 - # # 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 53b3033..137730b 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -199,6 +199,26 @@ SmartmicroRadarNode::SmartmicroRadarNode( std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; } + if (sensor.model == "umrr9d_can" && + com::types::ERROR_CODE_OK != + data_umrr9d->RegisterComTargetBaseListReceiveCallback( + sensor.id, + std::bind(&SmartmicroRadarNode::CAN_targetlist_callback_umrr9d, + this, i, std::placeholders::_1, + std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9d" + << std::endl; + } + if (sensor.model == "umrr9f_can_v2_1_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_1_1->RegisterComTargetBaseListReceiveCallback( + sensor.id, + std::bind(&SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1, + this, i, std::placeholders::_1, + std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f" + << std::endl; + } if (sensor.model == "umrr96" && com::types::ERROR_CODE_OK != data_umrr96->RegisterComTargetListReceiveCallback( @@ -226,17 +246,7 @@ SmartmicroRadarNode::SmartmicroRadarNode( std::bind(&SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr11" - << std::endl; - } - if (sensor.model == "umrr9d_can" && - com::types::ERROR_CODE_OK != - data_umrr9d->RegisterComTargetBaseListReceiveCallback( - sensor.id, - std::bind(&SmartmicroRadarNode::CAN_targetlist_callback_umrr9d, - this, i, std::placeholders::_1, - std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9d" + std::cout << "Failed to register targetlist callback for sensor umrr9f" << std::endl; } if (sensor.model == "umrr9d" && @@ -645,8 +655,46 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d( } } +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_can_umrr9f_v2_1_1, + const com::types::ClientId client_id) { + std::cout << "Targetlist callback is being called for umrr9f" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9f_v2_1_1->GetPortHeader(); + 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_can_umrr9f_v2_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->GetSignalLevel() - target->GetNoise(); + modifier.push_back({range_2d * std::cos(azimuth_angle), + range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), + target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + void SmartmicroRadarNode::update_config_files_from_params() { - RCLCPP_INFO(this->get_logger(), "Updating configs again."); const auto master_inst_serial_type = declare_parameter(kInstSerialTypeTag, std::string{}); const auto master_data_serial_type = From 5a61b28324866b7cde183ab6a4b3edd377fbb35a Mon Sep 17 00:00:00 2001 From: smartSRA Date: Thu, 6 Jul 2023 14:37:13 +0200 Subject: [PATCH 03/15] Update readme and simulators Add instructions on how to set-up can and ethernet interfaces. Lists new adapter and sensor parameters for node configuration. The simulators are updated to work with the new interface and docker environment respectively. The tests for the node have been updated also. --- Readme.md | 52 ++++++-- docker-compose.yml | 2 +- simulator/config_umrr9d/com_lib_config.json | 2 +- simulator/config_umrr9f/com_lib_config.json | 2 +- simulator/simulation/CMakeLists.txt | 16 +-- .../param/radar.adapter.example.yaml | 31 +++++ .../param/radar.adapter.template.yaml | 23 +--- .../param/radar.sensor.example.yaml | 122 ++++++++++++++++++ .../param/radar.sensor.template.yaml | 108 +++++++--------- .../src/smartmicro_radar_node.cpp | 46 +++++++ .../test/radar_node_test.launch.py | 12 +- .../test/test_smartmicro_radar_node.cpp | 14 +- 12 files changed, 321 insertions(+), 109 deletions(-) create mode 100644 umrr_ros2_driver/param/radar.adapter.example.yaml create mode 100644 umrr_ros2_driver/param/radar.sensor.example.yaml diff --git a/Readme.md b/Readme.md index 8976d81..1974f4e 100644 --- a/Readme.md +++ b/Readme.md @@ -28,11 +28,11 @@ sure the version used to publish the data is compatible with this version: - 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: `UMRR96 Type 153 AUTOMOTIVE v1.2.2` +- User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.2` - 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` +- User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.1.1` +- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.0.3` ### Sensor Firmwares This ROS2 driver release is compatible with the following sensor firmwares: @@ -65,16 +65,46 @@ callback is triggered a new point cloud message is created and published. The driver publishes `sensor_msgs::msg::PointCloud2` messages with the radar targets on the topic `umrr/targets` which can be remapped through the parameters. -### Configuration: +### Interface Configuration: +For setting up a sensor with ethernet or can, the interfaces of the should be set properly prior to configuring the node. +The sensor came equiped with two protocols: +- ethernet: to set-up an ethernet interface the following command could be used `ifconfig my_interface_name 192.168.11.17 netmask 255.255.255.0`. +The command above uses the default source ip address used by the sensors. +- can: to set-up a can interface the following commands could be used `slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx`and than `ip link set up my_interace_name`. +This uses the default baudrate of _500000_. + +### Node Configuration: The node is configured through the parameters. Here is a short recap of the most important parts. -For more details, see the [`radar.template.yaml`](umrr_ros2_driver/param/radar.template.yaml) file. -- `client_id`: the id of the client, must be a unique integer -- `ip`: the IP of the used sensor or the source -- `port`: port to be used to receive the packets -- `iface_name`: name of the used network interface +For more details, see the [`radar.sensor.example.yaml`](umrr_ros2_driver/param/radar.sensor.example.yaml) and +[`radar.adapter.example.yaml`](umrr_ros2_driver/param/radar.adapter.example.yaml) files. + +For the setting up the ***sensors***: +- `link_type`: the type of hardware connection +- `model`: the model of the sensor being used + - can: 'umrr11_can', 'umrr9d_can', 'umrr96_can', 'umrr9f_can_v1_1_1', 'umrr9f_can_v2_1_1' + - port: 'umrr11', 'umrr9d', 'umrr96', 'umrr9f_v1_1_1', 'umrr9f_v2_1_1' +- `dev_id`: adapter id to which sensor is connected. ***The adapter and sensor should have the same dev_id*** +- `id`: the client_id of the sensor/source, ***must be a _unique_ integer***. +- `ip`: the ***_unique_*** ip address of the sensor or of the source acting as a sensor, required only for sensors using _ethernet_. +- `port`: port to be used to receive the packets, default is _55555_ - `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('umrr11', 'umrr9d', 'umrr96', 'umrr9f_v1_1_1', 'umrr9f_v2_0_0') of the sensor being used +- `inst_type`: the type of instruction serialization type, relevant to sensors using ethernet and should be _port_based_ +- `data_type`: the type of data serialization type, relevant to sensors using ethernet and should be _port_based_ +- `uifname`: the user interface name of the sensor +- `uifmajorv`: the major version of the sensor user interface +- `uifminorv`: the minor version of the sensor user interface +- `uifpatchv`: the patch version of the sensor user interface + +For setting up the ***adapters***: +- `master_inst_serial_type`: the instruction serilization type of the master. When using a hybrid of 'can' and 'port' use _can_based_ +- `master_data_serial_type`: the data serilization type of the master. When using a hybrid of 'can' and 'port' use _can_based_ +- `hw_type`: the type of the hardware connection +- `hw_dev_id`: adapter id of the hardware, the sensor and ***adapter should use the same id*** +- `hw_iface_name`: name of the used network interface +- `baudrate`: the baudrate of the sensor connected with can, default is _500000_ +- `port`: port to be used to receive the packets, default is _55555_ + ## 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/docker-compose.yml b/docker-compose.yml index f3c20a2..fc54d8f 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -60,7 +60,7 @@ services: - SMART_ACCESS_CFG_FILE_PATH=/code/simulator/config_umrr9f/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: ["9", "1", "1", "B"] + command: ["9", "2", "1", "B"] networks: device_network: ipv4_address: 172.22.10.103 diff --git a/simulator/config_umrr9d/com_lib_config.json b/simulator/config_umrr9d/com_lib_config.json index d71df6d..efebc3a 100644 --- a/simulator/config_umrr9d/com_lib_config.json +++ b/simulator/config_umrr9d/com_lib_config.json @@ -8,7 +8,7 @@ "user_interface_name": "umrr9d_t152_automotive", "user_interface_major_v": 1, "user_interface_minor_v": 0, - "user_interface_patch_v": 2, + "user_interface_patch_v": 3, "download_path": "", "instruction_serialization_type": "port_based", "data_serialization_type": "port_based", diff --git a/simulator/config_umrr9f/com_lib_config.json b/simulator/config_umrr9f/com_lib_config.json index caaff38..5a444bb 100644 --- a/simulator/config_umrr9f/com_lib_config.json +++ b/simulator/config_umrr9f/com_lib_config.json @@ -6,7 +6,7 @@ "shared_lib_path": "/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9", "config_path": "/code/simulator/config_umrr9f", "user_interface_name": "umrr9f_t169_automotive", - "user_interface_major_v": 1, + "user_interface_major_v": 2, "user_interface_minor_v": 1, "user_interface_patch_v": 1, "download_path": "", diff --git a/simulator/simulation/CMakeLists.txt b/simulator/simulation/CMakeLists.txt index fbd282d..5bfab9b 100644 --- a/simulator/simulation/CMakeLists.txt +++ b/simulator/simulation/CMakeLists.txt @@ -16,18 +16,18 @@ add_executable(simulator target_include_directories(simulator PUBLIC ../../umrr_ros2_driver/smartmicro/include - umrr_ros2_driver/smartmicro/include/umrr11_t132_automotive_v1_1_1 - umrr_ros2_driver/smartmicro/include/umrr96_t153_automotive_v1_2_1 - umrr_ros2_driver/smartmicro/include/umrr9f_t169_automotive_v1_1_1 - umrr_ros2_driver/smartmicro/include/umrr9d_t152_automotive_v1_0_2) + umrr_ros2_driver/smartmicro/include/umrr11_t132_automotive_v1_1_2 + umrr_ros2_driver/smartmicro/include/umrr96_t153_automotive_v1_2_2 + umrr_ros2_driver/smartmicro/include/umrr9f_t169_automotive_v2_1_1 + umrr_ros2_driver/smartmicro/include/umrr9d_t152_automotive_v1_0_3) target_link_libraries(simulator com_lib osal - umrr11_t132_automotivev1.1.1_user_interface - umrr96_t153_automotivev1.2.1_user_interface - umrr9f_t169_automotivev1.1.1_user_interface - umrr9d_t152_automotivev1.0.2_user_interface + umrr11_t132_automotivev1.1.2_user_interface + umrr96_t153_automotivev1.2.2_user_interface + umrr9f_t169_automotivev2.1.1_user_interface + umrr9d_t152_automotivev1.0.3_user_interface dl) install(TARGETS simulator DESTINATION bin) diff --git a/umrr_ros2_driver/param/radar.adapter.example.yaml b/umrr_ros2_driver/param/radar.adapter.example.yaml new file mode 100644 index 0000000..31206fc --- /dev/null +++ b/umrr_ros2_driver/param/radar.adapter.example.yaml @@ -0,0 +1,31 @@ +--- +/**: + ros__parameters: + # This sets the iface_name in the hw_inventory.json + # sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx + # sudo ip link set up slcan0 + # One sensor per interface for can, for ethernet we can use one interface/dev for multiple sensors + master_inst_serial_type: "can_based" + master_data_serial_type: "can_based" + adapters: + adapter_0: + hw_type: "can" + hw_dev_id: 2 + hw_iface_name: "slcan0" + baudrate: 500000 + adapter_1: + hw_type: "eth" + hw_dev_id: 3 + hw_iface_name: "enx002432163c8d" + port: 55555 + adapter_2: + hw_type: "can" + hw_dev_id: 4 + hw_iface_name: "slcan1" + baudrate: 500000 + adapter_3: + hw_type: "can" + hw_dev_id: 5 + hw_iface_name: "can0" + baudrate: 500000 + diff --git a/umrr_ros2_driver/param/radar.adapter.template.yaml b/umrr_ros2_driver/param/radar.adapter.template.yaml index ea0a624..708c62e 100644 --- a/umrr_ros2_driver/param/radar.adapter.template.yaml +++ b/umrr_ros2_driver/param/radar.adapter.template.yaml @@ -5,27 +5,12 @@ # sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx # sudo ip link set up slcan0 # One sensor per interface - master_inst_serial_type: "can_based" - master_data_serial_type: "can_based" + master_inst_serial_type: "port_based" + master_data_serial_type: "port_based" adapters: adapter_0: - hw_type: "can" - hw_dev_id: 2 - hw_iface_name: "slcan0" - baudrate: 500000 - adapter_1: hw_type: "eth" hw_dev_id: 3 - hw_iface_name: "enx002432163c8d" + hw_iface_name: "eth0" port: 55555 - adapter_2: - hw_type: "can" - hw_dev_id: 4 - hw_iface_name: "slcan1" - baudrate: 500000 - #adapter_3: - # hw_type: "can" - # hw_dev_id: 5 - # hw_iface_name: "can0" - # baudrate: 500000 - + \ No newline at end of file diff --git a/umrr_ros2_driver/param/radar.sensor.example.yaml b/umrr_ros2_driver/param/radar.sensor.example.yaml new file mode 100644 index 0000000..d493ff4 --- /dev/null +++ b/umrr_ros2_driver/param/radar.sensor.example.yaml @@ -0,0 +1,122 @@ +--- +/**: + ros__parameters: +# An array of sensors to subscribe to. + sensors: + # 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. Available models umrr11, umrr9d, umrr96, + # umrr9f_v1_1_1, umrr9f_v2_0_0 + link_type: "can" + model: "umrr9d_can" + # The client_id of the sensor/source, must be a unique integer. + id: 500 + # Adapter id to which sensor is connected + dev_id: 2 + # The interface name of the sensor + uifname: "umrr9d_t152_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 0 + # The pathc version of the interface + uifpatchv: 3 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + sensor_1: + # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # umrr9f_v1_1_1, umrr9f_v2_0_0 + link_type: "can" + model: "umrr9f_can_v2_1_1" + # The client_id of the sensor/source, must be a unique integer. + id: 600 + # Adapter id to which sensor is connected + dev_id: 4 + # The interface name of the sensor + uifname: "umrr9f_t169_automotive" + # The major version of the interface + uifmajorv: 2 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 1 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + sensor_2: + link_type: "eth" + # The model of the connected sensor. + model: "umrr9f_v2_1_1" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 700 + # The ip address of the sensor or of the source acting as a sensor. + ip: "192.168.11.11" + # 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 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr9f_t169_automotive" + # The major version of the interface + uifmajorv: 2 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 1 + sensor_3: + link_type: "eth" + # The model of the connected sensor. + model: "umrr11" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 800 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.101" + # 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 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr11_t132_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 2 + sensor_4: + # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # umrr9f_v1_1_1, umrr9f_v2_0_0 + link_type: "can" + model: "umrr9d_can" + # The client_id of the sensor/source, must be a unique integer. + id: 900 + # Adapter id to which sensor is connected + dev_id: 5 + # The interface name of the sensor + uifname: "umrr9d_t152_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 0 + # The pathc version of the interface + uifpatchv: 3 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 \ No newline at end of file diff --git a/umrr_ros2_driver/param/radar.sensor.template.yaml b/umrr_ros2_driver/param/radar.sensor.template.yaml index 0e618a3..aeed182 100644 --- a/umrr_ros2_driver/param/radar.sensor.template.yaml +++ b/umrr_ros2_driver/param/radar.sensor.template.yaml @@ -5,36 +5,24 @@ sensors: # 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. Available models umrr11, umrr9d, umrr96, - # # umrr9f_v1_1_1, umrr9f_v2_0_0 - # link_type: "can" - # model: "umrr96_can" - # # The client_id of the sensor/source, must be a unique integer. - # id: 500 - # # Adapter id to which sensor is connected - # dev_id: 2 - # # The interface name of the sensor - # uifname: "umrr96_t153_automotive" - # # The major version of the interface - # uifmajorv: 1 - # # The minor version of the interface - # uifminorv: 2 - # # The pathc version of the interface - # uifpatchv: 2 - # # The frame_id to be set to the published messages. - # frame_id: "umrr" - # # Specify the history size. - # history_size: 10 sensor_0: - # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, - # umrr9f_v1_1_1, umrr9f_v2_0_0 - link_type: "can" - model: "umrr11_can" - # The client_id of the sensor/source, must be a unique integer. - id: 600 + link_type: "eth" + # The model of the connected sensor. + model: "umrr11" # Adapter id to which sensor is connected - dev_id: 4 + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 100 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.101" + # 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 + inst_type: "port_based" + data_type: "port_based" # The interface name of the sensor uifname: "umrr11_t132_automotive" # The major version of the interface @@ -43,20 +31,42 @@ uifminorv: 1 # The pathc version of the interface uifpatchv: 2 + sensor_1: + link_type: "eth" + # The model of the connected sensor. + model: "umrr96" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 200 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.102" + # 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 - sensor_1: + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr96_t153_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 2 + # The pathc version of the interface + uifpatchv: 2 + sensor_2: link_type: "eth" # The model of the connected sensor. model: "umrr9f_v2_1_1" # Adapter id to which sensor is connected dev_id: 3 # The client_id of the sensor/source, must be a unique integer. - id: 700 + id: 300 # The ip address of the sensor or of the source acting as a sensor. - ip: "192.168.11.11" + ip: "172.22.10.103" # The port to be used. port: 55555 # The frame_id to be set to the published messages. @@ -73,16 +83,16 @@ uifminorv: 1 # The pathc version of the interface uifpatchv: 1 - sensor_2: + sensor_3: link_type: "eth" # The model of the connected sensor. - model: "umrr11" + model: "umrr9d" # Adapter id to which sensor is connected dev_id: 3 # The client_id of the sensor/source, must be a unique integer. - id: 800 + id: 400 # The ip address of the sensor or of the source acting as a sensor. - ip: "192.168.11.153" + ip: "172.22.10.104" # The port to be used. port: 55555 # The frame_id to be set to the published messages. @@ -92,31 +102,11 @@ inst_type: "port_based" data_type: "port_based" # The interface name of the sensor - uifname: "umrr11_t132_automotive" + uifname: "umrr9d_t152_automotive" # The major version of the interface uifmajorv: 1 # The minor version of the interface - uifminorv: 1 + uifminorv: 0 # The pathc version of the interface - uifpatchv: 2 - # sensor_3: - # # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, - # # umrr9f_v1_1_1, umrr9f_v2_0_0 - # link_type: "can" - # model: "umrr9d_can" - # # The client_id of the sensor/source, must be a unique integer. - # id: 900 - # # Adapter id to which sensor is connected - # dev_id: 5 - # # The interface name of the sensor - # uifname: "umrr9d_t152_automotive" - # # The major version of the interface - # uifmajorv: 1 - # # The minor version of the interface - # uifminorv: 0 - # # The pathc version of the interface - # uifpatchv: 3 - # # The frame_id to be set to the published messages. - # frame_id: "umrr" - # # Specify the history size. - # history_size: 10 \ No newline at end of file + uifpatchv: 3 + \ No newline at end of file diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 137730b..8057216 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -166,6 +166,7 @@ SmartmicroRadarNode::SmartmicroRadarNode( com::master::umrr96_t153_automotive_v1_2_2::DataStreamServiceIface::Get(); data_umrr9d = com::master::umrr9d_t152_automotive_v1_0_3::DataStreamServiceIface::Get(); + data_umrr9f_v1_1_1 = com::master::umrr9f_t169_automotive_v1_1_1::DataStreamServiceIface::Get(); data_umrr9f_v2_1_1 = com::master::umrr9f_t169_automotive_v2_1_1::DataStreamServiceIface::Get(); @@ -239,6 +240,15 @@ SmartmicroRadarNode::SmartmicroRadarNode( std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; } + if ( + sensor.model == "umrr9f_v1_1_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v1_1_1->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &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_v1_1_1" << std::endl; + } if (sensor.model == "umrr9f_v2_1_1" && com::types::ERROR_CODE_OK != data_umrr9f_v2_1_1->RegisterComTargetListReceiveCallback( @@ -499,6 +509,42 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d( } } +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_v1_1_1, + const com::types::ClientId client_id) +{ + 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_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_1_1( const std::uint32_t sensor_idx, const std::shared_ptr(node_options), std::runtime_error); - node_options.append_parameter_override("master_client_id", 1); - node_options.append_parameter_override("hw_dev_id", 2); - node_options.append_parameter_override("hw_port", 55555); - node_options.append_parameter_override("hw_iface_name", "eth0"); + node_options.append_parameter_override("adapters.adapter_0.hw_type", "eth"); + node_options.append_parameter_override("adapters.adapter_0.hw_dev_id", 2); + node_options.append_parameter_override("adapters.adapter_0.port", 55555); + node_options.append_parameter_override("adapters.adapter_0.hw_iface_name", "eth0"); node_options.append_parameter_override("sensors.sensor_0.id", 10); node_options.append_parameter_override("sensors.sensor_0.ip", "172.22.10.102"); node_options.append_parameter_override("sensors.sensor_0.model", "umrr96"); + node_options.append_parameter_override("sensors.sensor_0.inst_type", "port_based"); + node_options.append_parameter_override("sensors.sensor_0.data_type", "port_based"); + node_options.append_parameter_override("sensors.sensor_0.uifname", "umrr96_t153_automotive"); + node_options.append_parameter_override("sensors.sensor_0.uifmajorv", 1); + node_options.append_parameter_override("sensors.sensor_0.uifminorv", 2); + node_options.append_parameter_override("sensors.sensor_0.uifpatchv", 2); auto node = std::make_shared(node_options); ASSERT_TRUE(node != nullptr); rclcpp::Rate sleepRate(std::chrono::seconds(5)); From 3f97cdbb8f3f1c1c241998f07f4b3a7a269a4056 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Tue, 11 Jul 2023 10:09:37 +0200 Subject: [PATCH 04/15] formatting of key words --- Readme.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Readme.md b/Readme.md index 1974f4e..46c9865 100644 --- a/Readme.md +++ b/Readme.md @@ -89,16 +89,16 @@ For the setting up the ***sensors***: - `port`: port to be used to receive the packets, default is _55555_ - `frame_id`: name of the frame in which the messages will be published - `history_size`: size of history for the message publisher -- `inst_type`: the type of instruction serialization type, relevant to sensors using ethernet and should be _port_based_ -- `data_type`: the type of data serialization type, relevant to sensors using ethernet and should be _port_based_ +- `inst_type`: the type of instruction serialization type, relevant to sensors using ethernet and should be 'port_based' +- `data_type`: the type of data serialization type, relevant to sensors using ethernet and should be 'port_based' - `uifname`: the user interface name of the sensor - `uifmajorv`: the major version of the sensor user interface - `uifminorv`: the minor version of the sensor user interface - `uifpatchv`: the patch version of the sensor user interface For setting up the ***adapters***: -- `master_inst_serial_type`: the instruction serilization type of the master. When using a hybrid of 'can' and 'port' use _can_based_ -- `master_data_serial_type`: the data serilization type of the master. When using a hybrid of 'can' and 'port' use _can_based_ +- `master_inst_serial_type`: the instruction serilization type of the master. When using a hybrid of 'can' and 'port' use 'can_based' +- `master_data_serial_type`: the data serilization type of the master. When using a hybrid of 'can' and 'port' use 'can_based' - `hw_type`: the type of the hardware connection - `hw_dev_id`: adapter id of the hardware, the sensor and ***adapter should use the same id*** - `hw_iface_name`: name of the used network interface From e8fd86f7da3aace90a3f84d6f43c6ecb8a284f2b Mon Sep 17 00:00:00 2001 From: smartSRA Date: Thu, 17 Aug 2023 12:30:52 +0200 Subject: [PATCH 05/15] Added new CAN interface for T171 --- Readme.md | 19 +- umrr_ros2_driver/CMakeLists.txt | 21 +- .../smartmicro_radar_node.hpp | 115 +- umrr_ros2_driver/launch/radar.launch.py | 2 +- .../{ => example}/radar.adapter.example.yaml | 0 .../{ => example}/radar.sensor.example.yaml | 0 .../src/smartmicro_radar_node.cpp | 1069 +++++++++++------ 7 files changed, 796 insertions(+), 430 deletions(-) rename umrr_ros2_driver/param/{ => example}/radar.adapter.example.yaml (100%) rename umrr_ros2_driver/param/{ => example}/radar.sensor.example.yaml (100%) diff --git a/Readme.md b/Readme.md index 46c9865..ac05cbf 100644 --- a/Readme.md +++ b/Readme.md @@ -70,8 +70,8 @@ For setting up a sensor with ethernet or can, the interfaces of the should be se The sensor came equiped with two protocols: - ethernet: to set-up an ethernet interface the following command could be used `ifconfig my_interface_name 192.168.11.17 netmask 255.255.255.0`. The command above uses the default source ip address used by the sensors. -- can: to set-up a can interface the following commands could be used `slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx`and than `ip link set up my_interace_name`. -This uses the default baudrate of _500000_. +- can: if using LAWICEL to set-up a can interface the following commands could be used `slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx`and than `ip link set up my_interace_name`. +This uses the default baudrate of _500000_. When using Peak CAN the interfaces are recognized by linux and is only needed to set the baudrate. ### Node Configuration: The node is configured through the parameters. Here is a short recap of the most important parts. @@ -81,8 +81,8 @@ For more details, see the [`radar.sensor.example.yaml`](umrr_ros2_driver/param/r For the setting up the ***sensors***: - `link_type`: the type of hardware connection - `model`: the model of the sensor being used - - can: 'umrr11_can', 'umrr9d_can', 'umrr96_can', 'umrr9f_can_v1_1_1', 'umrr9f_can_v2_1_1' - - port: 'umrr11', 'umrr9d', 'umrr96', 'umrr9f_v1_1_1', 'umrr9f_v2_1_1' + - can: 'umrra4_can', 'umrr11_can', 'umrr9d_can', 'umrr96_can', 'umrr9f_can_v1_1_1', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1' + - port: 'umrr11', 'umrr9d', 'umrr96', 'umrr9f_v1_1_1', 'umrr9f_v2_1_1', 'umrra4' - `dev_id`: adapter id to which sensor is connected. ***The adapter and sensor should have the same dev_id*** - `id`: the client_id of the sensor/source, ***must be a _unique_ integer***. - `ip`: the ***_unique_*** ip address of the sensor or of the source acting as a sensor, required only for sensors using _ethernet_. @@ -105,10 +105,9 @@ For setting up the ***adapters***: - `baudrate`: the baudrate of the sensor connected with can, default is _500000_ - `port`: port to be used to receive the packets, default is _55555_ - ## Mode of operations of the sensors The smartmicro radars come equipped with numerous features and modes of operation. Using the ros2 services provided one -may access these modes. A list of available sensor modes is given in the [`sensor_params.json`](umrr_ros2_driver/config/sensor_params.json). +may access these modes and send commands to the sensor. A list of available sensor operations is given in the [`user_interfaces`](umrr_ros2_driver/smartmicro/user_interfaces/). A ros2 `SetMode` service should be called to implement these mode changes. There are three inputs to a ros2 service call: - `param`: name of the mode instruction (specific to the sensor) @@ -118,6 +117,13 @@ A ros2 `SetMode` service should be called to implement these mode changes. There 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 /smart_radar/set_radar_mode umrr_ros2_msgs/srv/SetMode "{param: "tx_antenna_idx", value: 2, sensor_id: 100}"` +Similarly, a ros2 `SendCommand` service could be used to send commands to the sensors. There are two inputs for sending a command: +- `command`: name of the command (specific to the sensor interface) +- `sensor_id`: the id of the sensor to which the service call should be sent. + +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}"` + ## 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. The IP addresses of the sensors could be assigned using: @@ -138,6 +144,7 @@ The call for such a service would be as follows: 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). diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index 87a0450..91436d3 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -58,7 +58,10 @@ install(FILES "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr96_t153_automotivev1.2.2_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev1.1.1_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.1.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.2.1_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.0.3_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.2.2_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_automotivev1.0.1_user_interface.so" DESTINATION lib) set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") @@ -66,19 +69,11 @@ set(CONFIG_FOLDER_PATH "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config") 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( "${PROJECT_SOURCE_DIR}/cmake/config_path.hpp.in" "${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/config_path.hpp" @ONLY) -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") @@ -88,7 +83,10 @@ smartmicro/include/umrr11_t132_automotive_v1_1_2 smartmicro/include/umrr96_t153_automotive_v1_2_2 smartmicro/include/umrr9f_t169_automotive_v1_1_1 smartmicro/include/umrr9f_t169_automotive_v2_1_1 -smartmicro/include/umrr9d_t152_automotive_v1_0_3) +smartmicro/include/umrr9f_t169_automotive_v2_2_1 +smartmicro/include/umrr9d_t152_automotive_v1_0_3 +smartmicro/include/umrr9d_t152_automotive_v1_2_2 +smartmicro/include/umrra4_automotive_v1_0_1) # link smart_access_lib-linux-x86_64-gcc_9 to the node target_link_libraries(smartmicro_radar_node @@ -100,7 +98,10 @@ target_link_libraries(smartmicro_radar_node umrr96_t153_automotivev1.2.2_user_interface umrr9f_t169_automotivev1.1.1_user_interface umrr9f_t169_automotivev2.1.1_user_interface - umrr9d_t152_automotivev1.0.3_user_interface) + umrr9f_t169_automotivev2.2.1_user_interface + umrr9d_t152_automotivev1.0.3_user_interface + umrr9d_t152_automotivev1.2.2_user_interface + umrra4_automotivev1.0.1_user_interface) rclcpp_components_register_node(smartmicro_radar_node PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" 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 37878eb..f6f6041 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 @@ -27,8 +27,11 @@ #include #include #include +#include #include #include +#include +#include #include #include @@ -93,7 +96,20 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { private: /// /// @brief A callback that is called when a new target list port for - /// umrr11 arrives. + /// umrra4 T171 arrives. + /// @param[in] sensor_idx The sensor id for the respected published topic. + /// @param[in] targetlist_port_umrra4 The targetlist port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra4( + const std::uint32_t sensor_idx, + const std::shared_ptr &targetlist_port_umrra4, + const com::types::ClientId client_id + ); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr11 T132 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. /// @param[in] target_list_port The target list port @@ -108,7 +124,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { /// /// @brief A callback that is called when a new target list port for - /// umrr96 arrives. + /// umrr96 T153 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. /// @param[in] target_list_port The target list port @@ -123,7 +139,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { /// /// @brief A callback that is called when a new target list port for - /// umrr9f arrives. + /// umrr9f_v1_1_1 T169 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. /// @param[in] target_list_port The target list port @@ -138,7 +154,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { /// /// @brief A callback that is called when a new target list port for - /// umrr9f arrives. + /// umrr9f_v2_1_1 T169 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. /// @param[in] target_list_port The target list port @@ -153,22 +169,37 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { /// /// @brief A callback that is called when a new target list port for - /// umrr96 arrives. + /// umrr9d_v1_0_3 T152 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( + void targetlist_callback_umrr9d_v1_0_3( const std::uint32_t sensor_idx, const std::shared_ptr - &targetlist_port_umrr9d, + &targetlist_port_umrr9d_v1_0_3, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9d_v1_2_2 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_v1_2_2( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_port_umrr9d_v1_2_2, const com::types::ClientId client_id); /// /// @brief A callback that is called when a new CAN target list for - /// umrr96 arrives. + /// umrr96_v1_0_3 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. /// @param[in] target_list_port The target list port @@ -198,7 +229,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { /// /// @brief A callback that is called when a new CAN target list for - /// umrr9f arrives. + /// umrr9f_v2_1_1 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. /// @param[in] target_list_port The target list port @@ -210,22 +241,67 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { comtargetbaselist::ComTargetBaseList> &targetlist_can_umrr9f_v2_1_1, const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9f_v2_2_1 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 CAN_targetlist_callback_umrr9f_v2_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_can_umrr9f_v2_2_1, + const com::types::ClientId client_id); /// /// @brief A callback that is called when a new CAN target list for - /// umrr9d arrives. + /// umrr9d_v1_=_§ 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 CAN_targetlist_callback_umrr9d( + void CAN_targetlist_callback_umrr9d_v1_0_3( const std::uint32_t sensor_idx, const std::shared_ptr - &targetlist_can_umrr9d, + &targetlist_can_umrr9d_v1_0_3, const com::types::ClientId client_id); - + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9d_v1_2_2 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 CAN_targetlist_callback_umrr9d_v1_2_2( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_can_umrr9d_v1_2_2, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrra4 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 CAN_targetlist_callback_umrra4( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_can_umrra4, + const com::types::ClientId client_id); + /// /// @brief Read parameters and update the json config files required by /// Smart Access C++ API. @@ -265,8 +341,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { /// /// @brief Configure the sensor ip address. /// - void - ip_address(const std::shared_ptr request, + void ip_address(const std::shared_ptr request, std::shared_ptr response); /// @@ -293,18 +368,16 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { std::uint64_t response_type{}; }; -/// -/// @brief Set flag used in callbacks and call ros2 shutdown. -/// -void terminate_on_receive(int signal); - bool check_signal = false; std::shared_ptr m_services{}; std::shared_ptr data_umrr11{}; std::shared_ptr data_umrr96{}; std::shared_ptr data_umrr9f_v1_1_1{}; std::shared_ptr data_umrr9f_v2_1_1{}; -std::shared_ptr data_umrr9d{}; +std::shared_ptr data_umrr9f_v2_2_1{}; +std::shared_ptr data_umrr9d_v1_0_3{}; +std::shared_ptr data_umrr9d_v1_2_2{}; +std::shared_ptr data_umrra4{}; } // namespace radar } // namespace drivers diff --git a/umrr_ros2_driver/launch/radar.launch.py b/umrr_ros2_driver/launch/radar.launch.py index 92a6471..c953427 100644 --- a/umrr_ros2_driver/launch/radar.launch.py +++ b/umrr_ros2_driver/launch/radar.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): radar_node = Node( package=PACKAGE_NAME, executable='smartmicro_radar_node_exe', - name='smarty_pants', + name='smart_micro_radars', parameters=[radar_sensor_params, radar_adapter_params] ) diff --git a/umrr_ros2_driver/param/radar.adapter.example.yaml b/umrr_ros2_driver/param/example/radar.adapter.example.yaml similarity index 100% rename from umrr_ros2_driver/param/radar.adapter.example.yaml rename to umrr_ros2_driver/param/example/radar.adapter.example.yaml diff --git a/umrr_ros2_driver/param/radar.sensor.example.yaml b/umrr_ros2_driver/param/example/radar.sensor.example.yaml similarity index 100% rename from umrr_ros2_driver/param/radar.sensor.example.yaml rename to umrr_ros2_driver/param/example/radar.sensor.example.yaml diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 8057216..49b4d6b 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -16,30 +16,32 @@ #include "umrr_ros2_driver/smartmicro_radar_node.hpp" -#include -#include - +#include #include #include #include #include #include #include +#include +#include #include #include #include #include - -#include - -#include +#include +#include +#include +#include #include #include #include #include #include - +#include +#include +#include #include #include #include @@ -47,8 +49,6 @@ #include #include "umrr_ros2_driver/config_path.hpp" -#include "umrr_ros2_driver/sensor_commands.hpp" -#include "umrr_ros2_driver/sensor_params.hpp" using com::common::Instruction; using com::master::CmdRequest; @@ -62,7 +62,8 @@ using com::master::ResponseBatch; using com::master::SetParamRequest; using point_cloud_msg_wrapper::PointCloud2Modifier; -namespace { +namespace +{ constexpr auto kDefaultClientId = 0; constexpr auto kDefaultInterfaceName = "lo"; constexpr auto kDefaultIp = "127.0.0.1"; @@ -102,12 +103,14 @@ constexpr auto kUIMajorVTag = "user_interface_major_v"; constexpr auto kUIMinorVTag = "user_interface_minor_v"; constexpr auto kUIPatchVTag = "user_interface_patch_v"; -constexpr bool float_eq(const float a, const float b) noexcept { +constexpr bool float_eq(const float a, const float b) noexcept +{ const auto maximum = std::max(std::fabs(a), std::fabs(b)); return std::fabs(a - b) <= maximum * std::numeric_limits::epsilon(); } -struct RadarPoint { +struct RadarPoint +{ float x{}; float y{}; float z{}; @@ -116,12 +119,11 @@ struct RadarPoint { float rcs{}; float noise{}; float snr{}; - constexpr friend bool operator==(const RadarPoint &p1, - const RadarPoint &p2) noexcept { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && float_eq(p1.radial_speed, p2.radial_speed) && - float_eq(p1.power, p2.power) && float_eq(p1.rcs, p2.rcs) && - float_eq(p1.noise, p2.noise) && float_eq(p1.snr, p2.snr); + constexpr friend bool operator==(const RadarPoint & p1, const RadarPoint & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && float_eq(p1.z, p2.z) && + float_eq(p1.radial_speed, p2.radial_speed) && float_eq(p1.power, p2.power) && + float_eq(p1.rcs, p2.rcs) && float_eq(p1.noise, p2.noise) && float_eq(p1.snr, p2.snr); } }; @@ -130,23 +132,23 @@ LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(power); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(rcs); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(noise); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(snr); -using Generators = std::tuple; +using Generators = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, field_radial_speed_generator, field_rcs_generator, + field_noise_generator, field_power_generator, field_snr_generator>; using RadarCloudModifier = PointCloud2Modifier; -} // namespace +} // namespace -namespace smartmicro { -namespace drivers { -namespace radar { -SmartmicroRadarNode::SmartmicroRadarNode( - const rclcpp::NodeOptions &node_options) - : rclcpp::Node{"smartmicro_radar_node", node_options} { - // auto options = rclcpp::NodeOptions().allow_undeclared_parameters(true); +namespace smartmicro +{ +namespace drivers +{ +namespace radar +{ +SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node{"smartmicro_radar_node", node_options} +{ update_config_files_from_params(); const auto override = false; @@ -156,89 +158,117 @@ SmartmicroRadarNode::SmartmicroRadarNode( m_services = CommunicationServicesIface::Get(); if (!m_services->Init()) { throw std::runtime_error("Initialization failed"); - // return 1; } // Getting the data stream service - data_umrr11 = - com::master::umrr11_t132_automotive_v1_1_2::DataStreamServiceIface::Get(); - data_umrr96 = - com::master::umrr96_t153_automotive_v1_2_2::DataStreamServiceIface::Get(); - data_umrr9d = - com::master::umrr9d_t152_automotive_v1_0_3::DataStreamServiceIface::Get(); + data_umrr11 = com::master::umrr11_t132_automotive_v1_1_2::DataStreamServiceIface::Get(); + data_umrr96 = com::master::umrr96_t153_automotive_v1_2_2::DataStreamServiceIface::Get(); + data_umrr9d_v1_0_3 = com::master::umrr9d_t152_automotive_v1_0_3::DataStreamServiceIface::Get(); + data_umrr9d_v1_2_2 = com::master::umrr9d_t152_automotive_v1_2_2::DataStreamServiceIface::Get(); data_umrr9f_v1_1_1 = com::master::umrr9f_t169_automotive_v1_1_1::DataStreamServiceIface::Get(); - data_umrr9f_v2_1_1 = - com::master::umrr9f_t169_automotive_v2_1_1::DataStreamServiceIface::Get(); + data_umrr9f_v2_1_1 = com::master::umrr9f_t169_automotive_v2_1_1::DataStreamServiceIface::Get(); + data_umrr9f_v2_2_1 = com::master::umrr9f_t169_automotive_v2_2_1::DataStreamServiceIface::Get(); + data_umrra4 = com::master::umrra4_automotive_v1_0_1::DataStreamServiceIface::Get(); // Wait for initailization std::this_thread::sleep_for(std::chrono::seconds(2)); RCLCPP_INFO(this->get_logger(), "Data stream services have been received!"); for (auto i = 0UL; i < m_number_of_sensors; ++i) { - const auto &sensor = m_sensors[i]; + const auto & sensor = m_sensors[i]; m_publishers[i] = create_publisher( - "smart_radar/targets_" + std::to_string(i), sensor.history_size); - - if (sensor.model == "umrr96_can" && - com::types::ERROR_CODE_OK != - data_umrr96->RegisterComTargetBaseListReceiveCallback( - sensor.id, - std::bind(&SmartmicroRadarNode::CAN_targetlist_callback_umrr96, - this, i, std::placeholders::_1, - std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr96" - << std::endl; - } - if (sensor.model == "umrr11_can" && - com::types::ERROR_CODE_OK != - data_umrr11->RegisterComTargetBaseListReceiveCallback( - sensor.id, - std::bind(&SmartmicroRadarNode::CAN_targetlist_callback_umrr11, - this, i, std::placeholders::_1, - std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr11" - << std::endl; - } - if (sensor.model == "umrr9d_can" && - com::types::ERROR_CODE_OK != - data_umrr9d->RegisterComTargetBaseListReceiveCallback( - sensor.id, - std::bind(&SmartmicroRadarNode::CAN_targetlist_callback_umrr9d, - this, i, std::placeholders::_1, - std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9d" - << std::endl; - } - if (sensor.model == "umrr9f_can_v2_1_1" && - com::types::ERROR_CODE_OK != - data_umrr9f_v2_1_1->RegisterComTargetBaseListReceiveCallback( - sensor.id, - std::bind(&SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1, - this, i, std::placeholders::_1, - std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f" - << std::endl; - } - if (sensor.model == "umrr96" && - com::types::ERROR_CODE_OK != - data_umrr96->RegisterComTargetListReceiveCallback( - sensor.id, - std::bind(&SmartmicroRadarNode::targetlist_callback_umrr96, - this, i, std::placeholders::_1, - std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr96" - << std::endl; - } - if (sensor.model == "umrr11" && - com::types::ERROR_CODE_OK != - data_umrr11->RegisterComTargetListReceiveCallback( - sensor.id, - std::bind(&SmartmicroRadarNode::targetlist_callback_umrr11, - this, i, std::placeholders::_1, - std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr11" - << std::endl; + "smart_radar/targets_" + std::to_string(i), sensor.history_size); + + if ( + sensor.model == "umrra4_can" && + com::types::ERROR_CODE_OK != + data_umrra4->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrra4, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrra4" << std::endl; + } + if ( + sensor.model == "umrr96_can" && + com::types::ERROR_CODE_OK != + data_umrr96->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr96, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr96" << std::endl; + } + if ( + sensor.model == "umrr11_can" && + com::types::ERROR_CODE_OK != + data_umrr11->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr11, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; + } + if ( + sensor.model == "umrr9d_can_v1_0_3" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_0_3->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_0_3, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_0_3" << std::endl; + } + if ( + sensor.model == "umrr9d_can_v1_2_2" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_2_2->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_2_2" << std::endl; + } + if ( + sensor.model == "umrr9f_can_v2_1_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_1_1->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_1_1" << std::endl; + } + if ( + sensor.model == "umrr9f_can_v2_2_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_2_1->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_2_1" << std::endl; + } + if ( + sensor.model == "umrra4" && + com::types::ERROR_CODE_OK != + data_umrra4->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra4, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrra4" << std::endl; + } + if ( + sensor.model == "umrr96" && + com::types::ERROR_CODE_OK != + data_umrr96->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr96, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr96" << std::endl; + } + if ( + sensor.model == "umrr11" && + com::types::ERROR_CODE_OK != + data_umrr11->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr11, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; } if ( sensor.model == "umrr9f_v1_1_1" && @@ -249,40 +279,61 @@ SmartmicroRadarNode::SmartmicroRadarNode( std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrr9f_v1_1_1" << std::endl; } - if (sensor.model == "umrr9f_v2_1_1" && - com::types::ERROR_CODE_OK != - data_umrr9f_v2_1_1->RegisterComTargetListReceiveCallback( - sensor.id, - std::bind(&SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1, - this, i, std::placeholders::_1, - std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f" - << std::endl; + if ( + sensor.model == "umrr9f_v2_1_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_1_1->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f" << std::endl; + } + if ( + sensor.model == "umrr9d_v1_0_3" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_0_3->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_3, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_0_3" << std::endl; } - if (sensor.model == "umrr9d" && - com::types::ERROR_CODE_OK != - data_umrr9d->RegisterComTargetListReceiveCallback( - 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; + if ( + sensor.model == "umrr9d_v1_2_2" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_2_2->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_2, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_2_2" << std::endl; } } // create a ros2 service to change the radar parameters mode_srv_ = create_service( - "smart_radar/set_radar_mode", - std::bind(&SmartmicroRadarNode::radar_mode, this, std::placeholders::_1, - std::placeholders::_2)); + "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( + "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."); - rclcpp::on_shutdown( - std::bind(&SmartmicroRadarNode::on_shutdown_callback, this)); + + rclcpp::on_shutdown(std::bind(&SmartmicroRadarNode::on_shutdown_callback, this)); + } -void SmartmicroRadarNode::on_shutdown_callback() { +void SmartmicroRadarNode::on_shutdown_callback() +{ RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "SHUTDOWN CALLED!!!"); check_signal = true; rclcpp::Rate sleepRate(std::chrono::milliseconds(100)); @@ -291,33 +342,16 @@ void SmartmicroRadarNode::on_shutdown_callback() { } void SmartmicroRadarNode::radar_mode( - const std::shared_ptr request, - std::shared_ptr result) { + const std::shared_ptr request, + std::shared_ptr result) +{ std::string instruction_name{}; - bool check_flag_param = false; bool check_flag_id = false; - std::ifstream instr_file(KSensorParamFilePath); - auto param_table = nlohmann::json::parse(instr_file); - instruction_name = request->param; client_id = request->sensor_id; - for (const auto &item : param_table.items()) { - for (const auto ¶m : item.value().items()) { - if (instruction_name == param.value()["name"]) { - check_flag_param = true; - break; - } - } - } - - if (!check_flag_param) { - result->res = "Invalid instruction name or value! "; - return; - } - - for (auto &sensor : m_sensors) { + for (auto & sensor : m_sensors) { if (client_id == sensor.id) { check_flag_id = true; break; @@ -328,11 +362,9 @@ void SmartmicroRadarNode::radar_mode( return; } - std::shared_ptr inst{ - m_services->GetInstructionService()}; + std::shared_ptr inst{m_services->GetInstructionService()}; timer = this->create_wall_timer( - std::chrono::seconds(2), - std::bind(&SmartmicroRadarNode::my_timer_callback, this)); + std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); std::shared_ptr batch; @@ -342,12 +374,11 @@ void SmartmicroRadarNode::radar_mode( } std::shared_ptr> radar_mode_01 = - std::make_shared>( - "auto_interface_0dim", request->param, request->value); + std::make_shared>( + "auto_interface_0dim", request->param, request->value); std::shared_ptr> radar_mode_02 = - std::make_shared>("auto_interface_0dim", - request->param, request->value); + std::make_shared>("auto_interface_0dim", request->param, request->value); if (!batch->AddRequest(radar_mode_01)) { result->res = "Failed to add instruction to the batch! "; @@ -356,75 +387,238 @@ void SmartmicroRadarNode::radar_mode( result->res = "Failed to add instruction to the batch! "; } - if (com::types::ERROR_CODE_OK != - inst->SendInstructionBatch( - batch, std::bind(&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!"; + if ( + com::types::ERROR_CODE_OK != inst->SendInstructionBatch( + batch, std::bind( + &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; + } + result->res = "Service conducted successfully"; +} + +void SmartmicroRadarNode::ip_address( + const std::shared_ptr request, + std::shared_ptr result) +{ + std::shared_ptr inst{m_services->GetInstructionService()}; + timer = this->create_wall_timer( + std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); + bool check_flag = false; + client_id = request->sensor_id; + for (auto & sensor : m_sensors) { + if (client_id == sensor.id) { + check_flag = true; + break; + } + } + if (!check_flag) { + result->res_ip = "Sensor ID entered is not listed in the param file! "; + return; + } + + std::shared_ptr batch; + if (!inst->AllocateInstructionBatch(client_id, batch)) { + result->res_ip = "Failed to allocate instruction batch! "; + return; + } + + std::shared_ptr> ip_address = + std::make_shared>( + "auto_interface_0dim", "ip_source_address", request->value_ip); + + std::shared_ptr cmd = + std::make_shared("auto_interface_command", "comp_eeprom_ctrl_save_param_sec", 2010); + + if (!batch->AddRequest(ip_address)) { + result->res_ip = "Failed to add instruction to batch! "; + return; + } + if (!batch->AddRequest(cmd)) { + result->res_ip = "Failed to add instruction to batch! "; + return; + } + // send instruction batch to the device + if ( + com::types::ERROR_CODE_OK != + inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::sensor_response_ip, this, client_id, std::placeholders::_2))) { + result->res_ip = "Service not conducted"; + return; + } else { + RCLCPP_INFO( + this->get_logger(), + "Radar must be restarted and the parameters in the param file " + "must be updated !!."); + result->res_ip = "Service conducted successfully"; + } +} + +void SmartmicroRadarNode::radar_command( + const std::shared_ptr request, + std::shared_ptr result) +{ + std::string command_name{}; + bool check_flag_id = false; + + command_name = request->command; + client_id = request->sensor_id; + + 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) { + const com::types::ClientId client_id, + const std::shared_ptr & response, const std::string instruction_name) +{ std::vector>> myResp_1; std::vector>> myResp_2; - if (response->GetResponse("auto_interface_0dim", - instruction_name.c_str(), myResp_1)) { - for (auto &resp : myResp_1) { + if (response->GetResponse("auto_interface_0dim", instruction_name.c_str(), myResp_1)) { + for (auto & resp : myResp_1) { + response_type = resp->GetResponseType(); + RCLCPP_INFO( + this->get_logger(), "Response from '%s' : %i", instruction_name.c_str(), response_type); + } + } + if (response->GetResponse("auto_interface_0dim", instruction_name.c_str(), myResp_2)) { + for (auto & resp : myResp_2) { + response_type = resp->GetResponseType(); + RCLCPP_INFO( + this->get_logger(), "Response from '%s' : %i", instruction_name.c_str(), response_type); + } + } +} + +void SmartmicroRadarNode::sensor_response_ip( + const com::types::ClientId client_id, + const std::shared_ptr & response) +{ + std::vector>> myResp_2; + if (response->GetResponse("auto_interface_0dim", "ip_source_address", myResp_2)) { + for (auto & resp : myResp_2) { response_type = resp->GetResponseType(); - RCLCPP_INFO(this->get_logger(), "Response from '%s' : %i", - instruction_name.c_str(), response_type); + RCLCPP_INFO(this->get_logger(), "Response from sensor for ip change: %i", response_type); } } - if (response->GetResponse("auto_interface_0dim", - instruction_name.c_str(), myResp_2)) { - for (auto &resp : myResp_2) { +} + +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 '%s' : %i", - instruction_name.c_str(), response_type); + RCLCPP_INFO(this->get_logger(), "Response from sensor to command: %i", response_type); } } } +void SmartmicroRadarNode::targetlist_callback_umrra4( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrra4" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrra4->GetPortHeader(); + 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_umrra4->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->GetNoise(); + 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->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + void SmartmicroRadarNode::targetlist_callback_umrr96( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_port_umrr96, - const com::types::ClientId client_id) { + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr96, + const com::types::ClientId client_id) +{ std::cout << "Targetlist for umrr96" << std::endl; if (!check_signal) { - std::shared_ptr< - com::master::umrr96_t153_automotive_v1_2_2::comtargetlist::PortHeader> - port_header; + std::shared_ptr + port_header; port_header = targetlist_port_umrr96->GetPortHeader(); 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); + 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_umrr96->GetTargetList()) { + for (const auto & target : targetlist_port_umrr96->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->GetNoise(); - 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->GetNoise(), snr}); + 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->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); @@ -432,77 +626,101 @@ void SmartmicroRadarNode::targetlist_callback_umrr96( } void SmartmicroRadarNode::targetlist_callback_umrr11( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_port_umrr11, - const com::types::ClientId client_id) { + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr11, + const com::types::ClientId client_id) +{ std::cout << "Targetlist for umrr11" << std::endl; if (!check_signal) { - std::shared_ptr< - com::master::umrr11_t132_automotive_v1_1_2::comtargetlist::PortHeader> - port_header; + std::shared_ptr + port_header; port_header = targetlist_port_umrr11->GetPortHeader(); 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); + 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_umrr11->GetTargetList()) { + for (const auto & target : targetlist_port_umrr11->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->GetNoise(); - 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->GetNoise(), snr}); + 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->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); } } -void SmartmicroRadarNode::targetlist_callback_umrr9d( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_port_umrr9d, - const com::types::ClientId client_id) { - std::cout << "Targetlist for umrr9d" << std::endl; +void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_3( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9d_v1_0_3, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_0_3" << std::endl; if (!check_signal) { - std::shared_ptr< - com::master::umrr9d_t152_automotive_v1_0_3::comtargetlist::PortHeader> - port_header; - port_header = targetlist_port_umrr9d->GetPortHeader(); + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9d_v1_0_3->GetPortHeader(); 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); + 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()) { + for (const auto & target : targetlist_port_umrr9d_v1_0_3->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->GetNoise(); - 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->GetNoise(), snr}); + 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->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_2( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9d_v1_2_2, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_2_2" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9d_v1_2_2->GetPortHeader(); + 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_v1_2_2->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->GetNoise(); + 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->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); @@ -546,38 +764,68 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v1_1_1( } void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_port_umrr9f_v2_1_1, - const com::types::ClientId client_id) { + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_v2_1_1, + const com::types::ClientId client_id) +{ std::cout << "Targetlist for umrr9f" << std::endl; if (!check_signal) { - std::shared_ptr< - com::master::umrr9f_t169_automotive_v2_1_1::comtargetlist::PortHeader> - port_header; + std::shared_ptr + port_header; port_header = targetlist_port_umrr9f_v2_1_1->GetPortHeader(); 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); + 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_v2_1_1->GetTargetList()) { + for (const auto & target : targetlist_port_umrr9f_v2_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->GetNoise(); - 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->GetNoise(), snr}); + 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->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrra4( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrra4_automotive_v1_0_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrra4, + const com::types::ClientId client_id) +{ + std::cout << "CAN Targetlist for umrra4" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrra4->GetPortHeader(); + 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_can_umrra4->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->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); @@ -585,38 +833,34 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1( } void SmartmicroRadarNode::CAN_targetlist_callback_umrr96( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_can_umrr96, - const com::types::ClientId client_id) { + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr96_t153_automotive_v1_2_2::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr96, + const com::types::ClientId client_id) +{ std::cout << "CAN Targetlist for umrr96" << std::endl; if (!check_signal) { - std::shared_ptr - port_header; + std::shared_ptr + port_header; port_header = targetlist_can_umrr96->GetPortHeader(); 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); + 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_can_umrr96->GetTargetList()) { + for (const auto & target : targetlist_can_umrr96->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->GetSignalLevel() - target->GetNoise(); - modifier.push_back({range_2d * std::cos(azimuth_angle), - range_2d * std::sin(azimuth_angle), - range * std::sin(elevation_angle), - target->GetSpeedRadial(), target->GetSignalLevel(), - target->GetRCS(), target->GetNoise(), snr}); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); @@ -624,77 +868,104 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr96( } void SmartmicroRadarNode::CAN_targetlist_callback_umrr11( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_can_umrr11, - const com::types::ClientId client_id) { + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr11_t132_automotive_v1_1_2::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr11, + const com::types::ClientId client_id) +{ std::cout << "CAN Targetlist for umrr11" << std::endl; if (!check_signal) { - std::shared_ptr - port_header; + std::shared_ptr + port_header; port_header = targetlist_can_umrr11->GetPortHeader(); 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); + 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_can_umrr11->GetTargetList()) { + for (const auto & target : targetlist_can_umrr11->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->GetSignalLevel() - target->GetNoise(); - modifier.push_back({range_2d * std::cos(azimuth_angle), - range_2d * std::sin(azimuth_angle), - range * std::sin(elevation_angle), - target->GetSpeedRadial(), target->GetSignalLevel(), - target->GetRCS(), target->GetNoise(), snr}); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); } } -void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_can_umrr9d, - const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9d" << std::endl; +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_0_3( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_0_3::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9d_v1_0_3, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist callback is being called for umrr9d_v1_0_3" << std::endl; if (!check_signal) { - std::shared_ptr - port_header; - port_header = targetlist_can_umrr9d->GetPortHeader(); + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9d_v1_0_3->GetPortHeader(); 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); + 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_can_umrr9d->GetTargetList()) { + for (const auto & target : targetlist_can_umrr9d_v1_0_3->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->GetSignalLevel() - target->GetNoise(); - modifier.push_back({range_2d * std::cos(azimuth_angle), - range_2d * std::sin(azimuth_angle), - range * std::sin(elevation_angle), - target->GetSpeedRadial(), target->GetSignalLevel(), - target->GetRCS(), target->GetNoise(), snr}); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_2_2::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9d_v1_2_2, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist callback is being called for umrr9d_v1_2_2" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9d_v1_2_2->GetPortHeader(); + 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_can_umrr9d_v1_2_2->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->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); @@ -702,87 +973,109 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d( } void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_can_umrr9f_v2_1_1, - const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9f" << std::endl; + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_1_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v2_1_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist callback is being called for umrr9f_v2_1_1" << std::endl; if (!check_signal) { - std::shared_ptr - port_header; + std::shared_ptr + port_header; port_header = targetlist_can_umrr9f_v2_1_1->GetPortHeader(); 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); + 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_can_umrr9f_v2_1_1->GetTargetList()) { + for (const auto & target : targetlist_can_umrr9f_v2_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->GetSignalLevel() - target->GetNoise(); - modifier.push_back({range_2d * std::cos(azimuth_angle), - range_2d * std::sin(azimuth_angle), - range * std::sin(elevation_angle), - target->GetSpeedRadial(), target->GetSignalLevel(), - target->GetRCS(), target->GetNoise(), snr}); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); } } -void SmartmicroRadarNode::update_config_files_from_params() { - const auto master_inst_serial_type = - declare_parameter(kInstSerialTypeTag, std::string{}); - const auto master_data_serial_type = - declare_parameter(kDataSerialTypeTag, std::string{}); +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_2_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v2_2_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist callback is being called for umrr9f_v2_2_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9f_v2_2_1->GetPortHeader(); + 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_can_umrr9f_v2_2_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->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + +void SmartmicroRadarNode::update_config_files_from_params() +{ + const auto master_inst_serial_type = declare_parameter(kInstSerialTypeTag, std::string{}); + const auto master_data_serial_type = declare_parameter(kDataSerialTypeTag, std::string{}); auto read_adapter_params_if_possible = [&](const std::uint32_t index) { RCLCPP_INFO(this->get_logger(), "Updating ADAPTER params."); - auto ¤t_adapter = m_adapters[index]; + auto & current_adapter = m_adapters[index]; const auto prefix_2 = "adapters.adapter_" + std::to_string(index); - current_adapter.hw_dev_id = - this->declare_parameter(prefix_2 + ".hw_dev_id", kDefaultHwDevId); + current_adapter.hw_dev_id = this->declare_parameter(prefix_2 + ".hw_dev_id", kDefaultHwDevId); if (current_adapter.hw_dev_id == kDefaultHwDevId) { // The id was not set, so the adapter with this index was not defined. // Stop here. return false; } - current_adapter.hw_iface_name = this->declare_parameter( - prefix_2 + ".hw_iface_name", kDefaultHwDevIface); - current_adapter.hw_type = - this->declare_parameter(prefix_2 + ".hw_type", kDefaultHwLinkType); - current_adapter.baudrate = - this->declare_parameter(prefix_2 + ".baudrate", 500000); - current_adapter.port = - this->declare_parameter(prefix_2 + ".port", kDefaultPort); + current_adapter.hw_iface_name = + this->declare_parameter(prefix_2 + ".hw_iface_name", kDefaultHwDevIface); + current_adapter.hw_type = this->declare_parameter(prefix_2 + ".hw_type", kDefaultHwLinkType); + current_adapter.baudrate = this->declare_parameter(prefix_2 + ".baudrate", 500000); + current_adapter.port = this->declare_parameter(prefix_2 + ".port", kDefaultPort); return true; }; - // [] start the lambda function - // & for the capturing value by reference - // const std::uint32_t index specifies parameter list of lambda + auto read_sensor_params_if_possible = [&](const std::uint32_t index) { RCLCPP_INFO(this->get_logger(), "Updating SENSOR params."); - auto &sensor = m_sensors[index]; + auto & sensor = m_sensors[index]; const auto prefix_3 = "sensors.sensor_" + std::to_string(index); - sensor.dev_id = - this->declare_parameter(prefix_3 + ".dev_id", kDefaultHwDevId); + sensor.dev_id = this->declare_parameter(prefix_3 + ".dev_id", kDefaultHwDevId); sensor.uifname = this->declare_parameter(prefix_3 + ".uifname", ""); sensor.uifmajorv = this->declare_parameter(prefix_3 + ".uifmajorv", 0); sensor.uifminorv = this->declare_parameter(prefix_3 + ".uifminorv", 0); sensor.uifpatchv = this->declare_parameter(prefix_3 + ".uifpatchv", 0); - sensor.model = - this->declare_parameter(prefix_3 + ".model", kDefaultSensorType); + sensor.model = this->declare_parameter(prefix_3 + ".model", kDefaultSensorType); sensor.id = this->declare_parameter(prefix_3 + ".id", kDefaultClientId); if (sensor.id == kDefaultClientId) { // The id was not set, so the sensor with this index was not defined. Stop @@ -791,14 +1084,11 @@ void SmartmicroRadarNode::update_config_files_from_params() { } sensor.ip = this->declare_parameter(prefix_3 + ".ip", ""); sensor.port = this->declare_parameter(prefix_3 + ".port", 0); - sensor.frame_id = - this->declare_parameter(prefix_3 + ".frame_id", kDefaultFrameId); - sensor.history_size = this->declare_parameter(prefix_3 + ".history_size", - kDefaultHistorySize); + sensor.frame_id = this->declare_parameter(prefix_3 + ".frame_id", kDefaultFrameId); + sensor.history_size = this->declare_parameter(prefix_3 + ".history_size", kDefaultHistorySize); sensor.inst_type = this->declare_parameter(prefix_3 + ".inst_type", ""); sensor.data_type = this->declare_parameter(prefix_3 + ".data_type", ""); - sensor.link_type = - this->declare_parameter(prefix_3 + ".link_type", kDefaultHwLinkType); + sensor.link_type = this->declare_parameter(prefix_3 + ".link_type", kDefaultHwLinkType); return true; }; @@ -828,17 +1118,15 @@ void SmartmicroRadarNode::update_config_files_from_params() { config[kInstSerialTypeJsonTag] = master_inst_serial_type; std::ofstream{kConfigFilePath, std::ios::trunc} << config; - auto hw_inventory = - nlohmann::json::parse(std::ifstream{kHwInventoryFilePath}); - auto &hw_items = hw_inventory[kHwItemsJsonTag]; + auto hw_inventory = nlohmann::json::parse(std::ifstream{kHwInventoryFilePath}); + auto & hw_items = hw_inventory[kHwItemsJsonTag]; if (hw_items.empty()) { - throw std::runtime_error( - "There are no 'hwItems' defined in the hw_inventory.json file."); + throw std::runtime_error("There are no 'hwItems' defined in the hw_inventory.json file."); } auto hw_item = hw_items.front(); hw_items.clear(); for (auto j = 0UL; j < m_number_of_adapters; ++j) { - const auto &adapter = m_adapters[j]; + const auto & adapter = m_adapters[j]; hw_item[kPortTag] = adapter.port; hw_item[kHwDevLinkTag] = adapter.hw_type; hw_item[kHwDevIdTag] = adapter.hw_dev_id; @@ -848,19 +1136,17 @@ void SmartmicroRadarNode::update_config_files_from_params() { } std::ofstream{kHwInventoryFilePath, std::ios::trunc} << hw_inventory; - auto routing_table = - nlohmann::json::parse(std::ifstream{kRoutingTableFilePath}); - auto &clients = routing_table[kClientsJsonTag]; + auto routing_table = nlohmann::json::parse(std::ifstream{kRoutingTableFilePath}); + auto & clients = routing_table[kClientsJsonTag]; if (clients.empty()) { - throw std::runtime_error( - "There are no 'clients' defined in the routing_table.json file."); + throw std::runtime_error("There are no 'clients' defined in the routing_table.json file."); } - auto client = clients.front(); // Make a copy of the first client. + auto client = clients.front(); // Make a copy of the first client. clients.clear(); for (auto i = 0UL; i < m_number_of_sensors; ++i) { - const auto &sensor = m_sensors[i]; + const auto & sensor = m_sensors[i]; client[kClientLinkTag] = sensor.link_type; - client[kClientIdTag] = sensor.id; // + client[kClientIdTag] = sensor.id; // client[kHwDevIdTag] = sensor.dev_id; client[kPortTag] = sensor.port; client[kIpTag] = sensor.ip; @@ -873,12 +1159,11 @@ void SmartmicroRadarNode::update_config_files_from_params() { clients.push_back(client); } - std::ofstream{kRoutingTableFilePath, std::ios::trunc} << std::setw(4) - << routing_table; + std::ofstream{kRoutingTableFilePath, std::ios::trunc} << std::setw(4) << routing_table; } -} // namespace radar -} // namespace drivers -} // namespace smartmicro +} // namespace radar +} // namespace drivers +} // namespace smartmicro RCLCPP_COMPONENTS_REGISTER_NODE(smartmicro::drivers::radar::SmartmicroRadarNode) From 3f93a815f021295d3d12af24bce97b74d7678a9d Mon Sep 17 00:00:00 2001 From: smartSRA Date: Mon, 4 Sep 2023 11:00:39 +0200 Subject: [PATCH 06/15] remove test and add missig interface --- smart_extract.sh | 2 +- umrr_ros2_driver/CMakeLists.txt | 6 +- .../smartmicro_radar_node.hpp | 54 ++++-- .../src/smartmicro_radar_node.cpp | 181 +++++++++++------- .../test/test_smartmicro_radar_node.cpp | 47 ----- 5 files changed, 151 insertions(+), 139 deletions(-) delete mode 100644 umrr_ros2_driver/test/test_smartmicro_radar_node.cpp diff --git a/smart_extract.sh b/smart_extract.sh index 6034e6c..3e0f640 100755 --- a/smart_extract.sh +++ b/smart_extract.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -smart_pack=SmartAccessAutomotive_3_0_0.tgz +smart_pack=SmartAccessAutomotive_3_3_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 91436d3..697888a 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -44,7 +44,7 @@ if(NOT json_POPULATED) endif() ament_auto_find_build_dependencies() -set(SMARTMICRO_LIB_DIR "lib-linux-x86_64-gcc_7") +set(SMARTMICRO_LIB_DIR "lib-linux-x86_64-gcc_9") # specify the path to search for the libraries link_directories(${CMAKE_CURRENT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}) @@ -112,10 +112,6 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake) add_launch_test(test/radar_node_test.launch.py TIMEOUT "150") - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(test_smartmicro_radar_node test/test_smartmicro_radar_node.cpp) - target_link_libraries(test_smartmicro_radar_node smartmicro_radar_node) - # Not applying ros2 linters on external sources list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright 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 f6f6041..a439b0c 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 @@ -17,6 +17,7 @@ #ifndef UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ #define UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ + #include #include #include @@ -24,14 +25,14 @@ #include #include +#include #include #include -#include -#include #include #include #include -#include +#include +#include #include #include @@ -94,19 +95,6 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { void on_shutdown_callback(); private: - /// - /// @brief A callback that is called when a new target list port for - /// umrra4 T171 arrives. - /// @param[in] sensor_idx The sensor id for the respected published topic. - /// @param[in] targetlist_port_umrra4 The targetlist port - /// @param[in] client_id The client_id of the sensor - /// - void targetlist_callback_umrra4( - const std::uint32_t sensor_idx, - const std::shared_ptr &targetlist_port_umrra4, - const com::types::ClientId client_id - ); - /// /// @brief A callback that is called when a new target list port for /// umrr11 T132 arrives. @@ -167,6 +155,21 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { &targetlist_port_umrr9f_v2_1_1, const com::types::ClientId client_id); + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v2_2_1 T169 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_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_port_umrr9f_v2_2_1, + const com::types::ClientId client_id); + /// /// @brief A callback that is called when a new target list port for /// umrr9d_v1_0_3 T152 arrives. @@ -196,6 +199,19 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { comtargetlist::ComTargetList> &targetlist_port_umrr9d_v1_2_2, const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrra4 T171_v1_0_1 arrives. + /// @param[in] sensor_idx The sensor id for the respected published topic. + /// @param[in] targetlist_port_umrra4 The targetlist port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra4_v1_0_1( + const std::uint32_t sensor_idx, + const std::shared_ptr &targetlist_port_umrra4_v1_0_1, + const com::types::ClientId client_id + ); /// /// @brief A callback that is called when a new CAN target list for @@ -295,11 +311,11 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { /// @param[in] target_list_port The target list port /// @param[in] client_id The client_id of the sensor /// - void CAN_targetlist_callback_umrra4( + void CAN_targetlist_callback_umrra4_v1_0_1( const std::uint32_t sensor_idx, const std::shared_ptr - &targetlist_can_umrra4, + &targetlist_can_umrra4_v1_0_1, const com::types::ClientId client_id); /// @@ -370,6 +386,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { bool check_signal = false; std::shared_ptr m_services{}; +std::shared_ptr data_umrra4_v1_0_1{}; std::shared_ptr data_umrr11{}; std::shared_ptr data_umrr96{}; std::shared_ptr data_umrr9f_v1_1_1{}; @@ -377,7 +394,6 @@ std::shared_ptr data_umrr9f_v2_2_1{}; std::shared_ptr data_umrr9d_v1_0_3{}; std::shared_ptr data_umrr9d_v1_2_2{}; -std::shared_ptr data_umrra4{}; } // 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 49b4d6b..fed81ba 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -16,7 +16,11 @@ #include "umrr_ros2_driver/smartmicro_radar_node.hpp" -#include +#include +#include + +#include +#include #include #include #include @@ -31,17 +35,17 @@ #include #include #include -#include -#include + +#include + +#include #include #include #include #include #include -#include -#include -#include + #include #include #include @@ -161,14 +165,14 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option } // Getting the data stream service + data_umrra4_v1_0_1 = com::master::umrra4_automotive_v1_0_1::DataStreamServiceIface::Get(); data_umrr11 = com::master::umrr11_t132_automotive_v1_1_2::DataStreamServiceIface::Get(); data_umrr96 = com::master::umrr96_t153_automotive_v1_2_2::DataStreamServiceIface::Get(); - data_umrr9d_v1_0_3 = com::master::umrr9d_t152_automotive_v1_0_3::DataStreamServiceIface::Get(); - data_umrr9d_v1_2_2 = com::master::umrr9d_t152_automotive_v1_2_2::DataStreamServiceIface::Get(); data_umrr9f_v1_1_1 = com::master::umrr9f_t169_automotive_v1_1_1::DataStreamServiceIface::Get(); data_umrr9f_v2_1_1 = com::master::umrr9f_t169_automotive_v2_1_1::DataStreamServiceIface::Get(); data_umrr9f_v2_2_1 = com::master::umrr9f_t169_automotive_v2_2_1::DataStreamServiceIface::Get(); - data_umrra4 = com::master::umrra4_automotive_v1_0_1::DataStreamServiceIface::Get(); + data_umrr9d_v1_0_3 = com::master::umrr9d_t152_automotive_v1_0_3::DataStreamServiceIface::Get(); + data_umrr9d_v1_2_2 = com::master::umrr9d_t152_automotive_v1_2_2::DataStreamServiceIface::Get(); // Wait for initailization std::this_thread::sleep_for(std::chrono::seconds(2)); @@ -181,130 +185,139 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option "smart_radar/targets_" + std::to_string(i), sensor.history_size); if ( - sensor.model == "umrra4_can" && + sensor.model == "umrra4_v1_0_1" && com::types::ERROR_CODE_OK != - data_umrra4->RegisterComTargetBaseListReceiveCallback( + data_umrra4_v1_0_1->RegisterComTargetListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::CAN_targetlist_callback_umrra4, this, i, + &SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1, this, i, std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrra4" << std::endl; } if ( - sensor.model == "umrr96_can" && + sensor.model == "umrr96" && com::types::ERROR_CODE_OK != - data_umrr96->RegisterComTargetBaseListReceiveCallback( + data_umrr96->RegisterComTargetListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::CAN_targetlist_callback_umrr96, this, i, + &SmartmicroRadarNode::targetlist_callback_umrr96, this, i, std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrr96" << std::endl; } if ( - sensor.model == "umrr11_can" && + sensor.model == "umrr11" && com::types::ERROR_CODE_OK != - data_umrr11->RegisterComTargetBaseListReceiveCallback( + data_umrr11->RegisterComTargetListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::CAN_targetlist_callback_umrr11, this, i, + &SmartmicroRadarNode::targetlist_callback_umrr11, this, i, std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; } if ( - sensor.model == "umrr9d_can_v1_0_3" && + sensor.model == "umrr9f_v1_1_1" && com::types::ERROR_CODE_OK != - data_umrr9d_v1_0_3->RegisterComTargetBaseListReceiveCallback( + data_umrr9f_v1_1_1->RegisterComTargetListPortReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_0_3, 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 umrr9d_v1_0_3" << std::endl; + std::cout << "Failed to register targetlist callback for sensor umrr9f_v1_1_1" << std::endl; } if ( - sensor.model == "umrr9d_can_v1_2_2" && + sensor.model == "umrr9f_v2_1_1" && com::types::ERROR_CODE_OK != - data_umrr9d_v1_2_2->RegisterComTargetBaseListReceiveCallback( + data_umrr9f_v2_1_1->RegisterComTargetListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2, this, i, + &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_2_2" << std::endl; + std::cout << "Failed to register targetlist callback for sensor umrr9f" << std::endl; } if ( - sensor.model == "umrr9f_can_v2_1_1" && + sensor.model == "umrr9f_v2_2_1" && com::types::ERROR_CODE_OK != - data_umrr9f_v2_1_1->RegisterComTargetBaseListReceiveCallback( + data_umrr9f_v2_2_1->RegisterComTargetListPortReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1, this, i, + &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_1_1" << std::endl; + std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_2_1" << std::endl; } if ( - sensor.model == "umrr9f_can_v2_2_1" && + sensor.model == "umrr9d_v1_0_3" && com::types::ERROR_CODE_OK != - data_umrr9f_v2_2_1->RegisterComTargetBaseListReceiveCallback( + data_umrr9d_v1_0_3->RegisterComTargetListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1, this, i, + &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_3, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_2_1" << std::endl; + std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_0_3" << std::endl; + } + if ( + sensor.model == "umrr9d_v1_2_2" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_2_2->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_2, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_2_2" << std::endl; } if ( - sensor.model == "umrra4" && + sensor.model == "umrra4_can_v1_0_1" && com::types::ERROR_CODE_OK != - data_umrra4->RegisterComTargetListReceiveCallback( + data_umrra4_v1_0_1->RegisterComTargetBaseListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrra4, this, i, + &SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_0_1, this, i, std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrra4" << std::endl; } if ( - sensor.model == "umrr96" && + sensor.model == "umrr96_can" && com::types::ERROR_CODE_OK != - data_umrr96->RegisterComTargetListReceiveCallback( + data_umrr96->RegisterComTargetBaseListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr96, this, i, + &SmartmicroRadarNode::CAN_targetlist_callback_umrr96, this, i, std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrr96" << std::endl; } if ( - sensor.model == "umrr11" && + sensor.model == "umrr11_can" && com::types::ERROR_CODE_OK != - data_umrr11->RegisterComTargetListReceiveCallback( + data_umrr11->RegisterComTargetBaseListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr11, this, i, + &SmartmicroRadarNode::CAN_targetlist_callback_umrr11, this, i, std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; } if ( - sensor.model == "umrr9f_v1_1_1" && + sensor.model == "umrr9d_can_v1_0_3" && com::types::ERROR_CODE_OK != - data_umrr9f_v1_1_1->RegisterComTargetListPortReceiveCallback( + data_umrr9d_v1_0_3->RegisterComTargetBaseListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr9f_v1_1_1, this, i, + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_0_3, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f_v1_1_1" << std::endl; + std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_0_3" << std::endl; } if ( - sensor.model == "umrr9f_v2_1_1" && + sensor.model == "umrr9d_can_v1_2_2" && com::types::ERROR_CODE_OK != - data_umrr9f_v2_1_1->RegisterComTargetListReceiveCallback( + data_umrr9d_v1_2_2->RegisterComTargetBaseListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1, this, i, + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2, 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 umrr9d_v1_2_2" << std::endl; } if ( - sensor.model == "umrr9d_v1_0_3" && + sensor.model == "umrr9f_can_v2_1_1" && com::types::ERROR_CODE_OK != - data_umrr9d_v1_0_3->RegisterComTargetListReceiveCallback( + data_umrr9f_v2_1_1->RegisterComTargetBaseListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_3, this, i, + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_0_3" << std::endl; + std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_1_1" << std::endl; } if ( - sensor.model == "umrr9d_v1_2_2" && + sensor.model == "umrr9f_can_v2_2_1" && com::types::ERROR_CODE_OK != - data_umrr9d_v1_2_2->RegisterComTargetListReceiveCallback( + data_umrr9f_v2_2_1->RegisterComTargetBaseListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_2, this, i, + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_2_2" << std::endl; + std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_2_1" << std::endl; } } @@ -557,17 +570,17 @@ void SmartmicroRadarNode::command_response( } } -void SmartmicroRadarNode::targetlist_callback_umrra4( +void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1( const std::uint32_t sensor_idx, const std::shared_ptr & - targetlist_port_umrra4, + targetlist_port_umrra4_v1_0_1, const com::types::ClientId client_id) { std::cout << "Targetlist for umrra4" << std::endl; if (!check_signal) { std::shared_ptr port_header; - port_header = targetlist_port_umrra4->GetPortHeader(); + port_header = targetlist_port_umrra4_v1_0_1->GetPortHeader(); sensor_msgs::msg::PointCloud2 msg; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; @@ -575,7 +588,7 @@ void SmartmicroRadarNode::targetlist_callback_umrra4( 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_umrra4->GetTargetList()) { + for (const auto & target : targetlist_port_umrra4_v1_0_1->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); @@ -797,18 +810,52 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1( } } -void SmartmicroRadarNode::CAN_targetlist_callback_umrra4( +void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_v2_2_1, + const com::tyoes::CLientId client_id) +{ + std::cout << "Targetlist for umrr9f v2_2_1" << std::endl; + if(!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9f_v2_2_1->GetPortHeader(); + sensor_msgs::msgs::PointCloud2 msg; + RadarCloudModifier modifier{msg, m_sensor[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_v2_2_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range *n std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {rnage_2d * std:.cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range* std:.sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_0_1( const std::uint32_t sensor_idx, const std::shared_ptr< com::master::umrra4_automotive_v1_0_1::comtargetbaselist::ComTargetBaseList> & - targetlist_can_umrra4, + targetlist_can_umrra4_v1_0_1, const com::types::ClientId client_id) { std::cout << "CAN Targetlist for umrra4" << std::endl; if (!check_signal) { std::shared_ptr port_header; - port_header = targetlist_can_umrra4->GetPortHeader(); + port_header = targetlist_can_umrra4_v1_0_1->GetPortHeader(); sensor_msgs::msg::PointCloud2 msg; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; @@ -816,7 +863,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrra4( 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_can_umrra4->GetTargetList()) { + for (const auto & target : targetlist_can_umrra4_v1_0_1->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/test_smartmicro_radar_node.cpp b/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp deleted file mode 100644 index 14512c1..0000000 --- a/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp +++ /dev/null @@ -1,47 +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. - -#include - -#include -#include - -using smartmicro::drivers::radar::SmartmicroRadarNode; - -/// @test Test that if we publish one message, it generates a state estimate which is sent out. -TEST(SmartmicroRadarNodeTest, Create) -{ - rclcpp::init(0, nullptr); - ASSERT_TRUE(rclcpp::ok()); - rclcpp::NodeOptions node_options{}; - EXPECT_THROW(std::make_shared(node_options), std::runtime_error); - node_options.append_parameter_override("adapters.adapter_0.hw_type", "eth"); - node_options.append_parameter_override("adapters.adapter_0.hw_dev_id", 2); - node_options.append_parameter_override("adapters.adapter_0.port", 55555); - node_options.append_parameter_override("adapters.adapter_0.hw_iface_name", "eth0"); - node_options.append_parameter_override("sensors.sensor_0.id", 10); - node_options.append_parameter_override("sensors.sensor_0.ip", "172.22.10.102"); - node_options.append_parameter_override("sensors.sensor_0.model", "umrr96"); - node_options.append_parameter_override("sensors.sensor_0.inst_type", "port_based"); - node_options.append_parameter_override("sensors.sensor_0.data_type", "port_based"); - node_options.append_parameter_override("sensors.sensor_0.uifname", "umrr96_t153_automotive"); - node_options.append_parameter_override("sensors.sensor_0.uifmajorv", 1); - node_options.append_parameter_override("sensors.sensor_0.uifminorv", 2); - node_options.append_parameter_override("sensors.sensor_0.uifpatchv", 2); - auto node = std::make_shared(node_options); - ASSERT_TRUE(node != nullptr); - rclcpp::Rate sleepRate(std::chrono::seconds(5)); - sleepRate.sleep(); - rclcpp::shutdown(); -} From 79b1776f848e4f14ee22e4b0d9fd2a978d807454 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Mon, 4 Sep 2023 11:26:21 +0200 Subject: [PATCH 07/15] Update Readme and remove json files --- Readme.md | 27 +- umrr_ros2_driver/config/sensor_commands.json | 17 - umrr_ros2_driver/config/sensor_params.json | 343 ------------------ .../param/radar.sensor.template.yaml | 2 +- 4 files changed, 13 insertions(+), 376 deletions(-) delete mode 100644 umrr_ros2_driver/config/sensor_commands.json delete mode 100644 umrr_ros2_driver/config/sensor_params.json diff --git a/Readme.md b/Readme.md index ac05cbf..28d9caf 100644 --- a/Readme.md +++ b/Readme.md @@ -22,17 +22,20 @@ 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, UMRR11, DRVEGRD 152 or DRVEGRD 169 radar are +A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96, UMRR11, DRVEGRD 171, 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: `February 06, 2023` -- Smart Access Automotive version: `v3.0.0` +- Date of release: `September 15, 2023` +- Smart Access Automotive version: `v3.3.0` - User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.2` - User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.2` - User interface version: `UMRR9F Type 169 AUTOMOTIVE v1.1.1` - User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.1.1` +- User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.2.1` - User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.0.3` +- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.2.2` +- User interface version: `UMRRA4 Type 171 AUTOMOTIVE v1.0.1` ### Sensor Firmwares This ROS2 driver release is compatible with the following sensor firmwares: @@ -41,6 +44,8 @@ This ROS2 driver release is compatible with the following sensor firmwares: - UMRR9D Type 152: V2.1.0 - UMRR9F Type 169: V1.3.0 - UMRR9F Type 169: V2.0.2 +- UMRR9F Type 169: V2.2.0 +- UMRRA4 Type 171: V1.0.0 ### Point cloud message wrapper library To add targets to the point cloud in a safe and quick fashion a @@ -81,10 +86,10 @@ For more details, see the [`radar.sensor.example.yaml`](umrr_ros2_driver/param/r For the setting up the ***sensors***: - `link_type`: the type of hardware connection - `model`: the model of the sensor being used - - can: 'umrra4_can', 'umrr11_can', 'umrr9d_can', 'umrr96_can', 'umrr9f_can_v1_1_1', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1' - - port: 'umrr11', 'umrr9d', 'umrr96', 'umrr9f_v1_1_1', 'umrr9f_v2_1_1', 'umrra4' + - can: 'umrra4_can_v1_0_1', 'umrr96_can', 'umrr11_can', 'umrr9d_can_v1_0_3', 'umrr9d_can_v1_2_2', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1' + - port: 'umrra4_v1_0_1', 'umrr96', 'umrr11', 'umrr9d_v1_0_3', 'umrr9d_v1_2_2', 'umrr9f_v1_1_1', 'umrr9f_v2_1_1', 'umrr9f_v2_2_1', - `dev_id`: adapter id to which sensor is connected. ***The adapter and sensor should have the same dev_id*** -- `id`: the client_id of the sensor/source, ***must be a _unique_ integer***. +- `id`: the client_id of the sensor/source, ***must be a _unique_ integer and non-zero***. - `ip`: the ***_unique_*** ip address of the sensor or of the source acting as a sensor, required only for sensors using _ethernet_. - `port`: port to be used to receive the packets, default is _55555_ - `frame_id`: name of the frame in which the messages will be published @@ -100,7 +105,7 @@ For setting up the ***adapters***: - `master_inst_serial_type`: the instruction serilization type of the master. When using a hybrid of 'can' and 'port' use 'can_based' - `master_data_serial_type`: the data serilization type of the master. When using a hybrid of 'can' and 'port' use 'can_based' - `hw_type`: the type of the hardware connection -- `hw_dev_id`: adapter id of the hardware, the sensor and ***adapter should use the same id*** +- `hw_dev_id`: adapter id of the hardware, ***the sensor and adapter should use the same id*** - `hw_iface_name`: name of the used network interface - `baudrate`: the baudrate of the sensor connected with can, default is _500000_ - `port`: port to be used to receive the packets, default is _55555_ @@ -144,14 +149,6 @@ The call for such a service would be as follows: 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/umrr_ros2_driver/config/sensor_commands.json b/umrr_ros2_driver/config/sensor_commands.json deleted file mode 100644 index f8a9919..0000000 --- a/umrr_ros2_driver/config/sensor_commands.json +++ /dev/null @@ -1,17 +0,0 @@ -{ - "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 deleted file mode 100644 index c930190..0000000 --- a/umrr_ros2_driver/config/sensor_params.json +++ /dev/null @@ -1,343 +0,0 @@ -{ - "parameters": [ - { - "name": "tx_antenna_idx", - "comment": "Index of Transmit Antenna", - "sensors": [ - { - "model": "umrr11", - "comment": "0 and 1 are ACC, 2 is AEB", - "min": 0, - "max": 2 - }, - { - "model": "umrr96", - "min": 0, - "max": 2, - "default": 0 - }, - { - "model": "umrr9f", - "min": 0, - "max": 2, - "default": 0 - } - ] - }, - { - "name": "center_frequency_idx", - "comment": "Index of center frequency", - "sensors": [ - { - "model": "umrr11", - "comment": " 0..3 in ACC selectable. In AEB mode values 2 and 3 are similar to 1.", - "min": 0, - "max": 3 - }, - { - "model": "umrr96", - "min": 0, - "max": 2, - "default": 0 - }, - { - "model": "umrr9f", - "min": 1, - "max": 2, - "default": 1 - }, - { - "model": "umrr9d", - "min": 0, - "max": 3, - "default": 1 - } - ] - }, - { - "name": "frequency_sweep_idx", - "comment": "Index of sweep", - "sensors": [ - { - "model": "umrr11", - "comment": " currently always 0 because only one sweep for every TX-antenna", - "min": 0, - "max": 0 - }, - { - "model": "umrr96", - "comment": "0=226MHz, 1=512MHz, 2=1536MHz", - "min": 0, - "max": 2, - "default": 2 - }, - { - "model": "umrr9f", - "comment": "0=226MHz, 1=512MHz, 2=1536MHz, 3=3072MHz", - "min": 0, - "max": 3, - "default": 1 - }, - { - "model": "umrr9d", - "comment": "0=medium-range, 1=long-range", - "min": 0, - "max": 1, - "default": 0 - } - ] - }, - { - "name": "enable_tx_ant_toggle", - "comment": "Automatic toggle of TX-Antenna 0->2->0 every radar cycle", - "sensors": [ - { - "model": "umrr11", - "min": 0, - "max": 1, - "default": 0 - } - ] - }, - { - "name": "angular_separation", - "comment": "0=Disable Angular Separation, 1=Enable Angular Separation", - "sensors": [ - { - "model": "umrr11", - "min": 0, - "max": 1, - "default": 0 - } - ] - }, - { - "name": "range_toggle_mode", - "comment": "Automatic toggle of range:", - "sensors": [ - { - "model": "umrr96", - "comment": "0=off, 1=Short-Med, 2=Short-Long, 3=Med-Long", - "min": 0, - "max": 3, - "default": 0 - }, - { - "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, - "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 - }, - { - "model": "umrr9f_v2_0_0", - "min": 0, - "max": 2, - "default": 1 - } - ] - }, - { - "name": "tv_min_speed_sweep_idx_0", - "comment": "Target Validation: minimum speed of target at first sweep", - "sensors": [ - { - "model": "umrr96", - "min": -150.0, - "max": 150.0, - "default": -20 - }, - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0, - "default": -20 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": -112.0 - } - ] - }, - { - "name": "tv_min_speed_sweep_idx_1", - "comment": "Target Validation: minimum speed of target at second sweep", - "sensors": [ - { - "model": "umrr96", - "min": -150.0, - "max": 150.0, - "default": -20 - }, - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0, - "default": -20 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": -112.0 - } - ] - }, - { - "name": "tv_min_speed_sweep_idx_2", - "comment": "Target Validation: minimum speed of target at third sweep", - "sensors": [ - { - "model": "umrr96", - "min": -150.0, - "max": 150.0, - "default": -20 - }, - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0, - "default": -20 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": -112.0 - } - ] - }, - { - "name": "tv_min_speed_sweep_idx_3", - "comment": "Target Validation: minimum speed of target at fourth sweep", - "sensors": [ - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": -112.0 - } - ] - }, - { - "name": "tv_max_speed_sweep_idx_0", - "comment": "Target Validation: maximum speed of target at first sweep", - "sensors": [ - { - "model": "umrr96", - "min": -150.0, - "max": 150.0, - "default": 20 - }, - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0, - "default": 20 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": 56.0 - } - ] - }, - { - "name": "tv_max_speed_sweep_idx_1", - "comment": "Target Validation: maximum speed of target at second sweep", - "sensors": [ - { - "model": "umrr96", - "min": -150.0, - "max": 150.0, - "default": 20 - }, - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0, - "default": 20 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": 56.0 - } - ] - }, - { - "name": "tv_max_speed_sweep_idx_2", - "comment": "Target Validation: maximum speed of target at third sweep", - "sensors": [ - { - "model": "umrr96", - "min": -150.0, - "max": 150.0, - "default": 20 - }, - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0, - "default": 20 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": 56.0 - } - ] - }, - { - "name": "tv_max_speed_sweep_idx_3", - "comment": "Target Validation: maximum speed of target at fourth sweep", - "sensors": [ - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": 56.0 - } - ] - }, - { - "name": "ip_source_address", - "comment": "IP source address (32bit)", - "default": 3232238347 - } - ] -} diff --git a/umrr_ros2_driver/param/radar.sensor.template.yaml b/umrr_ros2_driver/param/radar.sensor.template.yaml index aeed182..0324ea5 100644 --- a/umrr_ros2_driver/param/radar.sensor.template.yaml +++ b/umrr_ros2_driver/param/radar.sensor.template.yaml @@ -86,7 +86,7 @@ sensor_3: link_type: "eth" # The model of the connected sensor. - model: "umrr9d" + model: "umrr9d_v1_0_3" # Adapter id to which sensor is connected dev_id: 3 # The client_id of the sensor/source, must be a unique integer. From 3103b131244e021cb2903fb3e05d27efc9684358 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Mon, 18 Sep 2023 10:01:08 +0200 Subject: [PATCH 08/15] Rectify typo mistakes in callback Update release version, reduce port stream time and update comment in test --- Readme.md | 5 +++-- docker-compose.yml | 2 +- simulator/simulation/src/simulator.cpp | 2 +- smart_extract.sh | 2 +- umrr_ros2_driver/CMakeLists.txt | 3 +-- umrr_ros2_driver/src/smartmicro_radar_node.cpp | 17 ++++++++--------- umrr_ros2_driver/test/radar_node_test.launch.py | 2 +- 7 files changed, 16 insertions(+), 17 deletions(-) diff --git a/Readme.md b/Readme.md index 89dc5ca..148c48d 100644 --- a/Readme.md +++ b/Readme.md @@ -26,8 +26,8 @@ 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: `September 15, 2023` -- Smart Access Automotive version: `v3.3.0` +- Date of release: `September 20, 2023` +- Smart Access Automotive version: `v3.4.0` - User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.2` - User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.2` - User interface version: `UMRR9F Type 169 AUTOMOTIVE v1.1.1` @@ -42,6 +42,7 @@ 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: V2.1.0 +- UMRR9D Type 152: V2.5.0 - UMRR9F Type 169: V1.3.0 - UMRR9F Type 169: V2.0.2 - UMRR9F Type 169: V2.2.0 diff --git a/docker-compose.yml b/docker-compose.yml index fc54d8f..2799005 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -60,7 +60,7 @@ services: - SMART_ACCESS_CFG_FILE_PATH=/code/simulator/config_umrr9f/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: ["9", "2", "1", "B"] + command: ["9", "2", "1", "C"] networks: device_network: ipv4_address: 172.22.10.103 diff --git a/simulator/simulation/src/simulator.cpp b/simulator/simulation/src/simulator.cpp index 39003fd..750445d 100644 --- a/simulator/simulation/src/simulator.cpp +++ b/simulator/simulation/src/simulator.cpp @@ -155,7 +155,7 @@ int main(int argc, char * argv[]) } 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(15)) { break; } } diff --git a/smart_extract.sh b/smart_extract.sh index 3e0f640..0895494 100755 --- a/smart_extract.sh +++ b/smart_extract.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -smart_pack=SmartAccessAutomotive_3_3_0.tgz +smart_pack=SmartAccessAutomotive_3_4_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 1ee1c6a..f64e3c5 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -44,7 +44,6 @@ if(NOT json_POPULATED) endif() ament_auto_find_build_dependencies() -set(SMARTMICRO_LIB_DIR "lib-linux-x86_64-gcc_9") set(SMARTMICRO_LIB_DIR "lib-linux-x86_64-gcc_9") @@ -112,7 +111,7 @@ rclcpp_components_register_node(smartmicro_radar_node if(BUILD_TESTING) find_package(launch_testing_ament_cmake) - add_launch_test(test/radar_node_test.launch.py TIMEOUT "150") + add_launch_test(test/radar_node_test.launch.py TIMEOUT "100") # Not applying ros2 linters on external sources list(APPEND AMENT_LINT_AUTO_EXCLUDE diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index fed81ba..411ca21 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -232,7 +232,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option if ( sensor.model == "umrr9f_v2_2_1" && com::types::ERROR_CODE_OK != - data_umrr9f_v2_2_1->RegisterComTargetListPortReceiveCallback( + data_umrr9f_v2_2_1->RegisterComTargetListReceiveCallback( sensor.id, std::bind( &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_1, this, i, std::placeholders::_1, std::placeholders::_2))) { @@ -342,7 +342,6 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option RCLCPP_INFO(this->get_logger(), "Radar services are ready."); rclcpp::on_shutdown(std::bind(&SmartmicroRadarNode::on_shutdown_callback, this)); - } void SmartmicroRadarNode::on_shutdown_callback() @@ -814,15 +813,15 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_1( const std::uint32_t sensor_idx, const std::shared_ptr & targetlist_port_umrr9f_v2_2_1, - const com::tyoes::CLientId client_id) + const com::types::ClientId client_id) { std::cout << "Targetlist for umrr9f v2_2_1" << std::endl; if(!check_signal) { std::shared_ptr port_header; port_header = targetlist_port_umrr9f_v2_2_1->GetPortHeader(); - sensor_msgs::msgs::PointCloud2 msg; - RadarCloudModifier modifier{msg, m_sensor[sensor_idx].frame_id}; + 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); @@ -831,12 +830,12 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_1( for (const auto & target : targetlist_port_umrr9f_v2_2_1->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); - const auto range_2d = range *n std::cos(elevation_angle); + const auto range_2d = range * std::cos(elevation_angle); const auto azimuth_angle = target->GetAzimuthAngle(); const auto snr = target->GetPower() - target->GetNoise(); modifier.push_back( - {rnage_2d * std:.cos(azimuth_angle), range_2d * std::sin(azimuth_angle), - range* std:.sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), target->GetRcs(), target->GetNoise(), snr}); } @@ -1112,7 +1111,7 @@ void SmartmicroRadarNode::update_config_files_from_params() return true; }; - + auto read_sensor_params_if_possible = [&](const std::uint32_t index) { RCLCPP_INFO(this->get_logger(), "Updating SENSOR params."); auto & sensor = m_sensors[index]; diff --git a/umrr_ros2_driver/test/radar_node_test.launch.py b/umrr_ros2_driver/test/radar_node_test.launch.py index b9090eb..16c4b14 100644 --- a/umrr_ros2_driver/test/radar_node_test.launch.py +++ b/umrr_ros2_driver/test/radar_node_test.launch.py @@ -125,7 +125,7 @@ def tearDown(self): self.test_node.destroy_node() def test_smart_node_publishes(self): - # Expect the smartnode to publish strings on '/umrr/targets_' + # Expect the smartnode to publish strings on '/smart_radar/targets_' data_rx_s1 = [] data_rx_s2 = [] data_rx_s3 = [] From 69758b31251665c68162054592934b2c60b8fcdb Mon Sep 17 00:00:00 2001 From: smartSRA Date: Tue, 19 Sep 2023 09:45:21 +0200 Subject: [PATCH 09/15] Update release date --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d1325b7..3283acd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -40,6 +40,6 @@ This release includes the new DRVEGRD 169 user-interface. The release includes n This release includes the new DRVEGRD 171 and DRVEGRD 152 user-interface. The release includes a complete list of all params and commands for all sensors which are accessed using the ros2 services. -## v5.0.0 - 2023-09-15 +## v5.0.0 - 2023-09-20 This release includes CAN communication for all the provided sensor types and sensors interfaces. The params are now extended to include the params/setting for the connected adapeters alongwith the sensor params. \ No newline at end of file From e118a255f3f94a04cd02bac39e1992bc3b6572fa Mon Sep 17 00:00:00 2001 From: smartSRA Date: Wed, 20 Sep 2023 11:04:19 +0200 Subject: [PATCH 10/15] Update doxygen description in param templates and resolve eol errors --- CHANGELOG.md | 2 +- umrr_ros2_driver/config/hw_inventory.json | 2 +- umrr_ros2_driver/config/routing_table.json | 2 +- umrr_ros2_driver/param/example/radar.adapter.example.yaml | 3 ++- umrr_ros2_driver/param/example/radar.sensor.example.yaml | 2 +- umrr_ros2_driver/param/radar.adapter.template.yaml | 5 ++--- umrr_ros2_driver/param/radar.sensor.template.yaml | 4 +++- 7 files changed, 11 insertions(+), 9 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3283acd..0a9cc44 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -42,4 +42,4 @@ This release includes the new DRVEGRD 171 and DRVEGRD 152 user-interface. The re ## v5.0.0 - 2023-09-20 -This release includes CAN communication for all the provided sensor types and sensors interfaces. The params are now extended to include the params/setting for the connected adapeters alongwith the sensor params. \ No newline at end of file +This release includes CAN communication for all the provided sensor types and sensors interfaces. The params are now extended to include the params/setting for the connected adapters alongwith the sensor params. \ No newline at end of file diff --git a/umrr_ros2_driver/config/hw_inventory.json b/umrr_ros2_driver/config/hw_inventory.json index 8e6627c..48d7e7a 100644 --- a/umrr_ros2_driver/config/hw_inventory.json +++ b/umrr_ros2_driver/config/hw_inventory.json @@ -6,4 +6,4 @@ ], "name": "HW Inventory List", "version": "1.1.0" -} \ No newline at end of file +} diff --git a/umrr_ros2_driver/config/routing_table.json b/umrr_ros2_driver/config/routing_table.json index 96643cc..bc6cee3 100644 --- a/umrr_ros2_driver/config/routing_table.json +++ b/umrr_ros2_driver/config/routing_table.json @@ -6,4 +6,4 @@ ], "name": "Client Routing Table", "version": "1.0.0" -} \ No newline at end of file +} diff --git a/umrr_ros2_driver/param/example/radar.adapter.example.yaml b/umrr_ros2_driver/param/example/radar.adapter.example.yaml index 31206fc..8d238f2 100644 --- a/umrr_ros2_driver/param/example/radar.adapter.example.yaml +++ b/umrr_ros2_driver/param/example/radar.adapter.example.yaml @@ -4,6 +4,7 @@ # This sets the iface_name in the hw_inventory.json # sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx # sudo ip link set up slcan0 + # For peak CAN adapters, only required to set baudrate # One sensor per interface for can, for ethernet we can use one interface/dev for multiple sensors master_inst_serial_type: "can_based" master_data_serial_type: "can_based" @@ -16,7 +17,7 @@ adapter_1: hw_type: "eth" hw_dev_id: 3 - hw_iface_name: "enx002432163c8d" + hw_iface_name: "eth0" port: 55555 adapter_2: hw_type: "can" diff --git a/umrr_ros2_driver/param/example/radar.sensor.example.yaml b/umrr_ros2_driver/param/example/radar.sensor.example.yaml index d493ff4..7a38608 100644 --- a/umrr_ros2_driver/param/example/radar.sensor.example.yaml +++ b/umrr_ros2_driver/param/example/radar.sensor.example.yaml @@ -119,4 +119,4 @@ # The frame_id to be set to the published messages. frame_id: "umrr" # Specify the history size. - history_size: 10 \ No newline at end of file + history_size: 10 diff --git a/umrr_ros2_driver/param/radar.adapter.template.yaml b/umrr_ros2_driver/param/radar.adapter.template.yaml index 708c62e..57e2666 100644 --- a/umrr_ros2_driver/param/radar.adapter.template.yaml +++ b/umrr_ros2_driver/param/radar.adapter.template.yaml @@ -2,9 +2,8 @@ /**: ros__parameters: # This sets the iface_name in the hw_inventory.json - # sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx - # sudo ip link set up slcan0 - # One sensor per interface + # One sensor per adapter in case of CAN + # For ethernet it is possible to have one adapter/interface for multiple sensors master_inst_serial_type: "port_based" master_data_serial_type: "port_based" adapters: diff --git a/umrr_ros2_driver/param/radar.sensor.template.yaml b/umrr_ros2_driver/param/radar.sensor.template.yaml index 0324ea5..98e5634 100644 --- a/umrr_ros2_driver/param/radar.sensor.template.yaml +++ b/umrr_ros2_driver/param/radar.sensor.template.yaml @@ -5,6 +5,9 @@ sensors: # 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. + # For sensor `model`if using : + # can: 'umrra4_can_v1_0_1', 'umrr96_can', 'umrr11_can', 'umrr9d_can_v1_0_3', 'umrr9d_can_v1_2_2', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1' + # port: 'umrra4_v1_0_1', 'umrr96', 'umrr11', 'umrr9d_v1_0_3', 'umrr9d_v1_2_2', 'umrr9f_v1_1_1', 'umrr9f_v2_1_1', 'umrr9f_v2_2_1' sensor_0: link_type: "eth" # The model of the connected sensor. @@ -109,4 +112,3 @@ uifminorv: 0 # The pathc version of the interface uifpatchv: 3 - \ No newline at end of file From 4da3e84980923216ce332b1ae54e172f3c3300d2 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Thu, 21 Sep 2023 11:53:43 +0200 Subject: [PATCH 11/15] Spelling checks, reinclude interface and adjust naming --- CHANGELOG.md | 12 +-- Readme.md | 4 +- umrr_ros2_driver/CMakeLists.txt | 3 + .../smartmicro_radar_node.hpp | 19 +++- umrr_ros2_driver/launch/radar.launch.py | 3 +- .../param/radar.adapter.template.yaml | 1 - .../src/smartmicro_radar_node.cpp | 88 ++++++++++++++----- 7 files changed, 98 insertions(+), 32 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0a9cc44..c02cf70 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,23 +14,23 @@ Requires new sensor firmware, if using UMRR11-T132: V5.1.4 and if using UMRR96-T ## v2.1.0 - 2022-06-02 This minor release of the driver offer ros2 services to communicate with the sensor. The driver now supports mode changes for UMRR96 and UMRR11. -This release also offers the possibilty of configuring the ip addresses of the sensor using ros2 services. The tests are further extended to inlcude ros2 services check. Requires new sensor firmware, if using UMRR11-T132: V5.1.4 and if using UMRR96-T153: V5.2.4. +This release also offers the possibility of configuring the ip addresses of the sensor using ros2 services. The tests are further extended to include ros2 services check. Requires new sensor firmware, if using UMRR11-T132: V5.1.4 and if using UMRR96-T153: V5.2.4. ## 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. +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 point cloud 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. +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 point cloud data. The callbacks for datastream now require a clientID. ## v3.2.0 - 2022-11-11 -This minor release introduces signal-to-noise field in the pointclouds and also fixes the max number of sensors that could be connected at once. +This minor release introduces signal-to-noise field in the point clouds and also fixes the max number of sensors that could be connected at once. ## 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. +This release fixes the offset which causes anomaly in point clouds 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 @@ -42,4 +42,4 @@ This release includes the new DRVEGRD 171 and DRVEGRD 152 user-interface. The re ## v5.0.0 - 2023-09-20 -This release includes CAN communication for all the provided sensor types and sensors interfaces. The params are now extended to include the params/setting for the connected adapters alongwith the sensor params. \ No newline at end of file +This release includes CAN communication for all the provided sensor types and sensors interfaces. The params are now extended to include the params/setting for the connected adapters along with the sensor params. \ No newline at end of file diff --git a/Readme.md b/Readme.md index 148c48d..f0c4ec9 100644 --- a/Readme.md +++ b/Readme.md @@ -73,10 +73,10 @@ The driver publishes `sensor_msgs::msg::PointCloud2` messages with the radar tar ### Interface Configuration: For setting up a sensor with ethernet or can, the interfaces of the should be set properly prior to configuring the node. -The sensor came equiped with two protocols: +The sensor is equipped with three physical layers (RS485, CAN and ethernet) however the driver uses only ethernet and can: - ethernet: to set-up an ethernet interface the following command could be used `ifconfig my_interface_name 192.168.11.17 netmask 255.255.255.0`. The command above uses the default source ip address used by the sensors. -- can: if using LAWICEL to set-up a can interface the following commands could be used `slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx`and than `ip link set up my_interace_name`. +- can: if using LAWICEL to set-up a can interface the following commands could be used `slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx`and than `ip link set up my_interface_name`. This uses the default baudrate of _500000_. When using Peak CAN the interfaces are recognized by linux and is only needed to set the baudrate. ### Node Configuration: diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index f64e3c5..04f4e1b 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -58,6 +58,7 @@ install(FILES "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr11_t132_automotivev1.1.2_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr96_t153_automotivev1.2.2_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev1.1.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.0.0_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.1.1_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.2.1_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.0.3_user_interface.so" @@ -83,6 +84,7 @@ smartmicro/include/ smartmicro/include/umrr11_t132_automotive_v1_1_2 smartmicro/include/umrr96_t153_automotive_v1_2_2 smartmicro/include/umrr9f_t169_automotive_v1_1_1 +smartmicro/include/umrr9f_t169_automotive_v2_0_0 smartmicro/include/umrr9f_t169_automotive_v2_1_1 smartmicro/include/umrr9f_t169_automotive_v2_2_1 smartmicro/include/umrr9d_t152_automotive_v1_0_3 @@ -98,6 +100,7 @@ target_link_libraries(smartmicro_radar_node umrr11_t132_automotivev1.1.2_user_interface umrr96_t153_automotivev1.2.2_user_interface umrr9f_t169_automotivev1.1.1_user_interface + umrr9f_t169_automotivev2.0.0_user_interface umrr9f_t169_automotivev2.1.1_user_interface umrr9f_t169_automotivev2.2.1_user_interface umrr9d_t152_automotivev1.0.3_user_interface 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 a439b0c..d9e3d4a 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 @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -124,7 +125,22 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { comtargetlist::ComTargetList> &targetlist_port_umrr96, const com::types::ClientId client_id); - + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v2_0_0 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 /// umrr9f_v1_1_1 T169 arrives. @@ -390,6 +406,7 @@ std::shared_ptr d std::shared_ptr data_umrr11{}; std::shared_ptr data_umrr96{}; std::shared_ptr data_umrr9f_v1_1_1{}; +std::shared_ptr data_umrr9f_v2_0_0{}; std::shared_ptr data_umrr9f_v2_1_1{}; std::shared_ptr data_umrr9f_v2_2_1{}; std::shared_ptr data_umrr9d_v1_0_3{}; diff --git a/umrr_ros2_driver/launch/radar.launch.py b/umrr_ros2_driver/launch/radar.launch.py index c953427..23946f3 100644 --- a/umrr_ros2_driver/launch/radar.launch.py +++ b/umrr_ros2_driver/launch/radar.launch.py @@ -37,10 +37,9 @@ def generate_launch_description(): radar_node = Node( package=PACKAGE_NAME, executable='smartmicro_radar_node_exe', - name='smart_micro_radars', + name='smart_radar', parameters=[radar_sensor_params, radar_adapter_params] ) - return LaunchDescription([ radar_node ]) diff --git a/umrr_ros2_driver/param/radar.adapter.template.yaml b/umrr_ros2_driver/param/radar.adapter.template.yaml index 57e2666..f1c3e36 100644 --- a/umrr_ros2_driver/param/radar.adapter.template.yaml +++ b/umrr_ros2_driver/param/radar.adapter.template.yaml @@ -12,4 +12,3 @@ hw_dev_id: 3 hw_iface_name: "eth0" port: 55555 - \ No newline at end of file diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 411ca21..91b4f91 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -31,6 +31,8 @@ #include #include #include +#include +#include #include #include #include @@ -169,6 +171,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option data_umrr11 = com::master::umrr11_t132_automotive_v1_1_2::DataStreamServiceIface::Get(); data_umrr96 = com::master::umrr96_t153_automotive_v1_2_2::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_umrr9f_v2_1_1 = com::master::umrr9f_t169_automotive_v2_1_1::DataStreamServiceIface::Get(); data_umrr9f_v2_2_1 = com::master::umrr9f_t169_automotive_v2_2_1::DataStreamServiceIface::Get(); data_umrr9d_v1_0_3 = com::master::umrr9d_t152_automotive_v1_0_3::DataStreamServiceIface::Get(); @@ -220,6 +223,15 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option std::placeholders::_1, std::placeholders::_2))) { 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 == "umrr9f_v2_1_1" && com::types::ERROR_CODE_OK != @@ -263,7 +275,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_0_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrra4" << std::endl; + std::cout << "Failed to register CAN targetlist callback for sensor umrra4" << std::endl; } if ( sensor.model == "umrr96_can" && @@ -272,7 +284,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrr96, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr96" << std::endl; + std::cout << "Failed to register CAN targetlist callback for sensor umrr96" << std::endl; } if ( sensor.model == "umrr11_can" && @@ -281,7 +293,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrr11, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; + std::cout << "Failed to register CAN targetlist callback for sensor umrr11" << std::endl; } if ( sensor.model == "umrr9d_can_v1_0_3" && @@ -290,7 +302,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_0_3, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_0_3" << std::endl; + std::cout << "Failed to register CAN targetlist callback for sensor umrr9d_v1_0_3" << std::endl; } if ( sensor.model == "umrr9d_can_v1_2_2" && @@ -299,7 +311,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_2_2" << std::endl; + std::cout << "Failed to register CAN targetlist callback for sensor umrr9d_v1_2_2" << std::endl; } if ( sensor.model == "umrr9f_can_v2_1_1" && @@ -308,7 +320,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_1_1" << std::endl; + std::cout << "Failed to register CAN targetlist callback for sensor umrr9f_v2_1_1" << std::endl; } if ( sensor.model == "umrr9f_can_v2_2_1" && @@ -317,7 +329,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_2_1" << std::endl; + std::cout << "Failed to register CAN targetlist callback for sensor umrr9f_v2_2_1" << std::endl; } } @@ -346,7 +358,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option void SmartmicroRadarNode::on_shutdown_callback() { - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "SHUTDOWN CALLED!!!"); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Shutdown called!!!"); check_signal = true; rclcpp::Rate sleepRate(std::chrono::milliseconds(100)); sleepRate.sleep(); @@ -575,7 +587,7 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1( targetlist_port_umrra4_v1_0_1, const com::types::ClientId client_id) { - std::cout << "Targetlist for umrra4" << std::endl; + std::cout << "Targetlist for umrra4_v1_0_1" << std::endl; if (!check_signal) { std::shared_ptr port_header; @@ -609,7 +621,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr96( targetlist_port_umrr96, const com::types::ClientId client_id) { - std::cout << "Targetlist for umrr96" << std::endl; + std::cout << "Targetlist for umrr96_v1_2_2" << std::endl; if (!check_signal) { std::shared_ptr port_header; @@ -643,7 +655,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr11( targetlist_port_umrr11, const com::types::ClientId client_id) { - std::cout << "Targetlist for umrr11" << std::endl; + std::cout << "Targetlist for umrr11_v1_1_2" << std::endl; if (!check_signal) { std::shared_ptr port_header; @@ -746,7 +758,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v1_1_1( targetlist_port_umrr9f_v1_1_1, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9f_v1_1_1" << std::endl; + std::cout << "Targetlist for umrr9f_v1_1_1" << std::endl; if (!check_signal) { std::shared_ptr< com::master::umrr9f_t169_automotive_v1_1_1::comtargetlistport::GenericPortHeader> @@ -775,13 +787,49 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v1_1_1( } } +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()}; + 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_v2_0_0->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_1_1( const std::uint32_t sensor_idx, const std::shared_ptr & targetlist_port_umrr9f_v2_1_1, const com::types::ClientId client_id) { - std::cout << "Targetlist for umrr9f" << std::endl; + std::cout << "Targetlist for umrr9f_v2_1_1" << std::endl; if (!check_signal) { std::shared_ptr port_header; @@ -850,7 +898,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_0_1( targetlist_can_umrra4_v1_0_1, const com::types::ClientId client_id) { - std::cout << "CAN Targetlist for umrra4" << std::endl; + std::cout << "CAN Targetlist for umrra4_v1_0_1" << std::endl; if (!check_signal) { std::shared_ptr port_header; @@ -885,7 +933,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr96( targetlist_can_umrr96, const com::types::ClientId client_id) { - std::cout << "CAN Targetlist for umrr96" << std::endl; + std::cout << "CAN Targetlist for umrr96_v1_2_2" << std::endl; if (!check_signal) { std::shared_ptr port_header; @@ -920,7 +968,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr11( targetlist_can_umrr11, const com::types::ClientId client_id) { - std::cout << "CAN Targetlist for umrr11" << std::endl; + std::cout << "CAN Targetlist for umrr11_v1_1_2" << std::endl; if (!check_signal) { std::shared_ptr port_header; @@ -955,7 +1003,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_0_3( targetlist_can_umrr9d_v1_0_3, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9d_v1_0_3" << std::endl; + std::cout << "Targetlist for umrr9d_v1_0_3" << std::endl; if (!check_signal) { std::shared_ptr port_header; @@ -990,7 +1038,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2( targetlist_can_umrr9d_v1_2_2, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9d_v1_2_2" << std::endl; + std::cout << "Targetlist for umrr9d_v1_2_2" << std::endl; if (!check_signal) { std::shared_ptr port_header; @@ -1025,7 +1073,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1( targetlist_can_umrr9f_v2_1_1, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9f_v2_1_1" << std::endl; + std::cout << "Targetlist for umrr9f_v2_1_1" << std::endl; if (!check_signal) { std::shared_ptr port_header; @@ -1060,7 +1108,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1( targetlist_can_umrr9f_v2_2_1, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9f_v2_2_1" << std::endl; + std::cout << "Targetlist for umrr9f_v2_2_1" << std::endl; if (!check_signal) { std::shared_ptr port_header; From 9b7d4024e176a57ceed7769f676a25c57cd738d0 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Thu, 21 Sep 2023 11:55:27 +0200 Subject: [PATCH 12/15] 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 c02cf70..6b8d770 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -40,6 +40,6 @@ This release includes the new DRVEGRD 169 user-interface. The release includes n This release includes the new DRVEGRD 171 and DRVEGRD 152 user-interface. The release includes a complete list of all params and commands for all sensors which are accessed using the ros2 services. -## v5.0.0 - 2023-09-20 +## v5.0.0 - 2023-09-21 This release includes CAN communication for all the provided sensor types and sensors interfaces. The params are now extended to include the params/setting for the connected adapters along with the sensor params. \ No newline at end of file diff --git a/Readme.md b/Readme.md index f0c4ec9..0c168a8 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: `September 20, 2023` +- Date of release: `September 21, 2023` - Smart Access Automotive version: `v3.4.0` - User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.2` - User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.2` From c69779061fa102fc3e3ff7adfd52744186361311 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Thu, 21 Sep 2023 11:58:46 +0200 Subject: [PATCH 13/15] Adjust line length --- umrr_ros2_driver/src/smartmicro_radar_node.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 91b4f91..6bc38f9 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -275,7 +275,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_0_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist callback for sensor umrra4" << std::endl; + std::cout << "Failed to register CAN targetlist for sensor umrra4" << std::endl; } if ( sensor.model == "umrr96_can" && @@ -284,7 +284,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrr96, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist callback for sensor umrr96" << std::endl; + std::cout << "Failed to register CAN targetlist for sensor umrr96" << std::endl; } if ( sensor.model == "umrr11_can" && @@ -293,7 +293,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrr11, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist callback for sensor umrr11" << std::endl; + std::cout << "Failed to register CAN targetlist for sensor umrr11" << std::endl; } if ( sensor.model == "umrr9d_can_v1_0_3" && @@ -302,7 +302,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_0_3, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist callback for sensor umrr9d_v1_0_3" << std::endl; + std::cout << "Failed to register CAN targetlist for sensor umrr9d_v1_0_3" << std::endl; } if ( sensor.model == "umrr9d_can_v1_2_2" && @@ -311,7 +311,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist callback for sensor umrr9d_v1_2_2" << std::endl; + std::cout << "Failed to register CAN targetlist for sensor umrr9d_v1_2_2" << std::endl; } if ( sensor.model == "umrr9f_can_v2_1_1" && @@ -320,7 +320,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist callback for sensor umrr9f_v2_1_1" << std::endl; + std::cout << "Failed to register CAN targetlist for sensor umrr9f_v2_1_1" << std::endl; } if ( sensor.model == "umrr9f_can_v2_2_1" && @@ -329,7 +329,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option sensor.id, std::bind( &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist callback for sensor umrr9f_v2_2_1" << std::endl; + std::cout << "Failed to register CAN targetlist for sensor umrr9f_v2_2_1" << std::endl; } } From 34b4fd3f97762b3d906cb827482b6b57fa67cb6d Mon Sep 17 00:00:00 2001 From: smartSRA Date: Fri, 22 Sep 2023 12:28:07 +0200 Subject: [PATCH 14/15] 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 6b8d770..f7e0fc2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -40,6 +40,6 @@ This release includes the new DRVEGRD 169 user-interface. The release includes n This release includes the new DRVEGRD 171 and DRVEGRD 152 user-interface. The release includes a complete list of all params and commands for all sensors which are accessed using the ros2 services. -## v5.0.0 - 2023-09-21 +## v5.0.0 - 2023-09-22 This release includes CAN communication for all the provided sensor types and sensors interfaces. The params are now extended to include the params/setting for the connected adapters along with the sensor params. \ No newline at end of file diff --git a/Readme.md b/Readme.md index 0c168a8..6e89702 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: `September 21, 2023` +- Date of release: `September 22, 2023` - Smart Access Automotive version: `v3.4.0` - User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.2` - User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.2` From b1c15c443afdf09458b75a58139a9028d5a438d5 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Fri, 22 Sep 2023 16:27:50 +0200 Subject: [PATCH 15/15] Add missing version --- Readme.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Readme.md b/Readme.md index 6e89702..fdd5f7d 100644 --- a/Readme.md +++ b/Readme.md @@ -31,6 +31,7 @@ sure the version used to publish the data is compatible with this version: - User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.2` - User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.2` - User interface version: `UMRR9F Type 169 AUTOMOTIVE v1.1.1` +- User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.0.0` - User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.1.1` - User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.2.1` - User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.0.3`