Merge pull request tier4#188 from tier4/sync-upstream

chore: sync upstream

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

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

perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py → 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,

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,
+        ]
+    )

perception_launch/launch/perception.launch.xml

@@ -47,7 +47,7 @@
     <!-- occupancy grid map module -->
     <group>
       <push-ros-namespace namespace="occupancy_grid_map"/>
-      <include file="$(find-pkg-share perception_launch)/launch/occupancy_grid_map/occupancy_grid_map.launch.py">
+      <include file="$(find-pkg-share perception_launch)/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py">
         <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/single_frame/pointcloud_raw" />
         <arg name="input/raw_pointcloud" value="$(var input/pointcloud)" />
         <arg name="output" value="/perception/occupancy_grid_map/map" />

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]

simulator_launch/CMakeLists.txt

@@ -11,4 +11,5 @@ endif()
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )

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"

simulator_launch/launch/simulator.launch.xml

@@ -13,7 +13,9 @@
   <let name="vehicle_model_pkg" value="$(find-pkg-share $(var vehicle_model)_description)"/>
 
   <group if="$(var scenario_simulation)">
-    <include file="$(find-pkg-share fault_injection)/launch/fault_injection.launch.xml"/>
+    <include file="$(find-pkg-share fault_injection)/launch/fault_injection.launch.xml">
+      <arg name="config_file" value="$(find-pkg-share simulator_launch)/config/fault_injection.param.yaml"/>
+    </include>
   </group>
 
   <!-- Dummy Perception -->
@@ -58,7 +60,7 @@
     <group>
       <push-ros-namespace namespace="occupancy_grid_map"/>
       <!-- Occupancy Grid -->
-      <include file="$(find-pkg-share perception_launch)/launch/occupancy_grid_map/occupancy_grid_map.launch.py">
+      <include file="$(find-pkg-share perception_launch)/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py">
         <arg name="input_obstacle_pointcloud" value="true" />
         <arg name="input_obstacle_and_raw_pointcloud" value="false" />
         <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/pointcloud" />