diff --git a/control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/longitudinal_controller.param.yaml index d502e0dc1..dcf289968 100644 --- a/control_launch/config/trajectory_follower/longitudinal_controller.param.yaml +++ b/control_launch/config/trajectory_follower/longitudinal_controller.param.yaml @@ -11,6 +11,7 @@ drive_state_stop_dist: 0.5 drive_state_offset_stop_dist: 1.0 stopping_state_stop_dist: 0.5 + stopped_state_entry_duration_time: 0.1 stopped_state_entry_vel: 0.01 stopped_state_entry_acc: 0.1 emergency_state_overshoot_stop_dist: 1.5 diff --git a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml index fcdfd8fc3..bb43b3844 100644 --- a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml +++ b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml @@ -5,6 +5,7 @@ external_emergency_stop_heartbeat_timeout: 0.8 stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 + stopped_state_entry_duration_time: 0.1 vel_lim: 25.0 lon_acc_lim: 5.0 lon_jerk_lim: 5.0 diff --git a/perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py b/perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py similarity index 98% rename from perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py rename to perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py index 4225d9df2..d371fa439 100644 --- a/perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py +++ b/perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py @@ -59,7 +59,7 @@ def add_launch_arg(name: str, default_value=None): "max_height": 2.0, "angle_min": -3.141592, # -M_PI "angle_max": 3.141592, # M_PI - "angle_increment": 0.0034906585, # 0.20*M_PI/180.0 + "angle_increment": 0.00349065850, # 0.20*M_PI/180.0 "scan_time": 0.0, "range_min": 1.0, "range_max": 200.0, diff --git a/perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py b/perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py new file mode 100644 index 000000000..2b7f9f8e8 --- /dev/null +++ b/perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py @@ -0,0 +1,93 @@ +# Copyright 2021 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.actions import DeclareLaunchArgument +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + composable_nodes = [ + ComposableNode( + package="probabilistic_occupancy_grid_map", + plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + name="occupancy_grid_map_node", + remappings=[ + ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), + ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), + ("~/output/occupancy_grid_map", LaunchConfiguration("output")), + ], + parameters=[ + { + "map_resolution": 0.5, + "use_height_filter": True, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ), + ] + + occupancy_grid_map_container = ComposableNodeContainer( + condition=LaunchConfigurationEquals("container", ""), + name="occupancy_grid_map_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=composable_nodes, + output="screen", + ) + + load_composable_nodes = LoadComposableNodes( + condition=LaunchConfigurationNotEquals("container", ""), + composable_node_descriptions=composable_nodes, + target_container=LaunchConfiguration("container"), + ) + + return LaunchDescription( + [ + add_launch_arg("container", ""), + add_launch_arg("use_multithread", "false"), + add_launch_arg("use_intra_process", "false"), + add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), + add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), + add_launch_arg("output", "occupancy_grid"), + set_container_executable, + set_container_mt_executable, + occupancy_grid_map_container, + load_composable_nodes, + ] + ) diff --git a/perception_launch/launch/perception.launch.xml b/perception_launch/launch/perception.launch.xml index 625ed6416..ef3053401 100644 --- a/perception_launch/launch/perception.launch.xml +++ b/perception_launch/launch/perception.launch.xml @@ -47,7 +47,7 @@ - + diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml index 231cfa134..3c48e0f8f 100644 --- a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml +++ b/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml @@ -2,6 +2,7 @@ ros__parameters: # motion state constraints max_velocity: 1.25 # max velocity limit [m/s] + stop_decel: 0.0 # deceleration at a stop point[m/ss] # external velocity limit parameter margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m] diff --git a/simulator_launch/CMakeLists.txt b/simulator_launch/CMakeLists.txt index bbabec7ab..05e5c27e2 100644 --- a/simulator_launch/CMakeLists.txt +++ b/simulator_launch/CMakeLists.txt @@ -11,4 +11,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/simulator_launch/config/fault_injection.param.yaml b/simulator_launch/config/fault_injection.param.yaml new file mode 100644 index 000000000..1a57b852f --- /dev/null +++ b/simulator_launch/config/fault_injection.param.yaml @@ -0,0 +1,37 @@ +/**: + ros__parameters: + event_diag_list: + vehicle_is_out_of_lane: "lane_departure" + trajectory_deviation_is_high: "trajectory_deviation" + localization_matching_score_is_low: "ndt_scan_matcher" + localization_accuracy_is_low: "localization_accuracy" + map_version_is_different: "map_version" + trajectory_is_invalid: "trajectory_point_validation" + cpu_temperature_is_high: "CPU Temperature" + cpu_usage_is_high: "CPU Usage" + cpu_is_in_thermal_throttling: "CPU Thermal Throttling" + storage_temperature_is_high: "HDD Temperature" + storage_usage_is_high: "HDD Usage" + network_usage_is_high: "Network Usage" + clock_error_is_large: "NTP Offset" + gpu_temperature_is_high: "GPU Temperature" + gpu_usage_is_high: "GPU Usage" + gpu_memory_usage_is_high: "GPU Memory Usage" + gpu_is_in_thermal_throttling: "GPU Thermal Throttling" + driving_recorder_storage_error: "driving_recorder" + debug_data_logger_storage_error: "bagpacker" + emergency_stop_operation: "emergency_stop_operation" + vehicle_error_occurred: "vehicle_errors" + vehicle_ecu_connection_is_lost: "can_bus_connection" + obstacle_crash_sensor_is_activated: "obstacle_crash" + /control/command_gate/node_alive_monitoring: "vehicle_cmd_gate: heartbeat" + /control/autonomous_driving/node_alive_monitoring: "control_topic_status" + /control/external_command_selector/node_alive_monitoring: "external_cmd_selector: heartbeat" + /localization/node_alive_monitoring: "localization_topic_status" + /map/node_alive_monitoring: "map_topic_status" + /planning/node_alive_monitoring: "planning_topic_status" + /sensing/lidar/node_alive_monitoring: "lidar_topic_status" + /sensing/imu/node_alive_monitoring: "imu_connection" + /sensing/gnss/node_alive_monitoring: "gnss_connection" + /system/node_alive_monitoring: "system_topic_status" + /vehicle/node_alive_monitoring: "vehicle_topic_status" diff --git a/simulator_launch/launch/simulator.launch.xml b/simulator_launch/launch/simulator.launch.xml index aba24c308..f66519d58 100644 --- a/simulator_launch/launch/simulator.launch.xml +++ b/simulator_launch/launch/simulator.launch.xml @@ -13,7 +13,9 @@ - + + + @@ -58,7 +60,7 @@ - +