Bug fiix import patron.xacro and append packages realsense2
This commit is contained in:
Даниил Грабарь
@@ -0,0 +1,63 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package realsense2_ros_mqtt_bridge
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
4.57.4 (2025-11-02)
|
||||
-------------------
|
||||
* Update realsense2__ros_mqtt_bridge package.xml to .57.4
|
||||
* PR `#3441 <https://github.com/IntelRealSense/realsense-ros/issues/3441>`_ from remibettan/ros2-development: merging 4.57.3 to ros2-development
|
||||
* Merge tag '4.57.3' into ros2-development
|
||||
* Contributors: Remi Bettan
|
||||
|
||||
4.57.3 (2025-09-15)
|
||||
-------------------
|
||||
* PR `#3428 <https://github.com/realsenseai/realsense-ros/issues/3428>`_ from remibettan: fixing mqtt readme
|
||||
* PR `#3417 <https://github.com/realsenseai/realsense-ros/issues/3417>`_ from remibettan: Merging ros2 hkr to ros2 dev final
|
||||
* PR `#38 <https://github.com/realsenseai/realsense-ros/issues/38>`_ from PrasRsRos: Implement hw reset in mqtt and add test
|
||||
* PR `#37 <https://github.com/realsenseai/realsense-ros/issues/37>`_ from PrasRsRos: Hmc test update
|
||||
* PR `#36 <https://github.com/realsenseai/realsense-ros/issues/36>`_ from PrasRsRos: Add hmc command tests to mqtt
|
||||
* PR `#32 <https://github.com/realsenseai/realsense-ros/issues/32>`_ from SamerKhshiboun: Support HWM command as ROS2 service and in the ROS-MQTT bridge node
|
||||
* PR `#34 <https://github.com/realsenseai/realsense-ros/issues/34>`_ from PrasRsRos: mqtt tf testcase
|
||||
* PR `#30 <https://github.com/realsenseai/realsense-ros/issues/30>`_ from SamerKhshiboun: Use new apis of SIC and SP that works directly with JSON inputs/outputs
|
||||
* PR `#31 <https://github.com/realsenseai/realsense-ros/issues/31>`_ from SamerKhshiboun: Support TF lookup for ROS-MQTT bridge node
|
||||
* PR `#29 <https://github.com/realsenseai/realsense-ros/issues/29>`_ from PrasRsRos/mqtt_negative__tests
|
||||
* PR `#28 <https://github.com/realsenseai/realsense-ros/issues/28>`_ from SamerKhshiboun: Fix MQTT Demo and update values for and update TC consecutives
|
||||
* PR `#26 <https://github.com/realsenseai/realsense-ros/issues/26>`_ from PrasRsRos: More system tests for mqtt
|
||||
* PR `#24 <https://github.com/realsenseai/realsense-ros/issues/24>`_ from PrasRsRos: Test TC on two cameras in parallel
|
||||
* PR `#23 <https://github.com/realsenseai/realsense-ros/issues/23>`_ from PrasRsRos: Ros tc implementation
|
||||
* PR `#22 <https://github.com/realsenseai/realsense-ros/issues/22>`_ from PrasRsRos: Added device info test at the system level
|
||||
* PR `#17 <https://github.com/realsenseai/realsense-ros/issues/17>`_ from SamerKhshiboun: Change sucess response "true/false" to be boolean
|
||||
* PR `#19 <https://github.com/realsenseai/realsense-ros/issues/19>`_ from PrasRsRos: DeviceInfo and TC Mqtt tests
|
||||
* PR `#16 <https://github.com/realsenseai/realsense-ros/issues/16>`_ from PrasRsRos: RS ROS Mqtt bridge unit tests
|
||||
* PR `#15 <https://github.com/realsenseai/realsense-ros/issues/15>`_ from SamerKhshiboun: Support device info service in ROS-MQTT bridge
|
||||
* PR `#14 <https://github.com/realsenseai/realsense-ros/issues/14>`_ from SamerKhshiboun: Merge ros2-development into hkr
|
||||
* PR `#13 <https://github.com/realsenseai/realsense-ros/issues/13>`_ from SamerKhshiboun: Sٍupport set/get application config as ROS service and in ROS-MQTT bridge
|
||||
* PR `#12 <https://github.com/realsenseai/realsense-ros/issues/12>`_ from SamerKhshiboun: TC Support in ROS-MQTT Bridge Node
|
||||
* PR `#10 <https://github.com/realsenseai/realsense-ros/issues/10>`_ from SamerKhshiboun: Add ROS MQTT Bridge (Python) Node Into realsense-ros-private
|
||||
* Contributors: Nir Azkiel, PrasRsRos, Remi Bettan, Samer Khshiboun
|
||||
|
||||
* PR `#3428 <https://github.com/realsenseai/realsense-ros/issues/3428>`_ from remibettan: fixing mqtt readme
|
||||
* readme fixed
|
||||
* PR `#3417 <https://github.com/realsenseai/realsense-ros/issues/3417>`_ from remibettan: Merging ros2 hkr to ros2 dev final
|
||||
* PR `#38 <https://github.com/realsenseai/realsense-ros/issues/38>`_ from PrasRsRos: Implement hw reset in mqtt and add test
|
||||
* PR `#37 <https://github.com/realsenseai/realsense-ros/issues/37>`_ from PrasRsRos: Hmc test update
|
||||
* PR `#36 <https://github.com/realsenseai/realsense-ros/issues/36>`_ from PrasRsRos: Add hmc command tests to mqtt
|
||||
* PR `#32 <https://github.com/realsenseai/realsense-ros/issues/32>`_ from SamerKhshiboun: Support HWM command as ROS2 service and in the ROS-MQTT bridge node
|
||||
* PR `#34 <https://github.com/realsenseai/realsense-ros/issues/34>`_ from PrasRsRos: mqtt tf testcase
|
||||
* PR `#30 <https://github.com/realsenseai/realsense-ros/issues/30>`_ from SamerKhshiboun: Use new apis of SIC and SP that works directly with JSON inputs/outputs
|
||||
* PR `#31 <https://github.com/realsenseai/realsense-ros/issues/31>`_ from SamerKhshiboun: Support TF lookup for ROS-MQTT bridge node
|
||||
* PR `#29 <https://github.com/realsenseai/realsense-ros/issues/29>`_ from PrasRsRos/mqtt_negative__tests
|
||||
* PR `#28 <https://github.com/realsenseai/realsense-ros/issues/28>`_ from SamerKhshiboun: Fix MQTT Demo and update values for and update TC consecutives failures threshold
|
||||
* PR `#26 <https://github.com/realsenseai/realsense-ros/issues/26>`_ from PrasRsRos: More system tests for mqtt
|
||||
* PR `#24 <https://github.com/realsenseai/realsense-ros/issues/24>`_ from PrasRsRos: Test TC on two cameras in parallel
|
||||
* PR `#23 <https://github.com/realsenseai/realsense-ros/issues/23>`_ from PrasRsRos: Ros tc implementation
|
||||
* PR `#22 <https://github.com/realsenseai/realsense-ros/issues/22>`_ from PrasRsRos: Added device info test at the system level
|
||||
* PR `#17 <https://github.com/realsenseai/realsense-ros/issues/17>`_ from SamerKhshiboun: Change sucess response "true/false" to be boolean
|
||||
* PR `#19 <https://github.com/realsenseai/realsense-ros/issues/19>`_ from PrasRsRos: DeviceInfo and TC Mqtt tests
|
||||
* PR `#16 <https://github.com/realsenseai/realsense-ros/issues/16>`_ from PrasRsRos: RS ROS Mqtt bridge unit tests
|
||||
* PR `#15 <https://github.com/realsenseai/realsense-ros/issues/15>`_ from SamerKhshiboun: Support device info service in ROS-MQTT bridge
|
||||
* PR `#14 <https://github.com/realsenseai/realsense-ros/issues/14>`_ from SamerKhshiboun: Merge ros2-development into hkr
|
||||
* PR `#13 <https://github.com/realsenseai/realsense-ros/issues/13>`_ from SamerKhshiboun: Sٍupport set/get application config as ROS service and in ROS-MQTT bridge
|
||||
* PR `#12 <https://github.com/realsenseai/realsense-ros/issues/12>`_ from SamerKhshiboun: TC Support in ROS-MQTT Bridge Node
|
||||
* PR `#10 <https://github.com/realsenseai/realsense-ros/issues/10>`_ from SamerKhshiboun: Add ROS MQTT Bridge (Python) Node Into realsense-ros-private
|
||||
* Contributors: Nir Azkiel, PrasRsRos, Remi Bettan, Samer Khshiboun
|
||||
@@ -0,0 +1,808 @@
|
||||
|
||||
<p align="center">
|
||||
<!-- Light mode -->
|
||||
<img src="../res/realsense-logo-light-mode.png#gh-light-mode-only" alt="Logo for light mode" width="70%"/>
|
||||
|
||||
<!-- Dark mode -->
|
||||
<img src="../res/realsense-logo-dark-mode.png#gh-dark-mode-only" alt="Logo for dark mode" width="70%"/>
|
||||
<br><br>
|
||||
</p>
|
||||
|
||||
|
||||
|
||||
<h1 align="center">
|
||||
MQTT <-> ROS bridge for Intel© RealSense™ Cameras<br>
|
||||
</h1>
|
||||
|
||||
|
||||
## Table of contents
|
||||
- [Installation](#installation)
|
||||
- [Starting the ros-mqtt-bridge node](#starting-the-ros-mqtt-bridge-node)
|
||||
- [ros2 run](#ros2-run)
|
||||
- [ros2 launch](#ros2-launch)
|
||||
- [Parameters](#parameters)
|
||||
- [Client Usage](#client-usage)
|
||||
- [Enumerate Devices](#enumerate-devices)
|
||||
- [Reset the device](#reset-the-device)
|
||||
- [Get Device Info](#get-device-info)
|
||||
- [Get Transformation](#get-transformation)
|
||||
- [Send HWM Command](#send-hwm-command)
|
||||
- [Get Parameter](#get-parameter)
|
||||
- [Set Parameter](#set-parameter)
|
||||
- [Get Frame](#get-frame)
|
||||
- [Get Safety Preset](#get-safety-preset)
|
||||
- [Set Safety Preset](#set-safety-preset)
|
||||
- [Get Safety Interface Config](#get-safety-interface-config)
|
||||
- [Set Safety Interface Config](#set-safety-interface-config)
|
||||
- [Get Calib Config](#get-calib-config)
|
||||
- [Set Calib Config](#set-calib-config)
|
||||
- [Get Safety Application Config](#get-application-config)
|
||||
- [Set Safety Application Config](#set-application-config)
|
||||
- [Triggered Calibration](#triggered-calibration)
|
||||
- [Supported Parameters For Set/Get](#supported-parameters-for-setget)
|
||||
- [Supported Streams](#supported-streams)
|
||||
- [Usage Example](#usage-example)
|
||||
|
||||
# Installation
|
||||
***This step assumes you have installed ROS environment and ROS Wrapper for Realsense Cameras (including RealSense SDK). For more info about these steps, click [here](https://github.com/realsenseai/realsense-ros/tree/ros2-development?tab=readme-ov-file)***
|
||||
|
||||
- Install paho-mqtt from https://pypi.org/project/paho-mqtt/2.1.0/
|
||||
```
|
||||
sudo pip3 install paho-mqtt==2.1.0
|
||||
```
|
||||
|
||||
- Create a ROS2 workspace
|
||||
```bash
|
||||
mkdir -p ~/ros2_ws/src
|
||||
cd ~/ros2_ws/src/
|
||||
```
|
||||
|
||||
- Build
|
||||
```bash
|
||||
colcon build
|
||||
```
|
||||
|
||||
- Source environment
|
||||
```bash
|
||||
ROS_DISTRO=<YOUR_SYSTEM_ROS_DISTRO> # set your ROS_DISTRO: iron, humble
|
||||
source /opt/ros/$ROS_DISTRO/setup.bash
|
||||
cd ~/ros2_ws
|
||||
./install/local_setup.bash
|
||||
```
|
||||
|
||||
# Starting the ros-mqtt-bridge node
|
||||
***this step assumes there is a running and configured MQTT broker, and at least one running realsense2_camera node***
|
||||
### ros2 run
|
||||
ros2 run realsense2_ros_mqtt_bridge realsense2_ros_mqtt_bridge
|
||||
# or, with parameters, for example
|
||||
ros2 run realsense2_ros_mqtt_bridge realsense2_ros_mqtt_bridge --ros-args -p broker_ip:='localhost'
|
||||
### ros2 launch
|
||||
ros2 launch realsense2_ros_mqtt_bridge rs_launch.py
|
||||
# or, with parameters, for example
|
||||
ros2 launch realsense2_ros_mqtt_bridge rs_launch.py broker_ip:='localhost'
|
||||
|
||||
# Parameters
|
||||
***All parameters can be configured or overriden by the `ros2 run` and `ros2 launch` commands (see examples in above section), or from the `rs_launch.py` file***
|
||||
- broker_ip
|
||||
- description: MQTT broker ip address
|
||||
- default value: 'localhost'
|
||||
- broker_port
|
||||
- description: MQTT port
|
||||
- default value: '1883'
|
||||
- log_level
|
||||
- description: log level [DEBUG|INFO|WARN|ERROR|FATAL]
|
||||
- default value: INFO
|
||||
|
||||
# Client Usage
|
||||
## Enumerate Devices
|
||||
* mqtt request message example
|
||||
```
|
||||
{
|
||||
"camera_namespace_prefix": "robot",
|
||||
"camera_names_prefix": "c_"
|
||||
}
|
||||
```
|
||||
* request topic
|
||||
```
|
||||
enumrete_devices_request
|
||||
```
|
||||
* response topic
|
||||
```
|
||||
enumerate_devices_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"success": True,
|
||||
"error_msg": "",
|
||||
"available_nodes_count": "1",
|
||||
"available_nodes":
|
||||
"[
|
||||
{camera_namespace: robot1, camera_name: c_333622320169}
|
||||
]"
|
||||
}
|
||||
```
|
||||
## Reset the Device
|
||||
* mqtt request message example
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
}
|
||||
```
|
||||
* request topic
|
||||
```
|
||||
send_hw_reset_request
|
||||
```
|
||||
* response topic
|
||||
```
|
||||
send_hw_reset_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "camera",
|
||||
"camera_name": "camera",
|
||||
"success": true,
|
||||
"error_msg": ""
|
||||
}
|
||||
```
|
||||
## Get Device Info
|
||||
* mqtt request message example
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
}
|
||||
```
|
||||
* request topic
|
||||
```
|
||||
get_device_info_request
|
||||
```
|
||||
* response topic
|
||||
```
|
||||
get_device_info_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "camera",
|
||||
"camera_name": "camera",
|
||||
"device_name": "intel_realsense_d585s",
|
||||
"serial_number": "333622320169",
|
||||
"firmware_version": "8.17.15566.148",
|
||||
"usb_type_descriptor": "3.2",
|
||||
"firmware_update_id": "333622320169",
|
||||
"sensors": "depth_module,rgb_camera,safety_camera,depth_mapping_camera,motion_module",
|
||||
"physical_port": "/sys/devices/pci0000:00/0000:00:14.0/usb2/2-1/2-1:1.0/video4linux/video0",
|
||||
}
|
||||
```
|
||||
|
||||
## Get Transformation
|
||||
* mqtt request message example
|
||||
```
|
||||
{
|
||||
"source": "c_353322320702_link",
|
||||
"destination": "c_353322320702_color_frame",
|
||||
}
|
||||
```
|
||||
* request topic
|
||||
```
|
||||
get_transformation_request
|
||||
```
|
||||
* response topic
|
||||
```
|
||||
get_transformation_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"rotation": {"x": -0.0022762807482196233, "y": -0.0011598517160371544, "z": -0.0011766824618274568, "w": 0.9999960443463445},
|
||||
"translation": {"x": -4.12423032685183e-05, "y": -0.04789295792579651, "z": 0.0005400447407737374},
|
||||
"success": true,
|
||||
"error_msg": ""
|
||||
}
|
||||
```
|
||||
|
||||
## Send HWM Command
|
||||
* mqtt request message example (read of safety interface config table)
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_353322320702",
|
||||
"opcode": 167, # opcode of GET_HKR_CONFIG_TABLE 0xA7
|
||||
"param1": 1, # read from flash (1)
|
||||
"param2": 49372, # table id (safety interface config) 0xC0DC
|
||||
"param3": 1, # 0 dynamic, 1 gold
|
||||
"param4": 0, # unused (ChunkId)
|
||||
"data": []
|
||||
}
|
||||
```
|
||||
* request topic
|
||||
```
|
||||
send_hwm_command_request
|
||||
```
|
||||
* response topic
|
||||
```
|
||||
send_hwm_command_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_353322320702",
|
||||
"success": true,
|
||||
"result": [167, 0, 0, 0, 0, 5, 220, 192, 148, 0, 0, 0, 0, 0, 0, 0, 250, 239, 161, 49, 0, 1, 1, 3, 1, 2, 0, 12, 0, 13, 0, 14, 0, 9, 0, 8, 0, 16, 0, 17, 0, 19, 0, 18, 0, 11, 1, 20, 0, 10, 0, 15, 0, 0, 150, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 128, 63, 0, 0, 128, 191, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 128, 191, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 113, 61, 138, 62, 20, 0, 12, 6, 4, 100, 0, 20, 100, 0, 20, 10, 15, 10, 95, 23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
|
||||
"error_msg": ""
|
||||
}
|
||||
```
|
||||
|
||||
## Get Parameter
|
||||
***See [available parameters and their types](#supported-parameters-for-setget)***
|
||||
* mqtt request message example
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"parameter_name": "rgb_camera.exposure"
|
||||
}
|
||||
```
|
||||
* request topic
|
||||
```
|
||||
get_param_request
|
||||
```
|
||||
* response topic
|
||||
```
|
||||
get_param_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"parameter_name": "rgb_camera.exposure",
|
||||
"success": True,
|
||||
"error_msg": "",
|
||||
"parameter_type": "integer",
|
||||
"parameter_value": "6012"
|
||||
}
|
||||
```
|
||||
|
||||
## Set Parameter
|
||||
***See [available parameters and their types](#supported-parameters-for-setget)***
|
||||
* mqtt request message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"parameter_name": "rgb_camera.exposure"
|
||||
"parameter_value": "6012",
|
||||
"parameter_type": "int"
|
||||
}
|
||||
```
|
||||
* request topic
|
||||
```
|
||||
set_param_request
|
||||
```
|
||||
* response topic
|
||||
```
|
||||
set_param_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"success": True,
|
||||
"error_msg": ""
|
||||
}
|
||||
```
|
||||
|
||||
## Get Frame
|
||||
***See [Supported Streams](#supported-streams)***
|
||||
* mqtt request message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"stream_name": "color"
|
||||
}
|
||||
```
|
||||
* request topic:
|
||||
```
|
||||
get_frame_request
|
||||
```
|
||||
* response topic:
|
||||
```
|
||||
get_frame_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"stream_name": "color"
|
||||
"success": True,
|
||||
"error_msg": "",
|
||||
"frame": "[0, 118, 124, 0, 0, ...., 255]" # array of bytes
|
||||
}
|
||||
```
|
||||
|
||||
## Get Safety Preset
|
||||
* mqtt request message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"index": "1"
|
||||
}
|
||||
```
|
||||
* request topic:
|
||||
```
|
||||
get_safety_preset_request
|
||||
```
|
||||
* response topic:
|
||||
```
|
||||
get_safety_preset_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"success": True,
|
||||
"error_msg": "",
|
||||
"safety_preset": "{safety preset as json}"
|
||||
}
|
||||
```
|
||||
## Set Safety Preset
|
||||
* mqtt request message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"safety_preset": "{safety preset as json}"
|
||||
"index": "1"
|
||||
}
|
||||
```
|
||||
* request topic:
|
||||
```
|
||||
set_safety_preset_request
|
||||
```
|
||||
* response topic:
|
||||
```
|
||||
set_safety_preset_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"success": True,
|
||||
"error_msg": "",
|
||||
}
|
||||
```
|
||||
|
||||
## Get Safety Interface Config
|
||||
* mqtt request message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
}
|
||||
```
|
||||
* request topic:
|
||||
```
|
||||
get_safety_interface_config_request
|
||||
```
|
||||
* response topic:
|
||||
```
|
||||
get_safety_interface_config_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"safety_inteface_config": "{safety interface config as JSON}",
|
||||
"success": True,
|
||||
"error_msg": "",
|
||||
}
|
||||
```
|
||||
|
||||
## Set Safety Interface Config
|
||||
* mqtt request message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"safety_inteface_config": "{safety interface config as JSON}",
|
||||
}
|
||||
```
|
||||
* request topic:
|
||||
```
|
||||
set_safety_interface_config_request
|
||||
```
|
||||
* response topic:
|
||||
```
|
||||
set_safety_interface_config_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"success": True,
|
||||
"error_msg": "",
|
||||
}
|
||||
```
|
||||
|
||||
## Get Calib Config
|
||||
* mqtt request message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
}
|
||||
```
|
||||
* request topic:
|
||||
```
|
||||
get_calib_config_request
|
||||
```
|
||||
* response topic:
|
||||
```
|
||||
get_calib_config_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"calib_config": "{calib config as JSON}",
|
||||
"success": True,
|
||||
"error_msg": "",
|
||||
}
|
||||
```
|
||||
|
||||
## Set Calib Config
|
||||
|
||||
* mqtt request message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"calib_config": "{calib config as JSON}"
|
||||
}
|
||||
```
|
||||
* request topic:
|
||||
```
|
||||
set_calib_config_request
|
||||
```
|
||||
* response topic:
|
||||
```
|
||||
set_calib_config_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"success": True,
|
||||
"error_msg": "",
|
||||
}
|
||||
```
|
||||
|
||||
## Get Application Config
|
||||
* mqtt request message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
}
|
||||
```
|
||||
* request topic:
|
||||
```
|
||||
get_application_config_request
|
||||
```
|
||||
* response topic:
|
||||
```
|
||||
get_application_config_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"application_config": "{application config as JSON}",
|
||||
"success": True,
|
||||
"error_msg": "",
|
||||
}
|
||||
```
|
||||
|
||||
## Set Application Config
|
||||
|
||||
* mqtt request message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"application_config": "{application config as JSON}"
|
||||
}
|
||||
```
|
||||
* request topic:
|
||||
```
|
||||
set_application_config_request
|
||||
```
|
||||
* response topic:
|
||||
```
|
||||
set_application_config_response
|
||||
```
|
||||
* mqtt response message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"success": True,
|
||||
"error_msg": "",
|
||||
}
|
||||
```
|
||||
|
||||
## Triggered Calibration
|
||||
|
||||
* Before calling triggered calibration, user should set the following parameters:
|
||||
* `safety_camera.safety_mode: 2` # switch to service mode
|
||||
* `depth_module.visual_preset: 1` # switch to visual preset #1 in depth module
|
||||
* `depth_module.emitter_enabled: true` # enable emitter in depth module
|
||||
* `depth_module.enable_auto_exposure: true` # enable AE in depth moudle
|
||||
* `enable_depth: false` # turn off depth stream
|
||||
* `enable_infra1: false` # turn off infra1 stream
|
||||
* `enable_infra2: false` # turn off infra2 stream
|
||||
* `enable_safety: false` # turn off safety stream
|
||||
* `enable_labeled_point_cloud: false` # turn off labeled pointcloud stream
|
||||
* `enable_occupancy: false` # turn off occupancy stream
|
||||
|
||||
* mqtt request message example:
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
}
|
||||
```
|
||||
* request topic:
|
||||
```
|
||||
triggered_calibration_request
|
||||
```
|
||||
* response topic:
|
||||
```
|
||||
triggered_calibration_response
|
||||
```
|
||||
* mqtt response message example: (user will get messages on the `triggered_calibration_response` on every progress update)
|
||||
```
|
||||
{
|
||||
"camera_namespace": "robot1",
|
||||
"camera_name": "c_333622320169",
|
||||
"success": True,
|
||||
"error_msg": "",
|
||||
"calibration": {calibration table as json}
|
||||
"health": 3.0
|
||||
"progress": 100.0 # [%0...%100]
|
||||
}
|
||||
```
|
||||
|
||||
# Supported Parameters For Set/Get
|
||||
|
||||
```
|
||||
accel_fps (type: integer)
|
||||
accel_info_qos (type: string)
|
||||
accel_qos (type: string)
|
||||
|
||||
align_depth.enable (type: boolean)
|
||||
align_depth.frames_queue_size (type: integer)
|
||||
|
||||
angular_velocity_cov (type: double)
|
||||
base_frame_id (type: string)
|
||||
camera_name (type: string)
|
||||
clip_distance (type: double)
|
||||
color_info_qos (type: string)
|
||||
color_qos (type: string)
|
||||
|
||||
colorizer.color_scheme (type: integer)
|
||||
colorizer.enable (type: boolean)
|
||||
colorizer.frames_queue_size (type: integer)
|
||||
colorizer.histogram_equalization_enabled (type: boolean)
|
||||
colorizer.max_distance (type: double)
|
||||
colorizer.min_distance (type: double)
|
||||
colorizer.stream_filter (type: integer)
|
||||
colorizer.stream_format_filter (type: integer)
|
||||
colorizer.stream_index_filter (type: integer)
|
||||
colorizer.visual_preset (type: integer)
|
||||
|
||||
decimation_filter.enable (type: boolean)
|
||||
decimation_filter.filter_magnitude (type: integer)
|
||||
decimation_filter.frames_queue_size (type: integer)
|
||||
decimation_filter.stream_filter (type: integer)
|
||||
decimation_filter.stream_format_filter (type: integer)
|
||||
decimation_filter.stream_index_filter (type: integer)
|
||||
|
||||
depth_info_qos (type: string)
|
||||
|
||||
depth_mapping_camera.frames_queue_size (type: integer)
|
||||
depth_mapping_camera.global_time_enabled (type: boolean)
|
||||
depth_mapping_camera.labeled_point_cloud_format (type: string)
|
||||
depth_mapping_camera.labeled_point_cloud_profile (type: string)
|
||||
depth_mapping_camera.occupancy_format (type: string)
|
||||
depth_mapping_camera.occupancy_profile (type: string)
|
||||
|
||||
depth_module.auto_exposure_roi.bottom (type: integer)
|
||||
depth_module.auto_exposure_roi.left (type: integer)
|
||||
depth_module.auto_exposure_roi.right (type: integer)
|
||||
depth_module.auto_exposure_roi.top (type: integer)
|
||||
depth_module.depth_format (type: string)
|
||||
depth_module.depth_profile (type: string)
|
||||
depth_module.emitter_always_on (type: boolean)
|
||||
depth_module.emitter_enabled (type: boolean)
|
||||
depth_module.enable_auto_exposure (type: boolean)
|
||||
depth_module.error_polling_enabled (type: boolean)
|
||||
depth_module.exposure (type: integer)
|
||||
depth_module.frames_queue_size (type: integer)
|
||||
depth_module.gain (type: integer)
|
||||
depth_module.global_time_enabled (type: boolean)
|
||||
depth_module.infra1_format (type: string)
|
||||
depth_module.infra2_format (type: string)
|
||||
depth_module.infra_profile (type: string)
|
||||
depth_module.inter_cam_sync_mode (type: integer)
|
||||
depth_module.laser_power (type: double)
|
||||
depth_module.visual_preset (type: integer)
|
||||
depth_qos (type: string)
|
||||
|
||||
device_type (type: string)
|
||||
diagnostics_period (type: double)
|
||||
disparity_filter.enable (type: boolean)
|
||||
disparity_to_depth.enable (type: boolean)
|
||||
|
||||
enable_accel (type: boolean)
|
||||
enable_color (type: boolean)
|
||||
enable_depth (type: boolean)
|
||||
enable_gyro (type: boolean)
|
||||
enable_infra1 (type: boolean)
|
||||
enable_infra2 (type: boolean)
|
||||
enable_labeled_point_cloud (type: boolean)
|
||||
enable_occupancy (type: boolean)
|
||||
enable_rgbd (type: boolean)
|
||||
enable_safety (type: boolean)
|
||||
enable_sync (type: boolean)
|
||||
|
||||
filter_by_sequence_id.enable (type: boolean)
|
||||
filter_by_sequence_id.frames_queue_size (type: integer)
|
||||
filter_by_sequence_id.sequence_id (type: integer)
|
||||
|
||||
gyro_fps (type: integer)
|
||||
gyro_info_qos (type: string)
|
||||
gyro_qos (type: string)
|
||||
|
||||
hdr_merge.enable (type: boolean)
|
||||
hdr_merge.frames_queue_size (type: integer)
|
||||
|
||||
hold_back_imu_for_frames (type: boolean)
|
||||
hole_filling_filter.enable (type: boolean)
|
||||
hole_filling_filter.frames_queue_size (type: integer)
|
||||
hole_filling_filter.holes_fill (type: integer)
|
||||
hole_filling_filter.stream_filter (type: integer)
|
||||
hole_filling_filter.stream_format_filter (type: integer)
|
||||
hole_filling_filter.stream_index_filter (type: integer)
|
||||
|
||||
infra1_info_qos (type: string)
|
||||
infra1_qos (type: string)
|
||||
infra2_info_qos (type: string)
|
||||
infra2_qos (type: string)
|
||||
|
||||
initial_reset (type: boolean)
|
||||
json_file_path (type: string)
|
||||
|
||||
labeled_point_cloud_info_qos (type: string)
|
||||
labeled_point_cloud_qos (type: string)
|
||||
|
||||
linear_accel_cov (type: double)
|
||||
|
||||
motion_module.enable_motion_correction (type: boolean)
|
||||
motion_module.frames_queue_size (type: integer)
|
||||
motion_module.global_time_enabled (type: boolean)
|
||||
|
||||
occupancy_info_qos (type: string)
|
||||
occupancy_qos (type: string)
|
||||
|
||||
pointcloud.allow_no_texture_points (type: boolean)
|
||||
pointcloud.enable (type: boolean)
|
||||
pointcloud.filter_magnitude (type: integer)
|
||||
pointcloud.frames_queue_size (type: integer)
|
||||
pointcloud.ordered_pc (type: boolean)
|
||||
pointcloud.pointcloud_qos (type: string)
|
||||
pointcloud.stream_filter (type: integer)
|
||||
pointcloud.stream_format_filter (type: integer)
|
||||
pointcloud.stream_index_filter (type: integer)
|
||||
|
||||
publish_tf (type: boolean)
|
||||
reconnect_timeout (type: double)
|
||||
|
||||
rgb_camera.auto_exposure_priority (type: boolean)
|
||||
rgb_camera.auto_exposure_roi.bottom (type: integer)
|
||||
rgb_camera.auto_exposure_roi.left (type: integer)
|
||||
rgb_camera.auto_exposure_roi.right (type: integer)
|
||||
rgb_camera.auto_exposure_roi.top (type: integer)
|
||||
rgb_camera.brightness (type: integer)
|
||||
rgb_camera.color_format (type: string)
|
||||
rgb_camera.color_profile (type: string)
|
||||
rgb_camera.contrast (type: integer)
|
||||
rgb_camera.enable_auto_exposure (type: boolean)
|
||||
rgb_camera.enable_auto_white_balance (type: boolean)
|
||||
rgb_camera.exposure (type: integer)
|
||||
rgb_camera.frames_queue_size (type: integer)
|
||||
rgb_camera.gain (type: integer)
|
||||
rgb_camera.gamma (type: integer)
|
||||
rgb_camera.global_time_enabled (type: boolean)
|
||||
rgb_camera.hue (type: integer)
|
||||
rgb_camera.power_line_frequency (type: integer)
|
||||
rgb_camera.saturation (type: integer)
|
||||
rgb_camera.sharpness (type: integer)
|
||||
rgb_camera.white_balance (type: double)
|
||||
|
||||
rosbag_filename (type: string)
|
||||
|
||||
safety_camera.frames_queue_size (type: integer)
|
||||
safety_camera.global_time_enabled (type: boolean)
|
||||
safety_camera.safety_format (type: string)
|
||||
safety_camera.safety_mode (type: integer)
|
||||
safety_camera.safety_preset_active_index (type: integer)
|
||||
safety_camera.safety_profile (type: string)
|
||||
safety_info_qos (type: string)
|
||||
safety_qos (type: string)
|
||||
|
||||
serial_no (type: string)
|
||||
|
||||
spatial_filter.enable (type: boolean)
|
||||
spatial_filter.filter_magnitude (type: integer)
|
||||
spatial_filter.filter_smooth_alpha (type: double)
|
||||
spatial_filter.filter_smooth_delta (type: integer)
|
||||
spatial_filter.frames_queue_size (type: integer)
|
||||
spatial_filter.holes_fill (type: integer)
|
||||
spatial_filter.stream_filter (type: integer)
|
||||
spatial_filter.stream_format_filter (type: integer)
|
||||
spatial_filter.stream_index_filter (type: integer)
|
||||
|
||||
temporal_filter.enable (type: boolean)
|
||||
temporal_filter.filter_smooth_alpha (type: double)
|
||||
temporal_filter.filter_smooth_delta (type: integer)
|
||||
temporal_filter.frames_queue_size (type: integer)
|
||||
temporal_filter.holes_fill (type: integer)
|
||||
temporal_filter.stream_filter (type: integer)
|
||||
temporal_filter.stream_format_filter (type: integer)
|
||||
temporal_filter.stream_index_filter (type: integer)
|
||||
|
||||
tf_publish_rate (type: double)
|
||||
unite_imu_method (type: integer)
|
||||
usb_port_id (type: string)
|
||||
use_sim_time (type: boolean)
|
||||
wait_for_device_timeout (type: double)
|
||||
|
||||
```
|
||||
|
||||
# Supported Streams
|
||||
- color
|
||||
- depth
|
||||
- infra1 (Left IR)
|
||||
- infra2 (Right IR)
|
||||
|
||||
# Usage Example
|
||||
[Minimal Python MQTT Client Example](examples/minimal_mqtt_client.py)
|
||||
|
||||
|
||||
[jazzy-badge]: https://img.shields.io/badge/-JAZZY-orange?style=flat-square&logo=ros
|
||||
[jazzy]: https://docs.ros.org/en/jazzy/index.html
|
||||
[humble-badge]: https://img.shields.io/badge/-HUMBLE-orange?style=flat-square&logo=ros
|
||||
[humble]: https://docs.ros.org/en/humble/index.html
|
||||
[iron-badge]: https://img.shields.io/badge/-IRON-orange?style=flat-square&logo=ros
|
||||
[iron]: https://docs.ros.org/en/iron/index.html
|
||||
[ubuntu24-badge]: https://img.shields.io/badge/-UBUNTU%2024%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white
|
||||
[ubuntu24]: https://releases.ubuntu.com/noble/
|
||||
[ubuntu22-badge]: https://img.shields.io/badge/-UBUNTU%2022%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white
|
||||
[ubuntu22]: https://releases.ubuntu.com/jammy/
|
||||
@@ -0,0 +1,695 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""Main module for interacting with the MQTT client."""
|
||||
import json
|
||||
import random
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
from paho.mqtt import client as paho_mqtt_client
|
||||
|
||||
|
||||
class DemoMQTTClient:
|
||||
"""
|
||||
Class that acts as Demo MQTT Client.
|
||||
|
||||
This class provides methods for initializing the MQTT client, handling
|
||||
MQTT connections, publishing messages, and interacting with MQTT topics.
|
||||
|
||||
Attributes:
|
||||
mqtt_broker_ip: The IP address of the MQTT broker.
|
||||
mqtt_broker_port: The port number of the MQTT broker.
|
||||
mqtt_client: The MQTT client instance.
|
||||
"""
|
||||
|
||||
def __init__(self, mqtt_broker_ip, mqtt_broker_port):
|
||||
"""
|
||||
Initialize the Demo MQTT client.
|
||||
|
||||
Args:
|
||||
mqtt_broker_ip: The IP address of the MQTT broker.
|
||||
mqtt_broker_port: The port number of the MQTT broker.
|
||||
"""
|
||||
self.mqtt_broker_ip = mqtt_broker_ip
|
||||
self.mqtt_broker_port = mqtt_broker_port
|
||||
|
||||
# Generate a Client ID with a random user-id suffix.
|
||||
mqtt_client_id = f'mqtt-client-user-{random.randint(0, 1000)}'
|
||||
|
||||
self.mqtt_client = paho_mqtt_client.Client(
|
||||
paho_mqtt_client.CallbackAPIVersion.VERSION1, mqtt_client_id)
|
||||
# client.username_pw_set(username, password)
|
||||
self.mqtt_client.on_connect = self.on_connect
|
||||
self.mqtt_client.connect(mqtt_broker_ip, mqtt_broker_port)
|
||||
self.tc_done = False
|
||||
|
||||
def on_connect(self, client, userdata, flags, rc):
|
||||
"""
|
||||
Handle the MQTT connection event.
|
||||
|
||||
Args:
|
||||
client: The MQTT client instance.
|
||||
userdata: The user data associated with the connection.
|
||||
flags: The connection flags.
|
||||
rc: The result code of the connection attempt.
|
||||
"""
|
||||
del client, userdata, flags # delete unused params
|
||||
if rc == 0:
|
||||
print(f'Connected to MQTT Broker on'
|
||||
f' ip:{ self.mqtt_broker_ip} port:{self.mqtt_broker_port}')
|
||||
else:
|
||||
print(f'Could not Connect to MQTT Broker on'
|
||||
f' ip:{ self.mqtt_broker_ip} port:{self.mqtt_broker_port}'
|
||||
f' return_code: {rc}')
|
||||
|
||||
def publish(self, msg, topic):
|
||||
"""
|
||||
Publish a message to the specified MQTT topic.
|
||||
|
||||
Args:
|
||||
msg: The message to publish.
|
||||
topic: The MQTT topic to publish to.
|
||||
"""
|
||||
msg_count = 1
|
||||
while True:
|
||||
time.sleep(1)
|
||||
result = self.mqtt_client.publish(topic, msg, qos=2)
|
||||
# result: [0, 1]
|
||||
status = result[0]
|
||||
if status == 0:
|
||||
print(f'Send {msg} to topic {topic}')
|
||||
else:
|
||||
print(f'Failed to send message to topic {topic}')
|
||||
msg_count += 1
|
||||
if msg_count > 1:
|
||||
break
|
||||
|
||||
def on_message(self, client, userdata, msg):
|
||||
"""
|
||||
Handle the MQTT message event.
|
||||
|
||||
Args:
|
||||
client: The MQTT client instance.
|
||||
userdata: The user data associated with the message.
|
||||
msg: The received MQTT message.
|
||||
"""
|
||||
del client, userdata # delete unused params
|
||||
content = msg.payload.decode('utf-8')
|
||||
if msg.topic == 'get_frame_response':
|
||||
print('got image... \
|
||||
not printing it since its raw data is huge...')
|
||||
else:
|
||||
if msg.topic == 'triggered_calibration_response':
|
||||
tc_json = json.loads(content)
|
||||
if tc_json['progress'] == 100.0:
|
||||
self.tc_done = True
|
||||
print(f'Received {content} to topic {msg.topic}')
|
||||
self.locked = False;
|
||||
|
||||
def start_client(self):
|
||||
"""Start the MQTT client."""
|
||||
self.mqtt_client.loop_start()
|
||||
self.mqtt_client.subscribe('enumerate_devices_response')
|
||||
self.mqtt_client.subscribe('get_transformation_response')
|
||||
self.mqtt_client.subscribe('send_hw_reset_response')
|
||||
self.mqtt_client.subscribe('send_hwm_command_response')
|
||||
self.mqtt_client.subscribe('get_device_info_response')
|
||||
self.mqtt_client.subscribe('get_parameter_response')
|
||||
self.mqtt_client.subscribe('set_parameter_response')
|
||||
self.mqtt_client.subscribe('get_frame_response')
|
||||
self.mqtt_client.subscribe('get_safety_preset_response')
|
||||
self.mqtt_client.subscribe('set_safety_preset_response')
|
||||
self.mqtt_client.subscribe('get_safety_interface_config_response')
|
||||
self.mqtt_client.subscribe('set_safety_interface_config_response')
|
||||
self.mqtt_client.subscribe('get_calib_config_response')
|
||||
self.mqtt_client.subscribe('set_calib_config_response')
|
||||
self.mqtt_client.subscribe('get_application_config_response')
|
||||
self.mqtt_client.subscribe('set_application_config_response')
|
||||
self.mqtt_client.subscribe('triggered_calibration_response')
|
||||
|
||||
self.mqtt_client.on_message = self.on_message
|
||||
|
||||
def stop_client(self):
|
||||
"""Stop the MQTT client."""
|
||||
self.mqtt_client.loop_stop()
|
||||
|
||||
def enumerate_devices(self, camera_namespace_prefix, camera_name_prefix):
|
||||
"""
|
||||
Send a request to enumerate devices.
|
||||
|
||||
Args:
|
||||
camera_namespace_prefix: The prefix of the camera namespace.
|
||||
camera_name_prefix: The prefix of the camera name.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace_prefix': camera_namespace_prefix,
|
||||
'camera_name_prefix': camera_name_prefix
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.publish(j, 'enumerate_devices_request')
|
||||
|
||||
def get_transformation(self, source, destination):
|
||||
"""
|
||||
Send a request to find the ROS2 transformation from source frame to destination frame
|
||||
|
||||
Args:
|
||||
source: source frame id.
|
||||
destination: destination frame id
|
||||
"""
|
||||
request_dict = {
|
||||
'source': source,
|
||||
'destination': destination
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'get_transformation_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def send_hw_reset_request(self, camera_namespace, camera_name):
|
||||
"""
|
||||
Send a request to reset the device
|
||||
|
||||
Args:
|
||||
camera_namespace
|
||||
camera_name
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'send_hw_reset_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def send_hwm_command(self, camera_namespace, camera_name, opcode,
|
||||
param1 = 0, param2 = 0, param3 = 0, param4 = 0,
|
||||
data = []):
|
||||
"""
|
||||
Send a hwm command request
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'opcode' : opcode,
|
||||
'param1' : param1,
|
||||
'param2' : param2,
|
||||
'param3' : param3,
|
||||
'param4' : param4,
|
||||
'data': np.array(data).tolist()
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'send_hwm_command_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def get_device_info(self, camera_namespace, camera_name):
|
||||
"""
|
||||
Send a request to get device info.
|
||||
|
||||
Args:
|
||||
camera_namespace: The namespace of the camera.
|
||||
camera_name: The name of the camera.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'get_device_info_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def set_param(self, camera_namespace, camera_name,
|
||||
parameter_name, parameter_value, parameter_type):
|
||||
"""
|
||||
Send a request to set a parameter.
|
||||
|
||||
Args:
|
||||
camera_namespace: The namespace of the camera.
|
||||
camera_name: The name of the camera.
|
||||
parameter_name: The name of the parameter to set.
|
||||
parameter_value: The value to set for the parameter.
|
||||
parameter_type: The type of the parameter.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'parameter_name': parameter_name,
|
||||
'parameter_value': parameter_value,
|
||||
'parameter_type': parameter_type
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'set_param_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def get_param(self, camera_namespace, camera_name, parameter_name):
|
||||
"""
|
||||
Send a request to get a parameter.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
parameter_name (str): The name of the parameter to get.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'parameter_name': parameter_name,
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'get_param_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def get_frame(self, camera_namespace, camera_name, stream_name):
|
||||
"""
|
||||
Send a request to get a frame.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
stream_name (str): The name of the stream.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'stream_name': stream_name,
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'get_frame_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def get_safety_preset(self, camera_namespace, camera_name, index):
|
||||
"""
|
||||
Send a request to get a safety preset.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
index (int): The index of the safety preset.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'index': index,
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'get_safety_preset_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def set_safety_preset(self, camera_namespace, camera_name, sp, index):
|
||||
"""
|
||||
Send a request to set a safety preset.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
sp (str): The safety preset.
|
||||
index (int): The index of the safety preset.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'safety_preset': sp,
|
||||
'index': str(index),
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'set_safety_preset_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def get_safety_interface_config(self, camera_namespace, camera_name, index=2):
|
||||
"""
|
||||
Send a request to get a safety inteface config.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'calib_location': index
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'get_safety_interface_config_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def set_safety_interface_config(self, camera_namespace, camera_name, sic):
|
||||
"""
|
||||
Send a request to set a safety interface config.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
sic (str): The safety interface config.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'safety_interface_config': sic,
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'set_safety_interface_config_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def get_calib_config(self, camera_namespace, camera_name):
|
||||
"""
|
||||
Send a request to get a calib config.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'get_calib_config_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def set_calib_config(self, camera_namespace, camera_name, calib_config):
|
||||
"""
|
||||
Send a request to set a calib config.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
calib_config (str): The calib config.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'calib_config': calib_config,
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'set_calib_config_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def get_application_config(self, camera_namespace, camera_name):
|
||||
"""
|
||||
Send a request to get an application config.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'get_application_config_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
def set_application_config(self, camera_namespace, camera_name, application_config):
|
||||
"""
|
||||
Send a request to set an application config.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
application_config (str): The application config.
|
||||
"""
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'application_config': application_config,
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.locked = True
|
||||
self.publish(j, 'set_application_config_request')
|
||||
while self.locked:
|
||||
pass
|
||||
|
||||
|
||||
def triggered_calibration(self, camera_namespace, camera_name):
|
||||
"""
|
||||
Run triggered calibration action.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
"""
|
||||
self.tc_done = False
|
||||
request_dict = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'json': 'calib run'
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
self.publish(j, 'triggered_calibration_request')
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
MQTT_BROKER_IP = 'localhost'
|
||||
MQTT_BROKER_PORT = 1883
|
||||
|
||||
jsons_dir = '../../realsense2_camera/examples/d500_tables/'
|
||||
|
||||
demo_mqtt_client = DemoMQTTClient(MQTT_BROKER_IP, MQTT_BROKER_PORT)
|
||||
demo_mqtt_client.start_client()
|
||||
|
||||
CAMERA_NAMESPACE_PREFIX = 'robot'
|
||||
CAMERA_NAME_PREFIX = 'c_'
|
||||
|
||||
# enumerate devices
|
||||
demo_mqtt_client.enumerate_devices(CAMERA_NAMESPACE_PREFIX,
|
||||
CAMERA_NAME_PREFIX)
|
||||
|
||||
# choose specific camera
|
||||
CAMERA_NAMESPACE = 'robot1'
|
||||
CAMERA_NAME = 'c_353322320702'
|
||||
|
||||
# reset the device
|
||||
demo_mqtt_client.send_hw_reset_request(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME)
|
||||
#needs time to reset
|
||||
import time
|
||||
time.sleep(8)
|
||||
|
||||
# get device info
|
||||
demo_mqtt_client.get_device_info(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME)
|
||||
|
||||
# get ROS2 transformation from 'c_353322320702_link' to 'c_353322320702_color_frame'
|
||||
demo_mqtt_client.get_transformation('c_353322320702_link', 'c_353322320702_color_frame')
|
||||
|
||||
|
||||
# send raw HWM command like GVD
|
||||
demo_mqtt_client.send_hwm_command(camera_namespace= CAMERA_NAMESPACE,
|
||||
camera_name = CAMERA_NAME,
|
||||
opcode = 0x10)
|
||||
|
||||
# switch to service mode
|
||||
demo_mqtt_client.set_param(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'safety_camera.safety_mode',
|
||||
'2',
|
||||
'int')
|
||||
|
||||
demo_mqtt_client.set_param(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'rgb_camera.exposure',
|
||||
'6012',
|
||||
'int')
|
||||
|
||||
demo_mqtt_client.get_param(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'rgb_camera.exposure')
|
||||
|
||||
demo_mqtt_client.get_frame(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'color')
|
||||
|
||||
###################################################################
|
||||
################ SAFETY PRESET GET/SET EXAMPLE ####################
|
||||
|
||||
demo_mqtt_client.get_safety_preset(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
1)
|
||||
|
||||
safety_preset_file = open(jsons_dir + 'safety_preset_example.json',
|
||||
mode='r',
|
||||
encoding='utf-8')
|
||||
safety_preset_json = json.load(safety_preset_file)
|
||||
SP_ESCAPED = str(safety_preset_json).replace('"', '\"')
|
||||
SP_ESCAPED = str(safety_preset_json).replace("'", '\"')
|
||||
|
||||
demo_mqtt_client.set_safety_preset(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
SP_ESCAPED,
|
||||
61)
|
||||
|
||||
|
||||
###################################################################
|
||||
########### SAFETY INTERFACE CONFIG GET/SET EXAMPLE ###############
|
||||
|
||||
demo_mqtt_client.get_safety_interface_config(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME)
|
||||
|
||||
safety_interface_config_file = open(jsons_dir + 'safety_interface_config_example.json',
|
||||
mode='r',
|
||||
encoding='utf-8')
|
||||
|
||||
safety_interface_config_json = json.load(safety_interface_config_file)
|
||||
SIC_ESCAPED = str(safety_interface_config_json).replace('"', '\"')
|
||||
SIC_ESCAPED = str(safety_interface_config_json).replace("'", '\"')
|
||||
|
||||
demo_mqtt_client.set_safety_interface_config(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
SIC_ESCAPED)
|
||||
|
||||
|
||||
###################################################################
|
||||
############## CALIB CONFIG GET/SET EXAMPLE #######################
|
||||
|
||||
demo_mqtt_client.get_calib_config(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME)
|
||||
|
||||
calib_config_file = open(jsons_dir + 'calib_config_example.json',
|
||||
mode='r',
|
||||
encoding='utf-8')
|
||||
calib_config_json = json.load(calib_config_file)
|
||||
CALIB_CONFIG_ESCAPED = str(calib_config_json).replace('"', '\"')
|
||||
CALIB_CONFIG_ESCAPED = str(calib_config_json).replace("'", '\"')
|
||||
|
||||
demo_mqtt_client.set_calib_config(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
CALIB_CONFIG_ESCAPED)
|
||||
|
||||
|
||||
##################################################################
|
||||
########## APPLICATION CONFIG GET/SET EXAMPLE ####################
|
||||
|
||||
demo_mqtt_client.get_application_config(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME)
|
||||
|
||||
application_config_file = open(jsons_dir + 'application_config_example.json',
|
||||
mode='r',
|
||||
encoding='utf-8')
|
||||
application_config_json = json.load(application_config_file)
|
||||
APPLICATION_CONFIG_ESCAPED = str(application_config_json).replace('"', '\"')
|
||||
APPLICATION_CONFIG_ESCAPED = str(application_config_json).replace("'", '\"')
|
||||
|
||||
demo_mqtt_client.set_application_config(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
APPLICATION_CONFIG_ESCAPED)
|
||||
|
||||
|
||||
###################################################################
|
||||
############## TRIGGERED CALIBRATION EXAMPLE ######################
|
||||
|
||||
# setup params for triggered calibration
|
||||
|
||||
# we are already in service mode, no need to switch
|
||||
|
||||
# switch to visual preset #1
|
||||
demo_mqtt_client.set_param(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'depth_module.visual_preset',
|
||||
'1',
|
||||
'int')
|
||||
|
||||
# enable emitter
|
||||
demo_mqtt_client.set_param(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'depth_module.emitter_enabled',
|
||||
'true',
|
||||
'bool')
|
||||
|
||||
# enable auto exposure
|
||||
demo_mqtt_client.set_param(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'depth_module.enable_auto_exposure',
|
||||
'true',
|
||||
'bool')
|
||||
|
||||
# turn off depth streaming
|
||||
demo_mqtt_client.set_param(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'enable_depth',
|
||||
'false',
|
||||
'bool')
|
||||
|
||||
# turn off infra1 streaming
|
||||
demo_mqtt_client.set_param(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'enable_infra1',
|
||||
'false',
|
||||
'bool')
|
||||
|
||||
# turn off infra2 streaming
|
||||
demo_mqtt_client.set_param(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'enable_infra2',
|
||||
'false',
|
||||
'bool')
|
||||
|
||||
# turn off safety streaming
|
||||
demo_mqtt_client.set_param(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'enable_safety',
|
||||
'false',
|
||||
'bool')
|
||||
|
||||
# turn off occupancy streaming
|
||||
demo_mqtt_client.set_param(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'enable_occupancy',
|
||||
'false',
|
||||
'bool')
|
||||
|
||||
# turn off lpcl streaming
|
||||
demo_mqtt_client.set_param(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME,
|
||||
'enable_labeled_point_cloud',
|
||||
'false',
|
||||
'bool')
|
||||
|
||||
# call the triggered calibration method
|
||||
demo_mqtt_client.triggered_calibration(CAMERA_NAMESPACE,
|
||||
CAMERA_NAME)
|
||||
|
||||
# check if TC is done, otherwise sleep for 2 seconds
|
||||
while not demo_mqtt_client.tc_done:
|
||||
time.sleep(2)
|
||||
|
||||
###################################################################
|
||||
demo_mqtt_client.stop_client()
|
||||
print(f'MQTT example completed, stopping the client and exiting')
|
||||
@@ -0,0 +1,57 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
"""Launch realsense2_ros_mqtt_bridge node"""
|
||||
import os
|
||||
import yaml
|
||||
from launch import LaunchDescription
|
||||
import launch_ros.actions
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
|
||||
configurable_parameters = [
|
||||
{'name': 'broker_ip', 'default': 'localhost', 'description': 'MQTT broker ip'},
|
||||
{'name': 'broker_port', 'default': '1883', 'description': 'MQTT port'},
|
||||
{'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'},
|
||||
]
|
||||
|
||||
|
||||
|
||||
def declare_configurable_parameters(parameters):
|
||||
return [DeclareLaunchArgument(param['name'], default_value=param['default'],
|
||||
description=param['description']) for param in parameters]
|
||||
|
||||
def set_configurable_parameters(parameters):
|
||||
return dict([(param['name'], LaunchConfiguration(param['name'])) for param in parameters])
|
||||
|
||||
def yaml_to_dict(path_to_yaml):
|
||||
with open(path_to_yaml, "r") as f:
|
||||
return yaml.load(f, Loader=yaml.SafeLoader)
|
||||
|
||||
def launch_setup(context, params):
|
||||
return [
|
||||
launch_ros.actions.Node(
|
||||
package='realsense2_ros_mqtt_bridge',
|
||||
executable='realsense2_ros_mqtt_bridge',
|
||||
parameters=[params],
|
||||
output='screen',
|
||||
arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
|
||||
emulate_tty=True,
|
||||
)
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [
|
||||
OpaqueFunction(function=launch_setup, kwargs = {'params' : set_configurable_parameters(configurable_parameters)})
|
||||
])
|
||||
@@ -0,0 +1,19 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>realsense2_ros_mqtt_bridge</name>
|
||||
<version>4.57.7</version>
|
||||
<description>ROS-MQTT Brdige for realsense2_camera node</description>
|
||||
<maintainer email="rsswsdk@realsensecloud.onmicrosoft.com">LibRealSense ROS Team</maintainer>
|
||||
<maintainer email="nir.azkiel@realsenseai.com">Nir Azkiel</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
+126
@@ -0,0 +1,126 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
# This script makes sure all files with the following extensions - .h, .hpp, .cpp, .js, .py, .bat, .sh, .txt -
|
||||
# include the Apache license reference and Intel copyright, as it shown in this file header.
|
||||
# The script checks also that these files are using Unix line-endings and spaces as delmiters (instead of tabs).
|
||||
# It is recommended to run this script when adding new files to the project.
|
||||
# For more info, run ./pr_check.sh --help
|
||||
|
||||
set +e
|
||||
|
||||
sudo apt-get install dos2unix
|
||||
|
||||
ok=0
|
||||
fixed=0
|
||||
|
||||
license_file=$PWD/../LICENSE
|
||||
|
||||
year_format="[[:digit:]][[:digit:]][[:digit:]][[:digit:]]"
|
||||
|
||||
function check_folder {
|
||||
for filename in $(find $1 -type f \( -iname \*.cpp -o -iname \*.h -o -iname \*.hpp -o -iname \*.js -o -iname \*.bat -o -iname \*.sh -o -iname \*.txt -o -iname \*.py \)); do
|
||||
|
||||
# Skip files of 3rd-party libraries which already have their own licenses and copyrights
|
||||
if [[ "$filename" == *"pr_check.sh"* ]]; then
|
||||
continue;
|
||||
fi
|
||||
|
||||
if [[ $(grep -oP "Software License Agreement" $filename | wc -l) -ne 0 ]]; then
|
||||
echo "[WARNING] $filename contains 3rd-party license agreement"
|
||||
else
|
||||
if [[ ! $filename == *"usbhost"* ]]; then
|
||||
# Only check files that are not .gitignore-d
|
||||
if [[ $(git check-ignore $filename | wc -l) -eq 0 ]]; then
|
||||
if [[ $(grep -oP "Copyright $year_format RealSense, Inc. All Rights Reserved" $filename | wc -l) -eq 0 &&
|
||||
$(grep -oP "Copyright $year_format-$year_format RealSense, Inc. All Rights Reserved" $filename | wc -l) -eq 0 ||
|
||||
$(grep -oP "Licensed under the Apache License, Version 2.0" $filename | wc -l) -eq 0
|
||||
]]; then
|
||||
echo "[ERROR] $filename is missing the copyright/license notice"
|
||||
ok=$((ok+1))
|
||||
|
||||
if [[ $2 == *"fix"* ]]; then
|
||||
# take last 13 linse from LICENSE file, and put them at beginning of $filename
|
||||
if [[ $filename == *".h"* || $filename == *".hpp"* || $filename == *".cpp"* || $filename == *".js"* ]]; then
|
||||
license_str="$(tail -13 ${license_file} | sed -e 's/^ / /' | sed -e 's/^/\/\//')"
|
||||
fi
|
||||
if [[ $filename == *".txt"* || $filename == *".py"* ]]; then
|
||||
license_str="$(tail -13 ${license_file} | sed -e 's/^ / /' | sed -e 's/^/#/')"
|
||||
fi
|
||||
echo "Trying to auto-resolve...";
|
||||
ed -s $filename << END
|
||||
0i
|
||||
${license_str}
|
||||
|
||||
.
|
||||
w
|
||||
q
|
||||
END
|
||||
fixed=$((fixed+1))
|
||||
fi
|
||||
fi
|
||||
|
||||
if [[ $(grep -o -P '\t' $filename | wc -l) -ne 0 ]]; then
|
||||
echo "[ERROR] $filename has tabs (this project is using spaces as delimiters)"
|
||||
ok=$((ok+1))
|
||||
|
||||
if [[ $2 == *"fix"* ]]; then
|
||||
echo "Trying to auto-resolve...";
|
||||
sed -i.bak $'s/\t/ /g' $filename
|
||||
fixed=$((fixed+1))
|
||||
fi
|
||||
fi
|
||||
|
||||
if [[ $(file ${filename} | grep -o -P 'CRLF' | wc -l) -ne 0 ]]; then
|
||||
echo "[ERROR] $filename is using DOS line endings (this project is using Unix line-endings)"
|
||||
ok=$((ok+1))
|
||||
|
||||
if [[ $2 == *"fix"* ]]; then
|
||||
echo "Trying to auto-resolve...";
|
||||
dos2unix $filename
|
||||
fixed=$((fixed+1))
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
done
|
||||
}
|
||||
|
||||
if [[ $1 == *"help"* ]]; then
|
||||
echo Pull-Request Check tool
|
||||
echo "Usage: (run from repo scripts directory)"
|
||||
echo " ./pr_check.sh [--help] [--fix]"
|
||||
echo " --fix Try to auto-fix defects"
|
||||
exit 0
|
||||
fi
|
||||
|
||||
|
||||
cd ..
|
||||
check_folder . $1
|
||||
cd scripts
|
||||
|
||||
if [[ ${fixed} -ne 0 ]]; then
|
||||
echo "Re-running pr_check..."
|
||||
./pr_check.sh
|
||||
else
|
||||
if [[ ${ok} -ne 0 ]]; then
|
||||
echo Pull-Request check failed, please address ${ok} the errors reported above
|
||||
exit 1
|
||||
fi
|
||||
fi
|
||||
|
||||
exit 0
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/realsense2_ros_mqtt_bridge
|
||||
[install]
|
||||
install_scripts=$base/lib/realsense2_ros_mqtt_bridge
|
||||
@@ -0,0 +1,42 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
from glob import glob
|
||||
import os
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'realsense2_ros_mqtt_bridge'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='4.57.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
(os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='Samer Khshiboun',
|
||||
maintainer_email='samer.khshiboun@intel.com',
|
||||
description='MQTT <-> ROS brdige for realsense2_camera node',
|
||||
license='Apache-2.0',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'realsense2_ros_mqtt_bridge = src.entry_point:main'
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -0,0 +1,13 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
@@ -0,0 +1,95 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""docstring."""
|
||||
import json
|
||||
|
||||
from realsense2_camera_msgs.srv import ApplicationConfigRead
|
||||
from realsense2_camera_msgs.srv import ApplicationConfigWrite
|
||||
|
||||
from .service_handler import ServiceHandler
|
||||
|
||||
|
||||
class ApplicationConfigHandler(ServiceHandler):
|
||||
"""docstring."""
|
||||
|
||||
def __init__(self, mqtt_ros_node):
|
||||
"""docstring."""
|
||||
super().__init__(mqtt_ros_node)
|
||||
|
||||
def handle_get_application_config_request(self, mqtt_request):
|
||||
"""docstring."""
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_application_config_request \
|
||||
message received')
|
||||
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
service_name = f'/{camera_namespace}/{camera_name}/application_config_read'
|
||||
|
||||
ros_client_application_config_read = self.create_ros_client(ApplicationConfigRead, service_name)
|
||||
if not ros_client_application_config_read:
|
||||
return
|
||||
|
||||
ros_request = ApplicationConfigRead.Request()
|
||||
|
||||
future = ros_client_application_config_read.call_async(ros_request)
|
||||
self.wait_for_future(future)
|
||||
|
||||
ros_response = future.result()
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'application_config': ros_response.application_config,
|
||||
'success': ros_response.success,
|
||||
'error_msg': ros_response.error_message
|
||||
}
|
||||
self.mqtt_ros_node.mqtt_client.publish('get_application_config_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_application_config_response message sent')
|
||||
ros_client_application_config_read.destroy()
|
||||
|
||||
def handle_set_application_config_request(self, mqtt_request):
|
||||
"""docstring."""
|
||||
self.mqtt_ros_node.ROS_DEBUG(
|
||||
'set_application_config_request message received')
|
||||
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
application_config = mqtt_request['application_config']
|
||||
service_name = f'/{camera_namespace}/{camera_name}/application_config_write'
|
||||
|
||||
ros_client_application_config_write = self.create_ros_client(ApplicationConfigWrite, service_name)
|
||||
if not ros_client_application_config_write:
|
||||
return
|
||||
|
||||
ros_request = ApplicationConfigWrite.Request()
|
||||
ros_request.application_config = application_config
|
||||
|
||||
future = ros_client_application_config_write.call_async(ros_request)
|
||||
self.wait_for_future(future)
|
||||
|
||||
ros_response = future.result()
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'success': ros_response.success,
|
||||
'error_msg': ros_response.error_message
|
||||
}
|
||||
self.mqtt_ros_node.mqtt_client.publish('set_application_config_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('set_application_config_response message sent')
|
||||
ros_client_application_config_write.destroy()
|
||||
@@ -0,0 +1,95 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""docstring."""
|
||||
import json
|
||||
|
||||
from realsense2_camera_msgs.srv import CalibConfigRead
|
||||
from realsense2_camera_msgs.srv import CalibConfigWrite
|
||||
|
||||
from .service_handler import ServiceHandler
|
||||
|
||||
|
||||
class CalibConfigHandler(ServiceHandler):
|
||||
"""docstring."""
|
||||
|
||||
def __init__(self, mqtt_ros_node):
|
||||
"""docstring."""
|
||||
super().__init__(mqtt_ros_node)
|
||||
|
||||
def handle_get_calib_config_request(self, mqtt_request):
|
||||
"""docstring."""
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_calib_config_request \
|
||||
message received')
|
||||
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
service_name = f'/{camera_namespace}/{camera_name}/calib_config_read'
|
||||
|
||||
ros_client_calib_config_read = self.create_ros_client(CalibConfigRead, service_name)
|
||||
if not ros_client_calib_config_read:
|
||||
return
|
||||
|
||||
ros_request = CalibConfigRead.Request()
|
||||
|
||||
future = ros_client_calib_config_read.call_async(ros_request)
|
||||
self.wait_for_future(future)
|
||||
|
||||
ros_response = future.result()
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'calib_config': ros_response.calib_config,
|
||||
'success': ros_response.success,
|
||||
'error_msg': ros_response.error_message
|
||||
}
|
||||
self.mqtt_ros_node.mqtt_client.publish('get_calib_config_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_calib_config_response message sent')
|
||||
ros_client_calib_config_read.destroy()
|
||||
|
||||
def handle_set_calib_config_request(self, mqtt_request):
|
||||
"""docstring."""
|
||||
self.mqtt_ros_node.ROS_DEBUG(
|
||||
'set_calib_config_request message received')
|
||||
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
calib_config = mqtt_request['calib_config']
|
||||
service_name = f'/{camera_namespace}/{camera_name}/calib_config_write'
|
||||
|
||||
ros_client_calib_config_write = self.create_ros_client(CalibConfigWrite, service_name)
|
||||
if not ros_client_calib_config_write:
|
||||
return
|
||||
|
||||
ros_request = CalibConfigWrite.Request()
|
||||
ros_request.calib_config = calib_config
|
||||
|
||||
future = ros_client_calib_config_write.call_async(ros_request)
|
||||
self.wait_for_future(future)
|
||||
|
||||
ros_response = future.result()
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'success': ros_response.success,
|
||||
'error_msg': ros_response.error_message
|
||||
}
|
||||
self.mqtt_ros_node.mqtt_client.publish('set_calib_config_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('set_calib_config_response message sent')
|
||||
ros_client_calib_config_write.destroy()
|
||||
@@ -0,0 +1,137 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""DeviceHandler for handling device-related MQTT requests."""
|
||||
import json
|
||||
|
||||
from .service_handler import ServiceHandler
|
||||
from realsense2_camera_msgs.srv import DeviceInfo
|
||||
|
||||
class DeviceHandler(ServiceHandler):
|
||||
"""
|
||||
Handler for device-related MQTT requests.
|
||||
|
||||
This class processes MQTT requests related to enumerating devices and
|
||||
retrieves available nodes that match the specified namespace and name
|
||||
prefixes.
|
||||
"""
|
||||
|
||||
def __init__(self, mqtt_ros_node):
|
||||
"""
|
||||
Initialize the DeviceHandler.
|
||||
|
||||
Args:
|
||||
mqtt_ros_node: Instance of the MQTTBridgeNode.
|
||||
"""
|
||||
self.mqtt_ros_node = mqtt_ros_node
|
||||
|
||||
def get_available_nodes(self, camera_namespace_prefix, camera_name_prefix):
|
||||
"""
|
||||
Get available nodes that match the given namespace and name prefixes.
|
||||
|
||||
Args:
|
||||
camera_namespace_prefix (str): Prefix for the camera namespace.
|
||||
camera_name_prefix (str): Prefix for the camera name.
|
||||
|
||||
Returns:
|
||||
tuple: A tuple containing the count of available nodes and a string
|
||||
representation of the available nodes.
|
||||
"""
|
||||
available_nodes_count = 0
|
||||
available_nodes_str = ''
|
||||
available_nodes = self.mqtt_ros_node.get_node_names_and_namespaces()
|
||||
for node_name, node_namespace in available_nodes:
|
||||
if node_namespace[1:].startswith(camera_namespace_prefix) and\
|
||||
node_name.startswith(camera_name_prefix):
|
||||
available_nodes_count += 1
|
||||
available_nodes_str += '{camera_namespace: ' +\
|
||||
node_namespace[1:] + \
|
||||
', camera_name: ' + node_name + '}, '
|
||||
|
||||
if available_nodes_count:
|
||||
available_nodes_str = available_nodes_str[:-2]
|
||||
|
||||
return available_nodes_count, available_nodes_str
|
||||
|
||||
def handle_enumerate_devices_request(self, mqtt_request):
|
||||
"""
|
||||
Handle the enumerate devices MQTT request.
|
||||
|
||||
Args:
|
||||
mqtt_request (dict): The MQTT request message containing the
|
||||
camera namespace and name prefixes.
|
||||
"""
|
||||
self.mqtt_ros_node.ROS_DEBUG('enumerate_devices_request \
|
||||
message received')
|
||||
|
||||
camera_namespace_prefix = mqtt_request['camera_namespace_prefix']
|
||||
camera_name_prefix = mqtt_request['camera_name_prefix']
|
||||
|
||||
nodes_count, nodes_str = self.get_available_nodes(
|
||||
camera_namespace_prefix,
|
||||
camera_name_prefix)
|
||||
|
||||
mqtt_response = {
|
||||
'success': True,
|
||||
'error_msg': '',
|
||||
'available_nodes_count': str(nodes_count),
|
||||
'available_nodes': '[' + nodes_str + ']'
|
||||
}
|
||||
|
||||
self.mqtt_ros_node.mqtt_client.publish('enumerate_devices_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('enumerate_devices_response message sent')
|
||||
|
||||
def handle_get_device_info_request(self, mqtt_request):
|
||||
"""
|
||||
Handle the get device info MQTT request.
|
||||
|
||||
Args:
|
||||
mqtt_request (dict): The MQTT request message containing the
|
||||
camera namespace and camera name of the device.
|
||||
"""
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_device_info message received')
|
||||
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
|
||||
service_name = f'/{camera_namespace}/{camera_name}/device_info'
|
||||
|
||||
ros_client_get_device_info = self.create_ros_client(DeviceInfo, service_name)
|
||||
if not ros_client_get_device_info:
|
||||
return
|
||||
|
||||
ros_request = DeviceInfo.Request()
|
||||
|
||||
future = ros_client_get_device_info.call_async(ros_request)
|
||||
self.wait_for_future(future)
|
||||
|
||||
ros_response = future.result()
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'device_name' : ros_response.device_name,
|
||||
'serial_number': ros_response.serial_number,
|
||||
'firmware_version': ros_response.firmware_version,
|
||||
'usb_type_descriptor': ros_response.usb_type_descriptor,
|
||||
'firmware_update_id': ros_response.firmware_update_id,
|
||||
'sensors': ros_response.sensors,
|
||||
'physical_port': ros_response.physical_port
|
||||
}
|
||||
self.mqtt_ros_node.mqtt_client.publish('get_device_info_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_device_info_response message sent')
|
||||
ros_client_get_device_info.destroy()
|
||||
@@ -0,0 +1,44 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""Main module for running the MQTT <-> ROS bridge node."""
|
||||
import rclpy
|
||||
|
||||
from .mqtt_bridge_node import MQTTBridgeNode
|
||||
|
||||
|
||||
def main(args=None):
|
||||
"""
|
||||
Initialize and run the MQTTBridgeNode.
|
||||
|
||||
This function initializes the ROS2 system, creates an instance of the
|
||||
MQTTBridgeNode, spins the node, and shuts down the ROS2 system when the
|
||||
node is finished.
|
||||
|
||||
Args:
|
||||
args: Command-line arguments (default is None).
|
||||
"""
|
||||
try:
|
||||
rclpy.init(args=args)
|
||||
mqtt_bridge_node = MQTTBridgeNode()
|
||||
rclpy.spin(mqtt_bridge_node)
|
||||
mqtt_bridge_node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
except Exception as e:
|
||||
print(e)
|
||||
print('Retry launching the node.')
|
||||
exit(1)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,135 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""FrameHandler for handling frame-related MQTT requests."""
|
||||
import json
|
||||
from functools import partial
|
||||
|
||||
from rclpy.qos import QoSDurabilityPolicy
|
||||
from rclpy.qos import QoSHistoryPolicy
|
||||
from rclpy.qos import QoSProfile
|
||||
from rclpy.qos import QoSReliabilityPolicy
|
||||
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
|
||||
class FrameHandler:
|
||||
"""
|
||||
Handler for frame-related MQTT requests.
|
||||
|
||||
This class processes MQTT requests related to getting frames from a camera
|
||||
and retrieves the corresponding image frames.
|
||||
"""
|
||||
|
||||
def __init__(self, mqtt_ros_node):
|
||||
"""
|
||||
Initialize the FrameHandler.
|
||||
|
||||
Args:
|
||||
mqtt_ros_node: Instance of the MQTTBridgeNode.
|
||||
"""
|
||||
self.mqtt_ros_node = mqtt_ros_node
|
||||
self.image = None
|
||||
self.topic_handle = {}
|
||||
|
||||
# Create a QoS profile for sensor data
|
||||
self.sensor_data_qos_profile = QoSProfile(
|
||||
history=QoSHistoryPolicy.KEEP_LAST,
|
||||
depth=10,
|
||||
reliability=QoSReliabilityPolicy.BEST_EFFORT,
|
||||
durability=QoSDurabilityPolicy.VOLATILE)
|
||||
|
||||
def handle_get_frame_request(self, mqtt_request):
|
||||
"""
|
||||
Handle the get frame MQTT request.
|
||||
|
||||
Args:
|
||||
mqtt_request (dict): The MQTT request message containing the
|
||||
camera namespace, camera name, and stream name.
|
||||
"""
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_frame_request message received')
|
||||
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
stream_name = mqtt_request['stream_name']
|
||||
topic_name = f'{camera_namespace}/{camera_name}/{stream_name}'
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'stream_name': stream_name,
|
||||
'success': True,
|
||||
'error_msg': '',
|
||||
'frame': ''
|
||||
}
|
||||
|
||||
if stream_name not in ['color', 'depth', 'infra1', 'infra2']:
|
||||
error_msg = f'Unsupported stream type: {stream_name}'
|
||||
self.mqtt_ros_node.ROS_ERROR(error_msg)
|
||||
mqtt_response['success'] = False
|
||||
mqtt_response['error_msg'] = error_msg
|
||||
|
||||
self.mqtt_ros_node.mqtt_client.publish('get_frame_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=1)
|
||||
return
|
||||
|
||||
if stream_name == 'depth' or\
|
||||
stream_name == 'infra1' or\
|
||||
stream_name == 'infra2':
|
||||
topic_name += '/image_rect_raw'
|
||||
elif stream_name == 'color':
|
||||
topic_name += '/image_raw'
|
||||
else:
|
||||
self.mqtt_ros_node.ROS_ERROR('Unsupported stream')
|
||||
return
|
||||
|
||||
if topic_name in self.topic_handle and self.topic_handle[topic_name] is not None:
|
||||
error_msg = f'One get_frame request is already in progress for {stream_name}, rejecting'
|
||||
self.mqtt_ros_node.ROS_ERROR(error_msg)
|
||||
mqtt_response['success'] = False
|
||||
mqtt_response['error_msg'] = error_msg
|
||||
|
||||
self.mqtt_ros_node.mqtt_client.publish('get_frame_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=1)
|
||||
return
|
||||
|
||||
self.topic_handle[topic_name] = self.mqtt_ros_node.create_subscription(Image,
|
||||
topic_name,
|
||||
partial(self.image_callback,mqtt_response=mqtt_response,topic_name=topic_name),
|
||||
self.sensor_data_qos_profile)
|
||||
|
||||
|
||||
def image_callback(self, msg,mqtt_response,topic_name):
|
||||
"""
|
||||
Image subscription callback.
|
||||
|
||||
Args:
|
||||
msg (Image): The received image message.
|
||||
"""
|
||||
if self.topic_handle[topic_name] is None:
|
||||
self.mqtt_ros_node.ROS_INFO(f'Ignoring the frame received for stream '+ mqtt_response['stream_name'])
|
||||
return
|
||||
|
||||
status = self.mqtt_ros_node.destroy_subscription(self.topic_handle[topic_name])
|
||||
if status == False:
|
||||
self.mqtt_ros_node.ROS_ERROR("Failed to destroy subscription")
|
||||
self.topic_handle[topic_name] = None
|
||||
self.image = msg
|
||||
self.mqtt_ros_node.ROS_INFO(f'Frame received successfully for stream '+ mqtt_response['stream_name'])
|
||||
mqtt_response['frame'] = str(self.image)
|
||||
self.mqtt_ros_node.mqtt_client.publish('get_frame_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_frame_response message sent')
|
||||
@@ -0,0 +1,74 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""HwResetHandler for handling device-related MQTT requests."""
|
||||
import json
|
||||
import numpy as np
|
||||
|
||||
from .service_handler import ServiceHandler
|
||||
from std_srvs.srv import Empty
|
||||
|
||||
|
||||
class HwResetHandler(ServiceHandler):
|
||||
"""
|
||||
Handler for hardware reset MQTT request.
|
||||
|
||||
This class processes MQTT requests related to sending hardware reset request to the device.
|
||||
"""
|
||||
|
||||
def __init__(self, mqtt_ros_node):
|
||||
"""
|
||||
Initialize the HwResetHandler.
|
||||
|
||||
Args:
|
||||
mqtt_ros_node: Instance of the MQTTBridgeNode.
|
||||
"""
|
||||
self.mqtt_ros_node = mqtt_ros_node
|
||||
|
||||
def handle_hw_reset_send_request(self, mqtt_request):
|
||||
"""
|
||||
Handle the get device info MQTT request.
|
||||
|
||||
Args:
|
||||
mqtt_request (dict): The MQTT request message containing the
|
||||
camera namespace and camera name of the device and the hwm command to run
|
||||
"""
|
||||
self.mqtt_ros_node.ROS_DEBUG('send_hw_reset_request message received')
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
|
||||
service_name = f'/{camera_namespace}/{camera_name}/hw_reset'
|
||||
|
||||
ros_client_hw_reset_command_send = self.create_ros_client(Empty, service_name)
|
||||
if not ros_client_hw_reset_command_send:
|
||||
return
|
||||
|
||||
ros_request = Empty.Request()
|
||||
|
||||
future = ros_client_hw_reset_command_send.call_async(ros_request)
|
||||
self.wait_for_future(future)
|
||||
ros_response = future.result()
|
||||
print("Pras", str(ros_response))
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'success' : True,
|
||||
'error_msg': ""
|
||||
}
|
||||
|
||||
self.mqtt_ros_node.mqtt_client.publish('send_hw_reset_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('send_hw_reset_response message sent')
|
||||
ros_client_hw_reset_command_send.destroy()
|
||||
@@ -0,0 +1,80 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""HWMCommandHandler for handling device-related MQTT requests."""
|
||||
import json
|
||||
import numpy as np
|
||||
|
||||
from .service_handler import ServiceHandler
|
||||
from realsense2_camera_msgs.srv import HardwareMonitorCommandSend
|
||||
|
||||
class HWMCommandHandler(ServiceHandler):
|
||||
"""
|
||||
Handler for HWM command-related MQTT requests.
|
||||
|
||||
This class processes MQTT requests related to sending hardware monitor commands to the device.
|
||||
"""
|
||||
|
||||
def __init__(self, mqtt_ros_node):
|
||||
"""
|
||||
Initialize the HWMCommandHandler.
|
||||
|
||||
Args:
|
||||
mqtt_ros_node: Instance of the MQTTBridgeNode.
|
||||
"""
|
||||
self.mqtt_ros_node = mqtt_ros_node
|
||||
|
||||
def handle_hwm_command_send_request(self, mqtt_request):
|
||||
"""
|
||||
Handle the get device info MQTT request.
|
||||
|
||||
Args:
|
||||
mqtt_request (dict): The MQTT request message containing the
|
||||
camera namespace and camera name of the device and the hwm command to run
|
||||
"""
|
||||
self.mqtt_ros_node.ROS_DEBUG('send_hwm_command_request message received')
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
|
||||
service_name = f'/{camera_namespace}/{camera_name}/hardware_monitor_command_send'
|
||||
|
||||
ros_client_hwm_command_send = self.create_ros_client(HardwareMonitorCommandSend, service_name)
|
||||
if not ros_client_hwm_command_send:
|
||||
return
|
||||
|
||||
ros_request = HardwareMonitorCommandSend.Request()
|
||||
# ros_request
|
||||
ros_request.opcode = mqtt_request['opcode']
|
||||
ros_request.param1 = mqtt_request.get('param1', 0)
|
||||
ros_request.param2 = mqtt_request.get('param2', 0)
|
||||
ros_request.param3 = mqtt_request.get('param3', 0)
|
||||
ros_request.param4 = mqtt_request.get('param4', 0)
|
||||
ros_request.data = np.array(mqtt_request.get('data', [])).tolist()
|
||||
|
||||
future = ros_client_hwm_command_send.call_async(ros_request)
|
||||
self.wait_for_future(future)
|
||||
ros_response = future.result()
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'success' : ros_response.success,
|
||||
'result': np.array(ros_response.result).tolist(),
|
||||
'error_msg': ros_response.error_message
|
||||
}
|
||||
|
||||
self.mqtt_ros_node.mqtt_client.publish('send_hwm_command_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('hwm_command_send_response message sent')
|
||||
ros_client_hwm_command_send.destroy()
|
||||
@@ -0,0 +1,356 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""docstring."""
|
||||
import json
|
||||
import random
|
||||
|
||||
import paho.mqtt.client as mqtt
|
||||
|
||||
from rclpy.node import Node
|
||||
|
||||
from .device_handler import DeviceHandler
|
||||
from .frame_handler import FrameHandler
|
||||
from .parameter_handler import ParameterHandler
|
||||
from .safety_preset_handler import SafetyPresetHandler
|
||||
from .safety_interface_config_handler import SafetyInterfaceConfigHandler
|
||||
from .calib_config_handler import CalibConfigHandler
|
||||
from .application_config_handler import ApplicationConfigHandler
|
||||
from .hw_reset_handler import HwResetHandler
|
||||
from .hwm_command_handler import HWMCommandHandler
|
||||
from .triggered_calibration_handler import TriggeredCalibrationHandler
|
||||
from .transformation_handler import TransformationHandler
|
||||
|
||||
class MQTTBridgeNode(Node):
|
||||
"""
|
||||
MQTT <-> ROS brdige node.
|
||||
|
||||
MQTTBridgeNode is a ROS Node that acts as a MQTT <-> ROS brdige for
|
||||
RealSense cameras. The node listens to pre-defined MQTT Broker
|
||||
messages, translates them into a ROS ones, handling these messages against
|
||||
the realsense2_camera node and then sending the response back to the
|
||||
MQTT Broker
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
default_mqtt_broker_ip='localhost',
|
||||
default_mqtt_broker_port=1883):
|
||||
"""
|
||||
Initialize the MQTTBridgeNode.
|
||||
|
||||
Args:
|
||||
default_mqtt_broker_ip (str): Default MQTT broker IP address.
|
||||
default_mqtt_broker_port (int): Default MQTT broker port.
|
||||
"""
|
||||
super().__init__('realsense_ros_mqtt_bridge_node')
|
||||
self.setup_node_parameters(default_mqtt_broker_ip, default_mqtt_broker_port)
|
||||
self.setup_mqtt_topics()
|
||||
self.setup_mqtt_handlers()
|
||||
self.setup_mqtt_connection()
|
||||
|
||||
def setup_node_parameters(self, default_mqtt_broker_ip, default_mqtt_broker_port):
|
||||
"""
|
||||
Declare and set node parameters.
|
||||
|
||||
Args:
|
||||
default_mqtt_broker_ip (str): Default MQTT broker IP address.
|
||||
default_mqtt_port (int): Default MQTT broker port.
|
||||
"""
|
||||
try:
|
||||
self.declare_parameter('broker_ip', default_mqtt_broker_ip)
|
||||
self.broker_ip = str(self.get_parameter(
|
||||
'broker_ip').get_parameter_value().string_value)
|
||||
|
||||
self.declare_parameter('broker_port', default_mqtt_broker_port)
|
||||
self.broker_port = self.get_parameter(
|
||||
'broker_port').get_parameter_value().integer_value
|
||||
except Exception as e:
|
||||
print(f'An unexpected error occurred: {e}')
|
||||
|
||||
def setup_mqtt_connection(self):
|
||||
"""Create MQTT client and connect it to MQTT broker."""
|
||||
self.mqtt_client_id = (f'realsense_ros_mqtt_bridge_node-'
|
||||
f'{random.randint(0, 1000)}')
|
||||
self.mqtt_client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION1,
|
||||
self.mqtt_client_id)
|
||||
self.mqtt_client.on_connect = self.on_mqtt_connect
|
||||
self.mqtt_client.on_message = self.on_mqtt_message
|
||||
try:
|
||||
self.ROS_INFO(f'Trying to connect to MQTT broker at '
|
||||
f'ip:{self.broker_ip} port:{self.broker_port}')
|
||||
self.mqtt_client.connect(self.broker_ip, self.broker_port)
|
||||
self.mqtt_client.loop_start()
|
||||
self.ROS_INFO('Successfully connected to MQTT Broker')
|
||||
except Exception as e:
|
||||
self.ROS_ERROR(f'Cannot connect to Broker ip:\
|
||||
{self.broker_ip} port:{self.broker_port}')
|
||||
self.ROS_ERROR(f'An exception occurred: {e}')
|
||||
raise e
|
||||
|
||||
def setup_mqtt_topics(self):
|
||||
"""Define the topics our MQTTBridgeNode needs to listen to."""
|
||||
self.mqtt_requests_topics = [
|
||||
'enumerate_devices_request',
|
||||
'send_hw_reset_request',
|
||||
'send_hwm_command_request',
|
||||
'get_device_info_request',
|
||||
'get_transformation_request',
|
||||
'get_param_request',
|
||||
'set_param_request',
|
||||
'get_frame_request',
|
||||
'get_safety_preset_request',
|
||||
'set_safety_preset_request',
|
||||
'get_safety_interface_config_request',
|
||||
'set_safety_interface_config_request',
|
||||
'get_calib_config_request',
|
||||
'set_calib_config_request',
|
||||
'get_application_config_request',
|
||||
'set_application_config_request',
|
||||
'hwm_command_send_request',
|
||||
'triggered_calibration_request'
|
||||
]
|
||||
|
||||
def setup_mqtt_handlers(self):
|
||||
"""
|
||||
Create handlers for each supported MQTT request type.
|
||||
|
||||
e.g., device_handler will handle enumerate device MQTT requests,
|
||||
which arrive on the 'enumerate_devices_request' topic
|
||||
"""
|
||||
self.device_handler = DeviceHandler(self)
|
||||
self.transformation_handler = TransformationHandler(self)
|
||||
self.parameter_handler = ParameterHandler(self)
|
||||
self.frame_handler = FrameHandler(self)
|
||||
self.safety_preset_handler = SafetyPresetHandler(self)
|
||||
self.safety_interface_config_handler = SafetyInterfaceConfigHandler(self)
|
||||
self.calib_config_handler = CalibConfigHandler(self)
|
||||
self.application_config_handler = ApplicationConfigHandler(self)
|
||||
self.hw_reset_handler = HwResetHandler(self)
|
||||
self.hwm_command_handler = HWMCommandHandler(self)
|
||||
self.triggered_calibration_handler = TriggeredCalibrationHandler(self)
|
||||
|
||||
def ROS_INFO(self, msg): # pylint: disable=invalid-name
|
||||
"""
|
||||
Log an info message.
|
||||
|
||||
Args:
|
||||
msg (str): Message to log.
|
||||
"""
|
||||
self.get_logger().info(msg)
|
||||
|
||||
def ROS_DEBUG(self, msg): # pylint: disable=invalid-name
|
||||
"""
|
||||
Log a debug message.
|
||||
|
||||
Args:
|
||||
msg (str): Message to log.
|
||||
"""
|
||||
self.get_logger().debug(msg)
|
||||
|
||||
def ROS_WARN(self, msg): # pylint: disable=invalid-name
|
||||
"""
|
||||
Log a warning message.
|
||||
|
||||
Args:
|
||||
msg (str): Message to log.
|
||||
"""
|
||||
self.get_logger().warn(msg)
|
||||
|
||||
def ROS_ERROR(self, msg): # pylint: disable=invalid-name
|
||||
"""
|
||||
Log an error message.
|
||||
|
||||
Args:
|
||||
msg (str): Message to log.
|
||||
"""
|
||||
self.get_logger().error(msg)
|
||||
|
||||
def on_mqtt_connect(self, client, userdata, flags, rc):
|
||||
"""
|
||||
Callback for when the MQTT client connects to the broker.
|
||||
|
||||
Args:
|
||||
client: The MQTT client instance.
|
||||
userdata: User-defined data of any type that is
|
||||
passed to the callbacks.
|
||||
flags: Response flags sent by the broker.
|
||||
rc: The connection result.
|
||||
"""
|
||||
del client, userdata, flags, rc # delete unsed params
|
||||
self.ROS_DEBUG('on_mqtt_connect')
|
||||
self.ROS_INFO('Trying to subscribe to requested MQTT topics')
|
||||
for topic in self.mqtt_requests_topics:
|
||||
self.mqtt_client.subscribe(topic)
|
||||
self.ROS_INFO('Successfully subscribed to requested MQTT topics')
|
||||
self.ROS_INFO('Waiting for new incoming MQTT requets')
|
||||
|
||||
def on_mqtt_message(self, client, userdata, msg):
|
||||
"""
|
||||
Callback for when a PUBLISH message is received from the server.
|
||||
|
||||
Args:
|
||||
client: The MQTT client instance.
|
||||
userdata: User-defined data of any type that is passed
|
||||
to the callbacks.
|
||||
msg: An instance of MQTTMessage, which has members
|
||||
topic, payload, qos, retain.
|
||||
"""
|
||||
del client, userdata # delete unsed params
|
||||
decoded_msg = msg.payload.decode('utf-8')
|
||||
self.ROS_INFO(f'Received new MQTT request on topic:'
|
||||
f'{msg.topic} msg:{decoded_msg}')
|
||||
mqtt_request = json.loads(decoded_msg)
|
||||
err_msg = ''
|
||||
response_topic = ''
|
||||
if msg.topic == 'enumerate_devices_request':
|
||||
if 'camera_namespace_prefix' not in mqtt_request:
|
||||
err_msg = "camera_namespace_prefix not found in the mqtt request"
|
||||
elif 'camera_name_prefix' not in mqtt_request:
|
||||
err_msg = "camera_name_prefix not found in the mqtt request"
|
||||
elif msg.topic != 'get_transformation_request':
|
||||
if 'camera_namespace' not in mqtt_request:
|
||||
err_msg = "camera_namespace not found in the mqtt request"
|
||||
elif 'camera_name' not in mqtt_request:
|
||||
err_msg = "camera_name not found in the mqtt request"
|
||||
if msg.topic == 'get_param_request':
|
||||
response_topic = 'get_parameter_response'
|
||||
if msg.topic == 'set_param_request':
|
||||
response_topic = 'set_parameter_response'
|
||||
|
||||
if err_msg == '':
|
||||
if msg.topic == 'enumerate_devices_request':
|
||||
self.device_handler.handle_enumerate_devices_request(mqtt_request)
|
||||
elif msg.topic == 'get_transformation_request':
|
||||
if 'source' not in mqtt_request:
|
||||
err_msg = "source not found in the mqtt request"
|
||||
elif 'destination' not in mqtt_request:
|
||||
err_msg = "destination not found in the mqtt request"
|
||||
else:
|
||||
self.transformation_handler.handle_get_transformation_request(mqtt_request)
|
||||
elif msg.topic == 'send_hw_reset_request':
|
||||
self.hw_reset_handler.handle_hw_reset_send_request(mqtt_request)
|
||||
elif msg.topic == 'send_hwm_command_request':
|
||||
if 'opcode' not in mqtt_request:
|
||||
err_msg = "opcode not found in the mqtt request"
|
||||
else:
|
||||
self.hwm_command_handler.handle_hwm_command_send_request(mqtt_request)
|
||||
elif msg.topic == 'get_device_info_request':
|
||||
self.device_handler.handle_get_device_info_request(mqtt_request)
|
||||
elif msg.topic == 'get_param_request':
|
||||
if 'parameter_name' not in mqtt_request:
|
||||
err_msg = "parameter_name not found in the mqtt request"
|
||||
response_topic = 'get_parameter_response'
|
||||
else:
|
||||
self.parameter_handler.handle_get_param_request(mqtt_request)
|
||||
elif msg.topic == 'set_param_request':
|
||||
response_topic = 'set_parameter_response'
|
||||
if 'parameter_name' not in mqtt_request:
|
||||
err_msg = "parameter_name not found in the mqtt request"
|
||||
elif 'parameter_value' not in mqtt_request:
|
||||
err_msg = "parameter_value not found in the mqtt request"
|
||||
elif 'parameter_type' not in mqtt_request:
|
||||
err_msg = "parameter_type not found in the mqtt request"
|
||||
else:
|
||||
self.parameter_handler.handle_set_param_request(mqtt_request)
|
||||
elif msg.topic == 'get_frame_request':
|
||||
if 'stream_name' not in mqtt_request:
|
||||
err_msg = "stream_name not found in the mqtt request"
|
||||
else:
|
||||
self.frame_handler.handle_get_frame_request(mqtt_request)
|
||||
elif msg.topic == 'get_safety_preset_request':
|
||||
if 'index' not in mqtt_request:
|
||||
err_msg = "index not found in the mqtt request"
|
||||
else:
|
||||
self.safety_preset_handler.handle_get_safety_preset_request(
|
||||
mqtt_request)
|
||||
elif msg.topic == 'set_safety_preset_request':
|
||||
if 'index' not in mqtt_request:
|
||||
err_msg = "index not found in the mqtt request"
|
||||
elif 'safety_preset' not in mqtt_request:
|
||||
err_msg = "preset not found in the mqtt request"
|
||||
else:
|
||||
self.safety_preset_handler.handle_set_safety_preset_request(
|
||||
mqtt_request)
|
||||
elif msg.topic == 'get_safety_interface_config_request':
|
||||
if 'calib_location' not in mqtt_request:
|
||||
mqtt_request['calib_location'] = 2
|
||||
self.ROS_WARN('calib_location was not found in MQTT request, using the default value 2')
|
||||
self.safety_interface_config_handler.handle_get_safety_interface_config_request(
|
||||
mqtt_request)
|
||||
elif msg.topic == 'set_safety_interface_config_request':
|
||||
if 'safety_interface_config' not in mqtt_request:
|
||||
err_msg = "safety_interface_config not found in the mqtt request"
|
||||
else:
|
||||
self.safety_interface_config_handler.handle_set_safety_interface_config_request(
|
||||
mqtt_request)
|
||||
elif msg.topic == 'get_calib_config_request':
|
||||
self.calib_config_handler.handle_get_calib_config_request(
|
||||
mqtt_request)
|
||||
elif msg.topic == 'set_calib_config_request':
|
||||
if 'calib_config' not in mqtt_request:
|
||||
err_msg = "calib_config not found in the mqtt request"
|
||||
else:
|
||||
self.calib_config_handler.handle_set_calib_config_request(
|
||||
mqtt_request)
|
||||
elif msg.topic == 'get_application_config_request':
|
||||
self.application_config_handler.handle_get_application_config_request(
|
||||
mqtt_request)
|
||||
elif msg.topic == 'set_application_config_request':
|
||||
if 'application_config' not in mqtt_request:
|
||||
err_msg = "application_config not found in the mqtt request"
|
||||
else:
|
||||
self.application_config_handler.handle_set_application_config_request(
|
||||
mqtt_request)
|
||||
elif msg.topic == 'triggered_calibration_request':
|
||||
#default is 'calib run'
|
||||
if 'json' not in mqtt_request:
|
||||
mqtt_request['json'] = 'calib run'
|
||||
self.triggered_calibration_handler.handle_triggered_calibration_request(
|
||||
mqtt_request)
|
||||
else:
|
||||
err_msg = 'Unsupported MQTT Message'
|
||||
|
||||
if err_msg == '':
|
||||
self.ROS_INFO('MQTT request handling done')
|
||||
self.ROS_INFO('Waiting for new incoming MQTT requets')
|
||||
else:
|
||||
mqtt_response = mqtt_request
|
||||
mqtt_response['success'] = False
|
||||
mqtt_response['error_msg'] = err_msg
|
||||
if response_topic == '':
|
||||
response_topic = msg.topic.replace('request', 'response')
|
||||
self.mqtt_client.publish(response_topic,
|
||||
json.dumps(mqtt_response),
|
||||
qos=1)
|
||||
self.ROS_ERROR(err_msg + f' Response in {response_topic}:{mqtt_response}')
|
||||
|
||||
|
||||
|
||||
def wait_for_service(self, client, service_name, timeout=5.0):
|
||||
"""
|
||||
Wait for a ROS service to become available.
|
||||
|
||||
Args:
|
||||
client: The service client instance.
|
||||
service_name (str): The name of the service to wait for.
|
||||
timeout (float): Time to wait for the service to
|
||||
become available.
|
||||
|
||||
Returns:
|
||||
bool: True if the service is available, False otherwise.
|
||||
"""
|
||||
if not client.wait_for_service(timeout):
|
||||
self.ROS_ERROR(f'Service not available: {service_name}')
|
||||
return False
|
||||
self.ROS_DEBUG(f'Service available: {service_name}')
|
||||
return True
|
||||
@@ -0,0 +1,221 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""ParameterHandler for handling parameter-related MQTT requests."""
|
||||
import json
|
||||
|
||||
from rcl_interfaces.msg import Parameter
|
||||
from rcl_interfaces.msg import ParameterType
|
||||
from rcl_interfaces.msg import ParameterValue
|
||||
from rcl_interfaces.srv import GetParameters
|
||||
from rcl_interfaces.srv import SetParameters
|
||||
|
||||
from .service_handler import ServiceHandler
|
||||
|
||||
|
||||
class ParameterHandler(ServiceHandler):
|
||||
"""
|
||||
Handler for parameter-related MQTT requests.
|
||||
|
||||
This class processes MQTT requests related to setting and getting parameters
|
||||
for a camera, and interacts with ROS services to handle these requests.
|
||||
"""
|
||||
|
||||
def __init__(self, mqtt_ros_node):
|
||||
"""
|
||||
Initialize the ParameterHandler.
|
||||
|
||||
Args:
|
||||
mqtt_ros_node: Instance of the MQTTBridgeNode.
|
||||
"""
|
||||
self.mqtt_ros_node = mqtt_ros_node
|
||||
self.parameter_type_map = {
|
||||
'float': ParameterType.PARAMETER_DOUBLE,
|
||||
'int': ParameterType.PARAMETER_INTEGER,
|
||||
'integer': ParameterType.PARAMETER_INTEGER,
|
||||
'string': ParameterType.PARAMETER_STRING,
|
||||
'bool': ParameterType.PARAMETER_BOOL,
|
||||
'boolean': ParameterType.PARAMETER_BOOL
|
||||
}
|
||||
self.type_map = {
|
||||
ParameterType.PARAMETER_BOOL: 'bool',
|
||||
ParameterType.PARAMETER_INTEGER: 'integer',
|
||||
ParameterType.PARAMETER_DOUBLE: 'double',
|
||||
ParameterType.PARAMETER_STRING: 'string'
|
||||
}
|
||||
|
||||
def handle_set_param_request(self, mqtt_request):
|
||||
"""
|
||||
Handle the set parameter MQTT request.
|
||||
|
||||
Args:
|
||||
mqtt_request (dict): The MQTT request message containing the
|
||||
camera namespace, camera name, parameter name, parameter value,
|
||||
and parameter type.
|
||||
"""
|
||||
self.mqtt_ros_node.ROS_DEBUG('set_param_request message received')
|
||||
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
parameter_name = mqtt_request['parameter_name']
|
||||
parameter_value = mqtt_request['parameter_value']
|
||||
parameter_type = mqtt_request['parameter_type'].lower()
|
||||
service_name = f'/{camera_namespace}/{camera_name}/set_parameters'
|
||||
|
||||
ros_client_set_parameter = self.create_ros_client(SetParameters, service_name)
|
||||
if not ros_client_set_parameter:
|
||||
return
|
||||
|
||||
parameter_value_obj = self.create_parameter_value(parameter_type, parameter_value)
|
||||
if not parameter_value_obj:
|
||||
self.mqtt_ros_node.ROS_ERROR(f'unsupported parameter_type: {parameter_type}')
|
||||
return
|
||||
|
||||
set_parameter_request = SetParameters.Request()
|
||||
set_parameter_request.parameters.append(
|
||||
Parameter(name=parameter_name, value=parameter_value_obj))
|
||||
|
||||
future = ros_client_set_parameter.call_async(set_parameter_request)
|
||||
|
||||
self.wait_for_future(future)
|
||||
|
||||
mqtt_response = self.create_set_param_response(camera_namespace, camera_name, future.result())
|
||||
|
||||
self.mqtt_ros_node.mqtt_client.publish('set_parameter_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('set_param_response message sent')
|
||||
ros_client_set_parameter.destroy()
|
||||
|
||||
def handle_get_param_request(self, mqtt_request):
|
||||
"""
|
||||
Handle the get parameter MQTT request.
|
||||
|
||||
Args:
|
||||
mqtt_request (dict): The MQTT request message containing the
|
||||
camera namespace, camera name, and parameter name.
|
||||
"""
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_param_request message received')
|
||||
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
parameter_name = mqtt_request['parameter_name']
|
||||
service_name = f'/{camera_namespace}/{camera_name}/get_parameters'
|
||||
|
||||
ros_client_get_parameter = self.create_ros_client(GetParameters, service_name)
|
||||
if not ros_client_get_parameter:
|
||||
return
|
||||
|
||||
ros_request = GetParameters.Request()
|
||||
ros_request.names.append(parameter_name)
|
||||
|
||||
future = ros_client_get_parameter.call_async(ros_request)
|
||||
|
||||
self.wait_for_future(future)
|
||||
|
||||
mqtt_response = self.create_get_param_response(camera_namespace, camera_name, parameter_name, future.result())
|
||||
|
||||
self.mqtt_ros_node.mqtt_client.publish('get_parameter_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_parameter_response message sent')
|
||||
ros_client_get_parameter.destroy()
|
||||
|
||||
def create_parameter_value(self, parameter_type, parameter_value):
|
||||
"""
|
||||
Create a ParameterValue object based on the type and value.
|
||||
|
||||
Args:
|
||||
parameter_type (str): The type of the parameter.
|
||||
parameter_value: The value of the parameter.
|
||||
|
||||
Returns:
|
||||
A ParameterValue object.
|
||||
"""
|
||||
if parameter_type in self.parameter_type_map:
|
||||
p = ParameterValue(type=self.parameter_type_map[parameter_type])
|
||||
if parameter_type == 'float':
|
||||
p.double_value = float(parameter_value)
|
||||
elif parameter_type in ['int', 'integer']:
|
||||
p.integer_value = int(parameter_value)
|
||||
elif parameter_type == 'string':
|
||||
p.string_value = parameter_value
|
||||
elif parameter_type == 'bool':
|
||||
p.bool_value = parameter_value.lower() == 'true'
|
||||
return p
|
||||
return None
|
||||
|
||||
def create_set_param_response(self, camera_namespace, camera_name, ros_response):
|
||||
"""
|
||||
Create a response for the set parameter request.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
ros_response: The ROS response.
|
||||
|
||||
Returns:
|
||||
A dictionary representing the MQTT response.
|
||||
"""
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name
|
||||
}
|
||||
|
||||
result = ros_response.results[0]
|
||||
|
||||
if result.successful:
|
||||
self.mqtt_ros_node.ROS_DEBUG('Parameter set successfully')
|
||||
mqtt_response['success'] = True
|
||||
mqtt_response['error_msg'] = ''
|
||||
else:
|
||||
self.mqtt_ros_node.ROS_WARN('Failed to set parameter')
|
||||
mqtt_response['success'] = False
|
||||
mqtt_response['error_msg'] = result.reason
|
||||
|
||||
return mqtt_response
|
||||
|
||||
def create_get_param_response(self, camera_namespace, camera_name, parameter_name, ros_response):
|
||||
"""
|
||||
Create a response for the get parameter request.
|
||||
|
||||
Args:
|
||||
camera_namespace (str): The namespace of the camera.
|
||||
camera_name (str): The name of the camera.
|
||||
parameter_name (str): The name of the parameter.
|
||||
ros_response: The ROS response.
|
||||
|
||||
Returns:
|
||||
A dictionary representing the MQTT response.
|
||||
"""
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'parameter_name': parameter_name,
|
||||
'success': True,
|
||||
'error_msg': ''
|
||||
}
|
||||
|
||||
result = ros_response.values[0]
|
||||
result_type = self.type_map.get(result.type, 'unsupported')
|
||||
if result_type != 'unsupported':
|
||||
self.mqtt_ros_node.ROS_DEBUG('Parameter retrieved successfully')
|
||||
mqtt_response['parameter_type'] = result_type
|
||||
mqtt_response['parameter_value'] = str(
|
||||
getattr(result, f'{result_type}_value'))
|
||||
else:
|
||||
self.mqtt_ros_node.ROS_WARN('Failed to retrieve parameter')
|
||||
mqtt_response['success'] = False
|
||||
mqtt_response['error_msg'] = 'unsupported type'
|
||||
|
||||
return mqtt_response
|
||||
@@ -0,0 +1,94 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""docstring."""
|
||||
import json
|
||||
|
||||
from realsense2_camera_msgs.srv import SafetyInterfaceConfigRead
|
||||
from realsense2_camera_msgs.srv import SafetyInterfaceConfigWrite
|
||||
|
||||
from .service_handler import ServiceHandler
|
||||
|
||||
|
||||
class SafetyInterfaceConfigHandler(ServiceHandler):
|
||||
"""docstring."""
|
||||
|
||||
def __init__(self, mqtt_ros_node):
|
||||
"""docstring."""
|
||||
super().__init__(mqtt_ros_node)
|
||||
|
||||
def handle_get_safety_interface_config_request(self, mqtt_request):
|
||||
"""docstring."""
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_safety_interface_config_request \
|
||||
message received')
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
service_name = f'/{camera_namespace}/{camera_name}/safety_interface_config_read'
|
||||
|
||||
ros_client_safety_interface_config_read = self.create_ros_client(SafetyInterfaceConfigRead, service_name)
|
||||
if not ros_client_safety_interface_config_read:
|
||||
return
|
||||
|
||||
ros_request = SafetyInterfaceConfigRead.Request(calib_location=mqtt_request['calib_location'])
|
||||
|
||||
future = ros_client_safety_interface_config_read.call_async(ros_request)
|
||||
self.wait_for_future(future)
|
||||
|
||||
ros_response = future.result()
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'safety_interface_config': ros_response.safety_interface_config,
|
||||
'success': ros_response.success,
|
||||
'error_msg': ros_response.error_message
|
||||
}
|
||||
self.mqtt_ros_node.mqtt_client.publish('get_safety_interface_config_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_safety_interface_config_response message sent')
|
||||
ros_client_safety_interface_config_read.destroy()
|
||||
|
||||
def handle_set_safety_interface_config_request(self, mqtt_request):
|
||||
"""docstring."""
|
||||
self.mqtt_ros_node.ROS_DEBUG(
|
||||
'set_safety_interface_config_request message received')
|
||||
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
safety_interface_config = mqtt_request['safety_interface_config']
|
||||
service_name = f'/{camera_namespace}/{camera_name}/safety_interface_config_write'
|
||||
|
||||
ros_client_safety_interface_config_write = self.create_ros_client(SafetyInterfaceConfigWrite, service_name)
|
||||
if not ros_client_safety_interface_config_write:
|
||||
return
|
||||
|
||||
ros_request = SafetyInterfaceConfigWrite.Request()
|
||||
ros_request.safety_interface_config = safety_interface_config
|
||||
|
||||
future = ros_client_safety_interface_config_write.call_async(ros_request)
|
||||
self.wait_for_future(future)
|
||||
|
||||
ros_response = future.result()
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'success': ros_response.success,
|
||||
'error_msg': ros_response.error_message
|
||||
}
|
||||
self.mqtt_ros_node.mqtt_client.publish('set_safety_interface_config_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('set_safety_interface_config_response message sent')
|
||||
ros_client_safety_interface_config_write.destroy()
|
||||
@@ -0,0 +1,99 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""docstring."""
|
||||
import json
|
||||
|
||||
from realsense2_camera_msgs.srv import SafetyPresetRead
|
||||
from realsense2_camera_msgs.srv import SafetyPresetWrite
|
||||
|
||||
from .service_handler import ServiceHandler
|
||||
|
||||
|
||||
class SafetyPresetHandler(ServiceHandler):
|
||||
"""docstring."""
|
||||
|
||||
def __init__(self, mqtt_ros_node):
|
||||
"""docstring."""
|
||||
super().__init__(mqtt_ros_node)
|
||||
|
||||
def handle_get_safety_preset_request(self, mqtt_request):
|
||||
"""docstring."""
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_safety_preset_request \
|
||||
message received')
|
||||
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
index = mqtt_request['index']
|
||||
service_name = f'/{camera_namespace}/{camera_name}/safety_preset_read'
|
||||
|
||||
ros_client_safety_preset_read = self.create_ros_client(SafetyPresetRead, service_name)
|
||||
if not ros_client_safety_preset_read:
|
||||
return
|
||||
|
||||
ros_request = SafetyPresetRead.Request()
|
||||
ros_request.index = int(index)
|
||||
|
||||
future = ros_client_safety_preset_read.call_async(ros_request)
|
||||
self.wait_for_future(future)
|
||||
|
||||
ros_response = future.result()
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'safety_preset': ros_response.safety_preset,
|
||||
'success': ros_response.success,
|
||||
'error_msg': ros_response.error_message
|
||||
}
|
||||
self.mqtt_ros_node.mqtt_client.publish('get_safety_preset_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_safety_preset_response message sent')
|
||||
ros_client_safety_preset_read.destroy()
|
||||
|
||||
def handle_set_safety_preset_request(self, mqtt_request):
|
||||
"""docstring."""
|
||||
self.mqtt_ros_node.ROS_DEBUG(
|
||||
'set_safety_preset_request message received')
|
||||
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
index = mqtt_request['index']
|
||||
preset = mqtt_request['safety_preset']
|
||||
service_name = f'/{camera_namespace}/{camera_name}/safety_preset_write'
|
||||
|
||||
ros_client_safety_preset_write = self.create_ros_client(SafetyPresetWrite, service_name)
|
||||
if not ros_client_safety_preset_write:
|
||||
return
|
||||
|
||||
ros_request = SafetyPresetWrite.Request()
|
||||
ros_request.index = int(index)
|
||||
ros_request.safety_preset = preset
|
||||
|
||||
future = ros_client_safety_preset_write.call_async(ros_request)
|
||||
self.wait_for_future(future)
|
||||
|
||||
ros_response = future.result()
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'success': ros_response.success,
|
||||
'error_msg': ros_response.error_message
|
||||
}
|
||||
self.mqtt_ros_node.mqtt_client.publish('set_safety_preset_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('set_safety_preset_response message sent')
|
||||
ros_client_safety_preset_write.destroy()
|
||||
@@ -0,0 +1,64 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""Handler for managing ROS service clients and futures."""
|
||||
import time
|
||||
|
||||
|
||||
class ServiceHandler:
|
||||
"""
|
||||
Handler for managing ROS service clients and futures.
|
||||
|
||||
This class provides methods for creating ROS service clients and waiting
|
||||
for futures to complete.
|
||||
"""
|
||||
|
||||
def __init__(self, mqtt_ros_node):
|
||||
"""
|
||||
Initialize the ServiceHandler.
|
||||
|
||||
Args:
|
||||
mqtt_ros_node: Instance of the MQTTBridgeNode.
|
||||
"""
|
||||
self.mqtt_ros_node = mqtt_ros_node
|
||||
|
||||
def create_ros_client(self, srv_type, service_name):
|
||||
"""
|
||||
Create a ROS service client and wait for the service to be available.
|
||||
|
||||
Args:
|
||||
srv_type: The type of the ROS service.
|
||||
service_name: The name of the ROS service.
|
||||
|
||||
Returns:
|
||||
The ROS service client if available, otherwise None.
|
||||
"""
|
||||
try:
|
||||
ros_client = self.mqtt_ros_node.create_client(srv_type, service_name)
|
||||
if not self.mqtt_ros_node.wait_for_service(ros_client, service_name):
|
||||
return None
|
||||
return ros_client
|
||||
except Exception as e:
|
||||
self.mqtt_ros_node.ROS_ERROR(f'[create_ros_client] An exception occurred: {e}')
|
||||
return None
|
||||
|
||||
def wait_for_future(self, future):
|
||||
"""
|
||||
Wait for a future to complete.
|
||||
|
||||
Args:
|
||||
future: The future to wait for.
|
||||
"""
|
||||
while not future.done():
|
||||
time.sleep(0.5)
|
||||
self.mqtt_ros_node.ROS_DEBUG('Waiting for ROS service call to finish...')
|
||||
@@ -0,0 +1,84 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
"""Handler for managing ROS service clients and futures."""
|
||||
import tf2_ros
|
||||
from rclpy.time import Time
|
||||
import json
|
||||
|
||||
class TransformationHandler:
|
||||
"""
|
||||
Handler for managing ROS service clients and futures.
|
||||
|
||||
This class provides methods for creating ROS service clients and waiting
|
||||
for futures to complete.
|
||||
"""
|
||||
|
||||
def __init__(self, mqtt_ros_node):
|
||||
"""
|
||||
Initialize the ServiceHandler.
|
||||
|
||||
Args:
|
||||
mqtt_ros_node: Instance of the MQTTBridgeNode.
|
||||
"""
|
||||
self.mqtt_ros_node = mqtt_ros_node
|
||||
|
||||
# Initialize transform listener
|
||||
self.mqtt_ros_node.tf_buffer = tf2_ros.Buffer()
|
||||
self.tf_listener = tf2_ros.TransformListener(self.mqtt_ros_node.tf_buffer, self.mqtt_ros_node)
|
||||
|
||||
def handle_get_transformation_request(self, mqtt_request):
|
||||
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_transformation_request message received')
|
||||
source = mqtt_request['source']
|
||||
destination = mqtt_request['destination']
|
||||
|
||||
try:
|
||||
# get transform from 'base_link' to 'child_link'
|
||||
transform_stamped = self.mqtt_ros_node.tf_buffer.lookup_transform(source, destination, Time())
|
||||
translation_dict = {
|
||||
"x": transform_stamped.transform.translation.x,
|
||||
"y": transform_stamped.transform.translation.y,
|
||||
"z": transform_stamped.transform.translation.z
|
||||
}
|
||||
|
||||
rotation_dict = {
|
||||
"x": transform_stamped.transform.rotation.x,
|
||||
"y": transform_stamped.transform.rotation.y,
|
||||
"z": transform_stamped.transform.rotation.z,
|
||||
"w": transform_stamped.transform.rotation.w
|
||||
}
|
||||
|
||||
self.mqtt_ros_node.get_logger().debug(f'Transform found: {transform_stamped.transform}')
|
||||
mqtt_response = {
|
||||
'rotation': rotation_dict,
|
||||
'translation': translation_dict,
|
||||
'success': True,
|
||||
'error_msg': ''
|
||||
}
|
||||
|
||||
except Exception as e:
|
||||
self.mqtt_ros_node.get_logger().error(f'Could not get transform: {e}')
|
||||
mqtt_response = {
|
||||
'rotation': '{}',
|
||||
'translation': '{}',
|
||||
'success': False,
|
||||
'error_msg': f'Could not get transform: {e}'
|
||||
}
|
||||
|
||||
self.mqtt_ros_node.mqtt_client.publish('get_transformation_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('get_transformation_response message sent')
|
||||
|
||||
|
||||
@@ -0,0 +1,142 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
import json
|
||||
from functools import partial
|
||||
|
||||
from rclpy.action import ActionClient
|
||||
from realsense2_camera_msgs.action import TriggeredCalibration
|
||||
|
||||
|
||||
class TriggeredCalibrationHandler:
|
||||
"""Brdige For Triggered Calibration ROS Action."""
|
||||
|
||||
def __init__(self, mqtt_ros_node):
|
||||
self.mqtt_ros_node = mqtt_ros_node
|
||||
self.action_clients = {}
|
||||
self.goal_handles = {}
|
||||
|
||||
def get_action_client(self, camera_namespace, camera_name):
|
||||
client_name = f"{camera_namespace}_{camera_name}"
|
||||
if client_name in self.action_clients:
|
||||
return self.action_clients[client_name]
|
||||
return None
|
||||
|
||||
def set_action_client(self, camera_namespace, camera_name, client):
|
||||
client_name = f"{camera_namespace}_{camera_name}"
|
||||
self.action_clients[client_name] = client
|
||||
|
||||
#assumption is that there is only one goal handle per action client.
|
||||
def get_goal_handle(self, camera_namespace, camera_name):
|
||||
client_name = f"{camera_namespace}_{camera_name}"
|
||||
if client_name in self.goal_handles:
|
||||
return self.goal_handles[client_name]
|
||||
return None
|
||||
|
||||
def set_goal_handle(self, camera_namespace, camera_name, client):
|
||||
client_name = f"{camera_namespace}_{camera_name}"
|
||||
self.goal_handles[client_name] = client
|
||||
|
||||
def ros_action_send_goal(self, goal, namespace, name):
|
||||
goal_msg = TriggeredCalibration.Goal(json=goal)
|
||||
self.get_action_client(namespace,name).wait_for_server()
|
||||
self.send_goal_future = self.get_action_client(namespace,name).send_goal_async(goal_msg, feedback_callback=partial(self.ros_action_feedback_callback, namespace=namespace,name=name))
|
||||
self.send_goal_future.add_done_callback(partial(self.ros_action_goal_response_callback, namespace=namespace,name=name))
|
||||
|
||||
def ros_action_goal_response_callback(self, future, namespace,name):
|
||||
goal_handle = future.result()
|
||||
self.set_goal_handle(namespace,name, goal_handle)
|
||||
if not goal_handle.accepted:
|
||||
self.mqtt_ros_node.ROS_DEBUG('TriggeredCalibrationHandler: Goal rejected')
|
||||
return
|
||||
|
||||
self.mqtt_ros_node.ROS_DEBUG('TriggeredCalibrationHandler: Goal accepted')
|
||||
|
||||
self.get_result_future = goal_handle.get_result_async()
|
||||
self.get_result_future.add_done_callback(partial(self.ros_action_result_callback, namespace=namespace,name=name))
|
||||
|
||||
def ros_action_result_callback(self, future, namespace,name):
|
||||
result = future.result().result
|
||||
self.mqtt_ros_node.ROS_DEBUG('ros_action_result_callback result{result}')
|
||||
self.mqtt_ros_node.ROS_DEBUG('Success: {0}'.format(result.success))
|
||||
self.mqtt_ros_node.ROS_DEBUG('Error: {0}'.format(result.error_msg))
|
||||
self.mqtt_ros_node.ROS_DEBUG('Calibration: {0}'.format(result.calibration))
|
||||
self.mqtt_ros_node.ROS_DEBUG('Health: {0}'.format(result.health))
|
||||
self.mqtt_ros_node.ROS_DEBUG('Progress: 100.0')
|
||||
|
||||
mqtt_response = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'success': result.success,
|
||||
'error_msg': result.error_msg,
|
||||
'calibration': result.calibration,
|
||||
'health': result.health,
|
||||
'progress': 100.0
|
||||
}
|
||||
self.mqtt_ros_node.mqtt_client.publish('triggered_calibration_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('triggered_calibration_response message sent')
|
||||
self.get_action_client(namespace,name).destroy()
|
||||
self.set_action_client(namespace,name, None)
|
||||
self.set_goal_handle(namespace,name, None)
|
||||
|
||||
def ros_action_feedback_callback(self, feedback_msg, namespace,name):
|
||||
feedback = feedback_msg.feedback
|
||||
self.mqtt_ros_node.ROS_DEBUG('TriggeredCalibrationHandler: Received feedback: {0}'.format(feedback.progress))
|
||||
mqtt_response = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'success': False,
|
||||
'error_msg': '',
|
||||
'calibration': '{}',
|
||||
'health': '0',
|
||||
'progress': feedback.progress
|
||||
}
|
||||
self.mqtt_ros_node.mqtt_client.publish('triggered_calibration_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
self.mqtt_ros_node.ROS_DEBUG('triggered_calibration_response message sent')
|
||||
|
||||
def handle_triggered_calibration_request(self, mqtt_request):
|
||||
self.mqtt_ros_node.ROS_DEBUG('triggered_calibration_request \
|
||||
message received')
|
||||
camera_namespace = mqtt_request['camera_namespace']
|
||||
camera_name = mqtt_request['camera_name']
|
||||
if mqtt_request['json'] == 'calib abort':
|
||||
#received am abort request, check if there was a pending goal.
|
||||
goal_handle = self.get_goal_handle(mqtt_request['camera_namespace'],mqtt_request['camera_name'])
|
||||
if self.get_action_client(camera_namespace,camera_name) is None or goal_handle is None:
|
||||
self.mqtt_ros_node.ROS_DEBUG("No goal in progress, cancelled too early?")
|
||||
mqtt_response = {
|
||||
'camera_namespace': camera_namespace,
|
||||
'camera_name': camera_name,
|
||||
'success': True,
|
||||
'error_msg': "No goal in progress, cancelled too early?",
|
||||
'calibration': '{}',
|
||||
'health': '0',
|
||||
'progress': 100.0
|
||||
}
|
||||
self.mqtt_ros_node.mqtt_client.publish('triggered_calibration_response',
|
||||
json.dumps(mqtt_response),
|
||||
qos=2)
|
||||
else:
|
||||
#cancel the goal in progress
|
||||
self.mqtt_ros_node.ROS_DEBUG("cancelling the goal..")
|
||||
future = goal_handle.cancel_goal_async()
|
||||
else:
|
||||
#all the requests other abort request is passed to the ActionServer
|
||||
action_name = f'/{camera_namespace}/{camera_name}/triggered_calibration'
|
||||
|
||||
self.set_action_client(mqtt_request['camera_namespace'],mqtt_request['camera_name'],ActionClient(self.mqtt_ros_node, TriggeredCalibration, action_name))
|
||||
self.ros_action_send_goal(mqtt_request['json'], camera_namespace, camera_name)
|
||||
@@ -0,0 +1,23 @@
|
||||
# Running the unit tests using pytest
|
||||
|
||||
## In one terminal:
|
||||
|
||||
```
|
||||
ros2 run realsense2_ros_mqtt_bridge realsense2_ros_mqtt_bridge
|
||||
```
|
||||
## In another terminal:
|
||||
```
|
||||
pytest-3 -s --log-cli-level=DEBUG realsense2_ros_mqtt_bridge/test/unittests
|
||||
```
|
||||
|
||||
# Running the system tests using pytest
|
||||
|
||||
```
|
||||
pytest-3 -s --log-cli-level=DEBUG realsense2_ros_mqtt_bridge/test/systemtests
|
||||
```
|
||||
|
||||
Note: To use the log level, user may have to upgrade the pytest to the latest, with the following command
|
||||
|
||||
```
|
||||
python -m pip install -U pytest
|
||||
```
|
||||
+91
@@ -0,0 +1,91 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_n_mqtt_nodes import CameraNMqttNodes as RSCameraSimulator
|
||||
from camera_n_mqtt_nodes import launch_descr_with_parameters
|
||||
import camera_n_mqtt_nodes
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
import pytest
|
||||
import rclpy
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
test_params_align_depth_color_d585s = {
|
||||
'camera_name': 'D585S',
|
||||
'device_type': 'D585S',
|
||||
}
|
||||
|
||||
@pytest.mark.parametrize("launch_descr_with_parameters",[
|
||||
pytest.param(test_params_align_depth_color_d585s, marks=pytest.mark.d585s),
|
||||
]
|
||||
,indirect=True)
|
||||
@pytest.mark.launch(fixture=launch_descr_with_parameters)
|
||||
def test_system_abort_triggered_calibration(launch_descr_with_parameters):
|
||||
#initialization starts....
|
||||
try:
|
||||
rclpy.init()
|
||||
params = launch_descr_with_parameters[1]
|
||||
namespace = 'camera'
|
||||
name = params['camera_name']
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
camera.start()
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
sds.prepare_for_calibration(namespace, name)
|
||||
sds.send_triggered_calibration_request(namespace,
|
||||
name)
|
||||
while True:
|
||||
response = sds.receive_triggered_calibration_response()
|
||||
LOGGER.debug(f"Response: {response}")
|
||||
if response['progress'] > 25.0:
|
||||
sds.abort_triggered_calibration_request(namespace, name)
|
||||
break
|
||||
while True:
|
||||
response = sds.receive_triggered_calibration_response()
|
||||
LOGGER.info(f"Response: {response}")
|
||||
if response['success'] == True or response['error_msg'] != '':
|
||||
if response['success'] == False and response['error_msg'] == 'Canceled':
|
||||
assert response['calibration'] == '{}', "Could not abort the calibration"
|
||||
LOGGER.info('Calibration abort successful')
|
||||
else:
|
||||
assert False, 'Aborting of triggered calibration failed. Response:' +str(response)
|
||||
break
|
||||
#cleanup starts....
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.warning("Test failed")
|
||||
LOGGER.warning(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
finally:
|
||||
camera.stop()
|
||||
rclpy.shutdown()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,105 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
|
||||
|
||||
import rclpy
|
||||
|
||||
import pytest
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_n_mqtt_nodes import CameraNMqttNodes as RSCameraSimulator
|
||||
from camera_n_mqtt_nodes import launch_descr_with_parameters
|
||||
import camera_n_mqtt_nodes
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
test_params_d585s = {
|
||||
'camera_name': 'D585S',
|
||||
'device_type': 'D585S',
|
||||
}
|
||||
|
||||
#@pytest.mark.timeout(20)
|
||||
@pytest.mark.parametrize("launch_descr_with_parameters",[
|
||||
pytest.param(test_params_d585s, marks=pytest.mark.d585s),
|
||||
]
|
||||
,indirect=True)
|
||||
@pytest.mark.launch(fixture=launch_descr_with_parameters)
|
||||
def test_system_application_config(launch_descr_with_parameters):
|
||||
#initialization starts....
|
||||
rclpy.init()
|
||||
params = launch_descr_with_parameters[1]
|
||||
namespace = 'camera'
|
||||
name = params['camera_name']
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
camera.create_application_config_service()
|
||||
|
||||
sds.set_safety_mode(namespace, name, 2)
|
||||
|
||||
sds.send_get_application_config_request(namespace,
|
||||
name)
|
||||
|
||||
response = sds.receive_get_application_config_response()
|
||||
|
||||
original_data = response['application_config']
|
||||
import json
|
||||
Application_data = json.loads(original_data)
|
||||
if Application_data['application_config']['developer_mode']['hkr'] == 1:
|
||||
Application_data['application_config']['developer_mode']['hkr'] = 0
|
||||
else:
|
||||
Application_data['application_config']['developer_mode']['hkr'] = 1
|
||||
Application_data1 = json.dumps(Application_data)
|
||||
|
||||
sds.send_set_application_config_request(namespace,
|
||||
name,
|
||||
Application_data1)
|
||||
|
||||
response = sds.receive_set_application_config_response()
|
||||
|
||||
sds.send_get_application_config_request(namespace,
|
||||
name)
|
||||
|
||||
response = sds.receive_get_application_config_response()
|
||||
ac_read = json.loads(response["application_config"])
|
||||
LOGGER.debug("Application data written: ",Application_data)
|
||||
LOGGER.debug("Application data readback:",ac_read)
|
||||
assert ac_read == Application_data, "Written Application config is not matching with the read one"
|
||||
|
||||
sds.send_set_application_config_request(namespace,
|
||||
name,
|
||||
original_data)
|
||||
|
||||
response = sds.receive_set_application_config_response()
|
||||
#cleanup starts....
|
||||
camera.stop()
|
||||
rclpy.shutdown()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,101 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
import numpy as np
|
||||
import rclpy
|
||||
|
||||
import pytest
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_n_mqtt_nodes import CameraNMqttNodes as RSCameraSimulator
|
||||
from camera_n_mqtt_nodes import launch_descr_with_parameters
|
||||
import camera_n_mqtt_nodes
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
test_params_d585s = {
|
||||
'camera_name': 'D585S',
|
||||
'device_type': 'D585S',
|
||||
}
|
||||
|
||||
#@pytest.mark.timeout(20)
|
||||
@pytest.mark.parametrize("launch_descr_with_parameters",[
|
||||
pytest.param(test_params_d585s, marks=pytest.mark.d585s),
|
||||
]
|
||||
,indirect=True)
|
||||
@pytest.mark.launch(fixture=launch_descr_with_parameters)
|
||||
def test_system_calib_config(launch_descr_with_parameters):
|
||||
#initialization starts....
|
||||
rclpy.init()
|
||||
params = launch_descr_with_parameters[1]
|
||||
namespace = 'camera'
|
||||
name = params['camera_name']
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
sds.set_safety_mode(namespace, name, 2)
|
||||
|
||||
sds.send_get_calib_config_request(namespace,
|
||||
name)
|
||||
|
||||
response = sds.receive_get_calib_config_response()
|
||||
print("Response:", response)
|
||||
original_data = response['calib_config']
|
||||
import json
|
||||
original_data = original_data.replace("null",str(np.finfo(np.float32).max))
|
||||
calib_config = json.loads(original_data)
|
||||
if calib_config['calibration_config']['roi_0']['vertex_0'][1] == 35:
|
||||
calib_config['calibration_config']['roi_0']['vertex_0'][1] = 36
|
||||
else:
|
||||
calib_config['calibration_config']['roi_0']['vertex_0'][1] = 35
|
||||
calib_config1 = json.dumps(calib_config)
|
||||
|
||||
sds.send_set_calib_config_request(namespace,
|
||||
name,
|
||||
calib_config1)
|
||||
|
||||
response = sds.receive_set_calib_config_response()
|
||||
|
||||
sds.send_get_calib_config_request(namespace,
|
||||
name)
|
||||
|
||||
response = sds.receive_get_calib_config_response()
|
||||
cc_read = json.loads(response['calib_config'])
|
||||
LOGGER.debug("calib config data written: ",calib_config)
|
||||
LOGGER.debug("calib config data readback:",cc_read)
|
||||
assert cc_read == calib_config, "Written calib config is not matching with the read one"
|
||||
sds.send_set_calib_config_request(namespace,
|
||||
name,
|
||||
original_data)
|
||||
|
||||
response = sds.receive_set_calib_config_response()
|
||||
#cleanup starts....
|
||||
camera.stop()
|
||||
rclpy.shutdown()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,84 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_n_mqtt_nodes import CameraNMqttNodes as RSCameraSimulator
|
||||
from camera_n_mqtt_nodes import launch_descr_with_parameters
|
||||
import camera_n_mqtt_nodes
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
import pytest
|
||||
import rclpy
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
test_params_align_depth_color_d585s = {
|
||||
'camera_name': 'D585S',
|
||||
'device_type': 'D585S',
|
||||
}
|
||||
|
||||
@pytest.mark.parametrize("launch_descr_with_parameters",[
|
||||
pytest.param(test_params_align_depth_color_d585s, marks=pytest.mark.d585s),
|
||||
]
|
||||
,indirect=True)
|
||||
@pytest.mark.launch(fixture=launch_descr_with_parameters)
|
||||
def test_system_device_info(launch_descr_with_parameters):
|
||||
#initialization starts....
|
||||
try:
|
||||
rclpy.init()
|
||||
params = launch_descr_with_parameters[1]
|
||||
namespace = 'camera'
|
||||
name = params['camera_name']
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
camera.start()
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
camera.create_device_info_service()
|
||||
|
||||
sds.send_get_device_info_request(namespace,
|
||||
name)
|
||||
response = sds.receive_get_device_info_response()
|
||||
LOGGER.info(f"response:{response}")
|
||||
device_info= camera_n_mqtt_nodes.get_camera_device_info(params['device_type'], response['serial_number'])
|
||||
assert device_info != None, "Could not find the device_info"
|
||||
for key, value in device_info.items():
|
||||
LOGGER.info(f"checking:{key}")
|
||||
assert response[key] == value, f"device_info read returned unexpected value in {key}:Received {response['serial_number']}"
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
finally:
|
||||
camera.stop()
|
||||
rclpy.shutdown()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,94 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_n_mqtt_nodes import CameraNMqttNodes as RSCameraSimulator
|
||||
from camera_n_mqtt_nodes import launch_descr_with_parameters
|
||||
import camera_n_mqtt_nodes
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
import pytest
|
||||
import rclpy
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
test_params_align_depth_color_d585s = {
|
||||
'camera_name': 'D585S',
|
||||
'device_type': 'D585S',
|
||||
}
|
||||
|
||||
@pytest.mark.parametrize("launch_descr_with_parameters",[
|
||||
pytest.param(test_params_align_depth_color_d585s, marks=pytest.mark.d585s),
|
||||
]
|
||||
,indirect=True)
|
||||
@pytest.mark.launch(fixture=launch_descr_with_parameters)
|
||||
def test_system_dryrun_calibration(launch_descr_with_parameters):
|
||||
#initialization starts....
|
||||
try:
|
||||
rclpy.init()
|
||||
params = launch_descr_with_parameters[1]
|
||||
namespace = 'camera'
|
||||
name = params['camera_name']
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
camera.start()
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
sds.prepare_for_calibration(namespace, name)
|
||||
|
||||
sds.send_triggered_calibration_request(namespace,
|
||||
name,
|
||||
dryrun=True)
|
||||
while True:
|
||||
response = sds.receive_triggered_calibration_response()
|
||||
LOGGER.info(f"Response: {response}")
|
||||
if response['success'] == True or response['error_msg'] != '':
|
||||
if response['success'] == True:
|
||||
LOGGER.info('Triggered calibraton was successful')
|
||||
assert (response['calibration'] != "{}"), "The calibration data received is empty"
|
||||
assert type(response['calibration']) != list, "The calibration data received is not a list"
|
||||
LOGGER.info('Triggered calibraton data:' + response['calibration'])
|
||||
elif response['progress'] == 100.0 and 'Calibration completed but algorithm failed' in response['error_msg']:
|
||||
#since it's an issue with the camera field of view, treating it as a success with warning.
|
||||
#Manual adjustment of camera is needed to pass the test.
|
||||
LOGGER.warning('Triggered calibraton completed, but algorithm failed. This is treated as a successful completion of the test')
|
||||
else:
|
||||
assert False, 'Triggered calibration failed with unexpected response:'+str(response)
|
||||
break
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
finally:
|
||||
camera.stop()
|
||||
rclpy.shutdown()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,101 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
|
||||
import rclpy
|
||||
|
||||
import pytest
|
||||
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_n_mqtt_nodes import CameraNMqttNodes as RSCameraSimulator
|
||||
from camera_n_mqtt_nodes import launch_descr_with_parameters
|
||||
import camera_n_mqtt_nodes
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
test_params_d585s = {
|
||||
'camera_name': 'D585S',
|
||||
'device_type': 'D585S',
|
||||
}
|
||||
|
||||
#@pytest.mark.timeout(20)
|
||||
@pytest.mark.parametrize("launch_descr_with_parameters",[
|
||||
pytest.param(test_params_d585s, marks=pytest.mark.d585s),
|
||||
]
|
||||
,indirect=True)
|
||||
@pytest.mark.launch(fixture=launch_descr_with_parameters)
|
||||
def test_system_frame_types(launch_descr_with_parameters):
|
||||
#initialization starts....
|
||||
try:
|
||||
rclpy.init()
|
||||
params = launch_descr_with_parameters[1]
|
||||
namespace = 'camera'
|
||||
name = params['camera_name']
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
params = [
|
||||
{"param_name":'safety_camera.safety_mode', "default_value":0, "param_type":"int"},
|
||||
]
|
||||
camera.add_parameters(params)
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
'''safety stream suport is not required
|
||||
LOGGER.info("Testing safety_frame...")
|
||||
sds.start_stop_safety_stream(namespace, name, True)
|
||||
frame = sds.get_frame_msg(namespace, name, "safety")
|
||||
LOGGER.debug(frame)
|
||||
'''
|
||||
LOGGER.info("Testing color_frame...")
|
||||
sds.start_stop_color_stream(namespace, name, True)
|
||||
frame = sds.get_frame_msg(namespace, name, "color")
|
||||
LOGGER.debug(frame)
|
||||
sds.start_stop_color_stream(namespace, name, False)
|
||||
|
||||
LOGGER.info("Testing depth_frame...")
|
||||
sds.start_stop_depth_stream(namespace, name, True)
|
||||
frame = sds.get_frame_msg(namespace, name, "depth")
|
||||
LOGGER.debug(frame)
|
||||
sds.start_stop_depth_stream(namespace, name, False)
|
||||
|
||||
LOGGER.info("Testing infra1_frame...")
|
||||
sds.start_stop_infra1_stream(namespace, name, True)
|
||||
frame = sds.get_frame_msg(namespace, name, "infra1")
|
||||
LOGGER.debug(frame)
|
||||
|
||||
LOGGER.info("Testing infra2_frame...")
|
||||
sds.start_stop_infra2_stream(namespace, name, True)
|
||||
frame = sds.get_frame_msg(namespace, name, "infra2")
|
||||
LOGGER.debug(frame)
|
||||
except:
|
||||
raise
|
||||
finally:
|
||||
#cleanup starts....
|
||||
camera.stop()
|
||||
rclpy.shutdown()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,116 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
import json
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_n_mqtt_nodes import CameraNMqttNodes as RSCameraSimulator
|
||||
from camera_n_mqtt_nodes import launch_descr_with_parameters
|
||||
import camera_n_mqtt_nodes
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
import pytest
|
||||
import rclpy
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
test_params_hmc_d585s = {
|
||||
'camera_name': 'D585S',
|
||||
'device_type': 'D585S',
|
||||
}
|
||||
|
||||
@pytest.mark.parametrize("launch_descr_with_parameters",[
|
||||
pytest.param(test_params_hmc_d585s, marks=pytest.mark.d585s),
|
||||
]
|
||||
,indirect=True)
|
||||
@pytest.mark.launch(fixture=launch_descr_with_parameters)
|
||||
def test_system_hmc_commands(launch_descr_with_parameters):
|
||||
#initialization starts....
|
||||
rclpy.init()
|
||||
params = launch_descr_with_parameters[1]
|
||||
namespace = 'camera'
|
||||
name = params['camera_name']
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
camera.start()
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
links = [
|
||||
{"opcode":0x76, "datasize_expected":8}, #GETRGBAEROI
|
||||
{"opcode":0x2A, "param1": 0, "datasize_expected":20}, #GTEMP
|
||||
{"opcode":0x2A, "param1": 1, "datasize_expected":20}, #GTEMP
|
||||
{"opcode":0x2A, "param1": 2, "datasize_expected":20}, #GTEMP
|
||||
{"opcode":0xAC, "datasize_expected":240}, #HEALTH Status
|
||||
{"opcode":0x76,"datasize_expected":8},#GETRGBAEROI
|
||||
{"opcode":0x75, "param1": 0x1c4, "param2": 0x2a0, "param3": 0x36d, "param4": 0x0ad},#SETRGBAEROI
|
||||
{"opcode":0x76,"datasize_expected":8},#GETRGBAEROI
|
||||
]
|
||||
for cmd in links:
|
||||
param1 = cmd.get("param1", None)
|
||||
param2 = cmd.get("param2", None)
|
||||
param3 = cmd.get("param3", None)
|
||||
param4 = cmd.get("param4", None)
|
||||
data = cmd.get("data", None)
|
||||
val = sds.send_hwm_command(namespace,name, cmd['opcode'], param1=param1, param2=param2, param3=param3, param4=param4, data=data)
|
||||
LOGGER.info(f"Data received {val['result']}")
|
||||
size = cmd.get("datasize_expected", 0)
|
||||
if size:
|
||||
in_data = val['result'][4:]
|
||||
assert val['result'] is not None, f"Expected {size} of data, but didn't get any"
|
||||
assert len(in_data) == size, f"Expected {size} of data, but got {len(in_data)}" #1st 4 bytes contain the opcode
|
||||
else:
|
||||
assert len(val['result']) == 4, f"Expected {size} of data, but got {len(val['result'])}" #1st 4 bytes contain the opcode
|
||||
header = int.from_bytes(val['result'][0:3], byteorder='little', signed=False)
|
||||
assert header == cmd['opcode'], f"Didn't get the opcode back, got {header} instead of {cmd['opcode']}"
|
||||
|
||||
val = sds.send_hwm_command(namespace,name, 0xBC, param1=0)
|
||||
LOGGER.debug(val)
|
||||
data = val['result'][4:]
|
||||
data[3] =1
|
||||
sds.set_safety_mode(namespace, name, 2)
|
||||
val = sds.send_hwm_command(namespace,name, 0xBC, param1=1,data=data)
|
||||
val = sds.send_hwm_command(namespace,name, 0xBC, param1=0)
|
||||
data = val['result'][4:]
|
||||
LOGGER.debug(val)
|
||||
assert data[3] ==1, "Error: couldn't read back the written data"
|
||||
|
||||
val = sds.send_hwm_command(namespace,name,0x76)#GETRGBAEROI
|
||||
val = sds.send_hwm_command(namespace,name,0x75, param1=0x1c2, param2= 0x2a0, param3 = 0x36d, param4 = 0x0ad)#SETRGBAEROI
|
||||
val = sds.send_hwm_command(namespace,name,0x76)#GETRGBAEROI
|
||||
param1l = val['result'][4:6]
|
||||
param1 = int.from_bytes(param1l, byteorder='little', signed=False)
|
||||
assert param1 == 0x1c2, "Error: written data is not matching with the read one"
|
||||
''' #taking too much of time and inconsistent
|
||||
val = sds.send_hwm_command(namespace,name,32, param1=1) #SoC reset
|
||||
import time
|
||||
time.sleep(15)
|
||||
mode = sds.get_integer_param(namespace, name, 'safety_camera.safety_mode')
|
||||
print("Mode:", mode)
|
||||
time.sleep(2)
|
||||
'''
|
||||
#cleanup starts....
|
||||
camera.stop()
|
||||
rclpy.shutdown()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,75 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
import json
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_n_mqtt_nodes import CameraNMqttNodes as RSCameraSimulator
|
||||
from camera_n_mqtt_nodes import launch_descr_with_parameters
|
||||
import camera_n_mqtt_nodes
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
import pytest
|
||||
import rclpy
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
test_params_hmc_d585s = {
|
||||
'camera_name': 'D585S',
|
||||
'device_type': 'D585S',
|
||||
'rgb_camera.color_profile': '640x360x30',
|
||||
|
||||
}
|
||||
|
||||
@pytest.mark.parametrize("launch_descr_with_parameters",[
|
||||
pytest.param(test_params_hmc_d585s, marks=pytest.mark.d585s),
|
||||
]
|
||||
,indirect=True)
|
||||
@pytest.mark.launch(fixture=launch_descr_with_parameters)
|
||||
def test_system_hw_reset(launch_descr_with_parameters):
|
||||
#initialization starts....
|
||||
rclpy.init()
|
||||
params = launch_descr_with_parameters[1]
|
||||
namespace = 'camera'
|
||||
name = params['camera_name']
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
camera.start()
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
sds.set_string_param(namespace, name,'rgb_camera.color_profile', '640x360x5')
|
||||
param = sds.get_string_param(namespace, name, 'rgb_camera.color_profile')
|
||||
assert param == '640x360x5', "Couldn't set the parameter"
|
||||
val = sds.send_hw_reset(namespace,name)
|
||||
import time
|
||||
#can't use the enumerate_devices for this purpose, so banking on sleep
|
||||
time.sleep(5)
|
||||
param = sds.get_string_param(namespace, name, 'rgb_camera.color_profile')
|
||||
assert param == '640x360x30', "Reset didn't work it seems. Please check the logs manually for reset..."
|
||||
time.sleep(2)
|
||||
#cleanup starts....
|
||||
camera.stop()
|
||||
rclpy.shutdown()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,730 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
import json
|
||||
import numpy as np
|
||||
|
||||
import rclpy
|
||||
|
||||
import pytest
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_n_mqtt_nodes import CameraNMqttNodes as RSCameraSimulator
|
||||
from camera_n_mqtt_nodes import launch_descr_with_parameters
|
||||
import camera_n_mqtt_nodes
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
test_params_d585s = {
|
||||
'camera_name': 'D585S',
|
||||
'device_type': 'D585S',
|
||||
}
|
||||
|
||||
#@pytest.mark.timeout(20)
|
||||
@pytest.mark.parametrize("launch_descr_with_parameters",[
|
||||
pytest.param(test_params_d585s, marks=pytest.mark.d585s),
|
||||
]
|
||||
,indirect=True)
|
||||
@pytest.mark.launch(fixture=launch_descr_with_parameters)
|
||||
def test_system_negative_json_strings(launch_descr_with_parameters):
|
||||
#initialization starts....
|
||||
rclpy.init()
|
||||
params = launch_descr_with_parameters[1]
|
||||
namespace = 'camera'
|
||||
name = params['camera_name']
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
|
||||
'''
|
||||
request_dict = {
|
||||
'camera_name_prefix': name
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'enumerate_devices_request')
|
||||
msg = sds.get_message()
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name_prefix in mqtt_request"
|
||||
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace_prefix': namespace
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'enumerate_devices_request')
|
||||
msg = sds.get_message()
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace_prefix in mqtt_request"
|
||||
|
||||
'''
|
||||
request_dict = {
|
||||
'camera_namespace_prefix': namespace,
|
||||
'camera_name_prefix': name
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'enumerate_devices_request')
|
||||
msg = sds.get_message()
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] == True, "Expected a success message for a proper mqtt_request"
|
||||
|
||||
|
||||
request_dict = {
|
||||
#'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'parameter_name': 'enable_safety',
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_param_request')
|
||||
msg = sds.get_message()
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
#'camera_name': name,
|
||||
'parameter_name': 'enable_safety',
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_param_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name in mqtt_request"
|
||||
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
#'parameter_name': 'enable_safety',
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_param_request')
|
||||
msg = sds.get_message()
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of parameter_name in mqtt_request"
|
||||
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
#'camera_name': name,
|
||||
'parameter_name': 'enable_safety',
|
||||
'parameter_value': False,
|
||||
'parameter_type': 'bool'
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_param_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
#'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'parameter_name': 'enable_safety',
|
||||
'parameter_value': False,
|
||||
'parameter_type': 'bool'
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_param_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace in mqtt_request"
|
||||
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
#'parameter_name': 'enable_safety',
|
||||
'parameter_value': False,
|
||||
'parameter_type': 'bool'
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_param_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of parameter_name in mqtt_request"
|
||||
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'parameter_name': 'enable_safety',
|
||||
#'parameter_value': False,
|
||||
'parameter_type': 'bool'
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_param_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of parameter_value in mqtt_request"
|
||||
|
||||
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'parameter_name': 'enable_safety',
|
||||
'parameter_value': False,
|
||||
#'parameter_type': 'bool'
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_param_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of parameter_type in mqtt_request"
|
||||
|
||||
#get_frame request
|
||||
request_dict = {
|
||||
#'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'stream_name': 'color',
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_frame_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
#'camera_name': name,
|
||||
'stream_name': 'color',
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_frame_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name in mqtt_request"
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
#'stream_name': 'color',
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_frame_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of stream_name in mqtt_request"
|
||||
|
||||
#get_safety_preset
|
||||
request_dict = {
|
||||
#'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'index': 0,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_safety_preset_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
#'camera_name': name,
|
||||
'index': 0,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_safety_preset_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name in mqtt_request"
|
||||
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
#'index': 0,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_safety_preset_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of index in mqtt_request"
|
||||
|
||||
#set_safety_preset
|
||||
preset_response = sds.get_safety_preset(namespace, name, 1)
|
||||
|
||||
request_dict = {
|
||||
#'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'index': 1,
|
||||
'safety_preset': preset_response['safety_preset']
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_safety_preset_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace in mqtt_request"
|
||||
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
#'camera_name': name,
|
||||
'index': 1,
|
||||
'safety_preset': preset_response['safety_preset']
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_safety_preset_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
#'index': 1,
|
||||
'safety_preset': preset_response['safety_preset']
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_safety_preset_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of index in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'index': 1,
|
||||
#'safety_preset': preset_response['safety_preset']
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_safety_preset_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of preset in mqtt_request"
|
||||
|
||||
#get_safety_interface_config
|
||||
request_dict = {
|
||||
#'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'calib_location': 2,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_safety_interface_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
#'camera_name': name,
|
||||
'calib_location': 2,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_safety_interface_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name in mqtt_request"
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
#'calib_location': 2,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_safety_interface_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] == True, "Expected a success message for the absence of calib_location in mqtt_request, it should use the default value 2"
|
||||
|
||||
#set_safety_interface_config
|
||||
si_cfg = response['safety_interface_config']
|
||||
request_dict = {
|
||||
#'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'safety_interface_config':si_cfg
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_safety_interface_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
#'camera_name': name,
|
||||
'safety_interface_config':si_cfg
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_safety_interface_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name in mqtt_request"
|
||||
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
#'safety_interface_config':si_cfg
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_safety_interface_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of safety_interface_config in mqtt_request"
|
||||
|
||||
|
||||
#get_calib_config
|
||||
request_dict = {
|
||||
#'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_calib_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
#'camera_name': name,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_calib_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name in mqtt_request"
|
||||
|
||||
sds.set_safety_mode(namespace, name, 2)
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_calib_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] == True, "Expected a success message for calib_config:" + response["error_msg"]
|
||||
original_data = response['calib_config'].replace("null",str(np.finfo(np.float32).max))
|
||||
|
||||
#set_calib_config_request
|
||||
request_dict = {
|
||||
#'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'calib_config':original_data
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_calib_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
#'camera_name': name,
|
||||
'calib_config':original_data
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_calib_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
#'calib_config':original_data
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_calib_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of calib_config in mqtt_request"
|
||||
|
||||
|
||||
#get_application_config
|
||||
request_dict = {
|
||||
#'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_application_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
#'camera_name': name,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_application_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'get_application_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] == True, "Expected a a successful application_config read"
|
||||
original_data = response['application_config']
|
||||
#set_application_config
|
||||
request_dict = {
|
||||
#'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'application_config':original_data
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_application_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
#'camera_name': name,
|
||||
'application_config':original_data
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_application_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name in mqtt_request"
|
||||
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
}
|
||||
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'set_application_config_request')
|
||||
msg = sds.get_message()
|
||||
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of application_config in mqtt_request"
|
||||
|
||||
|
||||
#triggered calibration
|
||||
sds.prepare_for_calibration(namespace, name)
|
||||
|
||||
request_dict = {
|
||||
#'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
'json':'calib run',
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'triggered_calibration_request')
|
||||
msg = sds.get_message()
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_namespace in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
#'camera_name': name,
|
||||
'json':'calib run',
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'triggered_calibration_request')
|
||||
msg = sds.get_message()
|
||||
response = json.loads(msg.payload)
|
||||
assert response["success"] != True, "Expected a failure message for the absence of camera_name in mqtt_request"
|
||||
|
||||
request_dict = {
|
||||
'camera_namespace': namespace,
|
||||
'camera_name': name,
|
||||
#'json':'calib run',
|
||||
}
|
||||
j = json.dumps(request_dict)
|
||||
sds.msg = None
|
||||
sds.locked = True
|
||||
sds.publish(j, 'triggered_calibration_request')
|
||||
msg = sds.get_message()
|
||||
sds.locked = True
|
||||
response = json.loads(msg.payload)
|
||||
assert response["error_msg"] == "", "default triggered calibration request is calib run, but failed with error:"+response["error_msg"]
|
||||
|
||||
while True:
|
||||
response = sds.receive_triggered_calibration_response()
|
||||
LOGGER.info(f"Response: {response}")
|
||||
if response['success'] == True or response['error_msg'] != '':
|
||||
if response['success'] == True:
|
||||
LOGGER.info('Triggered calibraton was successful')
|
||||
assert (response['calibration'] != "{}"), "The calibration data received is empty"
|
||||
assert type(response['calibration']) != list, "The calibration data received is not a list"
|
||||
LOGGER.info('Triggered calibraton data:' + response['calibration'])
|
||||
elif response['progress'] == 100.0 and 'Calibration completed but algorithm failed' in response['error_msg']:
|
||||
#since it's an issue with the camera field of view, treating it as a success with warning.
|
||||
#Manual adjustment of camera is needed to pass the test.
|
||||
LOGGER.warning('Triggered calibraton completed, but algorithm failed. This is treated as a successful completion of the test')
|
||||
else:
|
||||
assert False, 'Triggered calibration failed with unexpected response:'+str(response)
|
||||
break
|
||||
|
||||
#cleanup starts....
|
||||
camera.stop()
|
||||
rclpy.shutdown()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
+106
@@ -0,0 +1,106 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
import rclpy
|
||||
|
||||
import pytest
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_n_mqtt_nodes import CameraNMqttNodes as RSCameraSimulator
|
||||
from camera_n_mqtt_nodes import launch_descr_with_parameters
|
||||
import camera_n_mqtt_nodes
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
test_params_d585s = {
|
||||
'camera_name': 'D585S',
|
||||
'device_type': 'D585S',
|
||||
}
|
||||
|
||||
#@pytest.mark.timeout(20)
|
||||
@pytest.mark.parametrize("launch_descr_with_parameters",[
|
||||
pytest.param(test_params_d585s, marks=pytest.mark.d585s),
|
||||
]
|
||||
,indirect=True)
|
||||
@pytest.mark.launch(fixture=launch_descr_with_parameters)
|
||||
def test_system_safety_interface_config(launch_descr_with_parameters):
|
||||
#initialization starts....
|
||||
rclpy.init()
|
||||
params = launch_descr_with_parameters[1]
|
||||
namespace = 'camera'
|
||||
name = params['camera_name']
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
camera.create_safety_interface_config_service()
|
||||
#just to ensure no streams are running
|
||||
sds.set_safety_mode(namespace, name, 2)
|
||||
|
||||
#safety interface config read works only for RAM and FLASH.
|
||||
for index in [1,2]:
|
||||
sds.send_get_safety_interface_config_request(namespace,
|
||||
name,
|
||||
index)
|
||||
|
||||
response = sds.receive_get_safety_interface_config_response()
|
||||
original_data = response["safety_interface_config"]
|
||||
import json
|
||||
sc_data = json.loads(original_data)
|
||||
LOGGER.info(f"safety interface config {index} first read: {sc_data}")
|
||||
if sc_data["safety_interface_config"]['smcu_arbitration_params']['l_0_total_threshold'] == 10000:
|
||||
sc_data["safety_interface_config"]['smcu_arbitration_params']['l_0_total_threshold'] = 10
|
||||
else:
|
||||
sc_data["safety_interface_config"]['smcu_arbitration_params']['l_0_total_threshold'] = 10000
|
||||
sc_data1 = json.dumps(sc_data)
|
||||
|
||||
LOGGER.debug(f"safety interface config written: {sc_data}")
|
||||
|
||||
#change sp
|
||||
sds.send_set_safety_interface_config_request(namespace,
|
||||
name,
|
||||
sc_data1)
|
||||
|
||||
response = sds.receive_set_safety_interface_config_response()
|
||||
|
||||
sds.send_get_safety_interface_config_request(namespace,
|
||||
name,
|
||||
index)
|
||||
response = sds.receive_get_safety_interface_config_response()
|
||||
sc_read = json.loads(response["safety_interface_config"])
|
||||
LOGGER.info(f"safety interface config {index} second read: {sc_read}")
|
||||
assert sc_read == sc_data, "Written safety interface config is not matching with the read one"
|
||||
sds.send_set_safety_interface_config_request(namespace,
|
||||
name,
|
||||
original_data)
|
||||
response = sds.receive_set_safety_interface_config_response()
|
||||
#cleanup starts....
|
||||
camera.stop()
|
||||
rclpy.shutdown()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,84 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_n_mqtt_nodes import CameraNMqttNodes as RSCameraSimulator
|
||||
from camera_n_mqtt_nodes import launch_descr_with_parameters
|
||||
import camera_n_mqtt_nodes
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
import pytest
|
||||
import rclpy
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
test_params_tf_d585s = {
|
||||
'camera_name': 'D585S',
|
||||
'device_type': 'D585S',
|
||||
'publish_tf': 'true',
|
||||
'tf_publish_rate': '1.1',
|
||||
}
|
||||
|
||||
@pytest.mark.parametrize("launch_descr_with_parameters",[
|
||||
pytest.param(test_params_tf_d585s, marks=pytest.mark.d585s),
|
||||
]
|
||||
,indirect=True)
|
||||
@pytest.mark.launch(fixture=launch_descr_with_parameters)
|
||||
def test_system_tf(launch_descr_with_parameters):
|
||||
#initialization starts....
|
||||
rclpy.init()
|
||||
params = launch_descr_with_parameters[1]
|
||||
namespace = 'camera'
|
||||
name = params['camera_name']
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
camera.start()
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
links = {
|
||||
name+'_link':name+'_depth_frame',
|
||||
name+'_depth_frame':name+'_depth_optical_frame',
|
||||
name+'_link':name+'_color_frame',
|
||||
name+'_color_frame':name+'_color_optical_frame',
|
||||
}
|
||||
for source, destination in links.items():
|
||||
val = sds.get_transformation(namespace, name, source, destination)
|
||||
assert 'rotation' in val, "rotation values not found"
|
||||
assert 'x' in val['rotation'], "x in rotation values not found"
|
||||
assert 'y' in val['rotation'], "y in rotation values not found"
|
||||
assert 'z' in val['rotation'], "z in rotation values not found"
|
||||
assert 'w' in val['rotation'], "w in rotation values not found"
|
||||
|
||||
assert 'translation' in val, "translation values not found"
|
||||
assert 'x' in val['translation'], "x in translation values not found"
|
||||
assert 'y' in val['translation'], "y in translation values not found"
|
||||
assert 'z' in val['translation'], "z in translation values not found"
|
||||
|
||||
#cleanup starts....
|
||||
camera.stop()
|
||||
rclpy.shutdown()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,93 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_n_mqtt_nodes import CameraNMqttNodes as RSCameraSimulator
|
||||
from camera_n_mqtt_nodes import launch_descr_with_parameters
|
||||
import camera_n_mqtt_nodes
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
import pytest
|
||||
import rclpy
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
test_params_align_depth_color_d585s = {
|
||||
'camera_name': 'D585S',
|
||||
'device_type': 'D585S',
|
||||
}
|
||||
|
||||
@pytest.mark.parametrize("launch_descr_with_parameters",[
|
||||
pytest.param(test_params_align_depth_color_d585s, marks=pytest.mark.d585s),
|
||||
]
|
||||
,indirect=True)
|
||||
@pytest.mark.launch(fixture=launch_descr_with_parameters)
|
||||
def test_system_triggered_calibration(launch_descr_with_parameters):
|
||||
#initialization starts....
|
||||
try:
|
||||
rclpy.init()
|
||||
params = launch_descr_with_parameters[1]
|
||||
namespace = 'camera'
|
||||
name = params['camera_name']
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
camera.start()
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
sds.prepare_for_calibration(namespace, name)
|
||||
|
||||
sds.send_triggered_calibration_request(namespace,
|
||||
name)
|
||||
while True:
|
||||
response = sds.receive_triggered_calibration_response()
|
||||
LOGGER.info(f"Response: {response}")
|
||||
if response['success'] == True or response['error_msg'] != '':
|
||||
if response['success'] == True:
|
||||
LOGGER.info('Triggered calibraton was successful')
|
||||
assert (response['calibration'] != "{}"), "The calibration data received is empty"
|
||||
assert type(response['calibration']) != list, "The calibration data received is not a list"
|
||||
LOGGER.info('Triggered calibraton data:' + response['calibration'])
|
||||
elif response['progress'] == 100.0 and 'Calibration completed but algorithm failed' in response['error_msg']:
|
||||
#since it's an issue with the camera field of view, treating it as a success with warning.
|
||||
#Manual adjustment of camera is needed to pass the test.
|
||||
LOGGER.warning('Triggered calibraton completed, but algorithm failed. This is treated as a successful completion of the test')
|
||||
else:
|
||||
assert False, 'Triggered calibration failed with unexpected response:'+str(response)
|
||||
break
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
finally:
|
||||
camera.stop()
|
||||
rclpy.shutdown()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,85 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
import pytest
|
||||
|
||||
#@pytest.mark.skip(reason="under development")
|
||||
def test_triggered_calibration():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
camera.create_triggered_calibration_action()
|
||||
|
||||
#just call abort without a calibration run
|
||||
sds.abort_triggered_calibration_request(namespace, name)
|
||||
while True:
|
||||
response = sds.receive_triggered_calibration_response()
|
||||
LOGGER.debug(f"Response: {response}")
|
||||
if response['success'] == True:
|
||||
break
|
||||
assert response['error_msg'] == "No goal in progress, cancelled too early?", 'Unexpected error message received ' + response['error_msg']
|
||||
|
||||
|
||||
sds.send_triggered_calibration_request(namespace,
|
||||
name)
|
||||
while True:
|
||||
response = sds.receive_triggered_calibration_response()
|
||||
LOGGER.debug(f"Response: {response}")
|
||||
if response['progress'] > 2.0:
|
||||
sds.abort_triggered_calibration_request(namespace, name)
|
||||
break
|
||||
while True:
|
||||
response = sds.receive_triggered_calibration_response()
|
||||
LOGGER.debug(f"Response: {response}")
|
||||
if response['success'] == True or response['error_msg'] != "":
|
||||
break
|
||||
assert response['success'] == False, "Response.success received as True, expecting False"
|
||||
assert response['error_msg'] == "Canceled", 'Unexpected error message received ' + response['error_msg']
|
||||
assert response['calibration'] == "{}", 'Unexpected calibration value received ' + response['calibration']
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
camera.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,89 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
|
||||
|
||||
|
||||
def test_all_param_types():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
params = [
|
||||
{"param_name":'safety_camera.safety_mode', "default_value":0, "param_type":"int", "alternate_value":2},
|
||||
{"param_name":'accel_info_qos ', "default_value":"accel_info_qos", "param_type":"string", "alternate_value":'hello'},
|
||||
{"param_name":'align_depth.enable','default_value':True,'param_type': 'bool', "alternate_value":'False'},
|
||||
{"param_name":'align_depth.disable','default_value':False,'param_type': 'bool', "alternate_value":'True'}
|
||||
]
|
||||
|
||||
camera.add_parameters(params)
|
||||
for param in params:
|
||||
|
||||
LOGGER.info("Testing get_param for type " + param["param_type"])
|
||||
sds.send_get_param_request(namespace,
|
||||
name,
|
||||
param['param_name'])
|
||||
|
||||
response = sds.receive_get_param_response()
|
||||
assert str(response["parameter_value"]) == str(param['default_value']), "default value was not set for for param " + param['param_name']
|
||||
|
||||
LOGGER.info("Testing set_param for type " + param["param_type"])
|
||||
|
||||
sds.send_set_param_request(namespace,
|
||||
name,
|
||||
param['param_name'],
|
||||
param['alternate_value'],
|
||||
param['param_type'])
|
||||
response = sds.receive_set_param_response()
|
||||
|
||||
response = sds.get_param_msg(namespace,
|
||||
name,
|
||||
param['param_name'])
|
||||
assert response["parameter_value"] == str(param['alternate_value']), "Get or Set param failed, didn't get the written value for param " + param['param_name']
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
raise
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
camera.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,76 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
|
||||
|
||||
|
||||
def test_application_config():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
camera.create_application_config_service()
|
||||
|
||||
sds.send_get_application_config_request(namespace,
|
||||
name)
|
||||
|
||||
response = sds.receive_get_application_config_response()
|
||||
Application_data = "Application data"
|
||||
|
||||
sds.send_set_application_config_request(namespace,
|
||||
name,
|
||||
Application_data)
|
||||
|
||||
response = sds.receive_set_application_config_response()
|
||||
|
||||
sds.send_get_application_config_request(namespace,
|
||||
name)
|
||||
|
||||
response = sds.receive_get_application_config_response()
|
||||
assert response["application_config"] == Application_data, "Written Application config is not matching with the read one"
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
#LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
camera.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,90 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
|
||||
def test_bridge_instatiation():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
params = [
|
||||
{"param_name":'safety_camera.safety_mode', "default_value":0, "param_type":"int"},
|
||||
]
|
||||
camera.add_parameters(params)
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
|
||||
LOGGER.info("Testing get_param...")
|
||||
|
||||
sds.send_get_param_request(namespace,
|
||||
name,
|
||||
'safety_camera.safety_mode')
|
||||
|
||||
response = sds.receive_get_param_response()
|
||||
|
||||
LOGGER.debug("safety_camera.safety_mode Param received: " + str(response["parameter_value"]))
|
||||
|
||||
LOGGER.info("Testing set_param...")
|
||||
|
||||
sds.send_set_param_request(namespace,
|
||||
name,
|
||||
'safety_camera.safety_mode',
|
||||
'2',
|
||||
'int')
|
||||
response = sds.receive_set_param_response()
|
||||
|
||||
response = sds.get_param(namespace,
|
||||
name,
|
||||
'safety_camera.safety_mode')
|
||||
assert int(response["parameter_value"]) == 2, "Get or Set param failed, didn't get the written value"
|
||||
LOGGER.debug("safety_camera.safety_mode Param received: " + str(response))
|
||||
|
||||
LOGGER.info("Testing get_frame...")
|
||||
camera.start_publish_color_frame()
|
||||
frame = sds.get_frame(namespace, name, "color")
|
||||
LOGGER.debug(frame)
|
||||
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
camera.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,76 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
|
||||
|
||||
|
||||
def test_calib_config():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
camera.create_calib_config_service()
|
||||
|
||||
sds.send_get_calib_config_request(namespace,
|
||||
name)
|
||||
|
||||
response = sds.receive_get_calib_config_response()
|
||||
calib_data = "Calibration data"
|
||||
assert response['calib_config'] == 'Uninitialized', "Calib config read returned unexpected value: " + response['calib_config']
|
||||
sds.send_set_calib_config_request(namespace,
|
||||
name,
|
||||
calib_data)
|
||||
|
||||
response = sds.receive_set_calib_config_response()
|
||||
|
||||
sds.send_get_calib_config_request(namespace,
|
||||
name)
|
||||
|
||||
response = sds.receive_get_calib_config_response()
|
||||
assert response["calib_config"] == calib_data, "Written calib config is not matching with the read one"
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
camera.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,66 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
|
||||
|
||||
|
||||
def test_device_info():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
camera.create_device_info_service()
|
||||
|
||||
sds.send_get_device_info_request(namespace,
|
||||
name)
|
||||
|
||||
response = sds.receive_get_device_info_response()
|
||||
assert response['device_name'] == 'Camera', "device_info read returned unexpected value in device_name: " + response['device_name']
|
||||
assert response['serial_number'] == '1234', "device_info read returned unexpected value in serial_number: " + response['serial_number']
|
||||
LOGGER.debug("response:", response)
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
camera.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,72 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
import pytest
|
||||
|
||||
#@pytest.mark.skip(reason="under development")
|
||||
def test_triggered_calibration():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
camera.create_triggered_calibration_action()
|
||||
|
||||
sds.send_triggered_calibration_request(namespace,
|
||||
name, dryrun=True)
|
||||
while True:
|
||||
response = sds.receive_triggered_calibration_response()
|
||||
LOGGER.debug(f"Response: {response}")
|
||||
if response['progress'] == 100.0:
|
||||
#should we check for health?
|
||||
assert response['success'] == True, 'Triggered calibraton was not successful'
|
||||
assert response['calibration'] == "calib dry run", 'Unexpected calibration value received'
|
||||
break
|
||||
|
||||
#print(response.payload)
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
finally:
|
||||
#cleanup starts....
|
||||
camera.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,93 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
|
||||
|
||||
|
||||
def test_dual_camera_device_info():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
namespace1 = 'camera1'
|
||||
name1 = 'camera1'
|
||||
camera1 = RSCameraSimulator(namespace1, name1)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
camera1.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, f"Enumerate device failed, couldn't find the device {name}"
|
||||
|
||||
sds.send_enumerate_devices_request(namespace1, name1)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, f"Enumerate device failed, couldn't find the device {name1}"
|
||||
|
||||
camera.create_device_info_service()
|
||||
camera1.create_device_info_service()
|
||||
|
||||
sds.send_get_device_info_request(namespace,
|
||||
name)
|
||||
|
||||
response = sds.receive_get_device_info_response()
|
||||
assert response['device_name'] == 'Camera', "device_info read returned unexpected value in device_name: " + response['device_name']
|
||||
assert response['serial_number'] == '1234', "device_info read returned unexpected value in serial_number: " + response['serial_number']
|
||||
LOGGER.debug("response:", response)
|
||||
|
||||
sds.send_get_device_info_request(namespace1,
|
||||
name1)
|
||||
|
||||
response = sds.receive_get_device_info_response()
|
||||
assert response['device_name'] == 'Camera', "device_info read returned unexpected value in device_name: " + response['device_name']
|
||||
assert response['serial_number'] == '1234', "device_info read returned unexpected value in serial_number: " + response['serial_number']
|
||||
|
||||
|
||||
sds.send_get_device_info_request(namespace,
|
||||
name)
|
||||
|
||||
response = sds.receive_get_device_info_response()
|
||||
assert response['device_name'] == 'Camera', "device_info read returned unexpected value in device_name: " + response['device_name']
|
||||
assert response['serial_number'] == '1234', "device_info read returned unexpected value in serial_number: " + response['serial_number']
|
||||
LOGGER.debug("response:", response)
|
||||
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
camera.stop()
|
||||
camera1.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
+98
@@ -0,0 +1,98 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
|
||||
|
||||
|
||||
def test_dual_camera_triggered_calibration():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera2'
|
||||
camera2 = RSCameraSimulator(namespace, name)
|
||||
name1 = 'camera1'
|
||||
camera1 = RSCameraSimulator(namespace, name1)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera2.start()
|
||||
camera1.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, f"Enumerate device failed, couldn't find the device {name}"
|
||||
|
||||
sds.send_enumerate_devices_request(namespace, name1)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, f"Enumerate device failed, couldn't find the device {name1}"
|
||||
|
||||
camera2.create_triggered_calibration_action()
|
||||
camera1.create_triggered_calibration_action()
|
||||
|
||||
sds.send_triggered_calibration_request(namespace,
|
||||
name)
|
||||
|
||||
while True:
|
||||
response = sds.receive_triggered_calibration_response()
|
||||
LOGGER.info(f"Response: {response}")
|
||||
if response['progress'] > 5.0:
|
||||
break
|
||||
|
||||
sds.send_triggered_calibration_request(namespace,
|
||||
name1)
|
||||
camera1_response = False
|
||||
camera2_response = False
|
||||
|
||||
while True:
|
||||
response = sds.receive_triggered_calibration_response()
|
||||
LOGGER.info(f"Response: {response}")
|
||||
if response['progress'] == 100.0 and response['camera_name'] == 'camera2':
|
||||
assert response['success'] == True, 'Triggered calibraton was not successful'
|
||||
assert response['calibration'] == "calib run", 'Unexpected calibration value received'
|
||||
camera2_response = True
|
||||
if camera1_response:
|
||||
break
|
||||
if response['progress'] == 100.0 and response['camera_name'] == 'camera1':
|
||||
assert response['success'] == True, 'Triggered calibraton was not successful'
|
||||
assert response['calibration'] == "calib run", 'Unexpected calibration value received'
|
||||
camera1_response = True
|
||||
if camera2_response:
|
||||
break
|
||||
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.warning("Test failed")
|
||||
LOGGER.warning(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
camera2.stop()
|
||||
camera1.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,93 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
|
||||
def test_frame_types():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
params = [
|
||||
{"param_name":'safety_camera.safety_mode', "default_value":0, "param_type":"int"},
|
||||
]
|
||||
camera.add_parameters(params)
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
LOGGER.info("Testing color_frame...")
|
||||
camera.start_publish_color_frame()
|
||||
response = sds.get_frame_msg(namespace, name, "color")
|
||||
assert response['success'] == True, 'Receiving color frame was not successful'
|
||||
assert response['frame'] != "", 'Invalid frame received'
|
||||
|
||||
LOGGER.info("Testing depth_frame...")
|
||||
camera.start_publish_depth_frame()
|
||||
response = sds.get_frame_msg(namespace, name, "depth")
|
||||
assert response['success'] == True, 'Receiving depth frame was not successful'
|
||||
assert response['frame'] != "", 'Invalid frame received'
|
||||
|
||||
LOGGER.info("Testing infra1_frame...")
|
||||
camera.start_publish_infra1_frame()
|
||||
response = sds.get_frame_msg(namespace, name, "infra1")
|
||||
assert response['success'] == True, 'Receiving infra1 frame was not successful'
|
||||
assert response['frame'] != "", 'Invalid frame received'
|
||||
|
||||
LOGGER.info("Testing infra2_frame...")
|
||||
camera.start_publish_infra2_frame()
|
||||
response = sds.get_frame_msg(namespace, name, "infra2")
|
||||
assert response['success'] == True, 'Receiving infra2 frame was not successful'
|
||||
assert response['frame'] != "", 'Invalid frame received'
|
||||
|
||||
LOGGER.info("Testing invalid stream...")
|
||||
response = sds.get_frame_msg(namespace, name, "invalid")
|
||||
assert response['success'] != True, 'Expected a failure, but received frame'
|
||||
assert response['frame'] == "", 'Frame received for invalid stream'
|
||||
|
||||
LOGGER.info("Testing depth_frame...")
|
||||
camera.start_publish_depth_frame()
|
||||
response = sds.get_frame_msg(namespace, name, "depth")
|
||||
assert response['success'] == True, 'Receiving depth frame was not successful'
|
||||
assert response['frame'] != "", 'Invalid frame received'
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
camera.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,75 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
|
||||
|
||||
|
||||
def test_safety_interface_config():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
camera.create_safety_interface_config_service()
|
||||
for index in range(0,3):
|
||||
sds.send_get_safety_interface_config_request(namespace,
|
||||
name,
|
||||
index)
|
||||
|
||||
response = sds.receive_get_safety_interface_config_response()
|
||||
sp = "Index is " + str(index)
|
||||
sds.send_set_safety_interface_config_request(namespace,
|
||||
name,
|
||||
sp)
|
||||
response = sds.receive_set_safety_interface_config_response()
|
||||
|
||||
sds.send_get_safety_interface_config_request(namespace,
|
||||
name,
|
||||
index)
|
||||
response = sds.receive_get_safety_interface_config_response()
|
||||
assert response["safety_interface_config"] == sp, "Written safety interface config is not matching with the read one"
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
camera.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,78 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
|
||||
|
||||
|
||||
def test_safety_preset():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
camera.create_safety_preset_service()
|
||||
#for index in range(0,63):
|
||||
for index in [0,1,10,36,62,63]:
|
||||
sds.send_get_safety_preset_request(namespace,
|
||||
name,
|
||||
index)
|
||||
|
||||
response = sds.receive_get_safety_preset_response()
|
||||
assert response["safety_preset"] == "Uninitialized", "Safety preset read failed"
|
||||
sp = "Index is " + str(index)
|
||||
sds.send_set_safety_preset_request(namespace,
|
||||
name,
|
||||
sp,
|
||||
index)
|
||||
response = sds.receive_set_safety_preset_response()
|
||||
|
||||
sds.send_get_safety_preset_request(namespace,
|
||||
name,
|
||||
index)
|
||||
response = sds.receive_get_safety_preset_response()
|
||||
assert response["safety_preset"] == sp, "Written safety preset is not matching with the read one"
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
camera.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,69 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
|
||||
|
||||
from camera_node_simulator import RSCameraSimulator
|
||||
from mqtt_client_simulator import MQTTClientSimulator
|
||||
import logging
|
||||
|
||||
#LOGGER = logging.getLogger(__name__)
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
import pytest
|
||||
|
||||
#@pytest.mark.skip(reason="under development")
|
||||
def test_triggered_calibration():
|
||||
#initialization starts....
|
||||
try:
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
sds = MQTTClientSimulator("localhost", 1883)
|
||||
sds.start_client()
|
||||
camera.start()
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
os.system("ros2 node list")
|
||||
#initialization ends....
|
||||
|
||||
LOGGER.info("Testing enumerate_devices")
|
||||
sds.send_enumerate_devices_request(namespace, name)
|
||||
response = sds.get_enumerate_devices_response()
|
||||
assert int(response["available_nodes_count"]) > 0, "Enumerate device failed, couldn't find the device"
|
||||
|
||||
camera.create_triggered_calibration_action()
|
||||
|
||||
sds.send_triggered_calibration_request(namespace,
|
||||
name)
|
||||
while True:
|
||||
response = sds.receive_triggered_calibration_response()
|
||||
LOGGER.debug(f"Response: {response}")
|
||||
if response['progress'] == 100.0:
|
||||
assert response['success'] == True, 'Triggered calibraton was not successful'
|
||||
assert response['calibration'] == "calib run", 'Unexpected calibration value received'
|
||||
break
|
||||
|
||||
#cleanup starts....
|
||||
|
||||
except Exception as e:
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
LOGGER.error(exc_type, fname, exc_tb.tb_lineno)
|
||||
LOGGER.error("Test failed")
|
||||
LOGGER.error(e)
|
||||
camera.stop()
|
||||
LOGGER.info("Test completed")
|
||||
#cleanup ends....
|
||||
@@ -0,0 +1,250 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import threading
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import launch_ros
|
||||
from launch import LaunchDescription
|
||||
from launch import LaunchService
|
||||
|
||||
import launch_pytest
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
import logging
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
import rs_launch
|
||||
|
||||
class CameraNMqttNodes(Node):#, threading.Thread):
|
||||
def __init__(self, namespace="camera", name='camera', device_type_="D6585S"):
|
||||
super().__init__(name)
|
||||
self.dummy = True
|
||||
return
|
||||
def run(self):
|
||||
return
|
||||
|
||||
def wait_for_node(self, node_name, timeout=8.0):
|
||||
start = time.time()
|
||||
flag = False
|
||||
print('Waiting for node... ' + node_name)
|
||||
while time.time() - start < timeout:
|
||||
print(node_name + ": waiting for the node to come up")
|
||||
flag = node_name in self.get_node_names()
|
||||
if flag:
|
||||
return True, ""
|
||||
time.sleep(timeout/5)
|
||||
return False, "Timed out waiting for "+ str(timeout)+ "seconds"
|
||||
|
||||
def start(self):
|
||||
if self.dummy == True:
|
||||
self.wait_for_node("realsense_ros_mqtt_bridge_node")
|
||||
return
|
||||
|
||||
|
||||
|
||||
#start of dummy functions
|
||||
def stop(self):
|
||||
return
|
||||
def create_device_info_service(self):
|
||||
return
|
||||
def add_parameters(self,params):
|
||||
return
|
||||
def start_publish_color_frame(self):
|
||||
return
|
||||
def create_application_config_service(self):
|
||||
return
|
||||
def create_calib_config_service(self):
|
||||
return
|
||||
def create_safety_interface_config_service(self):
|
||||
return
|
||||
|
||||
#end of dummy functions
|
||||
|
||||
def get_camera_device_info(device_type, serial_no):
|
||||
short_data = os.popen("rs-enumerate-devices -S").read().splitlines()
|
||||
print(serial_no)
|
||||
line_found = 0
|
||||
found = False
|
||||
for line in short_data:
|
||||
print(line)
|
||||
if device_type in line:
|
||||
if serial_no in line:
|
||||
found = True
|
||||
break
|
||||
line_found += 1
|
||||
if found == True:
|
||||
'''
|
||||
rs-enumerate-devices -S
|
||||
Device Name Serial Number Firmware Version
|
||||
Intel RealSense D585S 416422320067 8.17.17151.218
|
||||
Device info:
|
||||
Name : Intel RealSense D585S
|
||||
Serial Number : 416422320067
|
||||
Firmware Version : 8.17.17151.218
|
||||
Physical Port : /sys/devices/pci0000:00/0000:00:14.0/usb2/2-5/2-5:1.0/video4linux/video0
|
||||
Debug Op Code : 180
|
||||
Advanced Mode : NO
|
||||
Product Id : 0B6B
|
||||
Camera Locked : YES
|
||||
Usb Type Descriptor : 3.2
|
||||
Product Line : D500
|
||||
Firmware Update Id : 416422320067
|
||||
Smcu Fw Version : 2.0.6.76
|
||||
'''
|
||||
#if this order changes, test will fail
|
||||
length = len(short_data)
|
||||
LOGGER.debug(device_type + " with serial_no " + serial_no +" found in " + short_data[line_found])
|
||||
for i in range(line_found, line_found+9):
|
||||
LOGGER.debug(short_data[i])
|
||||
device_info = {}
|
||||
#3rd line contains serial no
|
||||
line_no = line_found+3
|
||||
if not "Serial Number" in short_data[line_no]:
|
||||
return None
|
||||
split_line = short_data[line_no].split()
|
||||
device_info['serial_number'] = split_line[3]
|
||||
#4th line contains Firmware Version
|
||||
line_no = line_found+4
|
||||
if not "Firmware Version" in short_data[line_no]:
|
||||
return None
|
||||
split_line = short_data[line_no].split()
|
||||
device_info['firmware_version'] = split_line[3]
|
||||
#5th line contains physical port
|
||||
line_no = line_found+5
|
||||
if not "Physical Port" in short_data[line_no]:
|
||||
return None
|
||||
split_line = short_data[line_no].split()
|
||||
device_info['physical_port'] = split_line[3]
|
||||
#10th line contains Usb Type Descriptor
|
||||
line_no = line_found+10
|
||||
if not "Usb Type Descriptor" in short_data[line_no]:
|
||||
return None
|
||||
split_line = short_data[line_no].split()
|
||||
device_info['usb_type_descriptor'] = split_line[4]
|
||||
#12th line contains Usb Type Descriptor
|
||||
line_no = line_found+12
|
||||
if not "Firmware Update Id" in short_data[line_no]:
|
||||
return None
|
||||
split_line = short_data[line_no].split()
|
||||
device_info['firmware_update_id'] = split_line[4]
|
||||
return device_info
|
||||
return None
|
||||
|
||||
'''
|
||||
get the default parameters from the launch script so that the test doesn't have to
|
||||
get updated for each change to the parameter or default values
|
||||
'''
|
||||
def get_default_params():
|
||||
params = {}
|
||||
for param in rs_launch.configurable_parameters:
|
||||
params[param['name']] = param['default']
|
||||
return params
|
||||
|
||||
'''
|
||||
The format used by rs_launch.py and the LuanchConfiguration yaml files are different,
|
||||
so the params reused from the rs_launch has to be reformated to be added to yaml file.
|
||||
'''
|
||||
def convert_params(params):
|
||||
cparams = {}
|
||||
def strtobool (val):
|
||||
val = val.lower()
|
||||
if val == 'true':
|
||||
return True
|
||||
elif val == 'false':
|
||||
return False
|
||||
else:
|
||||
raise ValueError("invalid truth value %r" % (val,))
|
||||
for key, value in params.items():
|
||||
try:
|
||||
cparams[key] = int(value)
|
||||
except ValueError:
|
||||
try:
|
||||
cparams[key] = float(value)
|
||||
except ValueError:
|
||||
try:
|
||||
cparams[key] = strtobool(value)
|
||||
except ValueError:
|
||||
cparams[key] = value.replace("'","")
|
||||
return cparams
|
||||
|
||||
def get_rs_node_description(params):
|
||||
import tempfile
|
||||
import yaml
|
||||
tmp_yaml = tempfile.NamedTemporaryFile(prefix='launch_rs_',delete=False)
|
||||
params = convert_params(params)
|
||||
ros_params = {"ros__parameters":params}
|
||||
|
||||
camera_params = {params['camera_namespace'] +"/"+params['camera_name']: ros_params}
|
||||
with open(tmp_yaml.name, 'w') as f:
|
||||
yaml.dump(camera_params, f)
|
||||
|
||||
'''
|
||||
comment out the '#prefix' line, if you like gdb and want to debug the code, you may have to do more
|
||||
if you have more than one rs node.
|
||||
'''
|
||||
loglevel = "info"
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
loglevel = "debug"
|
||||
print("loglevel", loglevel)
|
||||
return launch_ros.actions.Node(
|
||||
package='realsense2_camera',
|
||||
namespace=params["camera_namespace"],
|
||||
name=params["camera_name"],
|
||||
#prefix=['xterm -e gdb -ex=run --args'],
|
||||
executable='realsense2_camera_node',
|
||||
parameters=[tmp_yaml.name],
|
||||
output='screen',
|
||||
arguments=['--ros-args', '--log-level', loglevel],
|
||||
emulate_tty=True,
|
||||
)
|
||||
|
||||
@launch_pytest.fixture
|
||||
def launch_descr_with_parameters(request):
|
||||
params = request.param
|
||||
changed_params = request.param
|
||||
params = get_default_params()
|
||||
for key, value in changed_params.items():
|
||||
params[key] = value
|
||||
if 'camera_name' not in changed_params:
|
||||
params['camera_name'] = 'camera_with_params'
|
||||
device_type = LaunchConfiguration('device_type', default=params['device_type'])
|
||||
loglevel = "info"
|
||||
if LOGGER.getEffectiveLevel() <= logging.DEBUG:
|
||||
loglevel = "debug"
|
||||
ld = LaunchDescription([
|
||||
launch_ros.actions.Node(
|
||||
package='realsense2_ros_mqtt_bridge',
|
||||
executable='realsense2_ros_mqtt_bridge',
|
||||
name='realsense2_ros_mqtt_bridge',
|
||||
arguments=['--ros-args', '--log-level', loglevel],
|
||||
output='screen',
|
||||
respawn=True,
|
||||
respawn_delay=1,
|
||||
),
|
||||
get_rs_node_description(params),
|
||||
DeclareLaunchArgument(
|
||||
'device_type',
|
||||
default_value=device_type,
|
||||
description='Specifying the device type'),
|
||||
launch_pytest.actions.ReadyToTest(),
|
||||
]),params
|
||||
return ld
|
||||
@@ -0,0 +1,429 @@
|
||||
# Copyright 2024 RealSense, Inc. All Rights Reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
import collections
|
||||
import time
|
||||
import threading
|
||||
import numpy as np
|
||||
from rclpy.node import Node
|
||||
import rclpy
|
||||
|
||||
from rcl_interfaces.msg import SetParametersResult
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
from realsense2_camera_msgs.srv import SafetyPresetRead
|
||||
from realsense2_camera_msgs.srv import SafetyPresetWrite
|
||||
from realsense2_camera_msgs.srv import SafetyInterfaceConfigRead
|
||||
from realsense2_camera_msgs.srv import SafetyInterfaceConfigWrite
|
||||
from realsense2_camera_msgs.srv import CalibConfigRead
|
||||
from realsense2_camera_msgs.srv import CalibConfigWrite
|
||||
from realsense2_camera_msgs.srv import ApplicationConfigRead
|
||||
from realsense2_camera_msgs.srv import ApplicationConfigWrite
|
||||
from realsense2_camera_msgs.srv import DeviceInfo
|
||||
from realsense2_camera_msgs.action import TriggeredCalibration
|
||||
from rclpy.action import ActionServer, CancelResponse, GoalResponse
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
|
||||
import logging
|
||||
LOGGER = logging.getLogger()
|
||||
|
||||
'''
|
||||
This is that holds the test node that listens to a subscription created by a test.
|
||||
'''
|
||||
class RSCameraSimulator(Node, threading.Thread):
|
||||
def __init__(self, namespace="camera", name='RSCameraSimulator'):
|
||||
LOGGER.debug('Creating node... /' + namespace + '/' + name)
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
Node.__init__(self,namespace=namespace, node_name=name)
|
||||
threading.Thread.__init__(self)
|
||||
self._stop_event = threading.Event()
|
||||
self.add_on_set_parameters_callback(self.parameter_callback)
|
||||
self.color_frame = None
|
||||
self.depth_frame = None
|
||||
self.infra1_frame = None
|
||||
self.infra2_frame = None
|
||||
self.namespace = namespace
|
||||
self.name = name
|
||||
self.goal_queue = collections.deque()
|
||||
self.goal_queue_lock = threading.Lock()
|
||||
self.current_goal = None
|
||||
|
||||
def run(self):
|
||||
LOGGER.debug("Thread started...")
|
||||
loop_count = 0
|
||||
executor = MultiThreadedExecutor()
|
||||
while(self._stop_event.is_set() == False):
|
||||
if loop_count > 10:
|
||||
LOGGER.debug("Spinning...")
|
||||
loop_count = 0
|
||||
if self.color_frame != None:
|
||||
self.publish_color_frame()
|
||||
if self.depth_frame != None:
|
||||
self.publish_depth_frame()
|
||||
if self.infra1_frame != None:
|
||||
self.publish_infra1_frame()
|
||||
if self.infra2_frame != None:
|
||||
self.publish_infra2_frame()
|
||||
rclpy.spin_once(self, timeout_sec=0.01, executor=executor)
|
||||
|
||||
LOGGER.info("destroying the publisher")
|
||||
if self.color_frame != None:
|
||||
self.destroy_publisher(self.color_frame)
|
||||
if self.depth_frame != None:
|
||||
self.destroy_publisher(self.depth_frame)
|
||||
if self.infra1_frame != None:
|
||||
self.destroy_publisher(self.infra1_frame)
|
||||
if self.infra2_frame != None:
|
||||
self.destroy_publisher(self.infra2_frame)
|
||||
self.destroy_node()
|
||||
|
||||
|
||||
def stop(self):
|
||||
LOGGER.debug("Setting the stop event...")
|
||||
self._stop_event.set()
|
||||
self.join()
|
||||
|
||||
def publish_color_frame(self):
|
||||
msg = Image()
|
||||
frame = np.zeros((4,4,0), dtype=int)
|
||||
msg.header.stamp = Node.get_clock(self).now().to_msg()
|
||||
msg.header.frame_id = 'test'
|
||||
msg.height = np.shape(frame)[0]
|
||||
msg.width = np.shape(frame)[1]
|
||||
msg.encoding = "rgb"
|
||||
msg.is_bigendian = False
|
||||
msg.step = np.shape(frame)[2] * np.shape(frame)[1]
|
||||
msg.data = np.array(frame).tobytes()
|
||||
# publishes message
|
||||
# LOGGER.debug("Publishing color frame...")
|
||||
self.color_frame.publish(msg)
|
||||
|
||||
def publish_depth_frame(self):
|
||||
msg = Image()
|
||||
frame = np.zeros((4,4,0), dtype=int)
|
||||
msg.header.stamp = Node.get_clock(self).now().to_msg()
|
||||
msg.header.frame_id = 'test'
|
||||
msg.height = np.shape(frame)[0]
|
||||
msg.width = np.shape(frame)[1]
|
||||
msg.encoding = "rgb"
|
||||
msg.is_bigendian = False
|
||||
msg.step = np.shape(frame)[2] * np.shape(frame)[1]
|
||||
msg.data = np.array(frame).tobytes()
|
||||
# publishes message
|
||||
# LOGGER.debug("Publishing depth frame...")
|
||||
self.depth_frame.publish(msg)
|
||||
|
||||
def publish_infra1_frame(self):
|
||||
msg = Image()
|
||||
frame = np.zeros((4,4,0), dtype=int)
|
||||
msg.header.stamp = Node.get_clock(self).now().to_msg()
|
||||
msg.header.frame_id = 'test'
|
||||
msg.height = np.shape(frame)[0]
|
||||
msg.width = np.shape(frame)[1]
|
||||
msg.encoding = "rgb"
|
||||
msg.is_bigendian = False
|
||||
msg.step = np.shape(frame)[2] * np.shape(frame)[1]
|
||||
msg.data = np.array(frame).tobytes()
|
||||
# publishes message
|
||||
# LOGGER.debug("Publishing infra1 frame...")
|
||||
self.infra1_frame.publish(msg)
|
||||
|
||||
def publish_infra2_frame(self):
|
||||
msg = Image()
|
||||
frame = np.zeros((4,4,0), dtype=int)
|
||||
msg.header.stamp = Node.get_clock(self).now().to_msg()
|
||||
msg.header.frame_id = 'test'
|
||||
msg.height = np.shape(frame)[0]
|
||||
msg.width = np.shape(frame)[1]
|
||||
msg.encoding = "rgb"
|
||||
msg.is_bigendian = False
|
||||
msg.step = np.shape(frame)[2] * np.shape(frame)[1]
|
||||
msg.data = np.array(frame).tobytes()
|
||||
# publishes message
|
||||
# LOGGER.debug("Publishing infra2 frame...")
|
||||
self.infra2_frame.publish(msg)
|
||||
|
||||
def start_publish_color_frame(self):
|
||||
if self.color_frame != None:
|
||||
LOGGER.warning(f'Color frame is already being published..')
|
||||
return
|
||||
queue = 1
|
||||
self.color_frame = self.create_publisher(Image, '/' + self.namespace + '/' + self.name + '/color/image_raw', queue)
|
||||
|
||||
|
||||
def start_publish_depth_frame(self):
|
||||
if self.depth_frame != None:
|
||||
LOGGER.warning(f'Depth frame is already being published..')
|
||||
return
|
||||
queue = 1
|
||||
self.depth_frame = self.create_publisher(Image, '/' + self.namespace + '/' + self.name + '/depth/image_rect_raw', queue)
|
||||
|
||||
|
||||
def start_publish_infra1_frame(self):
|
||||
if self.infra1_frame != None:
|
||||
LOGGER.warning(f'Infra1 frame is already being published..')
|
||||
return
|
||||
queue = 1
|
||||
self.infra1_frame = self.create_publisher(Image, '/' + self.namespace + '/' + self.name + '/infra1/image_rect_raw', queue)
|
||||
|
||||
def start_publish_infra2_frame(self):
|
||||
if self.infra2_frame != None:
|
||||
LOGGER.warning(f'Infra2 frame is already being published..')
|
||||
return
|
||||
queue = 1
|
||||
self.infra2_frame = self.create_publisher(Image, '/' + self.namespace + '/' + self.name + '/infra2/image_rect_raw', queue)
|
||||
|
||||
def add_parameters(self, params):
|
||||
try:
|
||||
for param in params:
|
||||
self.declare_parameter(param['param_name'], param['default_value'])
|
||||
except Exception as e:
|
||||
LOGGER.warning(f'An unexpected error occurred: {e}')
|
||||
|
||||
def parameter_callback(self, params):
|
||||
LOGGER.debug("Params changed: " + str(params))
|
||||
for param in params:
|
||||
if param.type_ == rclpy.Parameter.Type.INTEGER:
|
||||
if param.name == 'safety_camera.safety_mode':
|
||||
self.safety_camera_safety_mode = param.value
|
||||
LOGGER.info("Safety mode changed to " + str(self.safety_camera_safety_mode))
|
||||
else:
|
||||
LOGGER.info(param.name + " (int)value changed to " + str(param.value))
|
||||
elif param.type_ == rclpy.Parameter.Type.STRING:
|
||||
LOGGER.info(param.name + " (string)value changed to " + str(param.value))
|
||||
elif param.type_ == rclpy.Parameter.Type.BOOL:
|
||||
LOGGER.info(param.name + " (bool)value changed to " + str(param.value))
|
||||
else:
|
||||
LOGGER.warning("Unexpected param type: " + str(param.type_) + ". "+ param.name + " (bool)value changed to " + str(param.value))
|
||||
return SetParametersResult(successful=False)
|
||||
return SetParametersResult(successful=True)
|
||||
|
||||
def create_safety_preset_service(self):
|
||||
service_name = f'/{self.namespace}/{self.name}/safety_preset_read'
|
||||
self.safety_preset_read_srv = self.create_service(SafetyPresetRead, service_name, self.safety_preset_read_cb)
|
||||
service_name = f'/{self.namespace}/{self.name}/safety_preset_write'
|
||||
self.safety_preset_write_srv = self.create_service(SafetyPresetWrite, service_name, self.safety_preset_write_cb)
|
||||
self.safety_preset = ["Uninitialized"] * 64
|
||||
|
||||
def safety_preset_read_cb(self, request, response):
|
||||
LOGGER.info(f'Safety preset read for index {request.index}')
|
||||
response.success = True
|
||||
response.error_message = ''
|
||||
if self.safety_preset[request.index] == None:
|
||||
response.safety_preset = str(request.index)
|
||||
else:
|
||||
response.safety_preset = self.safety_preset[request.index]
|
||||
return response
|
||||
|
||||
def safety_preset_write_cb(self, request, response):
|
||||
LOGGER.info(f'Safety preset write for index {request.index} with data {request.safety_preset}')
|
||||
response.success = True
|
||||
response.error_message = ''
|
||||
self.safety_preset[request.index] = request.safety_preset
|
||||
return response
|
||||
|
||||
def create_safety_interface_config_service(self):
|
||||
service_name = f'/{self.namespace}/{self.name}/safety_interface_config_read'
|
||||
self.safety_interface_config_read_srv = self.create_service(SafetyInterfaceConfigRead, service_name, self.safety_interface_config_read_cb)
|
||||
service_name = f'/{self.namespace}/{self.name}/safety_interface_config_write'
|
||||
self.safety_interface_config_write_srv = self.create_service(SafetyInterfaceConfigWrite, service_name, self.safety_interface_config_write_cb)
|
||||
self.safety_interface_config = ["Uninitialized"] * 3
|
||||
|
||||
def safety_interface_config_read_cb(self, request, response):
|
||||
LOGGER.info(f'Safety interface config read for calib location {request.calib_location}')
|
||||
response.success = True
|
||||
response.error_message = ''
|
||||
response.safety_interface_config = self.safety_interface_config[request.calib_location]
|
||||
return response
|
||||
|
||||
def safety_interface_config_write_cb(self, request, response):
|
||||
LOGGER.info(f'Safety interface config write with data {request.safety_interface_config}')
|
||||
response.success = True
|
||||
response.error_message = ''
|
||||
self.safety_interface_config[2] = request.safety_interface_config
|
||||
return response
|
||||
|
||||
def create_calib_config_service(self):
|
||||
service_name = f'/{self.namespace}/{self.name}/calib_config_read'
|
||||
self.calib_config_read_srv = self.create_service(CalibConfigRead, service_name, self.calib_config_read_cb)
|
||||
service_name = f'/{self.namespace}/{self.name}/calib_config_write'
|
||||
self.calib_config_srv = self.create_service(CalibConfigWrite, service_name, self.calib_config_write_cb)
|
||||
self.calib_config = "Uninitialized"
|
||||
|
||||
def calib_config_read_cb(self, request, response):
|
||||
LOGGER.info(f'Calib config read called {response}')
|
||||
response.success = True
|
||||
response.error_message = ''
|
||||
response.calib_config = self.calib_config
|
||||
return response
|
||||
|
||||
def calib_config_write_cb(self, request, response):
|
||||
LOGGER.info(f'Calibration config write data {request.calib_config}')
|
||||
response.success = True
|
||||
response.error_message = ''
|
||||
self.calib_config = request.calib_config
|
||||
return response
|
||||
|
||||
def create_application_config_service(self):
|
||||
service_name = f'/{self.namespace}/{self.name}/application_config_read'
|
||||
self.application_config_read_srv = self.create_service(ApplicationConfigRead, service_name, self.application_config_read_cb)
|
||||
service_name = f'/{self.namespace}/{self.name}/application_config_write'
|
||||
self.application_config_srv = self.create_service(ApplicationConfigWrite, service_name, self.application_config_write_cb)
|
||||
self.application_config = "Uniinitialized"
|
||||
|
||||
def application_config_read_cb(self, request, response):
|
||||
LOGGER.info(f'Application config read called')
|
||||
response.success = True
|
||||
response.error_message = ''
|
||||
response.application_config = self.application_config
|
||||
return response
|
||||
|
||||
def application_config_write_cb(self, request, response):
|
||||
LOGGER.info(f'Application config write with data {request.application_config}')
|
||||
response.success = True
|
||||
response.error_message = ''
|
||||
self.application_config = request.application_config
|
||||
return response
|
||||
|
||||
def create_device_info_service(self):
|
||||
LOGGER.info(f'Created the device info service')
|
||||
service_name = f'/{self.namespace}/{self.name}/device_info'
|
||||
self.device_info_srv = self.create_service(DeviceInfo, service_name, self.device_info_read_cb)
|
||||
|
||||
|
||||
def device_info_read_cb(self, request, response):
|
||||
LOGGER.debug(f'DeviceInfo read called \n{request} \n{response}')
|
||||
response.device_name = "Camera" #string device_name
|
||||
response.serial_number = "1234" #string serial_number
|
||||
response.firmware_version = "1.2.3" #string firmware_version
|
||||
response.usb_type_descriptor = "USB 3.1" #string usb_type_descriptor
|
||||
response.firmware_update_id = "1" #string firmware_update_id
|
||||
response.sensors = "color"#string sensors
|
||||
response.physical_port = "1" #string physical_port
|
||||
LOGGER.info(f'DeviceInfo response: {response}')
|
||||
return response
|
||||
|
||||
def create_triggered_calibration_action(self):
|
||||
action_name = f'/{self.namespace}/{self.name}/triggered_calibration'
|
||||
'''
|
||||
self._action_server = ActionServer(
|
||||
self,
|
||||
TriggeredCalibration,
|
||||
action_name,
|
||||
self.triggered_calibration_handler)
|
||||
'''
|
||||
self._action_server = ActionServer(
|
||||
self,
|
||||
TriggeredCalibration,
|
||||
action_name,
|
||||
handle_accepted_callback=self.handle_accepted_callback,
|
||||
execute_callback=self.triggered_calibration_handler,
|
||||
goal_callback=self.goal_callback,
|
||||
cancel_callback=self.cancel_callback,
|
||||
callback_group=ReentrantCallbackGroup())
|
||||
|
||||
def handle_accepted_callback(self, goal_handle):
|
||||
"""Start or defer execution of an already accepted goal."""
|
||||
with self.goal_queue_lock:
|
||||
if self.current_goal is not None:
|
||||
# Put incoming goal in the queue
|
||||
self.goal_queue.append(goal_handle)
|
||||
LOGGER.info('Goal put in the queue')
|
||||
else:
|
||||
# Start goal execution right away
|
||||
self.current_goal = goal_handle
|
||||
LOGGER.info('Start the execution')
|
||||
self.current_goal.execute()
|
||||
|
||||
def goal_callback(self, goal_request):
|
||||
"""Accept or reject a client request to begin an action."""
|
||||
LOGGER.info(f'Received goal request {goal_request}')
|
||||
return GoalResponse.ACCEPT
|
||||
|
||||
def cancel_callback(self, goal_handle):
|
||||
"""Accept or reject a client request to cancel an action."""
|
||||
LOGGER.info('Received cancel request')
|
||||
return CancelResponse.ACCEPT
|
||||
|
||||
def triggered_calibration_handler(self, goal_handle):
|
||||
LOGGER.info(f'Calibration request: {goal_handle.request.json}')
|
||||
"""Execute a goal."""
|
||||
try:
|
||||
LOGGER.info('Executing goal...')
|
||||
# Append the seeds for the Fibonacci sequence
|
||||
feedback_msg = TriggeredCalibration.Feedback()
|
||||
|
||||
# Start executing the action
|
||||
for i in range(1, 100):
|
||||
if goal_handle.is_cancel_requested:
|
||||
goal_handle.canceled()
|
||||
LOGGER.info('Goal canceled')
|
||||
result = TriggeredCalibration.Result()
|
||||
result.success = False
|
||||
result.error_msg = 'Canceled'
|
||||
result.calibration = '{}'
|
||||
LOGGER.warning('Calibration aborted and senting status success and error message aborted. This needs to be ratified after understanding the actual implementation')
|
||||
return result
|
||||
|
||||
# Update Fibonacci sequence
|
||||
feedback_msg.progress = float(i)
|
||||
|
||||
LOGGER.info(
|
||||
'Publishing feedback: {0}'.format(feedback_msg.progress))
|
||||
|
||||
# Publish the feedback
|
||||
goal_handle.publish_feedback(feedback_msg)
|
||||
|
||||
# Sleep for demonstration purposes
|
||||
time.sleep(0.01)
|
||||
|
||||
goal_handle.succeed()
|
||||
|
||||
# Populate result message
|
||||
result = TriggeredCalibration.Result()
|
||||
result.success = True
|
||||
result.calibration = f'{goal_handle.request.json}'
|
||||
|
||||
LOGGER.info(
|
||||
'Returning result: {0}'.format(result))
|
||||
|
||||
return result
|
||||
finally:
|
||||
with self.goal_queue_lock:
|
||||
try:
|
||||
# Start execution of the next goal in the queue.
|
||||
self.current_goal = self.goal_queue.popleft()
|
||||
LOGGER.info('Next goal pulled from the queue')
|
||||
self.current_goal.execute()
|
||||
except IndexError:
|
||||
# No goal in the queue.
|
||||
self.current_goal = None
|
||||
|
||||
if __name__ == '__main__':
|
||||
rclpy.init()
|
||||
import os
|
||||
namespace = 'camera'
|
||||
name = 'camera'
|
||||
camera = RSCameraSimulator(namespace, name)
|
||||
camera.start()
|
||||
os.system("ros2 node list")
|
||||
import time
|
||||
rclpy.spin(camera)
|
||||
time.sleep(10)
|
||||
rclpy.shutdown()
|
||||
camera.stop()
|
||||
LOGGER.info("Test completed")
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user