Skip to content

Commit

Permalink
adapt CPM sample scripts according to msg changes
Browse files Browse the repository at this point in the history
  • Loading branch information
jpbusch committed Nov 29, 2024
1 parent 7099454 commit 2ab6be5
Show file tree
Hide file tree
Showing 3 changed files with 9 additions and 13 deletions.
5 changes: 2 additions & 3 deletions etsi_its_msgs_utils/samples/publish_cpm_ts.py
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,7 @@ def publish(self):


cpm_container = WrappedCpmContainer()
cpm_container.container_id.value = cpm_container.container_id.PERCEIVED_OBJECT_CONTAINER
cpm_container.container_data.choice.value = cpm_container.container_id.value
cpm_container.container_id.value = cpm_container.CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER

perceived_object_container = PerceivedObjectContainer()
perceived_object_container.number_of_perceived_objects.value = 2
Expand All @@ -76,7 +75,7 @@ def publish(self):
perceived_object.object_dimension_z.confidence.value = perceived_object.object_dimension_z.confidence.UNAVAILABLE
perceived_object_container.perceived_objects.array.append(perceived_object)

cpm_container.container_data.perceived_object_container = perceived_object_container
cpm_container.container_data_perceived_object_container = perceived_object_container
msg.payload.cpm_containers.value.array.append(cpm_container)

self.get_logger().info(f"Publishing CPM")
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -135,19 +135,17 @@ def publish(self) -> None:

# Sensor information container
si_container = WrappedCpmContainer()
si_container.container_id.value = CpmContainerId.SENSOR_INFORMATION_CONTAINER
si_container.container_data.choice.value = si_container.container_id.value
si_container.container_data.sensor_information_container.array = [lidar_sensor, local_aggretation_sensor]
si_container.container_id.value = si_container.CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER
si_container.container_data_sensor_information_container.array = [lidar_sensor, local_aggretation_sensor]

# Get perceived object
perceived_object = self.get_perceived_object()

# Perceived object container
po_container = WrappedCpmContainer()
po_container.container_id.value = CpmContainerId.PERCEIVED_OBJECT_CONTAINER
po_container.container_data.choice.value = po_container.container_id.value
po_container.container_data.perceived_object_container.number_of_perceived_objects.value = 1
po_container.container_data.perceived_object_container.perceived_objects.array = [perceived_object]
po_container.container_id.value = po_container.CHOICE_CONTAINER_DATA_PERCEIVED_OBJECT_CONTAINER
po_container.container_data_perceived_object_container.number_of_perceived_objects.value = 1
po_container.container_data_perceived_object_container.perceived_objects.array = [perceived_object]

# Append wrapped containers
msg.payload.cpm_containers.value.array = [si_container, po_container]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -113,14 +113,13 @@ def publish(self) -> None:
msg.payload.management_container.reference_position.longitude.value = int(6.047076274316094 * 1e7)

cpm_container = WrappedCpmContainer()
cpm_container.container_id.value = CpmContainerId.SENSOR_INFORMATION_CONTAINER
cpm_container.container_data.choice.value = cpm_container.container_id.value
cpm_container.container_id.value = cpm_container.CHOICE_CONTAINER_DATA_SENSOR_INFORMATION_CONTAINER

# Get sensor informations
lidar_sensor = self.get_lidar_sensor_information()
local_aggretation_sensor = self.get_local_aggregation_sensor_information()

cpm_container.container_data.sensor_information_container.array = [lidar_sensor, local_aggretation_sensor]
cpm_container.container_data_sensor_information_container.array = [lidar_sensor, local_aggretation_sensor]
msg.payload.cpm_containers.value.array.append(cpm_container)

self.get_logger().info(f"Publishing CPM")
Expand Down

0 comments on commit 2ab6be5

Please sign in to comment.