chore: sync upstream

common/glog_component/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.14)
+project(glog_component)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+
+ament_auto_add_library(glog_component SHARED
+  src/glog_component.cpp
+)
+target_link_libraries(glog_component glog)
+
+rclcpp_components_register_node(glog_component
+  PLUGIN "GlogComponent"
+  EXECUTABLE glog_component_node
+)
+
+ament_auto_package()

common/glog_component/README.md

@@ -0,0 +1,29 @@
+# glog_component
+
+This package provides the glog (google logging library) feature as a ros2 component library. This is used to dynamically load the glog feature with container.
+
+See the [glog github](https://github.com/google/glog) for the details of its features.
+
+## Example
+
+When you load the `glog_component` in container, the launch file can be like below:
+
+```py
+glog_component = ComposableNode(
+    package="glog_component",
+    plugin="GlogComponent",
+    name="glog_component",
+)
+
+container = ComposableNodeContainer(
+    name="my_container",
+    namespace="",
+    package="rclcpp_components",
+    executable=LaunchConfiguration("container_executable"),
+    composable_node_descriptions=[
+        component1,
+        component2,
+        glog_component,
+    ],
+)
+```

common/glog_component/include/glog_component/glog_component.hpp

@@ -0,0 +1,28 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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.
+
+#ifndef GLOG_COMPONENT__GLOG_COMPONENT_HPP_
+#define GLOG_COMPONENT__GLOG_COMPONENT_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <glog/logging.h>
+
+class GlogComponent : public rclcpp::Node
+{
+public:
+  explicit GlogComponent(const rclcpp::NodeOptions & node_options);
+};
+
+#endif  // GLOG_COMPONENT__GLOG_COMPONENT_HPP_

sensing/geo_pos_conv/package.xml → common/glog_component/package.xml

@@ -1,20 +1,21 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>geo_pos_conv</name>
-  <version>2.0.0</version>
-  <description>The ROS 2 geo_pos_conv package</description>
-  <maintainer email="[email protected]">Yamato Ando</maintainer>
-
+  <name>glog_component</name>
+  <version>0.1.0</version>
+  <description>The glog_component package</description>
+  <maintainer email="[email protected]">Takamasa Horibe</maintainer>
   <license>Apache License 2.0</license>
 
+  <author email="[email protected]">Takamasa Horibe</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>libgoogle-glog-dev</depend>
   <depend>rclcpp</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>autoware_lint_common</test_depend>
+  <depend>rclcpp_components</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

common/glog_component/src/glog_component.cpp

@@ -0,0 +1,25 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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.
+
+#include "glog_component/glog_component.hpp"
+
+GlogComponent::GlogComponent(const rclcpp::NodeOptions & node_options)
+: Node("glog_component", node_options)
+{
+  google::InitGoogleLogging("glog_component");
+  google::InstallFailureSignalHandler();
+}
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(GlogComponent)

common/traffic_light_utils/CMakeLists.txt

@@ -5,9 +5,18 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 find_package(Boost REQUIRED)
+find_package(autoware_cmake REQUIRED)
 
 ament_auto_add_library(traffic_light_utils SHARED
   src/traffic_light_utils.cpp
 )
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
+  file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
+  ament_add_ros_isolated_gtest(test_traffic_light_utils ${TEST_SOURCES})
+  target_include_directories(test_traffic_light_utils PRIVATE src/include)
+  target_link_libraries(test_traffic_light_utils traffic_light_utils)
+endif()
+
 ament_auto_package()

common/traffic_light_utils/package.xml

@@ -11,6 +11,10 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
   <depend>lanelet2_extension</depend>
   <depend>tier4_perception_msgs</depend>
 

common/traffic_light_utils/test/test_traffic_light_utils.cpp

@@ -0,0 +1,96 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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.
+
+#include "gtest/gtest.h"
+#include "traffic_light_utils/traffic_light_utils.hpp"
+
+namespace traffic_light_utils
+{
+
+TEST(isRoiValid, roi_validity)
+{
+  tier4_perception_msgs::msg::TrafficLightRoi test_roi;
+  test_roi.roi.x_offset = 300;
+  test_roi.roi.y_offset = 200;
+  test_roi.roi.width = 340;
+  test_roi.roi.height = 200;
+  uint32_t img_width = 640;
+  uint32_t img_heigh = 480;
+  EXPECT_FALSE(isRoiValid(test_roi, img_width, img_heigh));
+  test_roi.roi.width = 339;
+  EXPECT_TRUE(isRoiValid(test_roi, img_width, img_heigh));
+}
+
+TEST(setRoiInvalid, set_roi_size)
+{
+  tier4_perception_msgs::msg::TrafficLightRoi test_roi;
+  test_roi.roi.x_offset = 300;
+  test_roi.roi.y_offset = 200;
+  test_roi.roi.width = 300;
+  test_roi.roi.height = 200;
+  EXPECT_EQ(test_roi.roi.width, (uint32_t)300);
+  EXPECT_EQ(test_roi.roi.height, (uint32_t)200);
+  setRoiInvalid(test_roi);
+  EXPECT_EQ(test_roi.roi.width, (uint32_t)0);
+  EXPECT_EQ(test_roi.roi.height, (uint32_t)0);
+}
+
+TEST(isSignalUnknown, signal_element)
+{
+  tier4_perception_msgs::msg::TrafficSignal test_signal;
+  tier4_perception_msgs::msg::TrafficLightElement element;
+  element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
+  element.shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
+  test_signal.elements.push_back(element);
+  EXPECT_TRUE(isSignalUnknown(test_signal));
+  test_signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::RED;
+  EXPECT_FALSE(isSignalUnknown(test_signal));
+}
+
+TEST(setSignalUnknown, set_signal_element)
+{
+  tier4_perception_msgs::msg::TrafficSignal test_signal;
+  tier4_perception_msgs::msg::TrafficLightElement element;
+  element.color = tier4_perception_msgs::msg::TrafficLightElement::RED;
+  element.shape = tier4_perception_msgs::msg::TrafficLightElement::CROSS;
+  test_signal.elements.push_back(element);
+  EXPECT_EQ(test_signal.elements[0].color, tier4_perception_msgs::msg::TrafficLightElement::RED);
+  EXPECT_EQ(test_signal.elements[0].shape, tier4_perception_msgs::msg::TrafficLightElement::CROSS);
+  setSignalUnknown(test_signal, 1.23f);
+  EXPECT_EQ(
+    test_signal.elements[0].color, tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN);
+  EXPECT_EQ(
+    test_signal.elements[0].shape, tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN);
+  EXPECT_FLOAT_EQ(test_signal.elements[0].confidence, (float)1.23);
+}
+
+TEST(getTrafficLightCenter, get_signal)
+{
+  lanelet::LineString3d lineString;
+  lanelet::Point3d p0(0, 0, 0, 0);
+  lanelet::Point3d p1(1, 1, 1, 1);
+  lanelet::Point3d p2(2, 2, 2, 2);
+  lanelet::Point3d p3(3, 3, 3, 3);
+  lineString.push_back(p0);
+  lineString.push_back(p1);
+  lineString.push_back(p2);
+  lineString.push_back(p3);
+
+  lanelet::ConstLineString3d test_light(lineString);
+  EXPECT_FLOAT_EQ(getTrafficLightCenter(test_light).x(), (float)1.5);
+  EXPECT_FLOAT_EQ(getTrafficLightCenter(test_light).y(), (float)1.5);
+  EXPECT_FLOAT_EQ(getTrafficLightCenter(test_light).z(), (float)1.5);
+}
+
+}  // namespace traffic_light_utils

control/vehicle_cmd_gate/CMakeLists.txt

@@ -13,13 +13,31 @@ ament_auto_add_library(vehicle_cmd_gate_node SHARED
 
 rclcpp_components_register_node(vehicle_cmd_gate_node
   PLUGIN "vehicle_cmd_gate::VehicleCmdGate"
-  EXECUTABLE vehicle_cmd_gate
+  EXECUTABLE vehicle_cmd_gate_exe
 )
 
+rosidl_generate_interfaces(
+  ${PROJECT_NAME}
+  "msg/IsFilterActivated.msg"
+  DEPENDENCIES builtin_interfaces
+)
+
+# to use same package defined message
+if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
+    rosidl_target_interfaces(vehicle_cmd_gate_node
+    ${PROJECT_NAME} "rosidl_typesupport_cpp")
+else()
+    rosidl_get_typesupport_target(
+            cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+    target_link_libraries(vehicle_cmd_gate_node "${cpp_typesupport_target}")
+endif()
+
+
 if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_vehicle_cmd_gate
     test/src/test_main.cpp
     test/src/test_vehicle_cmd_filter.cpp
+    test/src/test_filter_in_vehicle_cmd_gate_node.cpp
   )
   ament_target_dependencies(test_vehicle_cmd_gate
     rclcpp

control/vehicle_cmd_gate/README.md

@@ -43,26 +43,38 @@
 
 ## Parameters
 
-| Parameter                                   | Type   | Description                                                                 |
-| ------------------------------------------- | ------ | --------------------------------------------------------------------------- |
-| `update_period`                             | double | update period                                                               |
-| `use_emergency_handling`                    | bool   | true when emergency handler is used                                         |
-| `check_external_emergency_heartbeat`        | bool   | true when checking heartbeat for emergency stop                             |
-| `system_emergency_heartbeat_timeout`        | double | timeout for system emergency                                                |
-| `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency                                              |
-| `stop_hold_acceleration`                    | double | longitudinal acceleration cmd when vehicle should stop                      |
-| `emergency_acceleration`                    | double | longitudinal acceleration cmd when vehicle stop with emergency              |
-| `moderate_stop_service_acceleration`        | double | longitudinal acceleration cmd when vehicle stop with moderate stop service  |
-| `nominal.vel_lim`                           | double | limit of longitudinal velocity (activated in AUTONOMOUS operation mode)     |
-| `nominal.lon_acc_lim`                       | double | limit of longitudinal acceleration (activated in AUTONOMOUS operation mode) |
-| `nominal.lon_jerk_lim`                      | double | limit of longitudinal jerk (activated in AUTONOMOUS operation mode)         |
-| `nominal.lat_acc_lim`                       | double | limit of lateral acceleration (activated in AUTONOMOUS operation mode)      |
-| `nominal.lat_jerk_lim`                      | double | limit of lateral jerk (activated in AUTONOMOUS operation mode)              |
-| `on_transition.vel_lim`                     | double | limit of longitudinal velocity (activated in TRANSITION operation mode)     |
-| `on_transition.lon_acc_lim`                 | double | limit of longitudinal acceleration (activated in TRANSITION operation mode) |
-| `on_transition.lon_jerk_lim`                | double | limit of longitudinal jerk (activated in TRANSITION operation mode)         |
-| `on_transition.lat_acc_lim`                 | double | limit of lateral acceleration (activated in TRANSITION operation mode)      |
-| `on_transition.lat_jerk_lim`                | double | limit of lateral jerk (activated in TRANSITION operation mode)              |
+| Parameter                                   | Type     | Description                                                                                                                                                                                 |
+| ------------------------------------------- | -------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `update_period`                             | double   | update period                                                                                                                                                                               |
+| `use_emergency_handling`                    | bool     | true when emergency handler is used                                                                                                                                                         |
+| `check_external_emergency_heartbeat`        | bool     | true when checking heartbeat for emergency stop                                                                                                                                             |
+| `system_emergency_heartbeat_timeout`        | double   | timeout for system emergency                                                                                                                                                                |
+| `external_emergency_stop_heartbeat_timeout` | double   | timeout for external emergency                                                                                                                                                              |
+| `stop_hold_acceleration`                    | double   | longitudinal acceleration cmd when vehicle should stop                                                                                                                                      |
+| `emergency_acceleration`                    | double   | longitudinal acceleration cmd when vehicle stop with emergency                                                                                                                              |
+| `moderate_stop_service_acceleration`        | double   | longitudinal acceleration cmd when vehicle stop with moderate stop service                                                                                                                  |
+| `nominal.vel_lim`                           | double   | limit of longitudinal velocity (activated in AUTONOMOUS operation mode)                                                                                                                     |
+| `nominal.reference_speed_point`             | <double> | velocity point used as a reference when calculate control command limit (activated in AUTONOMOUS operation mode). The size of this array must be equivalent to the size of the limit array. |
+| `nominal.lon_acc_lim`                       | <double> | array of limits of longitudinal acceleration (activated in AUTONOMOUS operation mode)                                                                                                       |
+| `nominal.lon_jerk_lim`                      | <double> | array of limits of longitudinal jerk (activated in AUTONOMOUS operation mode)                                                                                                               |
+| `nominal.lat_acc_lim`                       | <double> | array of limits of lateral acceleration (activated in AUTONOMOUS operation mode)                                                                                                            |
+| `nominal.lat_jerk_lim`                      | <double> | array of limits of lateral jerk (activated in AUTONOMOUS operation mode)                                                                                                                    |
+| `on_transition.vel_lim`                     | double   | limit of longitudinal velocity (activated in TRANSITION operation mode)                                                                                                                     |
+| `on_transition.reference_speed_point`       | <double> | velocity point used as a reference when calculate control command limit (activated in TRANSITION operation mode). The size of this array must be equivalent to the size of the limit array. |
+| `on_transition.lon_acc_lim`                 | <double> | array of limits of longitudinal acceleration (activated in TRANSITION operation mode)                                                                                                       |
+| `on_transition.lon_jerk_lim`                | <double> | array of limits of longitudinal jerk (activated in TRANSITION operation mode)                                                                                                               |
+| `on_transition.lat_acc_lim`                 | <double> | array of limits of lateral acceleration (activated in TRANSITION operation mode)                                                                                                            |
+| `on_transition.lat_jerk_lim`                | <double> | array of limits of lateral jerk (activated in TRANSITION operation mode)                                                                                                                    |
+
+## Filter function
+
+This module incorporates a limitation filter to the control command right before its published. Primarily for safety, this filter restricts the output range of all control commands published through Autoware.
+
+The limitation values are calculated based on the 1D interpolation of the limitation array parameters. Here is an example for the longitudinal jerk limit.
+
+![filter-example](./image/filter.png)
+
+Notation: this filter is not designed to enhance ride comfort. Its main purpose is to detect and remove abnormal values in the control outputs during the final stages of Autoware. If this filter is frequently active, it implies the control module may need tuning. If you're aiming to smoothen the signal via a low-pass filter or similar techniques, that should be handled in the control module. When the filter is activated, the topic `~/is_filter_activated` is published.
 
 ## Assumptions / Known limits
 

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -14,15 +14,17 @@
     stop_check_duration: 1.0
     nominal:
       vel_lim: 25.0
-      lon_acc_lim: 5.0
-      lon_jerk_lim: 5.0
-      lat_acc_lim: 5.0
-      lat_jerk_lim: 7.0
-      actual_steer_diff_lim: 1.0
+      reference_speed_points: [20.0, 30.0]
+      lon_acc_lim: [5.0, 4.0]
+      lon_jerk_lim: [5.0, 4.0]
+      lat_acc_lim: [5.0, 4.0]
+      lat_jerk_lim: [7.0, 6.0]
+      actual_steer_diff_lim: [1.0, 0.8]
     on_transition:
       vel_lim: 50.0
-      lon_acc_lim: 1.0
-      lon_jerk_lim: 0.5
-      lat_acc_lim: 2.0
-      lat_jerk_lim: 7.0
-      actual_steer_diff_lim: 1.0
+      reference_speed_points: [20.0, 30.0]
+      lon_acc_lim: [1.0, 0.9]
+      lon_jerk_lim: [0.5, 0.4]
+      lat_acc_lim: [2.0, 1.8]
+      lat_jerk_lim: [7.0, 6.0]
+      actual_steer_diff_lim: [1.0, 0.8]

control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml

@@ -7,7 +7,7 @@
   <!-- vehicle info -->
   <arg name="vehicle_info_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml"/>
 
-  <node pkg="vehicle_cmd_gate" exec="vehicle_cmd_gate" name="vehicle_cmd_gate" output="screen">
+  <node pkg="vehicle_cmd_gate" exec="vehicle_cmd_gate_exe" name="vehicle_cmd_gate" output="screen">
     <remap from="input/steering" to="/vehicle/status/steering_status"/>
 
     <remap from="input/auto/control_cmd" to="trajectory_follower/control_cmd"/>