Skip to content

Commit

Permalink
feat(aip_x2_launch): add topic state monitor for camera, radar, gnss …
Browse files Browse the repository at this point in the history 
…and imu (#227)

* feat: add topic state monitor for sensors

Signed-off-by: Tomohito Ando <[email protected]>

* fix comments

Signed-off-by: Tomohito Ando <[email protected]>

* replace launch.xml with launch.py

Signed-off-by: Tomohito Ando <[email protected]>

* ci(pre-commit): autofix

* add copyright

Signed-off-by: Tomohito Ando <[email protected]>

---------

Signed-off-by: Tomohito Ando <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@TomohitoAndo @pre-commit-ci @kotaro-hihara
2 people authored and kotaro-hihara committed Aug 27, 2024
1 parent 3de052e commit 1a9b241
Show file tree
Hide file tree
Showing 2 changed files with 360 additions and 0 deletions.
4 changes: 4 additions & 0 deletions aip_x2_launch/launch/sensing.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,10 @@
<arg name="output_twist_with_covariance" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
<arg name="config_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/vehicle_velocity_converter.param.yaml" />
</include>

<!-- Topic state monitor for each sensor -->
<include file="$(find-pkg-share aip_x2_launch)/launch/topic_state_monitor.launch.py"/>

</group>

</launch>
356 changes: 356 additions & 0 deletions aip_x2_launch/launch/topic_state_monitor.launch.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,356 @@
# Copyright 2024 Tier IV, 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 launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode


def generate_launch_description():
# GNSS topic monitor
gnss_topic_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_gnss_pose",
parameters=[
{
"topic": "/sensing/gnss/pose",
"topic_type": "geometry_msgs/msg/PoseStamped",
"best_effort": True,
"diag_name": "gnss_topic_status",
"warn_rate": 2.5,
"error_rate": 0.5,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

# IMU topic monitor
imu_topic_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_imu_data",
parameters=[
{
"topic": "/sensing/imu/imu_data",
"topic_type": "sensor_msgs/msg/Imu",
"best_effort": True,
"diag_name": "imu_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

# Radar topic monitors
radar_front_center_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_radar_front_center",
parameters=[
{
"topic": "/sensing/radar/front_center/objects_raw",
"topic_type": "radar_msgs/msg/RadarTracks",
"best_effort": True,
"diag_name": "radar_front_center_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

radar_front_left_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_radar_front_left",
parameters=[
{
"topic": "/sensing/radar/front_left/objects_raw",
"topic_type": "radar_msgs/msg/RadarTracks",
"best_effort": True,
"diag_name": "radar_front_left_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

radar_front_right_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_radar_front_right",
parameters=[
{
"topic": "/sensing/radar/front_right/objects_raw",
"topic_type": "radar_msgs/msg/RadarTracks",
"best_effort": True,
"diag_name": "radar_front_right_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

radar_rear_center_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_radar_rear_center",
parameters=[
{
"topic": "/sensing/radar/rear_center/objects_raw",
"topic_type": "radar_msgs/msg/RadarTracks",
"best_effort": True,
"diag_name": "radar_rear_center_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

radar_rear_left_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_radar_rear_left",
parameters=[
{
"topic": "/sensing/radar/rear_left/objects_raw",
"topic_type": "radar_msgs/msg/RadarTracks",
"best_effort": True,
"diag_name": "radar_rear_left_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

radar_rear_right_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_radar_rear_right",
parameters=[
{
"topic": "/sensing/radar/rear_right/objects_raw",
"topic_type": "radar_msgs/msg/RadarTracks",
"best_effort": True,
"diag_name": "radar_rear_right_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

# Camera topic monitors
camera0_topic_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_camera0",
parameters=[
{
"topic": "/sensing/camera/camera0/camera_info",
"topic_type": "sensor_msgs/msg/CameraInfo",
"best_effort": True,
"diag_name": "camera0_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

camera1_topic_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_camera1",
parameters=[
{
"topic": "/sensing/camera/camera1/camera_info",
"topic_type": "sensor_msgs/msg/CameraInfo",
"best_effort": True,
"diag_name": "camera1_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

camera2_topic_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_camera2",
parameters=[
{
"topic": "/sensing/camera/camera2/camera_info",
"topic_type": "sensor_msgs/msg/CameraInfo",
"best_effort": True,
"diag_name": "camera2_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

camera3_topic_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_camera3",
parameters=[
{
"topic": "/sensing/camera/camera3/camera_info",
"topic_type": "sensor_msgs/msg/CameraInfo",
"best_effort": True,
"diag_name": "camera3_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

camera4_topic_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_camera4",
parameters=[
{
"topic": "/sensing/camera/camera4/camera_info",
"topic_type": "sensor_msgs/msg/CameraInfo",
"best_effort": True,
"diag_name": "camera4_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

camera5_topic_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_camera5",
parameters=[
{
"topic": "/sensing/camera/camera5/camera_info",
"topic_type": "sensor_msgs/msg/CameraInfo",
"best_effort": True,
"diag_name": "camera5_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

camera6_topic_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_camera6",
parameters=[
{
"topic": "/sensing/camera/camera6/camera_info",
"topic_type": "sensor_msgs/msg/CameraInfo",
"best_effort": True,
"diag_name": "camera6_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

camera7_topic_monitor = ComposableNode(
package="topic_state_monitor",
plugin="topic_state_monitor::TopicStateMonitorNode",
name="topic_state_monitor_camera7",
parameters=[
{
"topic": "/sensing/camera/camera7/camera_info",
"topic_type": "sensor_msgs/msg/CameraInfo",
"best_effort": True,
"diag_name": "camera7_topic_status",
"warn_rate": 5.0,
"error_rate": 1.0,
"timeout": 5.0,
"window_size": 10,
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)

# ComposableNodeContainer to run all ComposableNodes
container = ComposableNodeContainer(
name="topic_state_monitor_container",
namespace="topic_state_monitor",
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[
gnss_topic_monitor,
imu_topic_monitor,
radar_front_center_monitor,
radar_front_left_monitor,
radar_front_right_monitor,
radar_rear_center_monitor,
radar_rear_left_monitor,
radar_rear_right_monitor,
camera0_topic_monitor,
camera1_topic_monitor,
camera2_topic_monitor,
camera3_topic_monitor,
camera4_topic_monitor,
camera5_topic_monitor,
camera6_topic_monitor,
camera7_topic_monitor,
],
output="screen",
)

return LaunchDescription([container])

0 comments on commit 1a9b241

Please sign in to comment.