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 sensing diags #288

Merged
merged 3 commits into from
Aug 27, 2024
Merged

feat(aip_x2_launch): add topic state monitor for sensing diags #288

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1a9b241
feat(aip_x2_launch): add topic state monitor for camera, radar, gnss …
TomohitoAndo Mar 26, 2024
cc534b2
remove topic_state_monitor for camera
kotaro-hihara Aug 27, 2024
c5348b5
feat: add imu_monitor and dummy diag (#212)
h-ohta Feb 16, 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
3 changes: 3 additions & 0 deletions aip_x2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,9 @@
# gnss
gnss: default

# imu
yaw_rate_status: default

# lidar
pandar_connection: default
pandar_temperature: default
Expand Down
4 changes: 4 additions & 0 deletions aip_x2_launch/launch/imu.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,10 @@
<arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
<arg name="gyro_bias_estimator_param_file" value="$(var gyro_bias_estimator_param_file)"/>
</include>

<include file="$(find-pkg-share imu_monitor)/launch/imu_monitor.launch.xml">
<arg name="config_file" value="$(find-pkg-share imu_monitor)/config/imu_monitor.param.yaml"/>
</include>
</group>

</launch>
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 @@ -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>
195 changes: 195 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,195 @@
# 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}],
)

# 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,
],
output="screen",
)

return LaunchDescription([container])
Loading