Skip to content

Commit 9c500e7

Browse files
lreiherlreiher
authored
Merge pull request #106 from Tezozomoc47/make-converter-service-based
Add the service-based approach for message conversion
2 parents f0ef04b + 7b58905 commit 9c500e7

36 files changed

+1052
-86
lines changed

‎README.md‎

Lines changed: 33 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -156,7 +156,7 @@ The conversion node bridges all ETSI ITS message types at the same time in both
156156
```bash
157157
ros2 launch etsi_its_conversion converter.launch.py
158158
# or
159-
ros2 run etsi_its_conversion etsi_its_conversion_node --ros-args -p etsi_types:=[cam,cpm_ts,denm,mapem_ts,mcm_uulm,spatem_ts,vam_ts] -p has_btp_destination_port:=true -p btp_destination_port_offset:=8 -p etsi_message_payload_offset:=78
159+
ros2 run etsi_its_conversion etsi_its_conversion_node --ros-args -p etsi_types:=[cam,cpm_ts,denm,mapem_ts,mcm_uulm,spatem_ts,vam_ts] -p has_btp_destination_port:=true -p btp_destination_port_offset:=0 -p etsi_message_payload_offset:=4
160160
```
161161

162162
#### Subscribed Topics
@@ -189,17 +189,40 @@ ros2 run etsi_its_conversion etsi_its_conversion_node --ros-args -p etsi_types:=
189189
| `~/spatem_ts/out` | `etsi_its_spatem_ts_msgs/msg/SPATEM` | SPATEM (TS) converted from UDP payload |
190190
| `~/vam_ts/out` | `etsi_its_vam_ts_msgs/msg/VAM` | VAM (TS) converted from UDP payload |
191191

192+
#### Services
193+
194+
| Service | Type | Description |
195+
| --- | --- | --- |
196+
| `~/cam/udp` | `etsi_its_conversion_srvs/srv/ConvertCamToUdp` | Convert ROS CAM to UDP payload |
197+
| `~/cam_ts/udp` | `etsi_its_conversion_srvs/srv/ConvertCamTsoUdp` | Convert ROS CAM (TS) to UDP payload |
198+
| `~/cpm_ts/udp` | `etsi_its_conversion_srvs/srv/ConvertCpmTsToUdp` | Convert ROS CPM (TS) to UDP payload |
199+
| `~/denm/udp` | `etsi_its_conversion_srvs/srv/ConvertDenmToUdp` | Convert ROS DENM to UDP payload |
200+
| `~/denm_ts/udp` | `etsi_its_conversion_srvs/srv/ConvertDenmTsToUdp` | Convert ROS DENM (TS) to UDP payload |
201+
| `~/mapem_ts/udp` | `etsi_its_conversion_srvs/srv/ConvertMapemTsToUdp` | Convert ROS MAPEM (TS) to UDP payload |
202+
| `~/mcm_uulm/udp` | `etsi_its_conversion_srvs/srv/ConvertMcmUulmToUdp` | Convert ROS MCM (UULM) to UDP payload |
203+
| `~/spatem_ts/udp` | `etsi_its_conversion_srvs/srv/ConvertSpatemTsToUdp` | Convert ROS SPATEM (TS) to UDP payload |
204+
| `~/vam_ts/udp` | `etsi_its_conversion_srvs/srv/ConvertVamTsoUdp` | Convert ROS VAM (TS) to UDP payload |
205+
| `~/udp/cam` | `etsi_its_conversion_srvs/srv/ConvertUdpToCam` | Convert UDP payload to ROS CAM |
206+
| `~/udp/cam_ts` | `etsi_its_conversion_srvs/srv/ConvertUdpToCamTs` | Convert UDP payload to ROS CAM (TS) |
207+
| `~/udp/cpm_ts` | `etsi_its_conversion_srvs/srv/ConvertUdpToCpmTs` | Convert UDP payload to ROS CPM (TS) |
208+
| `~/udp/denm` | `etsi_its_conversion_srvs/srv/ConvertUdpToDenm` | Convert UDP payload to ROS DENM |
209+
| `~/udp/denm_ts` | `etsi_its_conversion_srvs/srv/ConvertUdpToDenmTs` | Convert UDP payload to ROS DENM (TS) |
210+
| `~/udp/mapem_ts` | `etsi_its_conversion_srvs/srv/ConvertUdpToMapemTs` | Convert UDP payload to ROS MAPEM (TS) |
211+
| `~/udp/mcm_uulm` | `etsi_its_conversion_srvs/srv/ConvertUdpToMcmUulm` | Convert UDP payload to ROS MCM (UULM) |
212+
| `~/udp/spatem_ts` | `etsi_its_conversion_srvs/srv/ConvertUdpToSpatemTs` | Convert UDP payload to ROS SPATEM (TS) |
213+
| `~/udp/vam_ts` | `etsi_its_conversion_srvs/srv/ConvertUdpToVamTs` | Convert UDP payload to ROS VAM (TS) |
214+
192215
#### Parameters
193216

