diff --git a/.github/workflows/dockerbuild.yml b/.github/workflows/dockerbuild.yml index dd714cb..1fb3e19 100644 --- a/.github/workflows/dockerbuild.yml +++ b/.github/workflows/dockerbuild.yml @@ -9,25 +9,27 @@ on: jobs: build-and-test: runs-on: ubuntu-latest + strategy: + matrix: + ros_distro: [foxy, humble, jazzy] steps: + - name: Checkout + uses: actions/checkout@v2 - - name: Checkout - uses: actions/checkout@v2 + - name: Get the Smart Access binaries + run: echo "yes" | ./smart_extract.sh - - name: Get the Smart Access binaries - run: echo "yes" | ./smart_extract.sh + - name: Build docker container + run: docker build --build-arg ROS_DISTRO=${{ matrix.ros_distro }} -t umrr-ros:${{ matrix.ros_distro }} . - - name: Build the docker container - run: docker build . -t umrr-ros:latest + - name: Build the driver + run: docker run --rm -v $(pwd):/code umrr-ros:${{ matrix.ros_distro }} colcon build --packages-skip smart_rviz_plugin - - name: Building the driver with the docker container - run: docker run --rm -v`pwd`:/code umrr-ros colcon build --packages-skip smart_rviz_plugin + - name: Run tests + run: ROS_DISTRO=${{ matrix.ros_distro }} docker compose up - - name: Running the unit/integration tests via the docker container and exit - run: docker compose up + - name: Collect test results + run: docker run --rm -v`pwd`:/code umrr-ros:${{ matrix.ros_distro }} colcon test-result --all --verbose - - name: Getting the test coverage - run: docker run --rm -v`pwd`:/code umrr-ros colcon test-result --all --verbose - - - name: Shut down docker containers and networks - run: docker compose down + - name: Shut down containers + run: docker compose down diff --git a/CHANGELOG.md b/CHANGELOG.md index b003d3e..dc97002 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -117,4 +117,19 @@ All notable changes to this project will be documented in this file. This projec - **New User Interfaces**: Added user interfaces for DRVEGRD 152 v1.5.0 and DRVEGRD 171 v1.4.0. - **RViz Plugins**: Extended service configurator with DRVEGRD 171 MSE instruction set. - **Adopted Testing**: Includes check for new DRVEGRD 171 v1.5.0. -- **Updated External libraries**: Updated external libraries compatible with the new smart access release. \ No newline at end of file +- **Updated External libraries**: Updated external libraries compatible with the new smart access release. + +## v8.0.0 - 2026-03-03 + +### New Features +- **User Interface for DRVEGRD 166 v1.0.0**: Introduced a new UI. +- **User Interface for DRVEGRD 166 v2.0.0**: Introduced a new UI. +- **New User Interfaces**: Added user interfaces for DRVEGRD 169 v3.0.0, DRVEGRD 169 MSE v1.3.0 and DRVEGRD 171 MSE v2.1.0. +- **Multi-Distro Support**: Added full compatibility for ROS 2 Foxy, Humble, and Jazzy via updated Docker configurations and CI/CD pipelines. +- **Batch Instruction Interface**: Introduced a new extensible service structure allowing multiple parameters of varying types (float, uint8/16/32) to be sent in a single request. +- **Asynchronous Firmware Updates**: Refactored the download protocol to use a thread-safe, non-blocking architecture, preventing RViz UI freezes during sensor updates. +- **Expanded Service Set**: Integrated new GetMode and GetStatus services into the smartmicro node logic. +- **Enhanced RViz Plugins**: Updated the Service Configurator and Download plugins with improved layouts, section-based parameters, and read/write capabilities. + +### Breaking Changes +- **Message Structure Refactor**: Replaced single-field param and value fields with array-based params, values, and value_types to support batch processing. Existing integrations must be updated to the new array format. \ No newline at end of file diff --git a/Dockerfile b/Dockerfile index 2ed0efc..21f6c49 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,20 +1,35 @@ -FROM ros:foxy +# ROS 2 distro (default = foxy) +ARG ROS_DISTRO=foxy +FROM ros:${ROS_DISTRO} -## Revert to snapshot once GPG key error is resolved -RUN rm /etc/apt/sources.list.d/ros2-snapshots.list -RUN apt-get update && apt-get install curl -y -RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg -RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null +# Fix repo keys ONLY for Foxy (snapshots issue). +RUN if [ "$ROS_DISTRO" = "foxy" ]; then \ + rm -f /etc/apt/sources.list.d/ros2-snapshots.list || true && \ + apt-get update && apt-get install -y curl gnupg2 lsb-release && \ + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \ + http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" \ + | tee /etc/apt/sources.list.d/ros2.list > /dev/null ; \ + fi +# Common dependencies RUN apt-get update && apt-get install -y \ iputils-ping \ python3 \ python3-pip \ - ros-foxy-point-cloud-msg-wrapper \ - ros-foxy-rviz2 \ - ros-foxy-rviz-common \ - ros-foxy-rviz-default-plugins \ - ros-foxy-rviz-rendering \ - wget + wget \ + git \ + ca-certificates \ + && update-ca-certificates \ + && rm -rf /var/lib/apt/lists/* + +# ROS-specific dependencies +RUN apt-get update && apt-get install -y \ + ros-${ROS_DISTRO}-point-cloud-msg-wrapper \ + ros-${ROS_DISTRO}-rviz2 \ + ros-${ROS_DISTRO}-rviz-common \ + ros-${ROS_DISTRO}-rviz-default-plugins \ + ros-${ROS_DISTRO}-rviz-rendering WORKDIR /code diff --git a/Readme.md b/Readme.md index e0cc2dc..7dbeab7 100644 --- a/Readme.md +++ b/Readme.md @@ -19,10 +19,10 @@ ros2 launch umrr_ros2_driver radar.launch.py ## How to launch the rviz with recorder plugin From a separate terminal and after sourcing workspace ``` -rviz2 -d [`recorder.rviz`](smart_rviz_plugin/config/rviz/recorder.rviz) +rviz2 -d smartmicro_ros2_radars/umrr_ros2_driver/config/rviz/smart_plugin.rviz ``` -![Recorder](smart_rviz_plugin/config/images/recorder_rviz.png "Rviz Outlook") +![Recorder](smart_rviz_plugin/config/images/smart_rviz_plugin.png "Rviz Outlook") ## How to start the custom can message sender From smart_rviz_plugin folder @@ -42,8 +42,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: `February 12, 2025` -- Smart Access Automotive version: `v3.9.0` +- Date of release: `March 03, 2026` +- Smart Access Automotive version: `v3.11.0` For each sensor user interface there is a corressponding sensor firmware. The following list all the possible combinations. @@ -64,14 +64,20 @@ For each sensor user interface there is a corressponding sensor firmware. The fo | UMRR9D Type 152 AUTOMOTIVE v1.2.2 | UMRR9D Type 152: V2.5.0 | | UMRR9D Type 152 AUTOMOTIVE v1.4.1 | UMRR9D Type 152: V2.7.0 | | UMRR9D Type 152 AUTOMOTIVE v1.5.0 | UMRR9D Type 152: V3.3.0 | +| UMRR9D Type 152 AUTOMOTIVE v1.5.0 | UMRR9D Type 152: V3.6.0 | | UMRRA4 Type 171 AUTOMOTIVE v1.0.0 | UMRRA4 Type 171: V1.0.0 | | UMRRA4 Type 171 AUTOMOTIVE v1.0.1 | UMRRA4 Type 171: V1.0.0 | | UMRRA4 Type 171 AUTOMOTIVE v1.2.1 | UMRRA4 Type 171: V1.2.1 | | UMRRA4 Type 171 AUTOMOTIVE v1.4.0 | UMRRA4 Type 171: V2.0.0 | +| UMRRA4 Type 171 AUTOMOTIVE v1.4.0 | UMRRA4 Type 171: V2.3.0 | | UMRR11 Type 132 MSE v1.1.1 | UMRR11 Type 132-MSE: V6.1.2 | | UMRR9F Type 169 MSE v1.0.0 | UMRR9F Type 169-MSE: V1.1.0 | | UMRR9F Type 169 MSE v1.1.0 | UMRR9F Type 169-MSE: V1.3.0 | +| UMRR9F Type 169 MSE v1.3.0 | UMRR9F Type 169-MSE: V1.5.0 | | UMRRA4 Type 171 MSE v1.0.0 | UMRR9F Type 171-MSE: V1.0.0 | +| UMRRA4 Type 171 MSE v1.3.0 | UMRR9F Type 171-MSE: V2.1.0 | +| UMRRA1 Type 166 AUTOMOTIVE v1.0.0 | UMRRA1 Type 166: V1.0.0 | +| UMRRA1 Type 166 AUTOMOTIVE v2.0.0 | UMRRA1 Type 166: V1.0.0 | ### Point cloud message wrapper library To add targets to the point cloud in a safe and quick fashion a @@ -120,11 +126,13 @@ To set up the ***sensors***, configure the following parameters: - **`model`**: Defines the model of the sensor being used. - **CAN Models**: - `umrra4_can_mse_v1_0_0`, `umrr9f_can_mse_v1_1_0`, `umrr9f_can_mse_v1_0_0`, `umrr96_can_v1_2_2`, `umrr11_can_v1_1_2`, `umrr9f_can_v2_1_1`, `umrr9f_can_v2_2_1`, - `umrr9f_can_v2_4_1`, `umrr9d_can_v1_0_3`, `umrr9d_can_v1_2_2`, `umrr9d_can_v1_4_1`, `umrr9d_can_v1_5_0`, `umrra4_can_v1_0_1`, `umrra4_can_v1_2_1`,`umrra4_can_v1_4_0` + 'umrra4_can_mse_v1_0_0', 'umrra4_can_mse_v2_1_0', 'umrr9f_can_mse_v1_1_0', 'umrr9f_can_mse_v1_0_0', 'umrr96_can_v1_2_2', + 'umrr11_can_v1_1_2', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1', 'umrr9f_can_v2_4_1', 'umrr9f_can_v3_0_0', 'umrr9d_can_v1_0_3', + 'umrr9d_can_v1_2_2', 'umrr9d_can_v1_4_1', 'umrr9d_can_v1_5_0', 'umrra4_can_v1_0_1', 'umrra4_can_v1_2_1', 'umrra4_can_v1_4_0' - **Port Models**: - `umrra4_mse_v1_0_0`, `umrr9f_mse_v1_1_0`, `umrr9f_mse_v1_0_0`, `umrr96_v1_2_2`, `umrr11_v1_1_2`, `umrr9f_v2_1_1`, `umrr9f_v2_2_1`, - `umrr9f_v2_4_1`, `umrr9d_v1_0_3`, `umrr9d_v1_2_2`, `umrr9d_v1_4_1`, `umrr9d_v1_5_0`, `umrra4_v1_0_1`, `umrra4_v1_2_1`,`umrra4_v1_4_0` + 'umrra1_v2_0_0', 'umrra1_v1_0_0', 'umrra4_mse_v1_0_0', 'umrra4_mse_v2_1_0', 'umrr9f_mse_v1_3_0', 'umrr9f_mse_v1_1_0', + 'umrr9f_mse_v1_0_0', 'umrr96_v1_2_2', 'umrr11_v1_1_2', 'umrr9f_v2_1_1', 'umrr9f_v2_2_1', 'umrr9f_v2_4_1','umrr9f_v3_0_0', + 'umrr9d_v1_0_3', 'umrr9d_v1_2_2', 'umrr9d_v1_4_1', 'umrr9d_v1_5_0', 'umrra4_v1_0_1', 'umrra4_v1_2_1', 'umrra4_v1_4_0' - **`dev_id`**: Adapter ID to which the sensor is connected. ***Note:*** The adapter and sensor must have the same `dev_id`. @@ -180,21 +188,42 @@ To set up the ***adapters***, configure the following parameters: The smartmicro radars come equipped with numerous features and modes of operation. Using the ros2 services provided one 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) -- `value`: the mode of operation (specific to sensor where the modes are same) +A ros2 `SetMode` service should be called to implement these mode changes. These are the inputs to a ros2 `SetMode` service call: +- `params`: name/names of the mode instructions (specific to the sensor). +- `values`: the mode of operation (specific to sensor where the modes are same). - `sensor_id`: the id of the sensor to which the service call should be sent. +- `value_types`: the data types for the params `0: float32, 1: uint32, 2: uint16, 3: uint8`. +- `section_name`: the name of the section in the instruction file. -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}"` +For instance, changing the `Index of center frequency (center_frequency_idx)` of a UMRR-A4 sensor to `(1)` mode would require the following call: +`ros2 service call /smart_radar/set_radar_mode umrr_ros2_msgs/srv/SetMode "{section_name: auto_interface_0dim, sensor_id: 100, params: ['center_frequency_idx'], values: ['1'], value_types: [3]}"` + +A ros2 'GetMode' service can be called to get the actual sensor modes. The inputs for this call are: +- `params`: name/names of the mode instructions (specific to the sensor). +- `sensor_id`: the id of the sensor to which the service call should be sent. +- `param_types`: the data types for the params `0: float32, 1: uint32, 2: uint16, 3: uint8`. +- `section_name`: the name of the section in the instruction file. + +For instance, getting the `Index of center frequency (center_frequency_idx)` of a UMRR-A4 sensor to `(1)` mode would require the following call: +`ros2 service call /smart_radar/get_radar_mode umrr_ros2_msgs/srv/GetMode "{section_name: auto_interface_0dim, sensor_id: 100, params: ['center_frequency_idx'], param_types: [3]}"` Similarly, a ros2 `SendCommand` service could be used to send commands to the sensors. There are three inputs for sending a command: - `command`: name of the command (specific to the sensor interface) - `value`: the value of the command - `sensor_id`: the id of the sensor to which the service call should be sent. +- `section_name`: the name of the section in the instruction file. 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", value: 2, sensor_id: 100}"` +`ros2 service call /smart_radar/send_command umrr_ros2_msgs/srv/SendCommand "{section_name: auto_interface_command, command: "comp_eeprom_ctrl_default_param_sec", value: 2, sensor_id: 100}"` + +Apart from commands and modes we can also access the status of the sensor using a ros2 `GetStatus` service. It has three inputs: +- `statuses`: name/names of the status (specific to the sensor). +- `sensor_id`: the id of the sensor to which the service call should be sent. +- `status_types`: the data types for the status `0: uint32, 1: uint16`. +- `section_name`: the name of the section in the instruction file. + +The call would be like follows: +`ros2 service call /smart_radar/get_radar_status umrr_ros2_msgs/srv/GetStatus "{section_name: auto_interface, sensor_id: 100, statuses: ["sw_version_major", "sw_version_minor"], status_types: [1, 1]}"` ## 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. @@ -242,10 +271,11 @@ The sensor services respond with certain value codes. The following is a lookup 7 | Value out of minimal bounds 8 | Value out of maximal bounds -## Recorder plugin and custom CAN sender -A custom plugin for rviz to log the target list has been provided. A config file is available which adds this plugin to the rviz. WIth the plugin -it is now possible to view the target list data for the desired sensor. Along with logging the data the plugin also gives the possibility to record -the target list data, convert it into a csv format and save it. +## RVIZ plugins and custom CAN sender +Custom plugins for rviz has been provided. This plugin provides logging of the target list, object list and their respective headers. +It provides a command configurator plugin through which commands, status and mode reqeust could be send. It also provides a plugin for initiating a firmware download. +A config file is available which adds this plugin to the rviz. Along with logging the data the plugin also gives the possibility to record +the target/object list data, convert it into a csv format and save it. Separately, a python GUI is also provided with which it is possible to send custom CAN messages. diff --git a/docker-compose.yml b/docker-compose.yml index 8e023c9..e139d33 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -15,7 +15,7 @@ version: "3" services: build_simulator: - image: umrr-ros:latest + image: umrr-ros:${ROS_DISTRO:-foxy} volumes: - .:/code entrypoint: ["/code/simulator/simulation/build_simulator.sh"] @@ -23,7 +23,7 @@ services: depends_on: build_simulator: condition: service_completed_successfully - image: umrr-ros:latest + image: umrr-ros:${ROS_DISTRO:-foxy} volumes: - .:/code environment: @@ -38,7 +38,7 @@ services: depends_on: build_simulator: condition: service_completed_successfully - image: umrr-ros:latest + image: umrr-ros:${ROS_DISTRO:-foxy} volumes: - .:/code environment: @@ -53,7 +53,7 @@ services: depends_on: build_simulator: condition: service_completed_successfully - image: umrr-ros:latest + image: umrr-ros:${ROS_DISTRO:-foxy} volumes: - .:/code environment: @@ -68,7 +68,7 @@ services: depends_on: build_simulator: condition: service_completed_successfully - image: umrr-ros:latest + image: umrr-ros:${ROS_DISTRO:-foxy} volumes: - .:/code environment: @@ -80,7 +80,7 @@ services: device_network: ipv4_address: 172.22.10.104 ros_node: - image: umrr-ros:latest + image: umrr-ros:${ROS_DISTRO:-foxy} depends_on: - "sensor_0" - "sensor_1" diff --git a/simulator/simulation/src/simulator.cpp b/simulator/simulation/src/simulator.cpp index cdc2a31..1e7d654 100644 --- a/simulator/simulation/src/simulator.cpp +++ b/simulator/simulation/src/simulator.cpp @@ -55,40 +55,23 @@ void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) auto instructions = receive->GetInstructions(); for (auto instruction : instructions) { - if (instruction->GetSectionId() == 3042 && instruction->GetId() == 20) { - std::cout << "Base user interface major version set!" << std::endl; - instruction->SetResponse(COM_INSTR_PORT_SUCCESS); - instruction->SetValue(1); - } else if (instruction->GetSectionId() == 3042 && instruction->GetId() == 21) { - std::cout << "Base user interface minor version set!" << std::endl; - instruction->SetResponse(COM_INSTR_PORT_SUCCESS); - instruction->SetValue(0); - } else if (instruction->GetSectionId() == 3042 && instruction->GetId() == 22) { - std::cout << "User interface identifier set!" << std::endl; - instruction->SetResponse(COM_INSTR_PORT_SUCCESS); - instruction->SetValue(identifier); - } else if (instruction->GetSectionId() == 3042 && instruction->GetId() == 23) { - std::cout << "User interface major version set!" << std::endl; - instruction->SetResponse(COM_INSTR_PORT_SUCCESS); - instruction->SetValue(majorVersion); - } else if (instruction->GetSectionId() == 3042 && instruction->GetId() == 24) { - std::cout << "User interface minor version set!" << std::endl; - instruction->SetResponse(COM_INSTR_PORT_SUCCESS); - instruction->SetValue(minorVersion); - } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 2) { - std::cout << "UMRR96 mode frequency_sweep set!" << std::endl; - instruction->SetResponse(COM_INSTR_PORT_SUCCESS); - } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 4) { - std::cout << "UMRR11 mode angular_separation set!" << std::endl; - instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + if (instruction->GetSectionId() == 2010 && instruction->GetId() == 2) { + std::cout << "UMRR96 mode frequency_sweep set!" << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 5) { - std::cout << "UMRR9F mode range_toggle_mode set!" << std::endl; - instruction->SetResponse(COM_INSTR_PORT_SUCCESS); - } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 1) { - std::cout << "UMRR9D mode center_frequency_idx set!" << std::endl; - instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + std::cout << "UMRR9F mode range_toggle_mode get!" << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 4) { + std::cout << "UMRR11 mode angular_separation set!" << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + } else if (instruction->GetSectionId() == 2012 && instruction->GetId() == 3) { + std::cout << "Software major version read!" << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + } else if (instruction->GetSectionId() == 2012 && instruction->GetId() == 4) { + std::cout << "Software minorr version read!" << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); } else { - std::cout << "Unknown instruction received from ROS driver!" << std::endl; + std::cout << "Unknown instruction received from ROS driver!" << std::endl; } } diff --git a/smart_extract.sh b/smart_extract.sh index 4633b05..3f19c6a 100755 --- a/smart_extract.sh +++ b/smart_extract.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -smart_pack=SmartAccessAutomotive_3_9_0.tgz +smart_pack=SmartAccessAutomotive_3_11_0.tgz URL_smartbinaries=https://www.smartmicro.com/fileadmin/media/Downloads/Automotive_Radar/Software/${smart_pack} cat << EOF diff --git a/smart_rviz_plugin/config/images/recorder_rviz.png b/smart_rviz_plugin/config/images/recorder_rviz.png deleted file mode 100644 index 3f73ca3..0000000 Binary files a/smart_rviz_plugin/config/images/recorder_rviz.png and /dev/null differ diff --git a/smart_rviz_plugin/config/images/smart_rviz_plugin.png b/smart_rviz_plugin/config/images/smart_rviz_plugin.png new file mode 100644 index 0000000..fb19657 Binary files /dev/null and b/smart_rviz_plugin/config/images/smart_rviz_plugin.png differ diff --git a/smart_rviz_plugin/config/rviz/recorder.rviz b/smart_rviz_plugin/config/rviz/smart_plugin.rviz similarity index 65% rename from smart_rviz_plugin/config/rviz/recorder.rviz rename to smart_rviz_plugin/config/rviz/smart_plugin.rviz index a99da53..d5e90ac 100644 --- a/smart_rviz_plugin/config/rviz/recorder.rviz +++ b/smart_rviz_plugin/config/rviz/smart_plugin.rviz @@ -1,16 +1,11 @@ Panels: - Class: rviz_common/Displays - Help Height: 81 + Help Height: 0 Name: Displays Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /PointCloud21 - - /PointCloud22 - - /PointCloud23 + Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 1140 + Tree Height: 295 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -30,12 +25,10 @@ Panels: Name: Smart Status - Class: smart_rviz_plugin/Smart Command Configurator Name: Smart Command Configurator - - Class: smart_rviz_plugin/Smart Firmware Download - Name: Smart Firmware Download - Class: smart_rviz_plugin/Smart Status Name: Smart Status - - Class: smart_rviz_plugin/Smart Recorder - Name: Smart Recorder + - Class: smart_rviz_plugin/Smart Firmware Download + Name: Smart Firmware Download Visualization Manager: Class: "" Displays: @@ -57,38 +50,12 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 204; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 + - Class: rviz_default_plugins/Axes Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 15 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /smart_radar/can_targets_1 - Use Fixed Frame: true - Use rainbow: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -97,7 +64,7 @@ Visualization Manager: Min Value: -10 Value: true Axis: Z - Channel Name: heading + Channel Name: power Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity @@ -105,21 +72,21 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: -999999 + Max Intensity: 88.64950561523438 Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: PointCloud2 + Min Intensity: 56.63246154785156 + Name: Port_Tgts_S0 Position Transformer: XYZ Selectable: true - Size (Pixels): 3 - Size (m): 1 - Style: Boxes + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /smart_radar/port_objects_0 + Value: /smart_radar/port_targets_0 Use Fixed Frame: true Use rainbow: true Value: true @@ -132,8 +99,8 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity + Color: 138; 226; 52 + Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false @@ -141,18 +108,18 @@ Visualization Manager: Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 - Name: PointCloud2 + Name: Can_Tgts_S1 Position Transformer: XYZ Selectable: true - Size (Pixels): 3 - Size (m): 1 - Style: Tiles + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /smart_radar/port_targets_0 + Value: /smart_radar/can_targets_1 Use Fixed Frame: true Use rainbow: true Value: true @@ -199,33 +166,33 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 197.11245727539062 + Distance: 38.8543701171875 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 33.84825134277344 - Y: -4.720839023590088 - Z: 7.3445963859558105 + X: 10.84825325012207 + Y: 0.366975337266922 + Z: 0.01084582507610321 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5047965049743652 + Pitch: 1.5697963237762451 Target Frame: Value: Orbit (rviz) - Yaw: 3.1354000568389893 + Yaw: 3.1353957653045654 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 2090 + Height: 1757 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Smart Command Configurator: @@ -240,6 +207,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 3840 - X: 0 - Y: 33 + Width: 3151 + X: 374 + Y: 214 diff --git a/smart_rviz_plugin/include/smart_rviz_plugin/smart_download.hpp b/smart_rviz_plugin/include/smart_rviz_plugin/smart_download.hpp index 7b2c464..b9dcfdf 100644 --- a/smart_rviz_plugin/include/smart_rviz_plugin/smart_download.hpp +++ b/smart_rviz_plugin/include/smart_rviz_plugin/smart_download.hpp @@ -10,6 +10,9 @@ #include #include #include +#include +#include + #include #include @@ -30,12 +33,8 @@ class SmartDownloadService : public rviz_common::Panel Q_OBJECT public: - /// - /// @brief Constructor for the SmartDownloadService class. - /// - /// @param parent The parent widget. Defaults to nullptr. - /// - SmartDownloadService(QWidget * parent = nullptr); + explicit SmartDownloadService(QWidget * parent = nullptr); + ~SmartDownloadService() override; private slots: /// @@ -46,7 +45,7 @@ private slots: /// and sensor ID. /// void download_firmware(); - + /// /// @brief Slot function to handle file browsing action. /// @@ -58,22 +57,31 @@ private slots: private: /// - /// @brief Initializes the panel's components and ROS2 client. + /// @brief Initializes the ROS2 clients. + /// + /// This function initializes the ROS2 node and + /// client for the firmware download service + /// + void initialize_ros(); + + /// + /// @brief Initializes the panel's components. /// - /// This function sets up the GUI elements, initializes the ROS2 node and - /// client for the firmware download service, and connects the signals and + /// This function sets up the GUI elements and connects the signals and /// slots. /// - void initialize(); + void setup_ui(); - rclcpp::Node::SharedPtr download_node; - rclcpp::Client::SharedPtr download_client; + rclcpp::Node::SharedPtr download_node_; + rclcpp::Client::SharedPtr download_client_; + std::shared_ptr executor_; + std::thread ros_thread_; - QLineEdit * file_path_input; - QLineEdit * sensor_id_input; - QPushButton * start_download_button; - QPushButton * browse_button; - QTextEdit * response_text_edit; + QLineEdit * file_path_input_; + QLineEdit * sensor_id_input_; + QPushButton * start_download_button_; + QPushButton * browse_button_; + QTextEdit * response_text_edit_; }; } // namespace smart_rviz_plugin diff --git a/smart_rviz_plugin/include/smart_rviz_plugin/smart_services.hpp b/smart_rviz_plugin/include/smart_rviz_plugin/smart_services.hpp index 40d30b9..0c484ee 100644 --- a/smart_rviz_plugin/include/smart_rviz_plugin/smart_services.hpp +++ b/smart_rviz_plugin/include/smart_rviz_plugin/smart_services.hpp @@ -2,7 +2,6 @@ #define SMART_RVIZ_PLUGIN__SMART_SERVICES_HPP_ #include -#include #include #include #include @@ -16,11 +15,14 @@ #include #include #include +#include #include #include #include "umrr_ros2_msgs/srv/send_command.hpp" #include "umrr_ros2_msgs/srv/set_mode.hpp" +#include "umrr_ros2_msgs/srv/get_status.hpp" +#include "umrr_ros2_msgs/srv/get_mode.hpp" namespace smart_rviz_plugin { @@ -29,92 +31,129 @@ namespace smart_rviz_plugin /// /// This class provides a graphical user interface (GUI) panel for sending /// instructions to the sensor within the RViz environment. It extends the -/// rviz_common::Panel class and includes functionalities for selecting, viewing -/// and sending the sensor instructions. +/// rviz_common::Panel class and includes functionalities for viewing, recording +/// sensor data and sending instructions to the sensor. /// class SmartRadarService : public rviz_common::Panel { Q_OBJECT public: - /// /// @brief Constructor for the SmartRadarService class. - /// /// @param parent The parent widget. Defaults to nullptr. - /// - SmartRadarService(QWidget * parent = nullptr); + explicit SmartRadarService(QWidget * parent = nullptr); private slots: - /// /// @brief Slot function to send a parameter instruction to the sensor. - /// void on_send_param(); - /// /// @brief Slot function to send a command instruction to the sensor. - /// void on_send_command(); - /// + /// @brief Slot function to send a status instruction to the sensor. + void on_get_status(); + /// @brief Slot function to display instruction set of selected sensor model. - /// + /// @param index The index of the selected file in the combo box. void on_file_selected(int index); + /// @brief Slot function to handle param selection from the table. + void on_param_selection(); + + /// @brief Slot function to handle command selection from the table. + void on_command_selection(); + + /// @brief Slot function to handle status selection from the table. + void on_status_selection(); + private: - /// - /// @brief Initializes the panel's components and ROS2 client. - /// - /// This function sets up the GUI elements, initializes the ROS2 node and - /// client for the instruction services, and connects the signals and - /// slots. - /// + /// @brief Initializes the panel's components. void initialize(); - /// + /// @brief Initializes the ROS2 client. + void setup_ros_clients(); + + /// @brief Create widgets for the plugin. + void create_widgets(); + + /// @brief Create layout. + void setup_layout(); + + /// @brief Set up the connections based on activity. + void setup_connections(); + /// @brief Read parameters from user interface json file. - /// void read_param_json_data(); - /// /// @brief Read commands from user interface json file. - /// void read_command_json_data(); - /// - /// @brief Populate the table with the read json files. - /// + /// @brief Read statuses from user interface json file. + void read_status_json_data(); + + /// @brief Populates the table with the available user interface files. void populate_file_menu(); + // File paths const QString current_directory = QDir::currentPath(); QString param_json_file_path; QString command_json_file_path; - - QLineEdit * command_value_line_edit; - QLineEdit * command_argument_line_edit; - - QVBoxLayout * main_layout; - QTabWidget * tab_widget; - QWidget * param_tab; - QLineEdit * param_name_line_edit; - QLineEdit * param_value_line_edit; - QLineEdit * param_min_line_edit; - QLineEdit * param_max_line_edit; - QLineEdit * param_sensor_id; - QLineEdit * command_name_line_edit; - QLineEdit * command_comment_line_edit; - QLineEdit * command_sensor_id; - - QPushButton * send_param_button; - QPushButton * send_command_button; - QTableWidget * param_table_widget; - QTableWidget * command_table_widget; - QWidget * command_tab; - QComboBox * file_selector_combo_box; - QTextEdit * response_text_edit; - + QString status_json_file_path; + + // Layout components + QVBoxLayout * main_layout{nullptr}; + QTabWidget * tab_widget{nullptr}; + QWidget * param_tab{nullptr}; + QWidget * command_tab{nullptr}; + QWidget * status_tab{nullptr}; + + // Parameter input fields + QLineEdit * param_name_line_edit{nullptr}; + QLineEdit * param_value_line_edit{nullptr}; + QLineEdit * param_sensor_id{nullptr}; + QLineEdit * param_section_name{nullptr}; + + // Command input fields + QLineEdit * command_name_line_edit{nullptr}; + //QLineEdit * command_comment_line_edit{nullptr}; + QLineEdit * command_sensor_id{nullptr}; + QLineEdit * command_value_line_edit{nullptr}; + QLineEdit * command_section_name{nullptr}; + + // Status input fields + QLineEdit * status_name_line_edit{nullptr}; + QLineEdit * status_sensor_id{nullptr}; + QLineEdit * status_section_name{nullptr}; + + // Buttons + QPushButton * send_param_button{nullptr}; + QPushButton * send_command_button{nullptr}; + QPushButton * send_status_button{nullptr}; + + // Tables and combo boxes + QTableWidget * param_table_widget{nullptr}; + QTableWidget * command_table_widget{nullptr}; + QTableWidget * status_table_widget{nullptr}; + QTextEdit * response_text_edit{nullptr}; + QComboBox * file_selector_combo_box{nullptr}; + QComboBox * param_value_type{nullptr}; + QComboBox * status_value_type{nullptr}; + QComboBox * param_action_combo{nullptr}; + + // Forms + QFormLayout * param_form{nullptr}; + QFormLayout * command_form{nullptr}; + QFormLayout * status_form{nullptr}; + + // ROS2 components rclcpp::Node::SharedPtr client_node; rclcpp::Client::SharedPtr mode_client; rclcpp::Client::SharedPtr command_client; + rclcpp::Client::SharedPtr status_client; + rclcpp::Client::SharedPtr get_param_client; + + // Constants + static constexpr auto SERVICE_AVAILABILITY_TIMEOUT = std::chrono::seconds(2); }; } // namespace smart_rviz_plugin diff --git a/smart_rviz_plugin/package.xml b/smart_rviz_plugin/package.xml index 0c7949a..c4266b8 100644 --- a/smart_rviz_plugin/package.xml +++ b/smart_rviz_plugin/package.xml @@ -2,7 +2,7 @@ smart_rviz_plugin - 2.0.0 + 3.0.0 TargetList Recorder shahrukh Apache 2.0 License diff --git a/smart_rviz_plugin/src/smart_download.cpp b/smart_rviz_plugin/src/smart_download.cpp index e776f2f..9104faa 100644 --- a/smart_rviz_plugin/src/smart_download.cpp +++ b/smart_rviz_plugin/src/smart_download.cpp @@ -1,101 +1,141 @@ #include "smart_rviz_plugin/smart_download.hpp" +#include namespace smart_rviz_plugin { -SmartDownloadService::SmartDownloadService(QWidget * parent) : rviz_common::Panel(parent) + +SmartDownloadService::SmartDownloadService(QWidget * parent) +: rviz_common::Panel(parent) +{ + initialize_ros(); + setup_ui(); +} + +SmartDownloadService::~SmartDownloadService() { - initialize(); + if (executor_) { + executor_->cancel(); + } + if (ros_thread_.joinable()) { + ros_thread_.join(); + } + rclcpp::shutdown(); } -void SmartDownloadService::initialize() +void SmartDownloadService::initialize_ros() { - // Initialize ROS 2 if (!rclcpp::ok()) { rclcpp::init(0, nullptr); } - // Declare node and client - download_node = rclcpp::Node::make_shared("smart_download_gui"); - download_client = download_node->create_client( + download_node_ = rclcpp::Node::make_shared("smart_download_gui"); + download_client_ = download_node_->create_client( "smart_radar/firmware_download"); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Download node created!"); + executor_ = std::make_shared(); + executor_->add_node(download_node_); - // GUI setup - file_path_input = new QLineEdit(this); - sensor_id_input = new QLineEdit(this); - start_download_button = new QPushButton("Start Download", this); - browse_button = new QPushButton("Browse", this); + ros_thread_ = std::thread([this]() { executor_->spin(); }); - response_text_edit = new QTextEdit(this); - response_text_edit->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - response_text_edit->setFixedSize(1200, 100); + RCLCPP_INFO(download_node_->get_logger(), "SmartDownloadService node initialized."); +} - QVBoxLayout * layout = new QVBoxLayout(this); - layout->addWidget(new QLabel("File Path:")); - layout->addWidget(file_path_input); - layout->addWidget(browse_button); +void SmartDownloadService::setup_ui() +{ + file_path_input_ = new QLineEdit(this); + sensor_id_input_ = new QLineEdit(this); + start_download_button_ = new QPushButton("Start Download", this); + browse_button_ = new QPushButton("Browse", this); + response_text_edit_ = new QTextEdit(this); + response_text_edit_->setReadOnly(true); + response_text_edit_->setFixedHeight(120); + + // Compact file path layout + auto * file_layout = new QHBoxLayout; + file_layout->addWidget(file_path_input_); + file_layout->addWidget(browse_button_); + + // Main layout + auto * layout = new QVBoxLayout(this); + layout->addWidget(new QLabel("Firmware File Path:")); + layout->addLayout(file_layout); layout->addWidget(new QLabel("Sensor ID:")); - layout->addWidget(sensor_id_input); - layout->addWidget(start_download_button); - layout->addWidget(response_text_edit); + layout->addWidget(sensor_id_input_); + layout->addWidget(start_download_button_); + layout->addWidget(response_text_edit_); + setLayout(layout); // Connect signals and slots - connect(start_download_button, SIGNAL(clicked()), this, SLOT(download_firmware())); - connect(browse_button, SIGNAL(clicked()), this, SLOT(browse_file())); + connect(start_download_button_, &QPushButton::clicked, this, &SmartDownloadService::download_firmware); + connect(browse_button_, &QPushButton::clicked, this, &SmartDownloadService::browse_file); } void SmartDownloadService::download_firmware() { - if (!download_client) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to create download_client"); + if (!download_client_) { + RCLCPP_ERROR(download_node_->get_logger(), "Download client not created."); return; } - // Get file path and sensor ID - QString filePath = file_path_input->text(); - int sensorId = sensor_id_input->text().toInt(); + QString file_path = file_path_input_->text().trimmed(); + QString sensor_id_str = sensor_id_input_->text().trimmed(); - if (filePath.isEmpty()) { - QMessageBox::warning(this, "Warning", "Please select a file to download.", QMessageBox::Ok); + if (file_path.isEmpty()) { + QMessageBox::warning(this, "Warning", "Please select a firmware file.", QMessageBox::Ok); + return; + } + if (sensor_id_str.isEmpty() || !sensor_id_str.toInt()) { + QMessageBox::warning(this, "Warning", "Please enter a valid numeric sensor ID.", QMessageBox::Ok); return; } - start_download_button->setText("Downloading..."); + int sensor_id = sensor_id_str.toInt(); + start_download_button_->setEnabled(false); + start_download_button_->setText("Downloading..."); - // Set up the request auto request = std::make_shared(); - request->file_path = filePath.toStdString(); - request->sensor_id = sensorId; - - auto result = download_client->async_send_request(request); - - // Wait for the result. - if ( - rclcpp::spin_until_future_complete(download_node, result) == - rclcpp::FutureReturnCode::SUCCESS) { - QString response_msg = QString::fromStdString(result.get()->res); - response_text_edit->append("Download request sent.\n Response from sensor: " + response_msg); - RCLCPP_INFO( - rclcpp::get_logger("rclcpp"), "Request successful\n. Response: %s ", - result.get()->res.c_str()); - start_download_button->setText("Start Download"); - } else { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to download sensor firmware"); - response_text_edit->append("Failed to downlaod sensor firmware!"); - start_download_button->setText("Start Download"); + request->file_path = file_path.toStdString(); + request->sensor_id = sensor_id; + + if (!download_client_->wait_for_service(std::chrono::seconds(2))) { + QMessageBox::critical(this, "Error", "Firmware download service not available."); + start_download_button_->setEnabled(true); + start_download_button_->setText("Start Download"); + return; } + + auto future = download_client_->async_send_request(request); + + // Async callback, do non block rviz + std::thread([this, future]() mutable { + try { + auto result = future.get(); + QString response_msg = QString::fromStdString(result->res); + QMetaObject::invokeMethod(this, [this, response_msg]() { + response_text_edit_->append("Response: " + response_msg); + start_download_button_->setEnabled(true); + start_download_button_->setText("Start Download"); + }); + RCLCPP_INFO(download_node_->get_logger(), "Firmware download succeeded: %s", result->res.c_str()); + } catch (const std::exception & e) { + QMetaObject::invokeMethod(this, [this, e]() { + response_text_edit_->append(QString("Error: %1").arg(e.what())); + start_download_button_->setEnabled(true); + start_download_button_->setText("Start Download"); + }); + RCLCPP_ERROR(download_node_->get_logger(), "Firmware download failed: %s", e.what()); + } + }).detach(); } void SmartDownloadService::browse_file() { - QString filePath = QFileDialog::getOpenFileName(this, tr("Open File"), "", tr("All Files (*)")); - if (!filePath.isEmpty()) { - file_path_input->setText(filePath); + QString file_path = QFileDialog::getOpenFileName(this, tr("Select Firmware File"), "", tr("All Files (*)")); + if (!file_path.isEmpty()) { + file_path_input_->setText(file_path); } } } // namespace smart_rviz_plugin -#include PLUGINLIB_EXPORT_CLASS(smart_rviz_plugin::SmartDownloadService, rviz_common::Panel) diff --git a/smart_rviz_plugin/src/smart_services.cpp b/smart_rviz_plugin/src/smart_services.cpp index 5879f3f..05522f4 100644 --- a/smart_rviz_plugin/src/smart_services.cpp +++ b/smart_rviz_plugin/src/smart_services.cpp @@ -18,77 +18,186 @@ void SmartRadarService::initialize() rclcpp::init(0, nullptr); } + setup_ros_clients(); + create_widgets(); + setup_layout(); + setup_connections(); +} + +void SmartRadarService::setup_ros_clients() +{ client_node = rclcpp::Node::make_shared("smart_service_gui"); - mode_client = - client_node->create_client("smart_radar/set_radar_mode"); - command_client = - client_node->create_client("smart_radar/send_command"); + mode_client = client_node->create_client("smart_radar/set_radar_mode"); + command_client = client_node->create_client("smart_radar/send_command"); + status_client = client_node->create_client("/smart_radar/get_radar_status"); + get_param_client = client_node->create_client("smart_radar/get_radar_mode"); RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Client node created!"); +} +void SmartRadarService::create_widgets() +{ + // Parameter tab widget param_name_line_edit = new QLineEdit(this); + param_name_line_edit->setPlaceholderText("Enter parameter name"); + param_value_line_edit = new QLineEdit(this); + param_value_line_edit->setPlaceholderText("Enter value"); + param_sensor_id = new QLineEdit(this); - send_param_button = new QPushButton("Send Param", this); + param_sensor_id->setPlaceholderText("Enter sensor ID"); + + param_section_name = new QLineEdit(this); + param_section_name->setPlaceholderText("Enter param section name"); + + param_action_combo = new QComboBox(this); + param_action_combo->addItem("Write Parameter"); + param_action_combo->addItem("Read Parameter"); + + param_value_type = new QComboBox(this); + param_value_type->addItem("float32, (0)"); + param_value_type->addItem("uint32, (1)"); + param_value_type->addItem("uint16, (2)"); + param_value_type->addItem("uint8, (3)"); + + send_param_button = new QPushButton("Send Parameter", this); param_table_widget = new QTableWidget(this); + param_table_widget->setSelectionBehavior(QAbstractItemView::SelectRows); + param_table_widget->setSelectionMode(QAbstractItemView::SingleSelection); + // Command tab widgets command_name_line_edit = new QLineEdit(this); - command_comment_line_edit = new QLineEdit(this); + command_name_line_edit->setPlaceholderText("Enter command name"); + command_value_line_edit = new QLineEdit(this); + command_value_line_edit->setPlaceholderText("Enter command value"); + command_sensor_id = new QLineEdit(this); + command_sensor_id->setPlaceholderText("Enter sensor ID"); + + command_section_name = new QLineEdit(this); + command_section_name->setPlaceholderText("Enter command sectionn name"); + send_command_button = new QPushButton("Send Command", this); command_table_widget = new QTableWidget(this); - + command_table_widget->setSelectionBehavior(QAbstractItemView::SelectRows); + command_table_widget->setSelectionMode(QAbstractItemView::SingleSelection); + + // Status tab widget + status_name_line_edit = new QLineEdit(this); + status_name_line_edit->setPlaceholderText("Enter status name"); + + status_sensor_id = new QLineEdit(this); + status_sensor_id->setPlaceholderText("Enter sensor ID"); + + status_section_name = new QLineEdit(this); + status_section_name->setPlaceholderText("Enter status section name"); + + status_value_type = new QComboBox(this); + status_value_type->addItem("uint32, (0)"); + status_value_type->addItem("uint16, (1)"); + + send_status_button = new QPushButton("Get Status", this); + status_table_widget = new QTableWidget(this); + status_table_widget->setSelectionBehavior(QAbstractItemView::SelectRows); + status_table_widget->setSelectionMode(QAbstractItemView::SingleSelection); + + // Create tabs tab_widget = new QTabWidget(this); param_tab = new QWidget(tab_widget); command_tab = new QWidget(tab_widget); + status_tab = new QWidget(tab_widget); + + // Add dropdown menu for file selection + file_selector_combo_box = new QComboBox(this); + populate_file_menu(); + + response_text_edit = new QTextEdit(this); + response_text_edit->setReadOnly(true); + response_text_edit->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed); + response_text_edit->setFixedHeight(100); + +} +void SmartRadarService::setup_layout() +{ + // Parameter tab layout QVBoxLayout * param_layout = new QVBoxLayout(param_tab); - QVBoxLayout * command_layout = new QVBoxLayout(command_tab); + param_layout->setSpacing(10); + param_layout->setContentsMargins(10, 10, 10, 10); + + QFormLayout * param_form = new QFormLayout(); + param_form->addRow("Action:", param_action_combo); + param_form->addRow("Parameter Name:", param_name_line_edit); + param_form->addRow("Section Name:", param_section_name); + param_form->addRow("Value Type:", param_value_type); + param_form->addRow("Value:", param_value_line_edit); + param_form->addRow("Sensor ID:", param_sensor_id); param_layout->addWidget(param_table_widget); - param_layout->addWidget(new QLabel("Enter Param:")); - param_layout->addWidget(param_name_line_edit); - param_layout->addWidget(new QLabel("Enter Value:")); - param_layout->addWidget(param_value_line_edit); - param_layout->addWidget(new QLabel("Enter Sensor ID:")); - param_layout->addWidget(param_sensor_id); + param_layout->addLayout(param_form); param_layout->addWidget(send_param_button); + + // Command tab widget + QVBoxLayout * command_layout = new QVBoxLayout(command_tab); + command_layout->setSpacing(10); + command_layout->setContentsMargins(10, 10, 10, 10); + + QFormLayout * command_form = new QFormLayout(); + command_form->addRow("Command Name:", command_name_line_edit); + command_form->addRow("Section Name:", command_section_name); + command_form->addRow("Value:", command_value_line_edit); + command_form->addRow("Sensor ID:", command_sensor_id); command_layout->addWidget(command_table_widget); - command_layout->addWidget(new QLabel("Enter Command:")); - command_layout->addWidget(command_name_line_edit); - command_layout->addWidget(new QLabel("Enter Value:")); - command_layout->addWidget(command_value_line_edit); - command_layout->addWidget(new QLabel("Enter Sensor ID:")); - command_layout->addWidget(command_sensor_id); + command_layout->addLayout(command_form); command_layout->addWidget(send_command_button); + // Status tab layout + QVBoxLayout * status_layout = new QVBoxLayout(status_tab); + status_layout->setSpacing(10); + status_layout->setContentsMargins(10, 10, 10, 10); + + QFormLayout * status_form = new QFormLayout(); + status_form->addRow("Status Name:", status_name_line_edit); + status_form->addRow("Section Name:", status_section_name); + status_form->addRow("Value Type:", status_value_type); + status_form->addRow("Sensor ID:", status_sensor_id); + + status_layout->addWidget(status_table_widget); + status_layout->addLayout(status_form); + status_layout->addWidget(send_status_button); + + // Add tabs to tab widget tab_widget->addTab(param_tab, "Parameter"); tab_widget->addTab(command_tab, "Command"); + tab_widget->addTab(status_tab, "Status"); - // Add dropdown menu for file selection - file_selector_combo_box = new QComboBox(this); - populate_file_menu(); - + // Main layout QVBoxLayout * main_layout = new QVBoxLayout(this); + main_layout->setSpacing(10); + main_layout->setContentsMargins(10, 10, 10, 10); + main_layout->addWidget(file_selector_combo_box); main_layout->addWidget(tab_widget); - - response_text_edit = new QTextEdit(this); - response_text_edit->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - response_text_edit->setFixedSize(1200, 100); main_layout->addWidget(response_text_edit); +} +void SmartRadarService::setup_connections() +{ // Connect dropdown menu signal to slot - connect( - file_selector_combo_box, QOverload::of(&QComboBox::activated), this, - &SmartRadarService::on_file_selected); - - read_param_json_data(); - read_command_json_data(); - connect(send_param_button, SIGNAL(clicked()), this, SLOT(on_send_param())); - connect(send_command_button, SIGNAL(clicked()), this, SLOT(on_send_command())); + connect(file_selector_combo_box, QOverload::of(&QComboBox::activated), this, &SmartRadarService::on_file_selected); + connect(send_param_button, &QPushButton::clicked, this, &SmartRadarService::on_send_param); + connect(send_command_button, &QPushButton::clicked, this, &SmartRadarService::on_send_command); + connect(send_status_button, &QPushButton::clicked, this, &SmartRadarService::on_get_status); + connect(param_table_widget, &QTableWidget::itemSelectionChanged, this, &SmartRadarService::on_param_selection); + connect(command_table_widget, &QTableWidget::itemSelectionChanged, this, &SmartRadarService::on_command_selection); + connect(status_table_widget, &QTableWidget::itemSelectionChanged, this, &SmartRadarService::on_status_selection); + connect(param_action_combo, QOverload::of(&QComboBox::currentIndexChanged), + [this](int index) { + // Disable value input when "Read Parameter" + param_value_line_edit->setEnabled(index == 0); + }); } void SmartRadarService::populate_file_menu() @@ -101,6 +210,37 @@ void SmartRadarService::populate_file_menu() file_selector_combo_box->addItem("UMRR9D"); file_selector_combo_box->addItem("UMRRA4"); file_selector_combo_box->addItem("UMRRA4 MSE"); + file_selector_combo_box->addItem("UMRRA1"); +} + +void SmartRadarService::on_param_selection() +{ + QList selected = param_table_widget->selectedItems(); + if (!selected.isEmpty()) { + int row = selected[0]->row(); + param_name_line_edit->setText(param_table_widget->item(row, 1)->text()); + param_section_name->setText(param_table_widget->item(row, 0)->text()); + } +} + +void SmartRadarService::on_command_selection() +{ + QList selected = command_table_widget->selectedItems(); + if (!selected.isEmpty()) { + int row = selected[0]->row(); + command_name_line_edit->setText(command_table_widget->item(row, 1)->text()); + command_section_name->setText(command_table_widget->item(row, 0)->text()); + } +} + +void SmartRadarService::on_status_selection() +{ + QList selected = status_table_widget->selectedItems(); + if (!selected.isEmpty()) { + int row = selected[0]->row(); + status_name_line_edit->setText(status_table_widget->item(row, 1)->text()); + status_section_name->setText(status_table_widget->item(row, 0)->text()); + } } void SmartRadarService::on_file_selected(int index) @@ -112,65 +252,81 @@ void SmartRadarService::on_file_selected(int index) param_name_line_edit->clear(); param_value_line_edit->clear(); param_sensor_id->clear(); + param_section_name->clear(); // Clear command tab command_table_widget->setRowCount(0); command_name_line_edit->clear(); - command_argument_line_edit->clear(); - - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Index Zero!"); - } else if (index == 1) { - param_json_file_path = - current_directory + - "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" - "UserInterfaceUmrr9f_t169_mseV1.0.0/instructions/params/auto_interface_0dim.param"; - command_json_file_path = - current_directory + - "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" - "UserInterfaceUmrr9f_t169_mseV1.0.0/instructions/command/auto_interface.command"; - } else if (index == 2) { - param_json_file_path = - current_directory + - "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" - "UserInterfaceUmrr9f_t169_automotiveV2.4.1/instructions/params/auto_interface_0dim.param"; - command_json_file_path = - current_directory + - "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" - "UserInterfaceUmrr9f_t169_automotiveV2.4.1/instructions/command/auto_interface.command"; - } else if (index == 3) { - param_json_file_path = - current_directory + - "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" - "UserInterfaceUmrr9d_t152_automotiveV1.4.1/instructions/params/auto_interface_0dim.param"; - command_json_file_path = - current_directory + - "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" - "UserInterfaceUmrr9d_t152_automotiveV1.4.1/instructions/command/auto_interface.command"; - } else if (index == 4) { - param_json_file_path = - current_directory + - "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" - "UserInterfaceUmrra4_automotiveV1.2.1/instructions/params/auto_interface_0dim.param"; - command_json_file_path = - current_directory + - "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" - "UserInterfaceUmrra4_automotiveV1.2.1/instructions/command/auto_interface.command"; - } else if (index == 5) { - param_json_file_path = - current_directory + - "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" - "UserInterfaceUmrra4_mseV1.0.0/instructions/params/auto_interface_0dim.param"; - command_json_file_path = - current_directory + - "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" - "UserInterfaceUmrra4_mseV1.0.0/instructions/command/auto_interface.command"; - } + command_value_line_edit->clear(); + command_sensor_id->clear(); + command_section_name->clear(); + + // Clear status tab + status_table_widget->setRowCount(0); + status_name_line_edit->clear(); + status_sensor_id->clear(); + status_section_name->clear(); - // Update tables with data from selected files - if (index != 0) { - read_param_json_data(); - read_command_json_data(); + return; + } + + QString base_path = current_directory + + "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/"; + + switch (index) { + case 1: + param_json_file_path = base_path + + "UserInterfaceUmrr9f_t169_mseV1.0.0/instructions/params/auto_interface_0dim.param"; + command_json_file_path = base_path + + "UserInterfaceUmrr9f_t169_mseV1.0.0/instructions/command/auto_interface.command"; + status_json_file_path = base_path + + "UserInterfaceUmrr9f_t169_mseV1.0.0/instructions/status/auto_interface.status"; + break; + case 2: + param_json_file_path = base_path + + "UserInterfaceUmrr9f_t169_automotiveV2.4.1/instructions/params/auto_interface_0dim.param"; + command_json_file_path = base_path + + "UserInterfaceUmrr9f_t169_automotiveV2.4.1/instructions/command/auto_interface.command"; + status_json_file_path = base_path + + "UserInterfaceUmrr9f_t169_automotiveV2.4.1/instructions/status/auto_interface.status"; + break; + case 3: + param_json_file_path = base_path + + "UserInterfaceUmrr9d_t152_automotiveV1.4.1/instructions/params/auto_interface_0dim.param"; + command_json_file_path = base_path + + "UserInterfaceUmrr9d_t152_automotiveV1.4.1/instructions/command/auto_interface.command"; + status_json_file_path = base_path + + "UserInterfaceUmrr9d_t152_automotiveV1.4.1/instructions/status/auto_interface.status"; + break; + case 4: + param_json_file_path = base_path + + "UserInterfaceUmrra4_automotiveV1.2.1/instructions/params/auto_interface_0dim.param"; + command_json_file_path = base_path + + "UserInterfaceUmrra4_automotiveV1.2.1/instructions/command/auto_interface.command"; + status_json_file_path = base_path + + "UserInterfaceUmrra4_automotiveV1.2.1/instructions/status/auto_interface.status"; + break; + case 5: + param_json_file_path = base_path + + "UserInterfaceUmrra4_mseV1.0.0/instructions/params/auto_interface_0dim.param"; + command_json_file_path = base_path + + "UserInterfaceUmrra4_mseV1.0.0/instructions/command/auto_interface.command"; + status_json_file_path = base_path + + "UserInterfaceUmrra4_mseV1.0.0/instructions/status/auto_interface.status"; + break; + case 6: + param_json_file_path = base_path + + "user_interface_umrra1_t166_b_automotive_v2_0_0/instructions/params/auto_interface_rrm.param"; + command_json_file_path = base_path + + "user_interface_umrra1_t166_b_automotive_v2_0_0/instructions/command/auto_interface_rrm.command"; + status_json_file_path = base_path + + "user_interface_umrra1_t166_b_automotive_v2_0_0/instructions/status/auto_interface_rrm.status"; + break; } + + read_param_json_data(); + read_command_json_data(); + read_status_json_data(); } void SmartRadarService::read_command_json_data() @@ -185,11 +341,14 @@ void SmartRadarService::read_command_json_data() QJsonDocument doc(QJsonDocument::fromJson(json_data)); QJsonObject json_object = doc.object(); + // Extract gloabl section name + QString global_section_name = json_object["name"].toString(); + // Extract the "commands" array QJsonArray commands_array = json_object["commands"].toArray(); // Set up table headers - QStringList command_header_labels = {"Name", "Argument", "Comment"}; + QStringList command_header_labels = {"Section", "Name", "Argument", "Comment"}; command_table_widget->setColumnCount(3); command_table_widget->setHorizontalHeaderLabels(command_header_labels); @@ -201,15 +360,16 @@ void SmartRadarService::read_command_json_data() QString argument = command_object["argument"].toString(); QString comment = command_object["comment"].toString(); + QTableWidgetItem * section_item = new QTableWidgetItem(global_section_name); QTableWidgetItem * name_item = new QTableWidgetItem(name); QTableWidgetItem * argument_item = new QTableWidgetItem(argument); QTableWidgetItem * comment_item = new QTableWidgetItem(comment); - command_table_widget->setItem(i, 0, name_item); - command_table_widget->setItem(i, 1, argument_item); - command_table_widget->setItem(i, 2, comment_item); + command_table_widget->setItem(i, 0, section_item); + command_table_widget->setItem(i, 1, name_item); + command_table_widget->setItem(i, 2, argument_item); + command_table_widget->setItem(i, 3, comment_item); } - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Commands parsed successfully!"); } void SmartRadarService::read_param_json_data() @@ -224,12 +384,15 @@ void SmartRadarService::read_param_json_data() QJsonDocument doc(QJsonDocument::fromJson(json_data)); QJsonObject json_object = doc.object(); + // Extract gloabl section name + QString global_section_name = json_object["name"].toString(); + // Extract the "parameters" array QJsonArray params_array = json_object["parameters"].toArray(); // Set up table headers - QStringList param_header_labels = {"Name", "Comment"}; - param_table_widget->setColumnCount(2); + QStringList param_header_labels = {"Section", "Name", "Comment", "Type"}; + param_table_widget->setColumnCount(3); param_table_widget->setHorizontalHeaderLabels(param_header_labels); // Populate the table with parameter data @@ -238,68 +401,260 @@ void SmartRadarService::read_param_json_data() QJsonObject param_object = params_array[i].toObject(); QString name = param_object["name"].toString(); QString comment = param_object["comment"].toString(); + QString type = param_object["type"].toString(); + QTableWidgetItem * section_item = new QTableWidgetItem(global_section_name); QTableWidgetItem * name_item = new QTableWidgetItem(name); QTableWidgetItem * comment_item = new QTableWidgetItem(comment); + QTableWidgetItem * type_item = new QTableWidgetItem(type); - param_table_widget->setItem(i, 0, name_item); - param_table_widget->setItem(i, 1, comment_item); + param_table_widget->setItem(i, 0, section_item); + param_table_widget->setItem(i, 1, name_item); + param_table_widget->setItem(i, 2, comment_item); + param_table_widget->setItem(i, 3, type_item); } - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Parameters parsed successfully!"); } +void SmartRadarService::read_status_json_data() +{ + QFile status_json_file(status_json_file_path); + if (!status_json_file.open(QIODevice::ReadOnly)) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Parse Failed!"); + return; + } + + QByteArray json_data = status_json_file.readAll(); + QJsonDocument doc(QJsonDocument::fromJson(json_data)); + QJsonObject json_object = doc.object(); + + // Extract gloabl section name + QString global_section_name = json_object["name"].toString(); + + // Extract the "status" array + QJsonArray status_array = json_object["status"].toArray(); + + // Set up table headers + QStringList status_header_labels = {"Section", "Name", "Comment", "Type"}; + status_table_widget->setColumnCount(3); + status_table_widget->setHorizontalHeaderLabels(status_header_labels); + + // Populate the table with status data + status_table_widget->setRowCount(status_array.size()); + for (int i = 0; i < status_array.size(); ++i) { + QJsonObject status_object = status_array[i].toObject(); + QString name = status_object["name"].toString(); + QString comment = status_object["comment"].toString(); + QString type = status_object["type"].toString(); + + QTableWidgetItem * section_item = new QTableWidgetItem(global_section_name); + QTableWidgetItem * name_item = new QTableWidgetItem(name); + QTableWidgetItem * comment_item = new QTableWidgetItem(comment); + QTableWidgetItem * type_item = new QTableWidgetItem(type); + + status_table_widget->setItem(i, 0, section_item); + status_table_widget->setItem(i, 1, name_item); + status_table_widget->setItem(i, 2, comment_item); + status_table_widget->setItem(i, 3, type_item); + } +} + void SmartRadarService::on_send_param() { - if (!mode_client) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to create mode_client"); + // Validate common inputs + if (param_name_line_edit->text().isEmpty() || param_sensor_id->text().isEmpty()) { + response_text_edit->append("Error: Parameter name and sensor ID fields must be filled!"); return; } - auto request = std::make_shared(); - request->param = param_name_line_edit->text().toStdString(); - request->value = std::stoi(param_value_line_edit->text().toUtf8().constData()); - request->sensor_id = std::stoi(param_sensor_id->text().toUtf8().constData()); - - auto result = mode_client->async_send_request(request); - - // Wait for the result. - if ( - rclcpp::spin_until_future_complete(client_node, result) == rclcpp::FutureReturnCode::SUCCESS) { - QString response_msg = QString::fromStdString(result.get()->res); - response_text_edit->append("Request sent. Response from sensor: " + response_msg); - RCLCPP_INFO( - rclcpp::get_logger("rclcpp"), "Request sent. Response from sensor: %s", - result.get()->res.c_str()); - } else { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service!"); - response_text_edit->append("Failed to call service!"); + if (param_action_combo->currentIndex() == 0) { + // Writing param + if (param_value_line_edit->text().isEmpty()) { + response_text_edit->append("Error: Value must be provided for write!"); + return; + } + if (!mode_client->wait_for_service(SERVICE_AVAILABILITY_TIMEOUT)) { + response_text_edit->append("SetMode service not available!"); + return; + } + auto request = std::make_shared(); + request->section_name = param_section_name->text().toStdString(); + request->params.push_back(param_name_line_edit->text().toStdString()); + request->sensor_id = std::stoi(param_sensor_id->text().toStdString()); + request->value_types.push_back(param_value_type->currentIndex()); + request->values.push_back(param_value_line_edit->text().toStdString()); + auto result = mode_client->async_send_request(request); + auto status = rclcpp::spin_until_future_complete(client_node, result); + switch (status) { + case rclcpp::FutureReturnCode::SUCCESS: + { + auto response = result.get(); + QString response_msg = QString::fromStdString(response->res); + response_text_edit->append(response_msg); + } + break; + + case rclcpp::FutureReturnCode::TIMEOUT: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service timed out!"); + response_text_edit->append("Service timed out!"); + break; + + case rclcpp::FutureReturnCode::INTERRUPTED: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call was interrupted!"); + response_text_edit->append("Service call was interrupted!"); + break; + + default: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call failed due to unknown reason!"); + response_text_edit->append("Service call failed due to unknown reason!"); + break; + } + } + else { + // Reading param + if (!get_param_client->wait_for_service(SERVICE_AVAILABILITY_TIMEOUT)) { + response_text_edit->append("GetMode service not available!"); + return; + } + auto request = std::make_shared(); + request->section_name = param_section_name->text().toStdString(); + request->params.push_back(param_name_line_edit->text().toStdString()); + request->sensor_id = std::stoi(param_sensor_id->text().toStdString()); + request->param_types.push_back(param_value_type->currentIndex()); + auto result = get_param_client->async_send_request(request); + auto status = rclcpp::spin_until_future_complete(client_node, result); + switch (status) { + case rclcpp::FutureReturnCode::SUCCESS: + { + auto response = result.get(); + QString response_msg = QString::fromStdString(response->res); + response_text_edit->append(response_msg); + } + break; + + case rclcpp::FutureReturnCode::TIMEOUT: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service timed out!"); + response_text_edit->append("Service timed out!"); + break; + + case rclcpp::FutureReturnCode::INTERRUPTED: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call was interrupted!"); + response_text_edit->append("Service call was interrupted!"); + break; + + default: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call failed due to unknown reason!"); + response_text_edit->append("Service call failed due to unknown reason!"); + break; + } } } void SmartRadarService::on_send_command() { + // Validate inputs + if (command_name_line_edit->text().isEmpty() || command_value_line_edit->text().isEmpty() || command_sensor_id->text().isEmpty()) { + response_text_edit->append("Error: All command fields must be filled. Add dummy value of command if not specified!"); + return; + } + if (!command_client) { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to create command client"); return; } + if(!command_client->wait_for_service(SERVICE_AVAILABILITY_TIMEOUT)) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service not available!"); + response_text_edit->append("Service not available! Is the radar node running?"); + return; + } + auto request = std::make_shared(); + request->section_name = command_section_name->text().toStdString(); request->command = command_name_line_edit->text().toStdString(); request->value = std::stoi(command_value_line_edit->text().toUtf8().constData()); request->sensor_id = std::stoi(command_sensor_id->text().toUtf8().constData()); auto result = command_client->async_send_request(request); + auto status = rclcpp::spin_until_future_complete(client_node, result); + + switch (status) { + case rclcpp::FutureReturnCode::SUCCESS: + { + auto response = result.get(); + QString response_msg = QString::fromStdString(response->res); + response_text_edit->append(response_msg); + } + break; + + case rclcpp::FutureReturnCode::TIMEOUT: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service timed out!"); + response_text_edit->append("Service timed out!"); + break; + + case rclcpp::FutureReturnCode::INTERRUPTED: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call was interrupted!"); + response_text_edit->append("Service call was interrupted!"); + break; + + default: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call failed due to unknown reason!"); + response_text_edit->append("Service call failed due to unknown reason!"); + break; + } +} + +void SmartRadarService::on_get_status() +{ + // Validate inputs + if (status_name_line_edit->text().isEmpty() || status_sensor_id->text().isEmpty()) { + response_text_edit->append("Error: Status name and sensor ID must be filled!"); + return; + } + + if (!status_client) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to create status_client"); + return; + } + + if(!status_client->wait_for_service(SERVICE_AVAILABILITY_TIMEOUT)) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service not available!"); + response_text_edit->append("Service not available! Is the radar node running?"); + return; + } - // Wait for the result. - if ( - rclcpp::spin_until_future_complete(client_node, result) == rclcpp::FutureReturnCode::SUCCESS) { - QString response_msg = QString::fromStdString(result.get()->res); - response_text_edit->append("Request sent. Response from sensor: " + response_msg); - RCLCPP_INFO( - rclcpp::get_logger("rclcpp"), "Request sent. Response: %s", result.get()->res.c_str()); - } else { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service!"); - response_text_edit->append("Failed to call service!"); + auto request = std::make_shared(); + + request->section_name = status_section_name->text().toStdString(); + request->statuses.push_back(status_name_line_edit->text().toStdString()); + request->sensor_id = std::stoi(status_sensor_id->text().toUtf8().constData()); + request->status_types.push_back(status_value_type->currentIndex()); + + auto result = status_client->async_send_request(request); + auto status = rclcpp::spin_until_future_complete(client_node, result); + + switch (status) { + case rclcpp::FutureReturnCode::SUCCESS: + { + auto response = result.get(); + QString response_msg = QString::fromStdString(response->res); + response_text_edit->append(response_msg); + } + break; + + case rclcpp::FutureReturnCode::TIMEOUT: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service timed out!"); + response_text_edit->append("Service timed out!"); + break; + + case rclcpp::FutureReturnCode::INTERRUPTED: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call was interrupted!"); + response_text_edit->append("Service call was interrupted!"); + break; + + default: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call failed due to unknown reason!"); + response_text_edit->append("Service call failed due to unknown reason!"); + break; } } diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index 288d829..c650596 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -34,14 +34,16 @@ find_package(Threads REQUIRED) find_package(visualization_msgs REQUIRED) fetchcontent_declare(json - GIT_REPOSITORY https://github.com/nlohmann/json.git - GIT_TAG v3.10.5 + URL https://github.com/nlohmann/json/releases/download/v3.12.0/include.zip + URL_HASH SHA256=b8cb0ef2dd7f57f18933997c9934bb1fa962594f701cd5a8d3c2c80541559372 ) fetchcontent_getproperties(json) if(NOT json_POPULATED) fetchcontent_populate(json) - add_subdirectory(${json_SOURCE_DIR} ${json_BINARY_DIR} EXCLUDE_FROM_ALL) + add_library(nlohmann_json INTERFACE) + target_include_directories(nlohmann_json INTERFACE ${json_SOURCE_DIR}/include) + add_library(nlohmann_json::nlohmann_json ALIAS nlohmann_json) endif() ament_auto_find_build_dependencies() @@ -63,6 +65,7 @@ install(FILES "${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}/libumrr9f_t169_automotivev2.4.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev3.0.0_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}/libumrr9d_t152_automotivev1.4.1_user_interface.so" @@ -72,7 +75,11 @@ install(FILES "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_automotivev1.4.0_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_msev1.0.0_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_msev1.1.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_msev1.3.0_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_msev1.0.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_msev2.1.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra1_t166_b_automotivev1.0.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra1_t166_b_automotivev2.0.0_user_interface.so" DESTINATION lib) set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") @@ -88,7 +95,7 @@ configure_file( ament_auto_add_library(smartmicro_radar_node SHARED "src/smartmicro_radar_node.cpp" - "src/UpdateService.cpp" + "src/update_service.cpp" ) target_include_directories(smartmicro_radar_node PUBLIC ${CMAKE_INSTALL_PREFIX}/include/ @@ -100,6 +107,7 @@ 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/umrr9f_t169_automotive_v2_4_1 +smartmicro/include/umrr9f_t169_automotive_v3_0_0 smartmicro/include/umrr9d_t152_automotive_v1_0_3 smartmicro/include/umrr9d_t152_automotive_v1_2_2 smartmicro/include/umrr9d_t152_automotive_v1_4_1 @@ -109,7 +117,11 @@ smartmicro/include/umrra4_automotive_v1_2_1 smartmicro/include/umrra4_automotive_v1_4_0 smartmicro/include/umrr9f_t169_mse_v1_0_0 smartmicro/include/umrr9f_t169_mse_v1_1_0 -smartmicro/include/umrra4_mse_v1_0_0) +smartmicro/include/umrr9f_t169_mse_v1_3_0 +smartmicro/include/umrra4_mse_v1_0_0 +smartmicro/include/umrra4_mse_v2_1_0 +smartmicro/include/umrra1_t166_b_automotive_v1_0_0 +smartmicro/include/umrra1_t166_b_automotive_v2_0_0) # link smart_access_lib-linux-x86_64-gcc_9 to the node target_link_libraries(smartmicro_radar_node @@ -124,6 +136,7 @@ target_link_libraries(smartmicro_radar_node umrr9f_t169_automotivev2.1.1_user_interface umrr9f_t169_automotivev2.2.1_user_interface umrr9f_t169_automotivev2.4.1_user_interface + umrr9f_t169_automotivev3.0.0_user_interface umrr9d_t152_automotivev1.0.3_user_interface umrr9d_t152_automotivev1.2.2_user_interface umrr9d_t152_automotivev1.4.1_user_interface @@ -133,7 +146,11 @@ target_link_libraries(smartmicro_radar_node umrra4_automotivev1.4.0_user_interface umrr9f_t169_msev1.0.0_user_interface umrr9f_t169_msev1.1.0_user_interface - umrra4_msev1.0.0_user_interface) + umrr9f_t169_msev1.3.0_user_interface + umrra4_msev1.0.0_user_interface + umrra4_msev2.1.0_user_interface + umrra1_t166_b_automotivev1.0.0_user_interface + umrra1_t166_b_automotivev2.0.0_user_interface) rclcpp_components_register_node(smartmicro_radar_node PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" @@ -158,7 +175,6 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() ament_copyright("src/smartmicro_radar_node.cpp") - ament_cpplint("src/smartmicro_radar_node.cpp") ament_cppcheck("src/smartmicro_radar_node.cpp") endif() diff --git a/umrr_ros2_driver/config/rviz/smart_plugin.rviz b/umrr_ros2_driver/config/rviz/smart_plugin.rviz new file mode 100644 index 0000000..d5e90ac --- /dev/null +++ b/umrr_ros2_driver/config/rviz/smart_plugin.rviz @@ -0,0 +1,212 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 295 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: smart_rviz_plugin/Smart Recorder + Name: Smart Recorder + - Class: smart_rviz_plugin/Smart Status + Name: Smart Status + - Class: smart_rviz_plugin/Smart Command Configurator + Name: Smart Command Configurator + - Class: smart_rviz_plugin/Smart Status + Name: Smart Status + - Class: smart_rviz_plugin/Smart Firmware Download + Name: Smart Firmware Download +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: power + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 88.64950561523438 + Min Color: 0; 0; 0 + Min Intensity: 56.63246154785156 + Name: Port_Tgts_S0 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /smart_radar/port_targets_0 + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 138; 226; 52 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Can_Tgts_S1 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /smart_radar/can_targets_1 + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: umrr + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 38.8543701171875 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 10.84825325012207 + Y: 0.366975337266922 + Z: 0.01084582507610321 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.1353957653045654 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1757 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 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 + Selection: + collapsed: false + Smart Command Configurator: + collapsed: false + Smart Firmware Download: + collapsed: false + Smart Recorder: + collapsed: false + Smart Status: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3151 + X: 374 + Y: 214 diff --git a/umrr_ros2_driver/include/umrr_ros2_driver/UpdateService.hpp b/umrr_ros2_driver/include/umrr_ros2_driver/UpdateService.hpp deleted file mode 100644 index 2b51044..0000000 --- a/umrr_ros2_driver/include/umrr_ros2_driver/UpdateService.hpp +++ /dev/null @@ -1,15 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -using namespace com::common; -using namespace com::types; -using namespace com::master; - -void MyUpdateServiceInfoHandler(SWUpdateInfo &updateInfoLocal); -void StartSoftwareUpdate(ClientId client_id, std::string update_image); 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 eee8e34..99eac67 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 @@ -21,6 +21,7 @@ #include #include #include +#include "umrr_ros2_driver/update_service.hpp" #include #include @@ -35,12 +36,17 @@ #include #include #include +#include #include #include +#include #include #include #include #include +#include +#include +#include #include #include @@ -54,6 +60,8 @@ #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/get_status.hpp" +#include "umrr_ros2_msgs/srv/get_mode.hpp" namespace smartmicro { @@ -148,6 +156,37 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node targetlist_port_umrra4_mse_v1_0_0, const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new object list port for + /// umrra4_v2_1_0 T171 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_port_umrra4_mse_v2_1_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + void objectlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrra4_mse_v2_1_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrra4_v2_1_0 T171 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrra4_mse_v2_1_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4_mse_v2_1_0, + const com::types::ClientId client_id); + + + /// /// @brief A callback that is called when a new object list port for /// umrr9f_v1_0_0 T169 MSE arrives. @@ -203,6 +242,34 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node const std::shared_ptr & targetlist_port_umrr9f_mse_v1_1_0, const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new object list port for + /// umrr9f_v1_3_0 T169 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_port_umrr9f_mse_v1_3_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + void objectlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v1_3_0 T169 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9f_mse_v1_3_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id); /// /// @brief A callback that is called when a new target list port for @@ -308,6 +375,21 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node com::master::umrr9f_t169_automotive_v2_4_1::comtargetlist::ComTargetList> & targetlist_port_umrr9f_v2_4_1, const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v3_0_0 T169 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9f_v3_0_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9f_v3_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v3_0_0::comtargetlist::ComTargetList> & + targetlist_port_umrr9f_v3_0_0, + const com::types::ClientId client_id); /// /// @brief A callback that is called when a new target list port for @@ -406,6 +488,32 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node targetlist_port_umrra4_v1_4_0, const com::types::ClientId client_id); + /// + /// @brief A callback that is called when a new target list port for + /// umrra1_v1_0_0 T166 arrives. + /// @param[in] sensor_idx The sensor id for the respected published topic. + /// @param[in] targetlist_port_umrra1_v1_0_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra1_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra1_v1_0_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrra1_v2_0_0 T166 arrives. + /// @param[in] sensor_idx The sensor id for the respected published topic. + /// @param[in] targetlist_port_umrra1_v2_0_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra1_v2_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra1_v2_0_0, + const com::types::ClientId client_id); + /// /// @brief A callback that is called when a new CAN object list for /// umrra4_v1_0_0 T171 MSE arrives. @@ -435,6 +543,37 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node targetlist_can_umrra4_mse_v1_0_0, const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN object list for + /// umrra4_v2_1_0 T171 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_can_umrra4_mse_v2_1_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + /// + void CAN_objectlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_can_umrra4_mse_v2_1_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrra4_mse_v2_1_0 T171 MSE arrives. + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrra4_mse_v2_1_0 The target list port + /// @param[in] client_id The client_id of the sensor. + /// + /// + void CAN_targetlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_can_umrra4_mse_v2_1_0, + const com::types::ClientId client_id); + + /// /// @brief A callback that is called when a new CAN object list for /// umrr9f_v1_0_0 T169 MSE arrives. @@ -496,6 +635,37 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node com::master::umrr9f_t169_mse_v1_1_0::comtargetbaselist::ComTargetBaseList> & targetlist_can_umrr9f_mse_v1_1_0, const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN object list for + /// umrr9f_v1_3_0 T169 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_can_umrr9f_mse_v1_3_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + /// + void CAN_objectlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_mse_v1_3_0::comobjectbaselist::ComObjectBaseList> & + objectlist_can_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9f_mse_v1_3_0 T169 MSE arrives. + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9f_mse_v1_3_0 The target list port + /// @param[in] client_id The client_id of the sensor. + /// + /// + void CAN_targetlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_mse_v1_3_0::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id); /// /// @brief A callback that is called when a new CAN target list for @@ -570,6 +740,23 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node com::master::umrr9f_t169_automotive_v2_4_1::comtargetbaselist::ComTargetBaseList> & targetlist_can_umrr9f_v2_4_1, const com::types::ClientId client_id); + + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9f_v3_0_0 T169 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9f_v3_0_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrr9f_v3_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v3_0_0::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v3_0_0, + const com::types::ClientId client_id); + /// /// @brief A callback that is called when a new CAN target list for @@ -708,7 +895,8 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node void mode_response( const com::types::ClientId client_id, const std::shared_ptr & response, - const std::string instruction_name); + const std::vector & instruction_names, + const std::string & section_name); /// /// @brief Callback for getting the command response. @@ -719,7 +907,8 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// void command_response( const com::types::ClientId client_id, - const std::shared_ptr & response, const std::string command_name); + const std::shared_ptr & response, const std::string command_name, + const std::string & section_name); /// /// @brief Callback for changing IP address. @@ -737,7 +926,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// @param[in] request The request. /// @param[out] response The response. /// - void radar_mode( + void set_radar_mode( const std::shared_ptr request, std::shared_ptr response); @@ -796,10 +985,59 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// void setup_publishers(); + /// + /// @brief Service to get sensor status. + /// + /// @param[in] request The request. + /// @param[out] response The response. + /// + void get_radar_status( + const std::shared_ptr request, + std::shared_ptr response); + + /// + /// @brief Service to get sensor modes. + /// + /// @param[in] request The request. + /// @param[out] response The response. + /// + void get_radar_mode( + const std::shared_ptr request, + std::shared_ptr response); + + /// + /// @brief Callback for getting the status response. + /// + /// @param[in] client_id The client identifier. + /// @param[in] response The response batch. + /// @param[in] statuses The status name of the sensor. + /// @param[in] section_name The section name of the interface. + /// + void status_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, + const std::vector & statuses, + const std::string & section_name); + + /// + /// @brief Callback for getting the reading param response. + /// + /// @param[in] client_id The client identifier. + /// @param[in] response The response batch. + /// @param[in] params The param name of the sensor. + /// + void param_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, + const std::vector & params, + const std::string & section_name); + rclcpp::Service::SharedPtr mode_srv_; rclcpp::Service::SharedPtr ip_addr_srv_; rclcpp::Service::SharedPtr command_srv_; rclcpp::Service::SharedPtr download_srv_; + rclcpp::Service::SharedPtr status_srv_; + rclcpp::Service::SharedPtr read_mode_srv_; std::array m_sensors{}; std::array m_adapters{}; @@ -831,6 +1069,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node rclcpp::TimerBase::SharedPtr timer; com::types::ClientId client_id; std::uint64_t response_type{}; + std::shared_ptr update_service; }; bool check_signal = false; @@ -851,6 +1090,8 @@ std::shared_ptr data_umrr9f_v2_4_1{}; +std::shared_ptr + data_umrr9f_v3_0_0{}; std::shared_ptr data_umrr9d_v1_0_3{}; std::shared_ptr @@ -863,7 +1104,14 @@ std::shared_ptr data_umrr9f_mse_v1_0_0{}; std::shared_ptr data_umrr9f_mse_v1_1_0{}; +std::shared_ptr + data_umrr9f_mse_v1_3_0{}; std::shared_ptr data_umrra4_mse_v1_0_0{}; +std::shared_ptr data_umrra4_mse_v2_1_0{}; +std::shared_ptr + data_umrra1_v1_0_0{}; +std::shared_ptr + data_umrra1_v2_0_0{}; } // namespace radar } // namespace drivers diff --git a/umrr_ros2_driver/include/umrr_ros2_driver/update_service.hpp b/umrr_ros2_driver/include/umrr_ros2_driver/update_service.hpp new file mode 100644 index 0000000..abd9ecc --- /dev/null +++ b/umrr_ros2_driver/include/umrr_ros2_driver/update_service.hpp @@ -0,0 +1,33 @@ +#ifndef UPDATE_SERVICE_HPP +#define UPDATE_SERVICE_HPP + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +class UpdateService { + public: + UpdateService(); + ~UpdateService() = default; + + void StartSoftwareUpdate(com::types::ClientId client_id, std::string &update_image); + + private: + void UpdateCallback(com::types::SWUpdateInfo &info); + void HandleResult(); + + com::types::SWUpdateInfo updateInfo_; + std::mutex mutex_; + std::condition_variable cv_; +}; + +#endif // UPDATE_SERVICE_HPP diff --git a/umrr_ros2_driver/package.xml b/umrr_ros2_driver/package.xml index dd1b695..a32d764 100644 --- a/umrr_ros2_driver/package.xml +++ b/umrr_ros2_driver/package.xml @@ -2,7 +2,7 @@ umrr_ros2_driver - 7.1.0 + 8.0.0 A node to publish data from a smartmicro radar. s.m.s, smart microwave sensors GmbH. Apache 2.0 License diff --git a/umrr_ros2_driver/param/example/radar.adapter.example.yaml b/umrr_ros2_driver/param/example/radar.adapter.example.yaml index 0b62adb..c517a94 100644 --- a/umrr_ros2_driver/param/example/radar.adapter.example.yaml +++ b/umrr_ros2_driver/param/example/radar.adapter.example.yaml @@ -6,7 +6,7 @@ # sudo ip link set up slcan0 # set buffer length using /sbin/ip link set slcan0 txqueuelen 4096 # For peak CAN adapters, only required to set baudrate - # sudo ip link set your_interface up type can bitrate 500000 + # sudo ip link set can0 up type can bitrate 500000 # 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" diff --git a/umrr_ros2_driver/param/radar.params.template.yaml b/umrr_ros2_driver/param/radar.params.template.yaml index 3298492..97bf6df 100644 --- a/umrr_ros2_driver/param/radar.params.template.yaml +++ b/umrr_ros2_driver/param/radar.params.template.yaml @@ -2,9 +2,9 @@ ros__parameters: adapters: adapter_0: - hw_dev_id: 3 - hw_iface_name: eth0 - hw_type: eth + hw_type: "eth" + hw_dev_id: 4 + hw_iface_name: "eth0" port: 55555 master_data_serial_type: port_based master_inst_serial_type: port_based @@ -13,32 +13,32 @@ # e.g., sensor_0, sensor_1, etc. The list must start with sensor_0. # # Sensor `model` names if using link type: - # can: 'umrra4_can_mse_v1_0_0', 'umrr9f_can_mse_v1_1_0', 'umrr9f_can_mse_v1_0_0', 'umrr96_can_v1_2_2', 'umrr11_can_v1_1_2', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1', - # 'umrr9f_can_v2_4_1'. 'umrr9d_can_v1_0_3', 'umrr9d_can_v1_2_2', 'umrr9d_can_v1_4_1', 'umrr9d_can_v1_5_0', 'umrra4_can_v1_0_1', 'umrra4_can_v1_2_1', 'umrra4_can_v1_4_0' + # can: 'umrra4_can_mse_v1_0_0', 'umrra4_can_mse_v2_1_0', 'umrr9f_can_mse_v1_1_0', 'umrr9f_can_mse_v1_0_0', 'umrr96_can_v1_2_2', 'umrr11_can_v1_1_2', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1', + # 'umrr9f_can_v2_4_1', 'umrr9f_can_v3_0_0', 'umrr9d_can_v1_0_3', 'umrr9d_can_v1_2_2', 'umrr9d_can_v1_4_1', 'umrr9d_can_v1_5_0', 'umrra4_can_v1_0_1', 'umrra4_can_v1_2_1', 'umrra4_can_v1_4_0' # - # port: 'umrra4_mse_v1_0_0', 'umrr9f_mse_v1_1_0', 'umrr9f_mse_v1_0_0', 'umrr96_v1_2_2', 'umrr11_v1_1_2', 'umrr9f_v2_1_1', 'umrr9f_v2_2_1', 'umrr9f_v2_4_1'. 'umrr9d_v1_0_3', - # 'umrr9d_v1_2_2', 'umrr9d_v1_4_1', 'umrr9d_v1_5_0', 'umrra4_v1_0_1', 'umrra4_v1_2_1', 'umrra4_v1_4_0' + # port: 'umrra1_v2_0_0', 'umrra1_v1_0_0', 'umrra4_mse_v1_0_0', 'umrra4_mse_v2_1_0', 'umrr9f_mse_v1_3_0', 'umrr9f_mse_v1_1_0', 'umrr9f_mse_v1_0_0', 'umrr96_v1_2_2', 'umrr11_v1_1_2', 'umrr9f_v2_1_1', 'umrr9f_v2_2_1', 'umrr9f_v2_4_1', + # 'umrr9f_v3_0_0', 'umrr9d_v1_0_3', 'umrr9d_v1_2_2', 'umrr9d_v1_4_1', 'umrr9d_v1_5_0', 'umrra4_v1_0_1', 'umrra4_v1_2_1', 'umrra4_v1_4_0' sensor_0: - # The link of the sensor either 'can' or 'eth' + # The link of the sensor either 'can' or 'eth' link_type: "eth" # The publisher type of the sensor pub_type: "target" # The model of the connected sensor. model: "umrr11_v1_1_2" # Adapter id to which sensor is connected - dev_id: 3 + dev_id: 4 # 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 + # The ip address of the sensor or of the source acting as a sensor. + ip: "192.168.11.11" inst_type: "port_based" data_type: "port_based" + # The port to be used. + port: 55555 # The interface name of the sensor uifname: "umrr11_t132_automotive" # The major version of the interface @@ -55,7 +55,7 @@ # The model of the connected sensor. model: "umrr96_v1_2_2" # Adapter id to which sensor is connected - dev_id: 3 + dev_id: 4 # 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. @@ -84,7 +84,7 @@ # The model of the connected sensor. model: "umrr9f_v2_1_1" # Adapter id to which sensor is connected - dev_id: 3 + dev_id: 4 # 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. @@ -113,7 +113,7 @@ # The model of the connected sensor. model: "umrr9d_v1_5_0" # Adapter id to which sensor is connected - dev_id: 3 + dev_id: 4 # The client_id of the sensor/source, must be a unique integer. id: 400 # The ip address of the sensor or of the source acting as a sensor. diff --git a/umrr_ros2_driver/smartmicro/include/DataServicesIface.h b/umrr_ros2_driver/smartmicro/include/DataServicesIface.h index 134229f..36e22bc 100644 --- a/umrr_ros2_driver/smartmicro/include/DataServicesIface.h +++ b/umrr_ros2_driver/smartmicro/include/DataServicesIface.h @@ -31,6 +31,9 @@ typedef std::function DataReceiverCallback; typedef std::function ResponseCallback; +typedef std::function + LogReceiverCallback; + typedef std::function UpdateCallback; typedef std::function GetLocalTimeCallback; typedef std::function SetTimeOffsetCallback; @@ -93,6 +96,19 @@ class EXPORT_COM_LIB DataServicesIface : public com::VersionedIface { virtual ErrorCode RegisterDataRecvCallback(IN ClientId clientId, IN PortId portId, IN DataReceiverCallback callback) = 0; + + /* + * Function: RegisterComHubDataRecvCallback + * this function will pass all data ports from comhub device to registered + * callback function + * Arguments: IN DataReceiverCallback callback - callback function which + * should be called upon a reception. Return: ErrorCode Description: Registers + * a receiver callback function , which should be called , when a new buffer + * arrives. + */ + virtual ErrorCode + RegisterComHubDataRecvCallback(IN DataReceiverCallback callback) = 0; + /* * Function: RegisterInstRecvCallback * Arguments: IN DataReceiverCallback callback - callback function will be @@ -176,6 +192,16 @@ class EXPORT_COM_LIB DataServicesIface : public com::VersionedIface { virtual ErrorCode RegisterSetTimeOffsetCallback(IN SetTimeOffsetCallback callback) = 0; + /* + * Function: RegisterLogMessageCallBack + * Arguments: LogReceiverCallback + * Return: ErrorCode + * Description: Register a callback to receive log messages from + * BareMetal sensor. + */ + virtual ErrorCode + RegisterLogMessageCallBack(IN LogReceiverCallback callback) = 0; + /* * Function: AddTimeSyncSlave * Arguments: uint8_t slaveDevId slave device id @@ -262,6 +288,18 @@ class EXPORT_COM_LIB DataServicesIface : public com::VersionedIface { */ virtual bool GetConnectedClients( OUT std::map> &clients) = 0; + + /* + * Function: GetConnectedClients + * Arguments: OUT std::map>& + * OUT std::map& + * clients Return: bool Description: Returns a map of connected client + * descriptions. + */ + virtual bool GetConnectedClients( + OUT std::map> &clients, + OUT std::map &client_ips) = 0; + /* * Function: SetClientUserInterfaceInfo * Arguments: IN ClientId clientId client id diff --git a/umrr_ros2_driver/smartmicro/include/Types.h b/umrr_ros2_driver/smartmicro/include/Types.h index af5bef8..e8f7c70 100644 --- a/umrr_ros2_driver/smartmicro/include/Types.h +++ b/umrr_ros2_driver/smartmicro/include/Types.h @@ -17,7 +17,9 @@ #include #include -#include +#include +#include +#include namespace com { @@ -34,28 +36,29 @@ const uint8_t MAX_NUM_OF_INST = 255; #endif #define CAN_MAX_DATA_BYTES 8 -enum SerializationType { +enum SerializationType { // NOLINT(*enum-size) SERIALIZATION_TYPE_CAN_SPEC = 0, SERIALIZATION_TYPE_PORT_BASED, SERIALIZATION_TYPE_UNKNOWN }; -typedef struct { +struct CanFormat { uint16_t u16_identifier; uint8_t u8_dlc; - uint8_t au8_data[CAN_MAX_DATA_BYTES]; -} CanFormat; + uint8_t au8_data[CAN_MAX_DATA_BYTES]; // NOLINT(*arrays) +}; -enum LinkType { +enum LinkType { // NOLINT(*enum-size) LINK_TYPE_UDP = 0, LINK_TYPE_UDP_DISCOVERY, LINK_TYPE_CAN, LINK_TYPE_CAN_DISCOVERY, LINK_TYPE_RS485, - LIMK_TYPE_UNKNOWN + LIMK_TYPE_UNKNOWN, + LINK_TYPE_UNKNOWN = LIMK_TYPE_UNKNOWN }; -enum ProtocolType { +enum ProtocolType { // NOLINT(*enum-size) PROTOCOL_TYPE_UNKNOWN = 0, PROTOCOL_TYPE_SMS_CAN_BASE_DATA_V1, PROTOCOL_TYPE_ATXMEGA_SDLC, @@ -72,50 +75,66 @@ enum ProtocolType { PROTOCOL_TYPE_INSTRUCTION }; -enum LibraryRole { +enum LibraryRole { // NOLINT(*enum-size) LIBRARY_ROLE_MASTER, LIBRARY_ROLE_SLAVE, LIBRARY_ROLE_UNKNOWN }; -enum TimeSyncRole { +enum TimeSyncRole { // NOLINT(*enum-size) TIME_SYNC_ROLE_MASTER, TIME_SYNC_ROLE_SLAVE, TIME_SYNC_ROLE_UNKNOWN }; -typedef uint16_t LinkId; -typedef uint32_t SequenceNumber; -typedef uint8_t CanNetId; -typedef uint16_t CanId; -typedef uint16_t UdtId; -typedef uint8_t PhyDeviceId; -struct TimeOutDeatils { +using LinkId = uint16_t; +using SequenceNumber = uint32_t; +using CanNetId = uint8_t; +using CanId = uint16_t; +using UdtId = uint16_t; +using PhyDeviceId = uint8_t; + +struct TimeOutDetails { uint16_t timeOutCount; uint64_t latestOffsetValue; }; -typedef std::map TimeOutMap; +using TimeOutDeatils = TimeOutDetails; // TODO(RKO): Remove in next release +using TimeOutMap = std::map; const PortId INSTRUCTION_PORT_ID = 46; class SharedLibDescriptor { public: + // NOLINTNEXTLINE(*explicit*) SharedLibDescriptor(const std::string& libName); + SharedLibDescriptor(const SharedLibDescriptor&) = delete; + SharedLibDescriptor(SharedLibDescriptor&&) = default; + SharedLibDescriptor& operator=(const SharedLibDescriptor&) = delete; + SharedLibDescriptor& operator=(SharedLibDescriptor&&) = default; virtual ~SharedLibDescriptor(); bool Link(); - inline void* GetHandle() { return _handle; } + void* GetHandle() { return _handle; } protected: + // NOLINTBEGIN(*non-private-member-variables-in-classes) std::string _libName; - void* _handle; + void* _handle{nullptr}; + // NOLINTEND(*non-private-member-variables-in-classes) + + private: + static std::mutex mutex_; }; class ReceiverKey { public: ReceiverKey(ClientId clientId, PortId portId); - ReceiverKey(); - ~ReceiverKey(); + ReceiverKey() = default; + ReceiverKey(const ReceiverKey&) = default; + ReceiverKey(ReceiverKey&&) = delete; + ReceiverKey& operator=(const ReceiverKey&) = default; + ReceiverKey& operator=(ReceiverKey&&) = delete; + ~ReceiverKey() = default; ClientId GetClientId() const; void SetClientId(const ClientId& clientId); @@ -124,24 +143,25 @@ class ReceiverKey { void SetPortId(const PortId& portId); private: - ClientId _clientId; - PortId _portId; + ClientId _clientId{0}; + PortId _portId{0}; }; inline bool operator<(const ReceiverKey& lhs, const ReceiverKey& rhs) { - if ((lhs.GetClientId() < rhs.GetClientId()) || - ((lhs.GetClientId() == rhs.GetClientId()) && - (lhs.GetPortId() < rhs.GetPortId()))) { - return true; - } - return false; + return (lhs.GetClientId() < rhs.GetClientId()) || + ((lhs.GetClientId() == rhs.GetClientId()) && + (lhs.GetPortId() < rhs.GetPortId())); } class ResponseKey { public: ResponseKey(ClientId clientId, SequenceNumber seqNum); - ResponseKey(); - ~ResponseKey(); + ResponseKey() = default; + ResponseKey(const ResponseKey&) = default; + ResponseKey(ResponseKey&&) = delete; + ResponseKey& operator=(const ResponseKey&) = default; + ResponseKey& operator=(ResponseKey&&) = delete; + ~ResponseKey() = default; ClientId GetClientId() const; void SetClientId(IN const ClientId& clientId); @@ -150,26 +170,26 @@ class ResponseKey { void SetSequenceNumber(IN const SequenceNumber& seqNum); private: - ClientId _clientId; - SequenceNumber _seqNum; + ClientId _clientId{0}; + SequenceNumber _seqNum{0}; }; inline bool operator<(IN const ResponseKey& lhs, IN const ResponseKey& rhs) { - if ((lhs.GetClientId() < rhs.GetClientId()) || - ((lhs.GetClientId() == rhs.GetClientId()) && - (lhs.GetSequenceNumber() < rhs.GetSequenceNumber()))) { - return true; - } - return false; + return (lhs.GetClientId() < rhs.GetClientId()) || + ((lhs.GetClientId() == rhs.GetClientId()) && + (lhs.GetSequenceNumber() < rhs.GetSequenceNumber())); } class EthNetDescriptor { public: EthNetDescriptor(const std::string& ip, uint32_t port, EthTransportType type); + EthNetDescriptor() = default; + EthNetDescriptor(const EthNetDescriptor&) = default; + EthNetDescriptor(EthNetDescriptor&&) = delete; + EthNetDescriptor& operator=(const EthNetDescriptor&) = default; + EthNetDescriptor& operator=(EthNetDescriptor&&) = delete; + ~EthNetDescriptor() = default; - EthNetDescriptor() {} - ~EthNetDescriptor() {} - // ACCESS void GetIp(OUT std::string& ip) const; void SetIp(IN const std::string& ip); @@ -181,11 +201,11 @@ class EthNetDescriptor { private: std::string _ip; - uint32_t _port; - EthTransportType _type; + uint32_t _port{0}; + EthTransportType _type{ETH_TRANSPORT_TYPE_TCP}; }; -enum ClientPhyType { +enum ClientPhyType { // NOLINT(*enum-size) CLIENT_PHY_CAN = 0, CLIENT_PHY_ETH, CLIENT_PHY_RS485, @@ -197,243 +217,249 @@ class ClientDescriptor { ClientDescriptor(ClientId clientId, ClientPhyType phyType, SerializationType instSerialType, SerializationType dataSerialType, uint8_t phyDevId); + ClientDescriptor(const ClientDescriptor&) = default; + ClientDescriptor(ClientDescriptor&&) = delete; + ClientDescriptor& operator=(const ClientDescriptor&) = default; + ClientDescriptor& operator=(ClientDescriptor&&) = delete; + virtual ~ClientDescriptor() = default; - virtual ~ClientDescriptor() {} - - inline ClientPhyType GetPhyType() const { return _phyType; } - inline void SetPhyType(IN ClientPhyType phyType) { _phyType = phyType; } - - inline ClientId GetId() const { return _clientId; } - - inline void SetId(IN ClientId clientId) { _clientId = clientId; } - - inline void SetPhyDevId(IN PhyDeviceId phyDevId) { _phyDevId = phyDevId; } + ClientPhyType GetPhyType() const; + void SetPhyType(IN ClientPhyType phyType); - inline SerializationType GetInstSerialType() const { return _instSerialType; } + ClientId GetId() const; + void SetId(IN ClientId clientId); - inline void SetInstSerialType(IN SerializationType type) { - _instSerialType = type; - } + PhyDeviceId GetPhyDevId() const; + void SetPhyDevId(IN PhyDeviceId phyDevId); - inline SerializationType GetDataSerialType() const { return _dataSerialType; } + SerializationType GetInstSerialType() const; + void SetInstSerialType(IN SerializationType type); - inline void SetDataSerialType(IN SerializationType type) { - _dataSerialType = type; - } + SerializationType GetDataSerialType() const; + void SetDataSerialType(IN SerializationType type); - inline PhyDeviceId GetPhyDevId() const { return _phyDevId; } + std::string GetUserIfName() const; + void SetUserIfName(IN const std::string& userIfName); - inline void SetUserIfName(IN const std::string userIfName) { - _userIfName = userIfName; - } + uint32_t GetUserIfMajorVer() const; + void SetUserIfMajorVer(IN uint32_t majorVer); - inline std::string GetUserIfName() const { return _userIfName; } + uint32_t GetUserIfMinorVer() const; + void SetUserIfMinorVer(IN uint32_t minorVer); - inline void SetUserIfMajorVer(IN uint32_t majorVer) { - _userIfMajorVer = majorVer; - } - - inline uint32_t GetUserIfMajorVer() const { return _userIfMajorVer; } - - inline void SetUserIfMinorVer(IN uint32_t minorVer) { - _userIfMinorVer = minorVer; - } - - inline uint32_t GetUserIfMinorVer() const { return _userIfMinorVer; } - - inline void SetUserIfPatchVer(IN uint32_t patchVer) { - _userIfPatchVer = patchVer; - } - - inline uint32_t GetUserIfPatchVer() const { return _userIfPatchVer; } + uint32_t GetUserIfPatchVer() const; + void SetUserIfPatchVer(IN uint32_t patchVer); private: - ClientId _clientId; - ClientPhyType _phyType; - SerializationType _instSerialType; - SerializationType _dataSerialType; - PhyDeviceId _phyDevId; + ClientId _clientId{0}; + ClientPhyType _phyType{CLIENT_PHY_UNKNOWN}; + SerializationType _instSerialType{SERIALIZATION_TYPE_UNKNOWN}; + SerializationType _dataSerialType{SERIALIZATION_TYPE_UNKNOWN}; + PhyDeviceId _phyDevId{0}; std::string _userIfName; - uint32_t _userIfMajorVer; - uint32_t _userIfMinorVer; - uint32_t _userIfPatchVer; + uint32_t _userIfMajorVer{0}; + uint32_t _userIfMinorVer{0}; + uint32_t _userIfPatchVer{0}; }; class CanClientDescriptor : public ClientDescriptor { public: CanClientDescriptor(ClientId clientId, CanNetId canNetId, PhyDeviceId phyDevId); - virtual ~CanClientDescriptor() {} - - inline CanNetId GetCanNetId() const { return _canNetId; } + CanClientDescriptor(const CanClientDescriptor&) = default; + CanClientDescriptor(CanClientDescriptor&&) = delete; + CanClientDescriptor& operator=(const CanClientDescriptor&) = default; + CanClientDescriptor& operator=(CanClientDescriptor&&) = delete; + ~CanClientDescriptor() override = default; - inline void SetCanNetId(IN CanNetId canNetId) { _canNetId = canNetId; } + CanNetId GetCanNetId() const; + void SetCanNetId(IN CanNetId canNetId); private: - CanNetId _canNetId; + CanNetId _canNetId{0}; }; class Rs485ClientDescriptor : public ClientDescriptor { public: Rs485ClientDescriptor(ClientId clientId, SerializationType instSerialType, SerializationType dataSerialType, PhyDeviceId phyDevId); - virtual ~Rs485ClientDescriptor() {} + Rs485ClientDescriptor(const Rs485ClientDescriptor&) = default; + Rs485ClientDescriptor(Rs485ClientDescriptor&&) = delete; + Rs485ClientDescriptor& operator=(const Rs485ClientDescriptor&) = default; + Rs485ClientDescriptor& operator=(Rs485ClientDescriptor&&) = delete; + ~Rs485ClientDescriptor() override = default; }; class EthClientDescriptor : public ClientDescriptor { public: EthClientDescriptor(ClientId clientId, SerializationType instSerialType, SerializationType dataSerialType); - virtual ~EthClientDescriptor() {} + EthClientDescriptor(const EthClientDescriptor&) = default; + EthClientDescriptor(EthClientDescriptor&&) = delete; + EthClientDescriptor& operator=(const EthClientDescriptor&) = default; + EthClientDescriptor& operator=(EthClientDescriptor&&) = delete; + ~EthClientDescriptor() override = default; }; class SerializerConfig { public: - SerializerConfig(ClientId clientId, SerializationType serialType) - : _clientId(clientId), _serialType(serialType) {} - - virtual ~SerializerConfig() {} - - inline ClientId GetClientId() const { return _clientId; } + SerializerConfig(ClientId clientId, SerializationType serialType); + SerializerConfig(const SerializerConfig&) = default; + SerializerConfig(SerializerConfig&&) = delete; + SerializerConfig& operator=(const SerializerConfig&) = default; + SerializerConfig& operator=(SerializerConfig&&) = delete; + virtual ~SerializerConfig() = default; - inline SerializationType GetType() const { return _serialType; } + ClientId GetClientId() const; + SerializationType GetType() const; private: - ClientId _clientId; - SerializationType _serialType; + ClientId _clientId{0}; + SerializationType _serialType{SERIALIZATION_TYPE_UNKNOWN}; }; class CanSerializerConfig : public SerializerConfig { public: - CanSerializerConfig(ClientId clientId, LibraryRole role, CanNetId canNetId) - : SerializerConfig(clientId, SERIALIZATION_TYPE_CAN_SPEC), - _canNetId(canNetId), - _role(role) {} - - virtual ~CanSerializerConfig() {} + CanSerializerConfig(ClientId clientId, LibraryRole role, CanNetId canNetId); + CanSerializerConfig(const CanSerializerConfig&) = default; + CanSerializerConfig(CanSerializerConfig&&) = delete; + CanSerializerConfig& operator=(const CanSerializerConfig&) = default; + CanSerializerConfig& operator=(CanSerializerConfig&&) = delete; + ~CanSerializerConfig() override = default; - inline CanNetId GetCanNetId() const { return _canNetId; } - - inline LibraryRole GetLibraryRole() const { return _role; } + CanNetId GetCanNetId() const; + LibraryRole GetLibraryRole() const; private: - CanNetId _canNetId; - LibraryRole _role; + CanNetId _canNetId{0}; + LibraryRole _role{LIBRARY_ROLE_UNKNOWN}; }; class PortBasedSerializerConfig : public SerializerConfig { public: - PortBasedSerializerConfig(ClientId clientId, LibraryRole role) - : SerializerConfig(clientId, SERIALIZATION_TYPE_PORT_BASED), - _role(role) {} - virtual ~PortBasedSerializerConfig() {} + PortBasedSerializerConfig(ClientId clientId, LibraryRole role); + PortBasedSerializerConfig(const PortBasedSerializerConfig&) = default; + PortBasedSerializerConfig(PortBasedSerializerConfig&&) = delete; + PortBasedSerializerConfig& operator=(const PortBasedSerializerConfig&) = + default; + PortBasedSerializerConfig& operator=(PortBasedSerializerConfig&&) = delete; + ~PortBasedSerializerConfig() override = default; - inline LibraryRole GetLibraryRole() const { return _role; } + LibraryRole GetLibraryRole() const; private: - LibraryRole _role; + LibraryRole _role{LIBRARY_ROLE_UNKNOWN}; }; class InterfaceConfig { public: - InterfaceConfig(LinkType linkType); - virtual ~InterfaceConfig() {} + InterfaceConfig(LinkType linkType); // NOLINT(*explicit*) + InterfaceConfig(const InterfaceConfig&) = default; + InterfaceConfig(InterfaceConfig&&) = delete; + InterfaceConfig& operator=(const InterfaceConfig&) = default; + InterfaceConfig& operator=(InterfaceConfig&&) = delete; + virtual ~InterfaceConfig() = default; LinkType GetLinkType() const; private: - LinkType _linkType; + LinkType _linkType{LINK_TYPE_UNKNOWN}; }; class CanInterfaceConfig : public InterfaceConfig { public: CanInterfaceConfig(CanNetId canNetId, CanDevId canDevId); - virtual ~CanInterfaceConfig() {} + CanInterfaceConfig(const CanInterfaceConfig&) = default; + CanInterfaceConfig(CanInterfaceConfig&&) = delete; + CanInterfaceConfig& operator=(const CanInterfaceConfig&) = default; + CanInterfaceConfig& operator=(CanInterfaceConfig&&) = delete; + ~CanInterfaceConfig() override = default; CanNetId GetNetId() const; CanDevId GetDevId() const; - inline void SetUserIfName(IN const std::string& userIfName) { - _userIfName = userIfName; - } - - inline std::string GetUserIfName() const { return _userIfName; } + std::string GetUserIfName() const; + void SetUserIfName(IN const std::string& userIfName); - inline void SetUserIfMajorVer(IN uint32_t majorVer) { - _userIfMajorVer = majorVer; - } + uint32_t GetUserIfMajorVer() const; + void SetUserIfMajorVer(IN uint32_t majorVer); - inline uint32_t GetUserIfMajorVer() const { return _userIfMajorVer; } + uint32_t GetUserIfMinorVer() const; + void SetUserIfMinorVer(IN uint32_t minorVer); - inline void SetUserIfMinorVer(IN uint32_t minorVer) { - _userIfMinorVer = minorVer; - } - - inline uint32_t GetUserIfMinorVer() const { return _userIfMinorVer; } - - inline void SetUserIfPatchVer(IN uint32_t patchVer) { - _userIfPatchVer = patchVer; - } - - inline uint32_t GetUserIfPatchVer() const { return _userIfPatchVer; } + uint32_t GetUserIfPatchVer() const; + void SetUserIfPatchVer(IN uint32_t patchVer); private: - CanNetId _canNetId; - CanDevId _canDevId; + CanNetId _canNetId{0}; + CanDevId _canDevId{0}; std::string _userIfName; - uint32_t _userIfMajorVer; - uint32_t _userIfMinorVer; - uint32_t _userIfPatchVer; + uint32_t _userIfMajorVer{0}; + uint32_t _userIfMinorVer{0}; + uint32_t _userIfPatchVer{0}; }; class Rs485InterfaceConfig : public InterfaceConfig { public: - Rs485InterfaceConfig(SerialDevId devId); - virtual ~Rs485InterfaceConfig() {} + Rs485InterfaceConfig(SerialDevId devId); // NOLINT(*explicit*) + Rs485InterfaceConfig(const Rs485InterfaceConfig&) = default; + Rs485InterfaceConfig(Rs485InterfaceConfig&&) = delete; + Rs485InterfaceConfig& operator=(const Rs485InterfaceConfig&) = default; + Rs485InterfaceConfig& operator=(Rs485InterfaceConfig&&) = delete; + ~Rs485InterfaceConfig() override = default; SerialDevId GetDevId() const; private: - SerialDevId _devId; + SerialDevId _devId{0}; }; class IpConfig : public InterfaceConfig { public: - IpConfig(const std::string& ip, uint16_t port, LinkType type) - : InterfaceConfig(type), _ip(ip), _port(port) {} - virtual ~IpConfig() {} - - inline std::string GetIp() const { return _ip; } + IpConfig(const std::string& ip, uint16_t port, LinkType type); + IpConfig(const IpConfig&) = default; + IpConfig(IpConfig&&) = delete; + IpConfig& operator=(const IpConfig&) = default; + IpConfig& operator=(IpConfig&&) = delete; + ~IpConfig() override = default; - inline uint16_t GetPort() const { return _port; } + std::string GetIp() const; + uint16_t GetPort() const; private: std::string _ip; - uint16_t _port; + uint16_t _port{0}; }; class UdpConfig : public IpConfig { public: - UdpConfig(const std::string& ip, uint16_t port) - : IpConfig(ip, port, LINK_TYPE_UDP) {} - virtual ~UdpConfig() {} + UdpConfig(const std::string& ip, uint16_t port); + UdpConfig(const UdpConfig&) = default; + UdpConfig(UdpConfig&&) = delete; + UdpConfig& operator=(const UdpConfig&) = default; + UdpConfig& operator=(UdpConfig&&) = delete; + ~UdpConfig() override = default; }; class MulticastConfig : public IpConfig { public: - MulticastConfig(const std::string& ip, uint16_t port) - : IpConfig(ip, port, LINK_TYPE_UDP_DISCOVERY) {} - virtual ~MulticastConfig() {} + MulticastConfig(const std::string& ip, uint16_t port); + MulticastConfig(const MulticastConfig&) = default; + MulticastConfig(MulticastConfig&&) = delete; + MulticastConfig& operator=(const MulticastConfig&) = default; + MulticastConfig& operator=(MulticastConfig&&) = delete; + ~MulticastConfig() override = default; }; class ComLibConfig { public: - ComLibConfig(); - + ComLibConfig() = default; ComLibConfig(uint16_t majorVer, uint16_t minorVer, uint16_t patchVer); - - ~ComLibConfig(); - // ACCESS + ComLibConfig(const ComLibConfig&) = default; + ComLibConfig(ComLibConfig&&) = delete; + ComLibConfig& operator=(const ComLibConfig&) = default; + ComLibConfig& operator=(ComLibConfig&&) = delete; + ~ComLibConfig() = default; LibraryRole GetRole() const; ClientId GetClientId() const; @@ -476,75 +502,53 @@ class ComLibConfig { void SetComLibConfigPatchVersion(uint16_t patchVer); private: - uint16_t _majorVer; - uint16_t _minorVer; - uint16_t _patchVer; + uint16_t _majorVer{0}; + uint16_t _minorVer{0}; + uint16_t _patchVer{0}; std::string _serialLibPath; std::string _configFilesPath; - ClientId _clientId; - LibraryRole _role; + ClientId _clientId{0}; + LibraryRole _role{LIBRARY_ROLE_UNKNOWN}; std::string _userIfName; std::string _downloadPath; - SerializationType _instSerialization; - SerializationType _dataSerialization; - ClientPhyType _timeSyncHwIfaceType; - uint8_t _timeSyncHwDevId; - bool _isTimeSyncSupported; - TimeSyncRole _timeSyncRole; - bool _isAliveSupported; - uint16_t _userIfMajorVer; - uint16_t _userIfMinorVer; - uint16_t _userIfPatchVer; + SerializationType _instSerialization{SERIALIZATION_TYPE_UNKNOWN}; + SerializationType _dataSerialization{SERIALIZATION_TYPE_UNKNOWN}; + ClientPhyType _timeSyncHwIfaceType{CLIENT_PHY_UNKNOWN}; + uint8_t _timeSyncHwDevId{}; + bool _isTimeSyncSupported{false}; + TimeSyncRole _timeSyncRole{TIME_SYNC_ROLE_UNKNOWN}; + bool _isAliveSupported{false}; + uint16_t _userIfMajorVer{0}; + uint16_t _userIfMinorVer{0}; + uint16_t _userIfPatchVer{0}; }; class SWUpdateConfigData { public: - SWUpdateConfigData() - : _pathAvailableFlag(false), - _dataPtrValidFlag(false), - _segmentBufferSize(0), - _segmentDataPtr((uint8_t*)0) {} - virtual ~SWUpdateConfigData() {} - - inline void SetData(IN bool pathFlag, IN std::string pathStr, - IN bool dataFlag, IN int32_t dataSize, - IN uint8_t dataEncrypt, IN uint8_t* dataPtr) { - _pathAvailableFlag = pathFlag; - _segmentPath = pathStr; - _dataPtrValidFlag = dataFlag; - _segmentBufferSize = dataSize; - _segmentEncryption = dataEncrypt; - _segmentDataPtr = dataPtr; - } + void SetData(IN bool pathFlag, IN std::string pathStr, IN bool dataFlag, + IN int32_t dataSize, IN uint8_t dataEncrypt, + IN uint8_t* dataPtr); void cleanData(); - inline bool GetPathAvailableFlag() const { return _pathAvailableFlag; } - - inline std::string GetSegmentFilePath() const { return _segmentPath; } - - inline bool GetDataPtrValidFlag() const { return _dataPtrValidFlag; } - - inline uint8_t GetSegmentEncryption() const { return _segmentEncryption; } - - inline int32_t GetSegmentBufferSize() const { return _segmentBufferSize; } - - inline uint8_t* GetSegmentDataPtr() { return _segmentDataPtr; } + bool GetPathAvailableFlag() const; + std::string GetSegmentFilePath() const; + bool GetDataPtrValidFlag() const; + uint8_t GetSegmentEncryption() const; + int32_t GetSegmentBufferSize() const; + uint8_t* GetSegmentDataPtr(); private: - bool _pathAvailableFlag; + bool _pathAvailableFlag{false}; std::string _segmentPath; - bool _dataPtrValidFlag; - int32_t _segmentBufferSize; - uint8_t _segmentEncryption; - uint8_t* _segmentDataPtr; + bool _dataPtrValidFlag{false}; + int32_t _segmentBufferSize{0}; + uint8_t _segmentEncryption{0}; + uint8_t* _segmentDataPtr{nullptr}; }; class InternalTransportHeader { public: - InternalTransportHeader(); - ~InternalTransportHeader(); - com::types::ProtocolType GetProtocolType() const; void SetProtocolType(IN com::types::ProtocolType protoType); @@ -557,11 +561,11 @@ class InternalTransportHeader { void SetDstClientId(IN ClientId clientId); private: - com::types::ProtocolType _protoType; - ClientId _srcClientId; - bool _srcClientIdValid; - ClientId _dstClientId; - bool _dstClientIdValid; + com::types::ProtocolType _protoType{PROTOCOL_TYPE_UNKNOWN}; + ClientId _srcClientId{0}; + bool _srcClientIdValid{false}; + ClientId _dstClientId{0}; + bool _dstClientIdValid{false}; }; } // namespace types diff --git a/umrr_ros2_driver/src/UpdateService.cpp b/umrr_ros2_driver/src/UpdateService.cpp deleted file mode 100644 index 8a028b0..0000000 --- a/umrr_ros2_driver/src/UpdateService.cpp +++ /dev/null @@ -1,100 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "umrr_ros2_driver/UpdateService.hpp" -#include - -#include -#include -#include -#include -#include -#include - -using namespace com::common; -using namespace com::types; -using namespace com::master; - -static SWUpdateInfo UpdateInfoStatic; - -void MyUpdateServiceInfoHandler(SWUpdateInfo &updateInfoLocal) { - UpdateInfoStatic = updateInfoLocal; - uint64_t downloadedBytes = updateInfoLocal.GetCurrentDownloadedBytes(); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Downloaded bytes: %lu", downloadedBytes); -} - -void StartSoftwareUpdate(ClientId client_id, std::string update_image) { - std::ifstream fileStream(update_image, std::ios::binary | std::ios::ate); - if (!fileStream.is_open()) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Couldn't open File"); - return; - } - const uint64_t size = fileStream.tellg(); - fileStream.close(); - - auto comServicesPtr = com::master::CommunicationServicesIface::Get(); - auto UpdateServicePtr = comServicesPtr->GetUpdateService(); - - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Start Download of the firmware"); - - UpdateInfoStatic.SetUpdateStatus(RUNNING); - UpdateInfoStatic.SetCurrentDownloadedBytes(0); - bool readyFlag = false; - - if (UpdateServicePtr->SoftwareUpdate(update_image, client_id, - MyUpdateServiceInfoHandler) != - ERROR_CODE_OK) { - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Start of SW-Download failed"); - return; - } - - if (UpdateInfoStatic.GetCurrentDownloadedBytes() == size) { - std::this_thread::sleep_for(std::chrono::seconds(3)); - readyFlag = true; - } - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - while (UpdateInfoStatic.GetUpdateStatus() == RUNNING && !readyFlag) { - // Waiting for the update to finish - } - - switch (UpdateInfoStatic.GetUpdateStatus()) { - case READY_SUCCESS: - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Successful download of firmware."); - //std::system("clear"); - break; - case STOPPED_BY_MASTER: - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Download is stopped by Master."); - break; - case STOPPED_BY_SLAVE: - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Download is stopped by Slave."); - break; - case STOPPED_BY_ERROR_TIMEOUT: - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Download-Error caused by timeout."); - break; - case STOPPED_BY_ERROR_BLOCK_REPEAT: - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Download-Error caused by Block-Repeat problem."); - break; - case STOPPED_BY_ERROR_IMAGE_INVALID: - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Download-Error caused by Invalid-Image problem."); - break; - default: - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Download-Error caused by Unknown-Reason."); - break; - } -} diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 3daf8fa..41b3e4a 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -16,9 +16,22 @@ #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 @@ -41,10 +54,18 @@ #include #include #include +#include +#include #include #include #include #include +#include +#include +#include +#include +#include +#include #include #include #include @@ -53,23 +74,9 @@ #include #include #include +#include +#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "umrr_ros2_driver/UpdateService.hpp" #include "umrr_ros2_driver/config_path.hpp" using com::common::Instruction; @@ -231,6 +238,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option : rclcpp::Node{"smartmicro_radar_node", node_options} { update_config_files_from_params(); + update_service = std::make_shared(); const auto override = false; setenv("SMART_ACCESS_CFG_FILE_PATH", kConfigFilePath, override); @@ -260,13 +268,18 @@ void SmartmicroRadarNode::initialize_services() 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_umrr9f_v2_4_1 = com::master::umrr9f_t169_automotive_v2_4_1::DataStreamServiceIface::Get(); + data_umrr9f_v3_0_0 = com::master::umrr9f_t169_automotive_v3_0_0::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_umrr9d_v1_4_1 = com::master::umrr9d_t152_automotive_v1_4_1::DataStreamServiceIface::Get(); data_umrr9d_v1_5_0 = com::master::umrr9d_t152_automotive_v1_5_0::DataStreamServiceIface::Get(); data_umrr9f_mse_v1_0_0 = com::master::umrr9f_t169_mse_v1_0_0::DataStreamServiceIface::Get(); data_umrr9f_mse_v1_1_0 = com::master::umrr9f_t169_mse_v1_1_0::DataStreamServiceIface::Get(); + data_umrr9f_mse_v1_3_0 = com::master::umrr9f_t169_mse_v1_3_0::DataStreamServiceIface::Get(); data_umrra4_mse_v1_0_0 = com::master::umrra4_mse_v1_0_0::DataStreamServiceIface::Get(); + data_umrra4_mse_v2_1_0 = com::master::umrra4_mse_v2_1_0::DataStreamServiceIface::Get(); + data_umrra1_v1_0_0 = com::master::umrra1_t166_b_automotive_v1_0_0::DataStreamServiceIface::Get(); + data_umrra1_v2_0_0 = com::master::umrra1_t166_b_automotive_v2_0_0::DataStreamServiceIface::Get(); // Wait for initailization std::this_thread::sleep_for(std::chrono::seconds(2)); @@ -276,7 +289,7 @@ void SmartmicroRadarNode::initialize_services() mode_srv_ = create_service( "smart_radar/set_radar_mode", std::bind( - &SmartmicroRadarNode::radar_mode, this, std::placeholders::_1, std::placeholders::_2)); + &SmartmicroRadarNode::set_radar_mode, this, std::placeholders::_1, std::placeholders::_2)); // create a ros2 service to change the IP address ip_addr_srv_ = create_service( @@ -296,6 +309,18 @@ void SmartmicroRadarNode::initialize_services() std::bind( &SmartmicroRadarNode::firmware_download, this, std::placeholders::_1, std::placeholders::_2)); + // create a ros2 service to read the radar status + status_srv_ = create_service( + "smart_radar/get_radar_status", + std::bind( + &SmartmicroRadarNode::get_radar_status, this, std::placeholders::_1, std::placeholders::_2)); + + // create a ros2 service to read the radar modes + read_mode_srv_ = create_service( + "smart_radar/get_radar_mode", + std::bind( + &SmartmicroRadarNode::get_radar_mode, this, std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO(this->get_logger(), "Radar services are ready."); } @@ -366,6 +391,26 @@ void SmartmicroRadarNode::port_publishers(const detail::SensorConfig & sensor, s throw; } + if (sensor.model == "umrra4_mse_v2_1_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrra4_mse_v2_1_0->RegisterComObjectListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::objectlist_callback_umrra4_mse_v2_1_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register objectlist callback for sensor umrra4_mse_v2_1_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrra4_mse_v2_1_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra4_mse_v2_1_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrra4_mse_v2_1_0"); + } + } if (sensor.model == "umrra4_mse_v1_0_0") { if ( com::types::ERROR_CODE_OK != @@ -386,6 +431,26 @@ void SmartmicroRadarNode::port_publishers(const detail::SensorConfig & sensor, s this->get_logger(), "Failed to register targetlist callback for sensor umrra4_mse_v1_0_0"); } } + if (sensor.model == "umrr9f_mse_v1_3_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_3_0->RegisterComObjectListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::objectlist_callback_umrr9f_mse_v1_3_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register objectlist callback for sensor umrr9f_mse_v1_3_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_3_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_mse_v1_3_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9f_mse_v1_3_0"); + } + } if (sensor.model == "umrr9f_mse_v1_1_0") { if ( com::types::ERROR_CODE_OK != @@ -496,6 +561,16 @@ void SmartmicroRadarNode::port_publishers(const detail::SensorConfig & sensor, s RCLCPP_INFO( this->get_logger(), "Failed to register targetlist callback for sensor umrr9f_v2_4_1"); } + if ( + sensor.model == "umrr9f_v3_0_0" && + com::types::ERROR_CODE_OK != + data_umrr9f_v3_0_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_v3_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9f_v3_0_0"); + } if ( sensor.model == "umrr9d_v1_0_3" && com::types::ERROR_CODE_OK != @@ -566,6 +641,26 @@ void SmartmicroRadarNode::port_publishers(const detail::SensorConfig & sensor, s RCLCPP_INFO( this->get_logger(), "Failed to register targetlist callback for sensor umrra4_v1_4_0"); } + if ( + sensor.model == "umrra1_v1_0_0" && + com::types::ERROR_CODE_OK != + data_umrra1_v1_0_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra1_v1_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrra1_v1_0_0"); + } + if ( + sensor.model == "umrra1_v2_0_0" && + com::types::ERROR_CODE_OK != + data_umrra1_v2_0_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra1_v2_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrra1_v2_0_0"); + } } void SmartmicroRadarNode::can_publishers(const detail::SensorConfig & sensor, size_t sensor_idx) @@ -602,6 +697,28 @@ void SmartmicroRadarNode::can_publishers(const detail::SensorConfig & sensor, si throw; } + if (sensor.model == "umrra4_can_mse_v2_1_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrra4_mse_v2_1_0->RegisterComObjectBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_objectlist_callback_umrra4_mse_v2_1_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register objectlist callback for sensor umrra4_can_mse_v2_1_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrra4_mse_v2_1_0->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrra4_mse_v2_1_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register targetlist callback for sensor umrra4_can_mse_v2_1_0"); + } + } if (sensor.model == "umrra4_can_mse_v1_0_0") { if ( com::types::ERROR_CODE_OK != @@ -624,6 +741,28 @@ void SmartmicroRadarNode::can_publishers(const detail::SensorConfig & sensor, si "Failed to register targetlist callback for sensor umrra4_can_mse_v1_0_0"); } } + if (sensor.model == "umrr9f_can_mse_v1_3_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_3_0->RegisterComObjectBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_objectlist_callback_umrr9f_mse_v1_3_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register objectlist callback for sensor umrr9f_can_mse_v1_3_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_3_0->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_mse_v1_3_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register targetlist callback for sensor umrr9f_can_mse_v1_3_0"); + } + } if (sensor.model == "umrr9f_can_mse_v1_1_0") { if ( com::types::ERROR_CODE_OK != @@ -718,6 +857,16 @@ void SmartmicroRadarNode::can_publishers(const detail::SensorConfig & sensor, si RCLCPP_INFO( this->get_logger(), "Failed to register CAN targetlist for sensor umrr9f_can_v2_4_1"); } + if ( + sensor.model == "umrr9f_can_v3_0_0" && + com::types::ERROR_CODE_OK != + data_umrr9f_v3_0_0->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v3_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrr9f_can_v3_0_0"); + } if ( sensor.model == "umrr9d_can_v1_0_3" && com::types::ERROR_CODE_OK != @@ -818,20 +967,17 @@ void SmartmicroRadarNode::firmware_download( return; } - StartSoftwareUpdate(client_id, update_image); + update_service->StartSoftwareUpdate(client_id, update_image); result->res = "Service ended, check the console output for status! "; } -void SmartmicroRadarNode::radar_mode( +void SmartmicroRadarNode::set_radar_mode( const std::shared_ptr request, std::shared_ptr result) { - std::string instruction_name{}; + // Validate sensor ID bool check_flag_id = false; - - instruction_name = request->param; client_id = request->sensor_id; - for (auto & sensor : m_sensors) { if (client_id == sensor.id) { check_flag_id = true; @@ -839,44 +985,130 @@ void SmartmicroRadarNode::radar_mode( } } if (!check_flag_id) { - result->res = "The sensor ID value entered is invalid! "; + result->res = "Error: Sensor ID is invalid! "; + return; + } + + auto section_name = request->section_name; + if ( + section_name != "auto_interface_0dim" && section_name != "auto_interface_rrm" && + section_name != "Parameter") { + result->res = + "Error: Invalid section name specified! Must be 'auto_interface_0dim', " + "'auto_interface_rrm', or 'Parameter'."; + return; + } + + // Check arrays have same length + if ( + request->params.size() != request->values.size() || + request->params.size() != request->value_types.size()) { + result->res = "Error: param, values and value_types arrays must have same length"; return; } std::shared_ptr inst{m_services->GetInstructionService()}; + if (!inst) { + result->res = "Error: Failed to get instruction service"; + return; + } + 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! "; + result->res = "Error: Failed to allocate instruction! "; return; } - std::shared_ptr> radar_mode_01 = - 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); + for (size_t i = 0; i < request->params.size(); i++) { + const auto & param = request->params[i]; + const auto & value = request->values[i]; + const auto & value_type = request->value_types[i]; + bool request_added = false; + try { + switch (value_type) { + case 0: { + float float_value = std::stof(value); + auto radar_mode_float = + std::make_shared>(section_name, param, float_value); + request_added = batch->AddRequest(radar_mode_float); + break; + } + case 1: { + if (value.find('.') != std::string::npos) { + result->res = "Error: uint32 value cannot contain decimal points"; + return; + } + uint32_t u32_value = static_cast(std::stoul(value)); + auto radar_mode_u32 = + std::make_shared>(section_name, param, u32_value); + request_added = batch->AddRequest(radar_mode_u32); + break; + } + case 2: { + if (value.find('.') != std::string::npos) { + result->res = "Error: uint16 value cannot contain decimal points"; + return; + } + uint64_t temp = std::stoul(value); + if (temp > 65535) { + result->res = "Error: uint16 value must be between 0 and 65535"; + return; + } + uint16_t u16_value = static_cast(temp); + auto radar_mode_u16 = + std::make_shared>(section_name, param, u16_value); + request_added = batch->AddRequest(radar_mode_u16); + break; + } + case 3: { + if (value.find('.') != std::string::npos) { + result->res = "Error: uint8 value cannot contain decimal points"; + return; + } + uint64_t temp = std::stoul(value); + if (temp > 255) { + result->res = "Error: uint8 value must be between 0 and 255"; + return; + } + uint8_t u8_value = static_cast(temp); + auto radar_mode_u8 = + std::make_shared>(section_name, param, u8_value); + request_added = batch->AddRequest(radar_mode_u8); + break; + } + default: + result->res = + "Error: Invalid value_type specified. Must be 0 (f32), 1 (u32), 2 (u16), 3 (u8)"; + return; + } + } catch (const std::invalid_argument & e) { + result->res = "Error: Failed to convert value string, invalid format"; + return; + } catch (const std::out_of_range & e) { + result->res = "Error: Value is out of range for the specified type"; + return; + } - if (!batch->AddRequest(radar_mode_01)) { - result->res = "Failed to add instruction to the batch! "; - } - if (!batch->AddRequest(radar_mode_02)) { - result->res = "Failed to add instruction to the batch! "; + if (!request_added) { + result->res = "Error: Failed to add instruction '" + param + "'! "; + return; + } } 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!"; + com::types::ERROR_CODE_OK != + inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::mode_response, this, client_id, std::placeholders::_2, + request->params, section_name))) { + result->res = "Error: Check params are valid for this sensor and values within range!"; return; } - result->res = "Service conducted successfully"; + result->res = "Success: Request sent successfully. Check main terminal for sensor response!"; + RCLCPP_INFO(this->get_logger(), "Service call result: %s", result->res.c_str()); } void SmartmicroRadarNode::ip_address( @@ -901,7 +1133,7 @@ void SmartmicroRadarNode::ip_address( std::shared_ptr batch; if (!inst->AllocateInstructionBatch(client_id, batch)) { - result->res_ip = "Failed to allocate instruction batch! "; + result->res_ip = "Failed to allocate instruction! "; return; } @@ -913,11 +1145,11 @@ void SmartmicroRadarNode::ip_address( 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! "; + result->res_ip = "Failed to add instruction! "; return; } if (!batch->AddRequest(cmd)) { - result->res_ip = "Failed to add instruction to batch! "; + result->res_ip = "Failed to add instruction! "; return; } // send instruction batch to the device @@ -933,7 +1165,9 @@ void SmartmicroRadarNode::ip_address( this->get_logger(), "Radar must be restarted and the parameters in the param file " "must be updated !!."); - result->res_ip = "Service conducted successfully"; + result->res_ip = + "Success: IP change executed successfully. Radar must be restarted " + "and the parameters in the param file must be updated"; } } @@ -958,6 +1192,16 @@ void SmartmicroRadarNode::radar_command( return; } + auto section_name = request->section_name; + if ( + section_name != "auto_interface_command" && section_name != "auto_interface_rrm_command" && + section_name != "Command") { + result->res = + "Error: Invalid section name specified! Must be 'auto_interface_command', " + "'auto_interface_rrm_command', or 'Command'."; + return; + } + std::shared_ptr inst{m_services->GetInstructionService()}; timer = this->create_wall_timer( std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); @@ -965,15 +1209,15 @@ void SmartmicroRadarNode::radar_command( std::shared_ptr batch; if (!inst->AllocateInstructionBatch(client_id, batch)) { - result->res = "Failed to allocate instruction batch! "; + result->res = "Failed to allocate instruction! "; return; } std::shared_ptr radar_command = - std::make_shared("auto_interface_command", request->command, request->value); + std::make_shared(section_name, request->command, request->value); if (!batch->AddRequest(radar_command)) { - result->res = "Failed to add instruction to the batch! "; + result->res = "Failed to add instruction! "; return; } @@ -981,32 +1225,252 @@ void SmartmicroRadarNode::radar_command( com::types::ERROR_CODE_OK != inst->SendInstructionBatch( batch, std::bind( &SmartmicroRadarNode::command_response, this, client_id, - std::placeholders::_2, command_name))) { + std::placeholders::_2, command_name, section_name))) { result->res = "Error in sending command to the sensor!"; return; } - result->res = "Service conducted successfully"; + result->res = "Success: Request sent successfully."; +} + +void SmartmicroRadarNode::get_radar_status( + const std::shared_ptr request, + std::shared_ptr result) +{ + // Validate sensor ID + bool check_flag_id = false; + 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 = "Error: Sensor ID is invalid! "; + return; + } + + auto section_name = request->section_name; + if ( + section_name != "auto_interface" && section_name != "auto_interface_rrm" && + section_name != "Status") { + result->res = + "Error: Invalid section name specified! Must be 'auto_interface', 'auto_interface_rrm', or " + "'Status'."; + return; + } + + // Check arrays have same length + if (request->statuses.size() != request->status_types.size()) { + result->res = "Error: status and status_types arrays must have same length"; + return; + } + + std::shared_ptr inst{m_services->GetInstructionService()}; + if (!inst) { + result->res = "Error: Failed to get instruction service"; + return; + } + + 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 = "Error: Failed to allocate instruction! "; + return; + } + + for (size_t i = 0; i < request->statuses.size(); i++) { + const auto & status = request->statuses[i]; + const auto & status_type = request->status_types[i]; + bool request_added = false; + + switch (status_type) { + case 0: { + auto radar_status_u32 = std::make_shared>(section_name, status); + request_added = batch->AddRequest(radar_status_u32); + break; + } + case 1: { + auto radar_status_u16 = std::make_shared>(section_name, status); + request_added = batch->AddRequest(radar_status_u16); + break; + } + default: + result->res = "Error: Invalid value_type specified. Must be 0 (u32) or 1 (u8)"; + return; + } + + if (!request_added) { + result->res = "Error: Failed to add instruction '" + status + "' ! "; + return; + } + } + + if ( + com::types::ERROR_CODE_OK != + inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::status_response, this, client_id, std::placeholders::_2, + request->statuses, section_name))) { + result->res = "Error: Check status are valid for this sensor!"; + return; + } + result->res = "Success: Request sent successfully. Check main terminal for sensor response!"; +} + +void SmartmicroRadarNode::get_radar_mode( + const std::shared_ptr request, + std::shared_ptr result) +{ + // Validate sensor ID + bool check_flag_id = false; + 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 = "Error: Sensor ID is invalid! "; + return; + } + + auto section_name = request->section_name; + if ( + section_name != "auto_interface_0dim" && section_name != "auto_interface_rrm" && + section_name != "Parameter") { + result->res = + "Error: Invalid section name specified! Must be 'auto_interface_0dim', 'auto_interface_rrm', " + "or 'Parameter'."; + return; + } + + // Check arrays have same length + if (request->params.size() != request->param_types.size()) { + result->res = "Error: param and value_types arrays must have same length"; + return; + } + + std::shared_ptr inst{m_services->GetInstructionService()}; + if (!inst) { + result->res = "Error: Failed to get instruction service"; + return; + } + + 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 = "Error: Failed to allocate instruction! "; + return; + } + + for (size_t i = 0; i < request->params.size(); i++) { + const auto & param = request->params[i]; + const auto & param_type = request->param_types[i]; + bool request_added = false; + + switch (param_type) { + case 0: { + auto radar_param_float = std::make_shared>(section_name, param); + request_added = batch->AddRequest(radar_param_float); + break; + } + case 1: { + auto radar_param_u32 = std::make_shared>(section_name, param); + request_added = batch->AddRequest(radar_param_u32); + break; + } + case 2: { + auto radar_param_u16 = std::make_shared>(section_name, param); + request_added = batch->AddRequest(radar_param_u16); + break; + } + case 3: { + auto radar_param_u8 = std::make_shared>(section_name, param); + request_added = batch->AddRequest(radar_param_u8); + break; + } + default: + result->res = + "Error: Invalid value_type specified. Must be 0(u32), 1(u16), 2(u8) or 3(float)"; + return; + } + + if (!request_added) { + result->res = "Error: Failed to add instruction '" + param + "' Check param types match! "; + return; + } + } + + if ( + com::types::ERROR_CODE_OK != + inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::param_response, this, client_id, std::placeholders::_2, + request->params, section_name))) { + result->res = "Error: Check params are valid for this sensor!"; + return; + } + result->res = "Success: Request sent successfully. Check main terminal for sensor response!"; } void SmartmicroRadarNode::mode_response( const com::types::ClientId client_id, - const std::shared_ptr & response, const std::string instruction_name) + const std::shared_ptr & response, + const std::vector & instruction_names, const std::string & section_name) { - std::vector>> myResp_1; - std::vector>> myResp_2; + for (const auto & instruction_name : instruction_names) { + std::vector>> resp_u8; + std::vector>> resp_u32; + std::vector>> resp_f; + bool response_found = false; + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u8)) { + response_found = true; + for (auto & resp : resp_u8) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } - 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(section_name, instruction_name.c_str(), resp_u32)) { + response_found = true; + for (auto & resp : resp_u32) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } } - } - 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); + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_f)) { + response_found = true; + for (auto & resp : resp_f) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %f\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (!response_found) { + RCLCPP_WARN(this->get_logger(), "No response received!"); } } } @@ -1015,44 +1479,272 @@ 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); + std::vector>> resp_ip; + if (response->GetResponse("auto_interface_0dim", "ip_source_address", resp_ip)) { + for (auto & resp : resp_ip) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); } } } void SmartmicroRadarNode::command_response( const com::types::ClientId client_id, - const std::shared_ptr & response, const std::string command_name) + const std::shared_ptr & response, const std::string command_name, + const std::string & section_name) { std::vector>> command_resp; - if (response->GetResponse( - "auto_interface_command", command_name.c_str(), command_resp)) { + if (response->GetResponse(section_name, 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); - uint32_t value = resp->GetValue(); - RCLCPP_INFO(this->get_logger(), "Response from sensor to command: %i", value); + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); } } } -void SmartmicroRadarNode::objectlist_callback_umrra4_mse_v1_0_0( - const std::uint32_t sensor_idx, - const std::shared_ptr & - objectlist_port_umrra4_mse_v1_0_0, - const com::types::ClientId client_id) +void SmartmicroRadarNode::status_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, + const std::vector & statuses, const std::string & section_name) { - std::cout << "Objectlist for umrra4_mse_v1_0_0" << std::endl; - if (!check_signal) { - std::shared_ptr port_header; - port_header = objectlist_port_umrra4_mse_v1_0_0->GetPortHeader(); - std::shared_ptr object_header; - object_header = objectlist_port_umrra4_mse_v1_0_0->GetObjectListHeader(); - sensor_msgs::msg::PointCloud2 msg; + for (const auto & instruction_name : statuses) { + std::vector>> resp_u16; + std::vector>> resp_u32; + bool response_found = false; + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u16)) { + response_found = true; + for (auto & resp : resp_u16) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u32)) { + response_found = true; + for (auto & resp : resp_u32) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (!response_found) { + RCLCPP_WARN(this->get_logger(), "No response received!"); + } + } +} + +void SmartmicroRadarNode::param_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, + const std::vector & statuses, const std::string & section_name) +{ + for (const auto & instruction_name : statuses) { + std::vector>> resp_u16; + std::vector>> resp_u32; + std::vector>> resp_u8; + std::vector>> resp_f; + bool response_found = false; + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u16)) { + response_found = true; + for (auto & resp : resp_u16) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u32)) { + response_found = true; + for (auto & resp : resp_u32) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u8)) { + response_found = true; + for (auto & resp : resp_u8) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_f)) { + response_found = true; + for (auto & resp : resp_f) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %f\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (!response_found) { + RCLCPP_WARN(this->get_logger(), "No response received!"); + } + } +} + +void SmartmicroRadarNode::objectlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrra4_mse_v2_1_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrra4_mse_v2_1_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_port_umrra4_mse_v2_1_0->GetPortHeader(); + std::shared_ptr object_header; + object_header = objectlist_port_umrra4_mse_v2_1_0->GetObjectListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = object_header->GetCycleTime(); + header.number_of_objects = object_header->GetNumberOfObjects(); + header.ts_measurement = object_header->GetTimestampOfMeasurement(); + for (const auto & object : objectlist_port_umrra4_mse_v2_1_0->GetObjectList()) { + const auto x_pos = object->GetPosX(); + const auto y_pos = object->GetPosY(); + const auto z_pos = object->GetPosZ(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeading(); + const auto length = object->GetLength(); + const auto mileage = object->GetMileage(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = object->GetObjectId(); + const auto idle_cycles = object->GetIdleCycles(); + const auto status = object->GetStatus(); + + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, mileage, quality, acceleration, object_id, + idle_cycles, status}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_port_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4_mse_v2_1_0, + const com::types::ClientId client_id) +{ + std::cout << "Port Targetlist for umrra4_mse_v2_1_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_port_umrra4_mse_v2_1_0->GetPortHeader(); + std::shared_ptr target_header; + target_header = targetlist_port_umrra4_mse_v2_1_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrra4_mse_v2_1_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->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, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::objectlist_callback_umrra4_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrra4_mse_v1_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrra4_mse_v1_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_port_umrra4_mse_v1_0_0->GetPortHeader(); + std::shared_ptr object_header; + object_header = objectlist_port_umrra4_mse_v1_0_0->GetObjectListHeader(); + sensor_msgs::msg::PointCloud2 msg; umrr_ros2_msgs::msg::PortObjectHeader header; ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; const auto [sec, nanosec] = @@ -1148,6 +1840,116 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_mse_v1_0_0( } } +void SmartmicroRadarNode::objectlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrr9f_mse_v1_3_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_port_umrr9f_mse_v1_3_0->GetPortHeader(); + std::shared_ptr + object_header; + object_header = objectlist_port_umrr9f_mse_v1_3_0->GetObjectListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = object_header->GetCycleTime(); + header.number_of_objects = object_header->GetNumberOfObjects(); + header.ts_measurement = object_header->GetTimestampOfMeasurement(); + for (const auto & object : objectlist_port_umrr9f_mse_v1_3_0->GetObjectList()) { + const auto x_pos = object->GetPosX(); + const auto y_pos = object->GetPosY(); + const auto z_pos = object->GetPosZ(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeading(); + const auto length = object->GetLength(); + const auto mileage = object->GetMileage(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = object->GetObjectId(); + const auto idle_cycles = object->GetIdleCycles(); + const auto status = object->GetStatus(); + + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, mileage, quality, acceleration, object_id, + idle_cycles, status}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_port_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id) +{ + std::cout << "Port Targetlist for umrr9f_mse_v1_3_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_port_umrr9f_mse_v1_3_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9f_mse_v1_3_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9f_mse_v1_3_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->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, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + void SmartmicroRadarNode::objectlist_callback_umrr9f_mse_v1_1_0( const std::uint32_t sensor_idx, const std::shared_ptr & @@ -1749,6 +2551,63 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_4_1( } } +void SmartmicroRadarNode::targetlist_callback_umrr9f_v3_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_v3_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f v3_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9f_v3_0_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9f_v3_0_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9f_v3_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->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, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_3( const std::uint32_t sensor_idx, const std::shared_ptr & @@ -1874,7 +2733,120 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_4_1( port_header = targetlist_port_umrr9d_v1_4_1->GetPortHeader(); std::shared_ptr target_header; - target_header = targetlist_port_umrr9d_v1_4_1->GetTargetListHeader(); + target_header = targetlist_port_umrr9d_v1_4_1->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9d_v1_4_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, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_5_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9d_v1_5_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_5_0" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9d_v1_5_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9d_v1_5_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9d_v1_5_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->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, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::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) +{ + std::cout << "Targetlist for umrra4_v1_0_1" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_port_umrra4_v1_0_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrra4_v1_0_1->GetTargetListHeader(); sensor_msgs::msg::PointCloud2 msg; umrr_ros2_msgs::msg::PortTargetHeader header; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; @@ -1901,7 +2873,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_4_1( header.prf = target_header->GetPrf(); header.umambiguous_speed = target_header->GetUmambiguousSpeed(); header.acquisition_start = target_header->GetAcquisitionStart(); - for (const auto & target : targetlist_port_umrr9d_v1_4_1->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); @@ -1918,20 +2890,19 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_4_1( } } -void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_5_0( +void SmartmicroRadarNode::targetlist_callback_umrra4_v1_2_1( const std::uint32_t sensor_idx, - const std::shared_ptr & - targetlist_port_umrr9d_v1_5_0, + const std::shared_ptr & + targetlist_port_umrra4_v1_2_1, const com::types::ClientId client_id) { - std::cout << "Targetlist for umrr9d_v1_5_0" << std::endl; + std::cout << "Targetlist for umrra4_v1_2_1" << std::endl; if (!check_signal) { - std::shared_ptr - port_header; - port_header = targetlist_port_umrr9d_v1_5_0->GetPortHeader(); - std::shared_ptr + std::shared_ptr port_header; + port_header = targetlist_port_umrra4_v1_2_1->GetPortHeader(); + std::shared_ptr target_header; - target_header = targetlist_port_umrr9d_v1_5_0->GetTargetListHeader(); + target_header = targetlist_port_umrra4_v1_2_1->GetTargetListHeader(); sensor_msgs::msg::PointCloud2 msg; umrr_ros2_msgs::msg::PortTargetHeader header; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; @@ -1939,8 +2910,6 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_5_0( convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); msg.header.stamp.sec = sec; msg.header.stamp.nanosec = nanosec; - header.frame_id = m_sensors[sensor_idx].frame_id; - header.port_identifier = port_header->GetPortIdentifier(); header.port_ver_major = port_header->GetPortVersionMajor(); header.port_ver_minor = port_header->GetPortVersionMinor(); @@ -1950,6 +2919,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_5_0( header.header_ver_major = port_header->GetHeaderVersionMajor(); header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.frame_id = m_sensors[sensor_idx].frame_id; header.cycle_time = target_header->GetCycleTime(); header.number_of_targets = target_header->GetNumberOfTargets(); header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); @@ -1958,7 +2928,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_5_0( header.prf = target_header->GetPrf(); header.umambiguous_speed = target_header->GetUmambiguousSpeed(); header.acquisition_start = target_header->GetAcquisitionStart(); - for (const auto & target : targetlist_port_umrr9d_v1_5_0->GetTargetList()) { + for (const auto & target : targetlist_port_umrra4_v1_2_1->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); @@ -1975,19 +2945,19 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_5_0( } } -void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1( +void SmartmicroRadarNode::targetlist_callback_umrra4_v1_4_0( const std::uint32_t sensor_idx, - const std::shared_ptr & - targetlist_port_umrra4_v1_0_1, + const std::shared_ptr & + targetlist_port_umrra4_v1_4_0, const com::types::ClientId client_id) { - std::cout << "Targetlist for umrra4_v1_0_1" << std::endl; + std::cout << "Targetlist for umrra4_v1_4_0" << std::endl; if (!check_signal) { - std::shared_ptr port_header; - port_header = targetlist_port_umrra4_v1_0_1->GetPortHeader(); - std::shared_ptr + std::shared_ptr port_header; + port_header = targetlist_port_umrra4_v1_4_0->GetPortHeader(); + std::shared_ptr target_header; - target_header = targetlist_port_umrra4_v1_0_1->GetTargetListHeader(); + target_header = targetlist_port_umrra4_v1_4_0->GetTargetListHeader(); sensor_msgs::msg::PointCloud2 msg; umrr_ros2_msgs::msg::PortTargetHeader header; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; @@ -1995,8 +2965,6 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1( convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); msg.header.stamp.sec = sec; msg.header.stamp.nanosec = nanosec; - header.frame_id = m_sensors[sensor_idx].frame_id; - header.port_identifier = port_header->GetPortIdentifier(); header.port_ver_major = port_header->GetPortVersionMajor(); header.port_ver_minor = port_header->GetPortVersionMinor(); @@ -2006,6 +2974,7 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1( header.header_ver_major = port_header->GetHeaderVersionMajor(); header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.frame_id = m_sensors[sensor_idx].frame_id; header.cycle_time = target_header->GetCycleTime(); header.number_of_targets = target_header->GetNumberOfTargets(); header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); @@ -2014,7 +2983,7 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1( header.prf = target_header->GetPrf(); header.umambiguous_speed = target_header->GetUmambiguousSpeed(); header.acquisition_start = target_header->GetAcquisitionStart(); - for (const auto & target : targetlist_port_umrra4_v1_0_1->GetTargetList()) { + for (const auto & target : targetlist_port_umrra4_v1_4_0->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); @@ -2031,19 +3000,21 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1( } } -void SmartmicroRadarNode::targetlist_callback_umrra4_v1_2_1( +void SmartmicroRadarNode::targetlist_callback_umrra1_v1_0_0( const std::uint32_t sensor_idx, - const std::shared_ptr & - targetlist_port_umrra4_v1_2_1, + const std::shared_ptr< + com::master::umrra1_t166_b_automotive_v1_0_0::comtargetlist::ComTargetList> & + targetlist_port_umrra1_v1_0_0, const com::types::ClientId client_id) { - std::cout << "Targetlist for umrra4_v1_2_1" << std::endl; + std::cout << "Targetlist for umrra1_v1_0_0" << std::endl; if (!check_signal) { - std::shared_ptr port_header; - port_header = targetlist_port_umrra4_v1_2_1->GetPortHeader(); - std::shared_ptr + std::shared_ptr + port_header; + port_header = targetlist_port_umrra1_v1_0_0->GetPortHeader(); + std::shared_ptr target_header; - target_header = targetlist_port_umrra4_v1_2_1->GetTargetListHeader(); + target_header = targetlist_port_umrra1_v1_0_0->GetTargetListHeader(); sensor_msgs::msg::PointCloud2 msg; umrr_ros2_msgs::msg::PortTargetHeader header; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; @@ -2069,7 +3040,7 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_2_1( header.prf = target_header->GetPrf(); header.umambiguous_speed = target_header->GetUmambiguousSpeed(); header.acquisition_start = target_header->GetAcquisitionStart(); - for (const auto & target : targetlist_port_umrra4_v1_2_1->GetTargetList()) { + for (const auto & target : targetlist_port_umrra1_v1_0_0->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); @@ -2086,19 +3057,21 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_2_1( } } -void SmartmicroRadarNode::targetlist_callback_umrra4_v1_4_0( +void SmartmicroRadarNode::targetlist_callback_umrra1_v2_0_0( const std::uint32_t sensor_idx, - const std::shared_ptr & - targetlist_port_umrra4_v1_4_0, + const std::shared_ptr< + com::master::umrra1_t166_b_automotive_v2_0_0::comtargetlist::ComTargetList> & + targetlist_port_umrra1_v2_0_0, const com::types::ClientId client_id) { - std::cout << "Targetlist for umrra4_v1_4_0" << std::endl; + std::cout << "Targetlist for umrra1_v2_0_0" << std::endl; if (!check_signal) { - std::shared_ptr port_header; - port_header = targetlist_port_umrra4_v1_4_0->GetPortHeader(); - std::shared_ptr + std::shared_ptr + port_header; + port_header = targetlist_port_umrra1_v2_0_0->GetPortHeader(); + std::shared_ptr target_header; - target_header = targetlist_port_umrra4_v1_4_0->GetTargetListHeader(); + target_header = targetlist_port_umrra1_v2_0_0->GetTargetListHeader(); sensor_msgs::msg::PointCloud2 msg; umrr_ros2_msgs::msg::PortTargetHeader header; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; @@ -2124,7 +3097,7 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_4_0( header.prf = target_header->GetPrf(); header.umambiguous_speed = target_header->GetUmambiguousSpeed(); header.acquisition_start = target_header->GetAcquisitionStart(); - for (const auto & target : targetlist_port_umrra4_v1_4_0->GetTargetList()) { + for (const auto & target : targetlist_port_umrra1_v2_0_0->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); @@ -2141,6 +3114,98 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_4_0( } } +void SmartmicroRadarNode::CAN_objectlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_can_umrra4_mse_v2_1_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrra4_mse_can_v2_1_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_can_umrra4_mse_v2_1_0->GetPortHeader(); + std::shared_ptr + object_header; + object_header = objectlist_can_umrra4_mse_v2_1_0->GetComObjectBaseListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = object_header->GetCycleDuration(); + header.cycle_count = object_header->GetCycleCount(); + header.number_of_objects = object_header->GetNoOfObjects(); + header.ego_speed = object_header->GetSpeed(); + header.ego_speed_quality = object_header->GetSpeedQuality(); + header.ego_yaw_rate = object_header->GetYawRate(); + header.ego_yaw_rate_quality = object_header->GetYawRateQuality(); + header.dyn_source = object_header->GetDynamicSource(); + for (const auto & object : objectlist_can_umrra4_mse_v2_1_0->GetObjectList()) { + const auto x_pos = object->GetXPoint1(); + const auto y_pos = object->GetYPoint1(); + const auto z_pos = object->GetZPoint1(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeadingDeg(); + const auto length = object->GetObjectLen(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = static_cast(object->GetObjectId()); + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, quality, acceleration, object_id}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_can_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_can_umrra4_mse_v2_1_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrra4_mse_can_v2_1_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_can_umrra4_mse_v2_1_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrra4_mse_v2_1_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + header.time_stamp = target_header->GetTimeStamp(); + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + for (const auto & target : targetlist_can_umrra4_mse_v2_1_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->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, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + void SmartmicroRadarNode::CAN_objectlist_callback_umrra4_mse_v1_0_0( const std::uint32_t sensor_idx, const std::shared_ptr & @@ -2417,6 +3482,98 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_mse_v1_1_0( } } +void SmartmicroRadarNode::CAN_objectlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_can_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrr9f_mse_can_v1_3_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_can_umrr9f_mse_v1_3_0->GetPortHeader(); + std::shared_ptr + object_header; + object_header = objectlist_can_umrr9f_mse_v1_3_0->GetComObjectBaseListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = object_header->GetCycleDuration(); + header.cycle_count = object_header->GetCycleCount(); + header.number_of_objects = object_header->GetNoOfObjects(); + header.ego_speed = object_header->GetSpeed(); + header.ego_speed_quality = object_header->GetSpeedQuality(); + header.ego_yaw_rate = object_header->GetYawRate(); + header.ego_yaw_rate_quality = object_header->GetYawRateQuality(); + header.dyn_source = object_header->GetDynamicSource(); + for (const auto & object : objectlist_can_umrr9f_mse_v1_3_0->GetObjectList()) { + const auto x_pos = object->GetXPoint1(); + const auto y_pos = object->GetYPoint1(); + const auto z_pos = object->GetZPoint1(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeadingDeg(); + const auto length = object->GetObjectLen(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = static_cast(object->GetObjectId()); + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, quality, acceleration, object_id}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_can_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_can_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_mse_can_v1_3_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_can_umrr9f_mse_v1_3_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9f_mse_v1_3_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + header.time_stamp = target_header->GetTimeStamp(); + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + for (const auto & target : targetlist_can_umrr9f_mse_v1_3_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->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, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + void SmartmicroRadarNode::CAN_targetlist_callback_umrr96( const std::uint32_t sensor_idx, const std::shared_ptr< @@ -2830,6 +3987,53 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_4_1( m_publishers_can_target_header[sensor_idx]->publish(header); } } + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v3_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v3_0_0::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v3_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_v3_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9f_v3_0_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9f_v3_0_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + header.time_stamp = target_header->GetTimeStamp(); + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + for (const auto & target : targetlist_can_umrr9f_v3_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->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, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_0_1( const std::uint32_t sensor_idx, const std::shared_ptr< diff --git a/umrr_ros2_driver/src/update_service.cpp b/umrr_ros2_driver/src/update_service.cpp new file mode 100644 index 0000000..67aaca2 --- /dev/null +++ b/umrr_ros2_driver/src/update_service.cpp @@ -0,0 +1,95 @@ +#include "umrr_ros2_driver/update_service.hpp" + +#include +#include + +using namespace com::types; +using namespace com::master; + +UpdateService::UpdateService() {} + +void UpdateService::StartSoftwareUpdate(ClientId client_id, std::string & update_image) +{ + std::ifstream fileStream(update_image, std::ios::binary | std::ios::ate); + if (!fileStream.is_open()) { + RCLCPP_ERROR( + rclcpp::get_logger("FirmwareUpdater"), "Couldn't open file: %s", update_image.c_str()); + return; + } + + const uint64_t totalSize = fileStream.tellg(); + + auto comServicesPtr = CommunicationServicesIface::Get(); + auto updateService = comServicesPtr->GetUpdateService(); + + { + std::lock_guard lock(mutex_); + updateInfo_.SetUpdateStatus(RUNNING); + updateInfo_.SetCurrentDownloadedBytes(0); + } // mutex(unlocked) + + RCLCPP_INFO( + rclcpp::get_logger("UpdateService"), "Starting firmware download of %lu bytes...", totalSize); + + if (updateService->SoftwareUpdate(update_image, client_id, [this](SWUpdateInfo & info) { + this->UpdateCallback(info); + }) != ERROR_CODE_OK) { + RCLCPP_ERROR(rclcpp::get_logger("FirmwareUpdater"), "Start of software download failed"); + return; + } + + { + std::unique_lock lock( + mutex_); + cv_.wait( + lock, + [this, + totalSize] { + return updateInfo_.GetUpdateStatus() != RUNNING || + updateInfo_.GetCurrentDownloadedBytes() >= totalSize; + }); + } + + HandleResult(); +} + +void UpdateService::UpdateCallback(SWUpdateInfo & info) +{ + { + std::lock_guard lock(mutex_); + updateInfo_ = info; + RCLCPP_INFO( + rclcpp::get_logger("FirmwareUpdater"), "Downloaded %lu bytes...", + info.GetCurrentDownloadedBytes()); + } + cv_.notify_one(); +} + +void UpdateService::HandleResult() +{ + std::lock_guard lock(mutex_); + switch (updateInfo_.GetUpdateStatus()) { + case READY_SUCCESS: + RCLCPP_INFO( + rclcpp::get_logger("FirmwareUpdater"), "Firmware download completed successfully."); + break; + case STOPPED_BY_MASTER: + RCLCPP_WARN(rclcpp::get_logger("FirmwareUpdater"), "Download stopped by master."); + break; + case STOPPED_BY_SLAVE: + RCLCPP_ERROR(rclcpp::get_logger("FirmwareUpdater"), "Download stopped by slave."); + break; + case STOPPED_BY_ERROR_TIMEOUT: + RCLCPP_ERROR(rclcpp::get_logger("FirmwareUpdater"), "Download failed: timeout."); + break; + case STOPPED_BY_ERROR_BLOCK_REPEAT: + RCLCPP_ERROR(rclcpp::get_logger("FirmwareUpdater"), "Download failed: block repeat error."); + break; + case STOPPED_BY_ERROR_IMAGE_INVALID: + RCLCPP_ERROR(rclcpp::get_logger("FirmwareUpdater"), "Download failed: invalid image."); + break; + default: + RCLCPP_ERROR(rclcpp::get_logger("FirmwareUpdater"), "Download failed: unknown error."); + break; + } +} diff --git a/umrr_ros2_driver/test/radar_node_test.launch.py b/umrr_ros2_driver/test/radar_node_test.launch.py index 7fcab34..bb4279c 100644 --- a/umrr_ros2_driver/test/radar_node_test.launch.py +++ b/umrr_ros2_driver/test/radar_node_test.launch.py @@ -42,57 +42,54 @@ def generate_test_description(): parameters=[radar__params] ) - frequency_sweep_service = ExecuteProcess( - cmd = [[ - 'ros2 service call ', - '/smart_radar/set_radar_mode ', - 'umrr_ros2_msgs/srv/SetMode ', - '"{param: "frequency_sweep_idx", value: 1, sensor_id: 200}"' - ]], + set_frequency_sweep_service = ExecuteProcess( + cmd = [ + 'ros2', 'service', 'call', + '/smart_radar/set_radar_mode', + 'umrr_ros2_msgs/srv/SetMode', + '{section_name: auto_interface_0dim, sensor_id: 200, params: ["frequency_sweep_idx"], values: ["1"], value_types: [3]}' + ], output='screen', - shell = True ) - angular_separation_service = ExecuteProcess( - cmd = [[ - 'ros2 service call ', - '/smart_radar/set_radar_mode ', - 'umrr_ros2_msgs/srv/SetMode ', - '"{param: "angular_separation", value: 1, sensor_id: 100}"' - ]], - output='screen', - shell = True + set_angular_separation_service = ExecuteProcess( + cmd=[ + 'ros2', 'service', 'call', + '/smart_radar/set_radar_mode', + 'umrr_ros2_msgs/srv/SetMode', + '{section_name: auto_interface_0dim, sensor_id: 100, params: ["angular_separation"], values: ["1"], value_types: [3]}' + ], + output='screen' ) - range_toggle_mode_service = ExecuteProcess( - cmd = [[ - 'ros2 service call ', - '/smart_radar/set_radar_mode ', - 'umrr_ros2_msgs/srv/SetMode ', - '"{param: "range_toggle_mode", value: 4, sensor_id: 300}"' - ]], + + get_range_toggle_mode_service = ExecuteProcess( + cmd = [ + 'ros2', 'service', 'call', + '/smart_radar/get_radar_mode', + 'umrr_ros2_msgs/srv/GetMode', + '{section_name: auto_interface_0dim, sensor_id: 300, params: ["range_toggle_mode"], param_types: [3]}' + ], output='screen', - shell = True ) - center_frequency_idx_service = ExecuteProcess( - cmd = [[ - 'ros2 service call ', - '/smart_radar/set_radar_mode ', - 'umrr_ros2_msgs/srv/SetMode ', - '"{param: "center_frequency_idx", value: 1, sensor_id: 400}"' - ]], + get_software_version_service = ExecuteProcess( + cmd = [ + 'ros2', 'service', 'call', + '/smart_radar/get_radar_status', + 'umrr_ros2_msgs/srv/GetStatus', + '{section_name: auto_interface, sensor_id: 400, statuses: ["sw_version_major", "sw_version_minor"], status_types: [1, 1]}' + ], output='screen', - shell = True ) return ( launch.LaunchDescription([ radar_node, - frequency_sweep_service, - angular_separation_service, - range_toggle_mode_service, - center_frequency_idx_service, + set_frequency_sweep_service, + set_angular_separation_service, + get_range_toggle_mode_service, + get_software_version_service, launch_testing.actions.ReadyToTest(), ]), { @@ -137,7 +134,7 @@ def data_rx_s3_callback(msg): sub_s1 = self.test_node.create_subscription( sensor_msgs.PointCloud2, - 'smart_radar/port_targets_0', + 'smart_radar/port_targets_3', data_rx_s1_callback, 10 ) diff --git a/umrr_ros2_msgs/CMakeLists.txt b/umrr_ros2_msgs/CMakeLists.txt index 69e4984..a21888d 100644 --- a/umrr_ros2_msgs/CMakeLists.txt +++ b/umrr_ros2_msgs/CMakeLists.txt @@ -23,6 +23,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetIp.srv" "srv/SendCommand.srv" "srv/FirmwareDownload.srv" + "srv/GetStatus.srv" + "srv/GetMode.srv" "msg/CanTargetHeader.msg" "msg/PortTargetHeader.msg" "msg/PortObjectHeader.msg" diff --git a/umrr_ros2_msgs/package.xml b/umrr_ros2_msgs/package.xml index 3bc3fe0..2b6bf47 100644 --- a/umrr_ros2_msgs/package.xml +++ b/umrr_ros2_msgs/package.xml @@ -2,7 +2,7 @@ umrr_ros2_msgs - 2.0.0 + 3.0.0 A package for smartmicro radar messages shah Apache 2.0 diff --git a/umrr_ros2_msgs/srv/GetMode.srv b/umrr_ros2_msgs/srv/GetMode.srv new file mode 100644 index 0000000..bf0cd88 --- /dev/null +++ b/umrr_ros2_msgs/srv/GetMode.srv @@ -0,0 +1,6 @@ +string section_name # name of the interface section +uint32 sensor_id # id of the sensor to send request +string[] params # list of parameters or parameter to read +uint8[] param_types # 0: float32, 1: uint32, 2: uint16, 3: uint8 +--- +string res # response from the sensor diff --git a/umrr_ros2_msgs/srv/GetStatus.srv b/umrr_ros2_msgs/srv/GetStatus.srv new file mode 100644 index 0000000..5d495b3 --- /dev/null +++ b/umrr_ros2_msgs/srv/GetStatus.srv @@ -0,0 +1,6 @@ +string section_name # name of the interface section +uint32 sensor_id # id of the sensor to send request +string[] statuses # list of status or status to send +uint8[] status_types # 0: uint32, 1: uint16 +--- +string res # response from the sensor diff --git a/umrr_ros2_msgs/srv/SendCommand.srv b/umrr_ros2_msgs/srv/SendCommand.srv index 6e16b52..adb43cc 100644 --- a/umrr_ros2_msgs/srv/SendCommand.srv +++ b/umrr_ros2_msgs/srv/SendCommand.srv @@ -1,5 +1,6 @@ -string command -float32 value -uint32 sensor_id +string section_name # name of the interface section +string command # name of command to send +float32 value # value to send +uint32 sensor_id # id of the sensor to send request --- string res diff --git a/umrr_ros2_msgs/srv/SetMode.srv b/umrr_ros2_msgs/srv/SetMode.srv index 2b33c77..9f7549c 100644 --- a/umrr_ros2_msgs/srv/SetMode.srv +++ b/umrr_ros2_msgs/srv/SetMode.srv @@ -1,5 +1,7 @@ -string param -float32 value -uint32 sensor_id +string section_name # name of the interface section +uint32 sensor_id # id of the sensor to send request +string[] params # list of params or param to send +string[] values # list of values or value to send +uint8[] value_types # 0: float32, 1: uint32, 2: uint16, 3: uint8 --- -string res +string res # response from the sensor