Sensor Services (#1)

Provide ros2 services to change sensor modes and configure the sensor

.github/workflows/dockerbuild.yml

@@ -21,7 +21,7 @@ jobs:
       run:  docker build . -t umrr-ros:latest
 
     - name: Building the driver with the docker container
-      run:  docker run --rm -v`pwd`:/code umrr-ros colcon build --packages-select umrr_ros2_driver
+      run:  docker run --rm -v`pwd`:/code umrr-ros colcon build --packages-select umrr_ros2_driver umrr_ros2_msgs
 
     - name: Running the unit/integration tests via the docker container and exit
       run:  docker-compose up

.gitignore

@@ -5,6 +5,7 @@ log
 umrr_ros2_driver/__pycache__
 simulator/simulation/build_dir
 simulator/simulation/out
+umrr_ros2_driver/include/umrr_ros2_driver/sensor_params.hpp
 umrr_ros2_driver/smartmicro/*
 !umrr_ros2_driver/smartmicro/include
 umrr_ros2_driver/smartmicro/include/*

CHANGELOG.md

@@ -8,4 +8,9 @@ This is the first official versioned release of the `smartmicro_ros2_radars`. It
 ## v2.0.0 - 2022-05-05
 
 This major release of the driver includes multi-user interfaces. The driver now supports and publishes data from UMRR96 and UMRR11.
-A new test approach has been implemented which simulates the sensors and interfaces with the node hence making the tests more robust. 
+A new test approach has been implemented which simulates the sensors and interfaces with the node hence making the tests more robust.
+
+## v2.1.0 - 2022-06-02
+
+This minor release of the driver offer ros2 services to communicate with the sensor. The driver now supports mode changes for UMRR96 and UMRR11.
+This release also offers the possibilty of configuring the ip addresses of the sensor using ros2 services. The tests are further extended to inlcude ros2 services check.

Readme.md

@@ -26,7 +26,7 @@ A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96 radar, UMRR11
 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: `March 25, 2022`
+- Date of release: `June 02, 2022`
 - Smart Access Automotive version: `v1.1.0`
 - User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.1`
 - User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.1`
@@ -55,7 +55,7 @@ The driver publishes `sensor_msgs::msg::PointCloud2` messages with the radar tar
 
 ### Configuration:
 The node is configured through the parameters. Here is a short recap of the most important parts.
-For more details, see the [`radar.template.yaml`](param/radar.template.yaml) file.
+For more details, see the [`radar.template.yaml`](umrr_ros2_driver/param/radar.template.yaml) file.
 - `client_id`: the id of the client, must be a unique integer
 - `ip`: the IP of the used sensor or the source
 - `port`: port to be used to receive the packets
@@ -64,14 +64,51 @@ For more details, see the [`radar.template.yaml`](param/radar.template.yaml) fil
 - `history_size`: size of history for the message publisher
 - `model`: the model of the sensor being used 
 
+## Mode of operations of the sensors
+The smartmicro radars come equipped with numerous features and modes of operation. Using the ros2 services provided one
+may access these modes. A list of available sensor modes is given in the [`sensor_params.json`](umrr_ros2_driver/config/sensor_params.json).
+
+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) 
+- `sensor_id`: the id of the sensor to which the service call should be sent.
+
+For instance, changing the `Index of Transmit Antenna (tx_antenna_idx)` of a UMRR-11 sensor to `AEB (2)` mode would require the following call:
+`ros2 service call /smartmicro_radar_node/set_radar_mode umrr_ros2_msgs/srv/SetMode "{param: "tx_antenna_idx", value: 2, sensor_id: 100}"`
+
 ## Configuration of the sensors
 In order to use multiple sensors (maximum of up to ten sensors) with the node the sensors should be configured separately.
 The IP addresses of the sensors could be assigned using:
 - The smartmicro tool `DriveRecorder`.
 - Using the `Smart Access C++ API`
+- Using `Sensor Services` provided by the node
 
 Each sensor has to be assigned a unique IP address!
 
+To use the ros2 `SetIp`service we require two inputs:
+- `value_ip`: the value of the ip address in decimal. For instance to set the IP to `192.168.11.64` its corresponding
+value in decimal `3232238400` should be used.
+- `sensor_id`: the sensor whose ip address is to be changed.
+
+The call for such a service would be as follows:
+`ros2 service call /smartmicro_radar_node/set_ip_address umrr_ros2_msgs/srv/SetIp "{value_ip: 3232238400, sensor_id: 100}"`
+
+Note: For successfull execution of this call it is important that the sensor is restarted, the ip address in the
+[`radar.template.yaml`](umrr_ros2_driver/param/radar.template.yaml) is updated and the driver is build again.
+
+## Sensor Service Responses
+
+The sensor services respond with certain value codes. The following is a lookup table for the possible responses:
+
+**Value**   |   **Description**
+--- | ---
+0   |    No instruction Response
+1   |    Instruction Response was processed successfully
+2   |    General error
+6   |    Invalid protection
+7   |    Value out of minimal bounds
+8   |    Value out of maximal bounds
+
 ## Development
 The dockerfile can be used to build and test the ros driver.
 
@@ -87,7 +124,7 @@ docker build . -t umrr-ros:latest
 
 Building the driver with the docker container
 ```bash
-docker run --rm -v`pwd`:/code umrr-ros colcon build --packages-select umrr_ros2_driver
+docker run --rm -v`pwd`:/code umrr-ros colcon build --packages-select umrr_ros2_driver umrr_ros2_msgs
 ```
 
 Running the unit and integration tests via the docker compose
@@ -105,7 +142,7 @@ Stop and remove docker containers and networks
 docker-compose down
 ```
 ## ARMv8 Support
-The Smart Access release which will be downloaded using the script also offers platform support for armv8. In order to build the driver on an armv8 machine, the `CmakeLists.txt` should be adopted.
+The Smart Access release which will be downloaded using the script also offers platform support for armv8. In order to build the driver on an armv8 machine, the [`CMakeLists.txt`](umrr_ros2_driver/CMakeLists.txt) should be adopted.
 Instead of using the default `lib-linux-x86_64_gcc_9` the user should plugin the `lib-linux-armv8-gcc_9` for armv8.
 
 ## Contribution

simulator/simulation/src/simulator.cpp

@@ -58,6 +58,16 @@ void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) {
       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() == 0) {
+      std::cout << "Instruction for mode change tx_antenna answered!"
+                << std::endl;
+      instruction->SetResponse(COM_INSTR_PORT_SUCCESS);
+    } else if (instruction->GetSectionId() == 2010 &&
+               instruction->GetId() == 2) {
+      std::cout << "Instruction for mode change sweep_idx answered!"
+                << std::endl;
+      instruction->SetResponse(COM_INSTR_PORT_SUCCESS);
     } else {
       std::cout << "Unknown instruction received from ROS driver!" << std::endl;
     }
@@ -110,7 +120,7 @@ int main(int argc, char *argv[]) {
     ifs.close();
     delete[] filebuffer;
 
-    if (std::chrono::steady_clock::now() - Start > std::chrono::seconds(30))
+    if (std::chrono::steady_clock::now() - Start > std::chrono::seconds(40))
       break;
   }
   return 0;

umrr_ros2_driver/CMakeLists.txt

@@ -26,7 +26,7 @@ if(NOT CMAKE_CXX_STANDARD)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-    add_compile_options(-Wall -Wextra -Wpedantic)
+    add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
 endif()
 
 find_package(ament_cmake_auto REQUIRED)
@@ -62,11 +62,15 @@ set(CONFIG_FOLDER_PATH "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config")
 set(CONFIG_FILE_PATH "${CONFIG_FOLDER_PATH}/smart_access_config.json")
 set(HW_INVENTORY_FILE_PATH "${CONFIG_FOLDER_PATH}/hw_inventory.json")
 set(ROUTING_TABLE_FILE_PATH "${CONFIG_FOLDER_PATH}/routing_table.json")
+set(SENSOR_PARAM_FILE_PATH "${PROJECT_SOURCE_DIR}/config/sensor_params.json")
 configure_file(
     "${PROJECT_SOURCE_DIR}/cmake/smart_access_config.json.in" "${CONFIG_FILE_PATH}" @ONLY)
 configure_file(
     "${PROJECT_SOURCE_DIR}/cmake/config_path.hpp.in"
     "${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/config_path.hpp" @ONLY)
+configure_file(
+    "${PROJECT_SOURCE_DIR}/cmake/sensor_params.hpp.in"
+    "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/sensor_params.hpp" @ONLY)
 
 ament_auto_add_library(smartmicro_radar_node SHARED "src/smartmicro_radar_node.cpp")
 

umrr_ros2_driver/cmake/sensor_params.hpp.in

@@ -0,0 +1,31 @@
+// Copyright 2021 Apex.AI, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef UMRR_ROS2_DRIVER__CMAKE_SENSOR_PARAM_PATH_H_
+#define UMRR_ROS2_DRIVER__CMAKE_SENSOR_PARAM_PATH_H_
+
+namespace smartmicro
+{
+namespace drivers
+{
+namespace radar
+{
+
+constexpr auto KSensorParamFilePath ="@SENSOR_PARAM_FILE_PATH@";
+
+}  // namespace radar
+}  // namespace drivers
+}  // namespace smartmicro
+
+#endif  // UMRR_ROS2_DRIVER__CMAKE_SENSOR_PARAM_PATH_H_

umrr_ros2_driver/config/sensor_params.json

@@ -0,0 +1,58 @@
+{
+    "parameters": [
+        {
+            "model": "umrr11/umrr96",
+            "name": "tx_antenna_idx",
+            "comment": "Index of Transmit Antenna (UMRR11 : 0 and 1 are ACC, 2 is AEB)",
+            "min": 0,
+            "max": 2,
+            "default": 2
+        },
+        {
+            "model": "umrr11/umrr96",
+            "name": "center_frequency_idx",
+            "type": "u8",
+            "comment": "Index of center frequency (UMRR11: 0..3 in ACC selectable. In AEB mode values 2 and 3 are similar to 1.)",
+            "min": 0,
+            "max": 3,
+            "default": 0
+        },
+        {   
+            "model": "umrr11/umrr96",
+            "name": "frequency_sweep_idx",
+            "comment": "Index of sweep (UMRR11: currently always 0 because only one sweep for every TX-antenna. UMRR96: 0=226MHz, 1=512MHz, 2=1536MHz)",
+            "min": 0,
+            "max": 2,
+            "default": 0
+        },
+        {   
+            "model": "umrr11",
+            "name": "enable_tx_ant_toggle",
+            "comment": "Automatic toggle of TX-Antenna 0->2->0 every radar cycle",
+            "min": 0,
+            "max": 1,
+            "default": 0
+        },
+        {   
+            "model": "umrr11",
+            "name": "angular_separation",
+            "comment": "0 = Disable Angular Separation, 1 = Enable Angular Separation",
+            "min": 0,
+            "max": 1,
+            "default": 0
+        },
+        {   
+            "model": "umrr96",
+            "name": "range_toggle_mode",
+            "comment": "Automatic toggle of range:0=off, 1=Short-Med, 2=Short-Long, 3=Med-Long",
+            "min": 0,
+            "max": 3,
+            "default": 0
+        },
+        {
+            "name": "ip_source_address",
+            "comment": "IP source address (32bit)",
+            "default": 3232238347
+        }
+    ]
+}