194-
| Parameter | Type | Description | Options |
195-
| --- | --- | --- | --- |
196-
| `has_btp_destination_port` | `bool` | whether incoming/outgoing UDP messages include a [2-byte BTP destination port](https://www.etsi.org/deliver/etsi_en/302600_302699/3026360501/02.01.00_20/en_3026360501v020100a.pdf) in their payload; for incoming UDP payloads: if `false`, destination port is expected in `src_port` field with fallback of `udp2ros_etsi_types[0]` |
197-
| `btp_destination_port_offset` | `int` | number of bytes before an optional 2-byte BTP destination port, see `has_btp_destination_port` (always `0` in outgoing UDP payload) |
198-
| `etsi_message_payload_offset` | `int` | number of bytes before actual ETSI message payload (always `0` or `4` (if `has_btp_destination_port`) in outgoing UDP payload) |
199-
| `ros2udp_etsi_types` | `string[]` | list of ETSI types to convert from `etsi_its_msgs` to `udp_msgs` (defaults to all supported ETSI types, including EN and TS versions) | `cam`, `cam_ts`, `cpm_ts`, `denm`, `denm_ts`, `mapem_ts`, `mcm_uulm`, `spatem_ts`, `vam_ts` |
200-
| `udp2ros_etsi_types` | `string[]` | list of ETSI types to convert from `udp_msgs` to `etsi_its_msgs` (defaults to all supported ETSI types, either EN or TS version) | `cam`, `cam_ts`, `cpm_ts`, `denm`, `denm_ts`, `mapem_ts`, `mcm_uulm`, `spatem_ts`, `vam_ts` |
201-
| `subscriber_queue_size` | `int` | queue size for incoming ROS messages |
202-
| `publisher_queue_size` | `int` | queue size for outgoing ROS messages |
217+
| Parameter | Type | Default | Options | Description |
218+
| --- | --- | --- | --- | --- |
219+
| `has_btp_destination_port` | `bool` | `true` | | whether incoming/outgoing UDP messages include a [2-byte BTP destination port](https://www.etsi.org/deliver/etsi_en/302600_302699/3026360501/02.01.00_20/en_3026360501v020100a.pdf) in their payload; for incoming UDP payloads: if `false`, destination port is expected in `src_port` field with fallback of `udp2ros_etsi_types[0]` |
220+
| `btp_destination_port_offset` | `int` | `0` | | number of bytes before an optional 2-byte BTP destination port, see `has_btp_destination_port` (always `0` in outgoing UDP payload) |
221+
| `etsi_message_payload_offset` | `int` | `4` | | number of bytes before actual ETSI message payload (always `0` or `4` (if `has_btp_destination_port`) in outgoing UDP payload) |
222+
| `ros2udp_etsi_types` | `string[]` | `cam`, `cam_ts`, `cpm_ts`, `denm`, `denm_ts`, `mapem_ts`, `mcm_uulm`, `spatem_ts`, `vam_ts` | `cam`, `cam_ts`, `cpm_ts`, `denm`, `denm_ts`, `mapem_ts`, `mcm_uulm`, `spatem_ts`, `vam_ts` | list of ETSI types to convert from `etsi_its_msgs` to `udp_msgs` |
223+
| `udp2ros_etsi_types` | `string[]` | `cam`, `cpm_ts`, `denm`, `mapem_ts`, `mcm_uulm`, `spatem_ts`, `vam_ts` | `cam`, `cam_ts`, `cpm_ts`, `denm`, `denm_ts`, `mapem_ts`, `mcm_uulm`, `spatem_ts`, `vam_ts` | list of ETSI types to convert from `udp_msgs` to `etsi_its_msgs` |
224+
| `subscriber_queue_size` | `int` | `10` | | queue size for incoming ROS messages |
225+
| `publisher_queue_size` | `int` | `10` | | queue size for outgoing ROS messages |
203226

204227

205228
## Sample Messages

‎etsi_its_conversion/etsi_its_conversion/CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@ project(etsi_its_conversion)
44
find_package(ament_cmake REQUIRED)
55
find_package(etsi_its_cam_conversion REQUIRED)
66
find_package(etsi_its_cam_ts_conversion REQUIRED)
7+
find_package(etsi_its_conversion_srvs REQUIRED)
78
find_package(etsi_its_cpm_ts_conversion REQUIRED)
89
find_package(etsi_its_denm_conversion REQUIRED)
910
find_package(etsi_its_denm_ts_conversion REQUIRED)
@@ -13,8 +14,13 @@ find_package(etsi_its_spatem_ts_conversion REQUIRED)
1314
find_package(etsi_its_vam_ts_conversion REQUIRED)
1415
find_package(rclcpp REQUIRED)
1516
find_package(rclcpp_components REQUIRED)
17+
find_package(ros_environment REQUIRED)
1618
find_package(udp_msgs REQUIRED)
1719

20+
if("$ENV{ROS_DISTRO}" STREQUAL "humble")
21+
add_compile_definitions(ROS_DISTRO_HUMBLE)
22+
endif()
23+
1824
add_library(${PROJECT_NAME} SHARED src/Converter.cpp)
1925

2026
rclcpp_components_register_node(${PROJECT_NAME}
@@ -35,6 +41,7 @@ target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17)
3541
ament_target_dependencies(${PROJECT_NAME}
3642
etsi_its_cam_conversion
3743
etsi_its_cam_ts_conversion
44+
etsi_its_conversion_srvs
3845
etsi_its_cpm_ts_conversion
3946
etsi_its_denm_conversion
4047
etsi_its_denm_ts_conversion

‎etsi_its_conversion/etsi_its_conversion/config/params.yml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,8 @@
11
/**/*:
22
ros__parameters:
33
has_btp_destination_port: true
4-
btp_destination_port_offset: 8 # default for Cohda Wireless MK-5 exampleETSI application
5-
etsi_message_payload_offset: 78 # default for Cohda Wireless MK-5 exampleETSI application
4+
btp_destination_port_offset: 0
5+
etsi_message_payload_offset: 4
66
ros2udp_etsi_types:
77
- cam
88
- cam_ts

‎etsi_its_conversion/etsi_its_conversion/include/etsi_its_conversion/Converter.hpp‎

Lines changed: 67 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,24 @@ SOFTWARE.
2828
#include <string>
2929
#include <unordered_map>
3030

31+
#include <etsi_its_conversion_srvs/srv/convert_cam_to_udp.hpp>
32+
#include <etsi_its_conversion_srvs/srv/convert_cam_ts_to_udp.hpp>
33+
#include <etsi_its_conversion_srvs/srv/convert_cpm_ts_to_udp.hpp>
34+
#include <etsi_its_conversion_srvs/srv/convert_denm_to_udp.hpp>
35+
#include <etsi_its_conversion_srvs/srv/convert_denm_ts_to_udp.hpp>
36+
#include <etsi_its_conversion_srvs/srv/convert_mapem_ts_to_udp.hpp>
37+
#include <etsi_its_conversion_srvs/srv/convert_mcm_uulm_to_udp.hpp>
38+
#include <etsi_its_conversion_srvs/srv/convert_spatem_ts_to_udp.hpp>
39+
#include <etsi_its_conversion_srvs/srv/convert_vam_ts_to_udp.hpp>
40+
#include <etsi_its_conversion_srvs/srv/convert_udp_to_cam.hpp>
41+
#include <etsi_its_conversion_srvs/srv/convert_udp_to_cam_ts.hpp>
42+
#include <etsi_its_conversion_srvs/srv/convert_udp_to_cpm_ts.hpp>
43+
#include <etsi_its_conversion_srvs/srv/convert_udp_to_denm.hpp>
44+
#include <etsi_its_conversion_srvs/srv/convert_udp_to_denm_ts.hpp>
45+
#include <etsi_its_conversion_srvs/srv/convert_udp_to_mapem_ts.hpp>
46+
#include <etsi_its_conversion_srvs/srv/convert_udp_to_mcm_uulm.hpp>
47+
#include <etsi_its_conversion_srvs/srv/convert_udp_to_spatem_ts.hpp>
48+
#include <etsi_its_conversion_srvs/srv/convert_udp_to_vam_ts.hpp>
3149
#include <etsi_its_cam_conversion/convertCAM.h>
3250
#include <etsi_its_cam_ts_conversion/convertCAM.h>
3351
#include <etsi_its_cpm_ts_conversion/convertCollectivePerceptionMessage.h>
@@ -49,11 +67,10 @@ SOFTWARE.
4967
#include <rclcpp/rclcpp.hpp>
5068
#include <udp_msgs/msg/udp_packet.hpp>
5169

52-
5370
namespace etsi_its_conversion {
5471

55-
5672
using namespace udp_msgs::msg;
73+
namespace conversion_srvs = etsi_its_conversion_srvs::srv;
5774
namespace cam_msgs = etsi_its_cam_msgs::msg;
5875
namespace cam_ts_msgs = etsi_its_cam_ts_msgs::msg;
5976
namespace cpm_ts_msgs = etsi_its_cpm_ts_msgs::msg;
@@ -64,15 +81,12 @@ namespace mcm_uulm_msgs = etsi_its_mcm_uulm_msgs::msg;
6481
namespace spatem_ts_msgs = etsi_its_spatem_ts_msgs::msg;
6582
namespace vam_ts_msgs = etsi_its_vam_ts_msgs::msg;
6683

67-
6884
class Converter : public rclcpp::Node {
6985

7086
public:
71-
7287
explicit Converter(const rclcpp::NodeOptions& options);
7388

7489
protected:
75-
7690
void loadParameters();
7791

7892
void setup();
@@ -99,34 +113,61 @@ class Converter : public rclcpp::Node {
99113
template <typename T_ros, typename T_struct>
100114
bool encodeRosMessageToUdpPacketMessage(const T_ros& msg, UdpPacket& udp_msg, const asn_TYPE_descriptor_t* type_descriptor, std::function<void(const T_ros&, T_struct&)> conversion_fn, const int btp_header_destination_port) const;
101115

116+
template <typename T_ros, typename T_struct, typename T_srv>
117+
void rosToUdpSrvCallback(const std::shared_ptr<typename T_srv::Request> request,
118+
std::shared_ptr<typename T_srv::Response> response,
119+
const std::string& type,
120+
const asn_TYPE_descriptor_t* asn_type_descriptor,
121+
std::function<void(const T_ros&, T_struct&)> conversion_fn) const;
122+
123+
template <typename T_ros, typename T_struct, typename T_srv>
124+
void udpToRosSrvCallback(const std::shared_ptr<typename T_srv::Request> request, std::shared_ptr<typename T_srv::Response> response, const std::string& type, const asn_TYPE_descriptor_t* asn_type_descriptor, std::function<void(const T_struct&, T_ros&)> conversion_fn) const;
125+
102126
void udpCallback(const UdpPacket::UniquePtr udp_msg) const;
103127

104128
template <typename T_ros, typename T_struct>
105129
void rosCallback(const typename T_ros::UniquePtr msg,
106130
const std::string& type, const asn_TYPE_descriptor_t* type_descriptor, std::function<void(const T_ros&, T_struct&)> conversion_fn) const;
107131

108132
protected:
109-
110133
static const std::string kInputTopicUdp;
111134
static const std::string kOutputTopicUdp;
112135
static const std::string kInputTopicCam;
113136
static const std::string kOutputTopicCam;
137+
static const std::string kServiceCamToUdp;
138+
static const std::string kServiceUdpToCam;
114139
static const std::string kInputTopicCamTs;
115140
static const std::string kOutputTopicCamTs;
141+
static const std::string kServiceCamTsToUdp;
142+
static const std::string kServiceUdpToCamTs;
116143
static const std::string kInputTopicCpmTs;
117144
static const std::string kOutputTopicCpmTs;
145+
static const std::string kServiceCpmTsToUdp;
146+
static const std::string kServiceUdpToCpmTs;
118147
static const std::string kInputTopicDenm;
119148
static const std::string kOutputTopicDenm;
149+
static const std::string kServiceDenmToUdp;
150+
static const std::string kServiceUdpToDenm;
120151
static const std::string kInputTopicDenmTs;
121152
static const std::string kOutputTopicDenmTs;
153+
static const std::string kServiceDenmTsToUdp;
154+
static const std::string kServiceUdpToDenmTs;
122155
static const std::string kInputTopicMapemTs;
123156
static const std::string kOutputTopicMapemTs;
157+
static const std::string kServiceMapemTsToUdp;
158+
static const std::string kServiceUdpToMapemTs;
124159
static const std::string kInputTopicMcmUulm;
125160
static const std::string kOutputTopicMcmUulm;
161+
static const std::string kServiceMcmUulmToUdp;
162+
static const std::string kServiceUdpToMcmUulm;
126163
static const std::string kInputTopicSpatemTs;
127164
static const std::string kOutputTopicSpatemTs;
165+
static const std::string kServiceSpatemTsToUdp;
166+
static const std::string kServiceUdpToSpatemTs;
128167
static const std::string kInputTopicVamTs;
129168
static const std::string kOutputTopicVamTs;
169+
static const std::string kServiceVamTsToUdp;
170+
static const std::string kServiceUdpToVamTs;
130171

131172
static const std::string kHasBtpDestinationPortParam;
132173
static const bool kHasBtpDestinationPortParamDefault;
@@ -158,6 +199,26 @@ class Converter : public rclcpp::Node {
158199
rclcpp::Subscription<UdpPacket>::SharedPtr subscriber_udp_;
159200
std::unordered_map<std::string, rclcpp::SubscriptionBase::SharedPtr> subscribers_;
160201

202+
rclcpp::Service<conversion_srvs::ConvertCamToUdp>::SharedPtr convert_cam_to_udp_service_;
203+
rclcpp::Service<conversion_srvs::ConvertCamTsToUdp>::SharedPtr convert_cam_ts_to_udp_service_;
204+
rclcpp::Service<conversion_srvs::ConvertCpmTsToUdp>::SharedPtr convert_cpm_ts_to_udp_service_;
205+
rclcpp::Service<conversion_srvs::ConvertDenmToUdp>::SharedPtr convert_denm_to_udp_service_;
206+
rclcpp::Service<conversion_srvs::ConvertDenmTsToUdp>::SharedPtr convert_denm_ts_to_udp_service_;
207+
rclcpp::Service<conversion_srvs::ConvertMapemTsToUdp>::SharedPtr convert_mapem_ts_to_udp_service_;
208+
rclcpp::Service<conversion_srvs::ConvertMcmUulmToUdp>::SharedPtr convert_mcm_uulm_to_udp_service_;
209+
rclcpp::Service<conversion_srvs::ConvertSpatemTsToUdp>::SharedPtr convert_spatem_ts_to_udp_service_;
210+
rclcpp::Service<conversion_srvs::ConvertVamTsToUdp>::SharedPtr convert_vam_ts_to_udp_service_;
211+
212+
rclcpp::Service<conversion_srvs::ConvertUdpToCam>::SharedPtr convert_udp_to_cam_service_;
213+
rclcpp::Service<conversion_srvs::ConvertUdpToCamTs>::SharedPtr convert_udp_to_cam_ts_service_;
214+
rclcpp::Service<conversion_srvs::ConvertUdpToCpmTs>::SharedPtr convert_udp_to_cpm_ts_service_;
215+
rclcpp::Service<conversion_srvs::ConvertUdpToDenm>::SharedPtr convert_udp_to_denm_service_;
216+
rclcpp::Service<conversion_srvs::ConvertUdpToDenmTs>::SharedPtr convert_udp_to_denm_ts_service_;
217+
rclcpp::Service<conversion_srvs::ConvertUdpToMapemTs>::SharedPtr convert_udp_to_mapem_ts_service_;
218+
rclcpp::Service<conversion_srvs::ConvertUdpToMcmUulm>::SharedPtr convert_udp_to_mcm_uulm_service_;
219+
rclcpp::Service<conversion_srvs::ConvertUdpToSpatemTs>::SharedPtr convert_udp_to_spatem_ts_service_;
220+
rclcpp::Service<conversion_srvs::ConvertUdpToVamTs>::SharedPtr convert_udp_to_vam_ts_service_;
221+
161222
mutable rclcpp::Publisher<cam_msgs::CAM>::SharedPtr publisher_cam_;
162223
mutable rclcpp::Publisher<cam_ts_msgs::CAM>::SharedPtr publisher_cam_ts_;
163224
mutable rclcpp::Publisher<cpm_ts_msgs::CollectivePerceptionMessage>::SharedPtr publisher_cpm_ts_;
@@ -170,5 +231,4 @@ class Converter : public rclcpp::Node {
170231
mutable rclcpp::Publisher<UdpPacket>::SharedPtr publisher_udp_;
171232
};
172233

173-
174234
}

‎etsi_its_conversion/etsi_its_conversion/package.xml‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -15,20 +15,22 @@
1515

1616
<license>MIT</license>
1717

18+
<buildtool_depend>ament_cmake</buildtool_depend>
19+
1820
<depend>etsi_its_cam_conversion</depend>
1921
<depend>etsi_its_cam_ts_conversion</depend>
22+
<depend>etsi_its_conversion_srvs</depend>
2023
<depend>etsi_its_cpm_ts_conversion</depend>
2124
<depend>etsi_its_denm_conversion</depend>
2225
<depend>etsi_its_denm_ts_conversion</depend>
2326
<depend>etsi_its_mapem_ts_conversion</depend>
2427
<depend>etsi_its_mcm_uulm_conversion</depend>
2528
<depend>etsi_its_spatem_ts_conversion</depend>
2629
<depend>etsi_its_vam_ts_conversion</depend>
27-
<depend>udp_msgs</depend>
28-
29-
<buildtool_depend>ament_cmake</buildtool_depend>
3030
<depend>rclcpp</depend>
3131
<depend>rclcpp_components</depend>
32+
<depend>ros_environment</depend>
33+
<depend>udp_msgs</depend>
3234

3335
<export>
3436
<build_type>ament_cmake</build_type>

0 commit comments

Comments
 (0)