Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(aip_x2_launch): add topic state monitor for camera, radar, gnss and imu #227

Merged
merged 5 commits into from
Mar 26, 2024
Merged

feat(aip_x2_launch): add topic state monitor for camera, radar, gnss and imu #227

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
b8bbcfd
feat: add topic state monitor for sensors
TomohitoAndo Mar 25, 2024
2999221
fix comments
TomohitoAndo Mar 25, 2024
722a4c0
replace launch.xml with launch.py
TomohitoAndo Mar 26, 2024
6b90c30
ci(pre-commit): autofix
pre-commit-ci[bot] Mar 26, 2024
c20e00b
add copyright
TomohitoAndo Mar 26, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions aip_x2_launch/launch/sensing.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,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
Open in desktop
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])
Loading