diff --git a/common/glog_component/CMakeLists.txt b/common/glog_component/CMakeLists.txt new file mode 100644 index 0000000000000..c2a68d0130b37 --- /dev/null +++ b/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() diff --git a/common/glog_component/README.md b/common/glog_component/README.md new file mode 100644 index 0000000000000..94a40fe32e40d --- /dev/null +++ b/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, + ], +) +``` diff --git a/common/glog_component/include/glog_component/glog_component.hpp b/common/glog_component/include/glog_component/glog_component.hpp new file mode 100644 index 0000000000000..d940363d75ac4 --- /dev/null +++ b/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 + +#include + +class GlogComponent : public rclcpp::Node +{ +public: + explicit GlogComponent(const rclcpp::NodeOptions & node_options); +}; + +#endif // GLOG_COMPONENT__GLOG_COMPONENT_HPP_ diff --git a/sensing/geo_pos_conv/package.xml b/common/glog_component/package.xml similarity index 52% rename from sensing/geo_pos_conv/package.xml rename to common/glog_component/package.xml index b82ea443ef7d5..0d6e7daac1de3 100644 --- a/sensing/geo_pos_conv/package.xml +++ b/common/glog_component/package.xml @@ -1,20 +1,21 @@ - geo_pos_conv - 2.0.0 - The ROS 2 geo_pos_conv package - Yamato Ando - + glog_component + 0.1.0 + The glog_component package + Takamasa Horibe Apache License 2.0 + Takamasa Horibe + + ament_cmake ament_cmake_auto autoware_cmake + libgoogle-glog-dev rclcpp - - ament_lint_auto - autoware_lint_common + rclcpp_components ament_cmake diff --git a/common/glog_component/src/glog_component.cpp b/common/glog_component/src/glog_component.cpp new file mode 100644 index 0000000000000..9e7e70da6c884 --- /dev/null +++ b/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(GlogComponent) diff --git a/common/traffic_light_utils/CMakeLists.txt b/common/traffic_light_utils/CMakeLists.txt index 567e7653b524f..741bdd585ed7f 100644 --- a/common/traffic_light_utils/CMakeLists.txt +++ b/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() diff --git a/common/traffic_light_utils/package.xml b/common/traffic_light_utils/package.xml index 48c6212a5909e..de05355eafd66 100644 --- a/common/traffic_light_utils/package.xml +++ b/common/traffic_light_utils/package.xml @@ -11,6 +11,10 @@ ament_cmake_auto autoware_cmake + ament_cmake_ros + ament_lint_auto + autoware_lint_common + lanelet2_extension tier4_perception_msgs diff --git a/common/traffic_light_utils/test/test_traffic_light_utils.cpp b/common/traffic_light_utils/test/test_traffic_light_utils.cpp new file mode 100644 index 0000000000000..bd777265b4195 --- /dev/null +++ b/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 diff --git a/control/vehicle_cmd_gate/CMakeLists.txt b/control/vehicle_cmd_gate/CMakeLists.txt index 81ccac1ea8b66..c5db663ccc77e 100644 --- a/control/vehicle_cmd_gate/CMakeLists.txt +++ b/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 diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index a8ac8c46ae782..231c2c5022138 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/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` | | 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` | | array of limits of longitudinal acceleration (activated in AUTONOMOUS operation mode) | +| `nominal.lon_jerk_lim` | | array of limits of longitudinal jerk (activated in AUTONOMOUS operation mode) | +| `nominal.lat_acc_lim` | | array of limits of lateral acceleration (activated in AUTONOMOUS operation mode) | +| `nominal.lat_jerk_lim` | | 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` | | 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` | | array of limits of longitudinal acceleration (activated in TRANSITION operation mode) | +| `on_transition.lon_jerk_lim` | | array of limits of longitudinal jerk (activated in TRANSITION operation mode) | +| `on_transition.lat_acc_lim` | | array of limits of lateral acceleration (activated in TRANSITION operation mode) | +| `on_transition.lat_jerk_lim` | | 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 diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 731c73feeba4d..92844c61f6f4f 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/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] diff --git a/control/vehicle_cmd_gate/image/filter.png b/control/vehicle_cmd_gate/image/filter.png new file mode 100644 index 0000000000000..ed1c7f772a932 Binary files /dev/null and b/control/vehicle_cmd_gate/image/filter.png differ diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index 644e937e0718d..a6cf7cdcf8c08 100644 --- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/control/vehicle_cmd_gate/msg/IsFilterActivated.msg b/control/vehicle_cmd_gate/msg/IsFilterActivated.msg new file mode 100644 index 0000000000000..fdb47c0a78f08 --- /dev/null +++ b/control/vehicle_cmd_gate/msg/IsFilterActivated.msg @@ -0,0 +1,10 @@ +builtin_interfaces/Time stamp + +bool is_activated + +# for additional information +bool is_activated_on_steering +bool is_activated_on_steering_rate +bool is_activated_on_speed +bool is_activated_on_acceleration +bool is_activated_on_jerk diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index df1de9f370cb6..36bb662fb2f53 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -13,6 +13,8 @@ ament_cmake autoware_cmake + rosidl_default_generators + autoware_adapi_v1_msgs autoware_auto_control_msgs autoware_auto_system_msgs @@ -33,10 +35,14 @@ tier4_vehicle_msgs vehicle_info_util + rosidl_default_runtime + ament_cmake_ros ament_lint_auto autoware_lint_common + rosidl_interface_packages + ament_cmake diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 49735da876d9a..8dec06c455868 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -24,6 +24,60 @@ VehicleCmdFilter::VehicleCmdFilter() { } +bool VehicleCmdFilter::setParameterWithValidation(const VehicleCmdFilterParam & p) +{ + const auto s = p.reference_speed_points.size(); + if ( + p.lon_acc_lim.size() != s || p.lon_jerk_lim.size() != s || p.lat_acc_lim.size() != s || + p.lat_jerk_lim.size() != s || p.actual_steer_diff_lim.size() != s) { + std::cerr << "VehicleCmdFilter::setParam() There is a size mismatch in the parameter. " + "Parameter initialization failed." + << std::endl; + return false; + } + + param_ = p; + return true; +} + +void VehicleCmdFilter::setLonAccLim(LimitArray v) +{ + auto tmp = param_; + tmp.lon_acc_lim = v; + setParameterWithValidation(tmp); +} +void VehicleCmdFilter::setLonJerkLim(LimitArray v) +{ + auto tmp = param_; + tmp.lon_jerk_lim = v; + setParameterWithValidation(tmp); +} +void VehicleCmdFilter::setLatAccLim(LimitArray v) +{ + auto tmp = param_; + tmp.lat_acc_lim = v; + setParameterWithValidation(tmp); +} +void VehicleCmdFilter::setLatJerkLim(LimitArray v) +{ + auto tmp = param_; + tmp.lat_jerk_lim = v; + setParameterWithValidation(tmp); +} +void VehicleCmdFilter::setActualSteerDiffLim(LimitArray v) +{ + auto tmp = param_; + tmp.actual_steer_diff_lim = v; + setParameterWithValidation(tmp); +} + +void VehicleCmdFilter::setParam(const VehicleCmdFilterParam & p) +{ + if (!setParameterWithValidation(p)) { + std::exit(EXIT_FAILURE); + } +} + void VehicleCmdFilter::limitLongitudinalWithVel(AckermannControlCommand & input) const { input.longitudinal.speed = std::max( @@ -33,28 +87,33 @@ void VehicleCmdFilter::limitLongitudinalWithVel(AckermannControlCommand & input) void VehicleCmdFilter::limitLongitudinalWithAcc( const double dt, AckermannControlCommand & input) const { + const auto lon_acc_lim = getLonAccLim(); input.longitudinal.acceleration = std::max( - std::min(static_cast(input.longitudinal.acceleration), param_.lon_acc_lim), - -param_.lon_acc_lim); + std::min(static_cast(input.longitudinal.acceleration), lon_acc_lim), -lon_acc_lim); input.longitudinal.speed = - limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, param_.lon_acc_lim * dt); + limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, lon_acc_lim * dt); } void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( const double dt, AckermannControlCommand & input) const { + const auto lon_jerk_lim = getLonJerkLim(); input.longitudinal.acceleration = limitDiff( - input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, param_.lon_jerk_lim * dt); + input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, lon_jerk_lim * dt); + input.longitudinal.jerk = + std::clamp(static_cast(input.longitudinal.jerk), -lon_jerk_lim, lon_jerk_lim); } void VehicleCmdFilter::limitLateralWithLatAcc( [[maybe_unused]] const double dt, AckermannControlCommand & input) const { + const auto lat_acc_lim = getLatAccLim(); + double latacc = calcLatAcc(input); - if (std::fabs(latacc) > param_.lat_acc_lim) { + if (std::fabs(latacc) > lat_acc_lim) { double v_sq = std::max(static_cast(input.longitudinal.speed * input.longitudinal.speed), 0.001); - double steer_lim = std::atan(param_.lat_acc_lim * param_.wheel_base / v_sq); + double steer_lim = std::atan(lat_acc_lim * param_.wheel_base / v_sq); input.lateral.steering_tire_angle = latacc > 0.0 ? steer_lim : -steer_lim; } } @@ -65,8 +124,10 @@ void VehicleCmdFilter::limitLateralWithLatJerk( double curr_latacc = calcLatAcc(input); double prev_latacc = calcLatAcc(prev_cmd_); - const double latacc_max = prev_latacc + param_.lat_jerk_lim * dt; - const double latacc_min = prev_latacc - param_.lat_jerk_lim * dt; + const auto lat_jerk_lim = getLatJerkLim(); + + const double latacc_max = prev_latacc + lat_jerk_lim * dt; + const double latacc_min = prev_latacc - lat_jerk_lim * dt; if (curr_latacc > latacc_max) { input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_max); @@ -78,23 +139,60 @@ void VehicleCmdFilter::limitLateralWithLatJerk( void VehicleCmdFilter::limitActualSteerDiff( const double current_steer_angle, AckermannControlCommand & input) const { + const auto actual_steer_diff_lim = getSteerDiffLim(); + auto ds = input.lateral.steering_tire_angle - current_steer_angle; - ds = std::clamp(ds, -param_.actual_steer_diff_lim, param_.actual_steer_diff_lim); + ds = std::clamp(ds, -actual_steer_diff_lim, actual_steer_diff_lim); input.lateral.steering_tire_angle = current_steer_angle + ds; } +void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const +{ + // TODO(Horibe): parametrize the max steering angle. + // TODO(Horibe): support steering greater than PI/2. Now the lateral acceleration + // calculation does not support bigger steering value than PI/2 due to tan/atan calculation. + constexpr float steer_limit = M_PI_2; + input.lateral.steering_tire_angle = + std::clamp(input.lateral.steering_tire_angle, -steer_limit, steer_limit); +} + void VehicleCmdFilter::filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & cmd) const + const double dt, const double current_steer_angle, AckermannControlCommand & cmd, + IsFilterActivated & is_activated) const { + const auto cmd_orig = cmd; + limitLateralSteer(cmd); limitLongitudinalWithJerk(dt, cmd); limitLongitudinalWithAcc(dt, cmd); limitLongitudinalWithVel(cmd); limitLateralWithLatJerk(dt, cmd); limitLateralWithLatAcc(dt, cmd); limitActualSteerDiff(current_steer_angle, cmd); + + is_activated = checkIsActivated(cmd, cmd_orig); return; } +IsFilterActivated VehicleCmdFilter::checkIsActivated( + const AckermannControlCommand & c1, const AckermannControlCommand & c2, const double tol) +{ + IsFilterActivated msg; + msg.is_activated_on_steering = + std::abs(c1.lateral.steering_tire_angle - c2.lateral.steering_tire_angle) > tol; + msg.is_activated_on_steering_rate = + std::abs(c1.lateral.steering_tire_rotation_rate - c2.lateral.steering_tire_rotation_rate) > tol; + msg.is_activated_on_speed = std::abs(c1.longitudinal.speed - c2.longitudinal.speed) > tol; + msg.is_activated_on_acceleration = + std::abs(c1.longitudinal.acceleration - c2.longitudinal.acceleration) > tol; + msg.is_activated_on_jerk = std::abs(c1.longitudinal.jerk - c2.longitudinal.jerk) > tol; + + msg.is_activated = + (msg.is_activated_on_steering || msg.is_activated_on_steering_rate || + msg.is_activated_on_speed || msg.is_activated_on_acceleration || msg.is_activated_on_jerk); + + return msg; +} + double VehicleCmdFilter::calcSteerFromLatacc(const double v, const double latacc) const { const double v_sq = std::max(v * v, 0.001); @@ -114,4 +212,56 @@ double VehicleCmdFilter::limitDiff( return prev + diff; } +double VehicleCmdFilter::interpolateFromSpeed(const LimitArray & limits) const +{ + // Consider only for the positive velocities. + const auto current = std::abs(current_speed_); + const auto reference = param_.reference_speed_points; + + // If the speed is out of range of the reference, apply zero-order hold. + if (current <= reference.front()) { + return limits.front(); + } + if (current >= reference.back()) { + return limits.back(); + } + + // Apply linear interpolation + for (size_t i = 0; i < reference.size() - 1; ++i) { + if (reference.at(i) <= current && current <= reference.at(i + 1)) { + auto ratio = + (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i)); + return interp; + } + } + + std::cerr << "VehicleCmdFilter::interpolateFromSpeed() interpolation logic is broken. Command " + "filter is not working. Please check the code." + << std::endl; + return reference.back(); +} + +double VehicleCmdFilter::getLonAccLim() const +{ + return interpolateFromSpeed(param_.lon_acc_lim); +} +double VehicleCmdFilter::getLonJerkLim() const +{ + return interpolateFromSpeed(param_.lon_jerk_lim); +} +double VehicleCmdFilter::getLatAccLim() const +{ + return interpolateFromSpeed(param_.lat_acc_lim); +} +double VehicleCmdFilter::getLatJerkLim() const +{ + return interpolateFromSpeed(param_.lat_jerk_lim); +} +double VehicleCmdFilter::getSteerDiffLim() const +{ + return interpolateFromSpeed(param_.actual_steer_diff_lim); +} + } // namespace vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index 00acb50080cfe..eb85fcbeb8606 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -16,22 +16,28 @@ #define VEHICLE_CMD_FILTER_HPP_ #include +#include #include +#include + namespace vehicle_cmd_gate { using autoware_auto_control_msgs::msg::AckermannControlCommand; +using vehicle_cmd_gate::msg::IsFilterActivated; +using LimitArray = std::vector; struct VehicleCmdFilterParam { double wheel_base; double vel_lim; - double lon_acc_lim; - double lon_jerk_lim; - double lat_acc_lim; - double lat_jerk_lim; - double actual_steer_diff_lim; + LimitArray reference_speed_points; + LimitArray lon_acc_lim; + LimitArray lon_jerk_lim; + LimitArray lat_acc_lim; + LimitArray lat_jerk_lim; + LimitArray actual_steer_diff_lim; }; class VehicleCmdFilter { @@ -41,12 +47,13 @@ class VehicleCmdFilter void setWheelBase(double v) { param_.wheel_base = v; } void setVelLim(double v) { param_.vel_lim = v; } - void setLonAccLim(double v) { param_.lon_acc_lim = v; } - void setLonJerkLim(double v) { param_.lon_jerk_lim = v; } - void setLatAccLim(double v) { param_.lat_acc_lim = v; } - void setLatJerkLim(double v) { param_.lat_jerk_lim = v; } - void setActualSteerDiffLim(double v) { param_.actual_steer_diff_lim = v; } - void setParam(const VehicleCmdFilterParam & p) { param_ = p; } + void setLonAccLim(LimitArray v); + void setLonJerkLim(LimitArray v); + void setLatAccLim(LimitArray v); + void setLatJerkLim(LimitArray v); + void setActualSteerDiffLim(LimitArray v); + void setCurrentSpeed(double v) { current_speed_ = v; } + void setParam(const VehicleCmdFilterParam & p); void setPrevCmd(const AckermannControlCommand & v) { prev_cmd_ = v; } void limitLongitudinalWithVel(AckermannControlCommand & input) const; @@ -56,18 +63,33 @@ class VehicleCmdFilter void limitLateralWithLatJerk(const double dt, AckermannControlCommand & input) const; void limitActualSteerDiff( const double current_steer_angle, AckermannControlCommand & input) const; + void limitLateralSteer(AckermannControlCommand & input) const; void filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & input) const; + const double dt, const double current_steer_angle, AckermannControlCommand & input, + IsFilterActivated & is_activated) const; + static IsFilterActivated checkIsActivated( + const AckermannControlCommand & c1, const AckermannControlCommand & c2, + const double tol = 1.0e-3); AckermannControlCommand getPrevCmd() { return prev_cmd_; } private: VehicleCmdFilterParam param_; AckermannControlCommand prev_cmd_; + double current_speed_ = 0.0; + + bool setParameterWithValidation(const VehicleCmdFilterParam & p); double calcLatAcc(const AckermannControlCommand & cmd) const; double calcSteerFromLatacc(const double v, const double latacc) const; double limitDiff(const double curr, const double prev, const double diff_lim) const; + + double interpolateFromSpeed(const LimitArray & limits) const; + double getLonAccLim() const; + double getLonJerkLim() const; + double getLatAccLim() const; + double getLatJerkLim() const; + double getSteerDiffLim() const; }; } // namespace vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 3f6a07b6d5555..9030521b6b334 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace vehicle_cmd_gate { @@ -70,6 +71,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) pub_external_emergency_ = create_publisher("output/external_emergency", durable_qos); operation_mode_pub_ = create_publisher("output/operation_mode", durable_qos); + is_filter_activated_pub_ = + create_publisher("~/is_filter_activated", durable_qos); + // Subscriber external_emergency_stop_heartbeat_sub_ = create_subscription( "input/external_emergency_stop_heartbeat", 1, @@ -79,7 +83,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) engage_sub_ = create_subscription( "input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1)); kinematics_sub_ = create_subscription( - "input/kinematics", 1, [this](Odometry::SharedPtr msg) { current_kinematics_ = *msg; }); + "/localization/kinematic_state", 1, + [this](Odometry::SharedPtr msg) { current_kinematics_ = *msg; }); acc_sub_ = create_subscription( "input/acceleration", 1, [this](AccelWithCovarianceStamped::SharedPtr msg) { current_acceleration_ = msg->accel.accel.linear.x; @@ -158,11 +163,14 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) VehicleCmdFilterParam p; p.wheel_base = vehicle_info.wheel_base_m; p.vel_lim = declare_parameter("nominal.vel_lim"); - p.lon_acc_lim = declare_parameter("nominal.lon_acc_lim"); - p.lon_jerk_lim = declare_parameter("nominal.lon_jerk_lim"); - p.lat_acc_lim = declare_parameter("nominal.lat_acc_lim"); - p.lat_jerk_lim = declare_parameter("nominal.lat_jerk_lim"); - p.actual_steer_diff_lim = declare_parameter("nominal.actual_steer_diff_lim"); + p.reference_speed_points = + declare_parameter>("nominal.reference_speed_points"); + p.lon_acc_lim = declare_parameter>("nominal.lon_acc_lim"); + p.lon_jerk_lim = declare_parameter>("nominal.lon_jerk_lim"); + p.lat_acc_lim = declare_parameter>("nominal.lat_acc_lim"); + p.lat_jerk_lim = declare_parameter>("nominal.lat_jerk_lim"); + p.actual_steer_diff_lim = + declare_parameter>("nominal.actual_steer_diff_lim"); filter_.setParam(p); } @@ -170,11 +178,14 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) VehicleCmdFilterParam p; p.wheel_base = vehicle_info.wheel_base_m; p.vel_lim = declare_parameter("on_transition.vel_lim"); - p.lon_acc_lim = declare_parameter("on_transition.lon_acc_lim"); - p.lon_jerk_lim = declare_parameter("on_transition.lon_jerk_lim"); - p.lat_acc_lim = declare_parameter("on_transition.lat_acc_lim"); - p.lat_jerk_lim = declare_parameter("on_transition.lat_jerk_lim"); - p.actual_steer_diff_lim = declare_parameter("on_transition.actual_steer_diff_lim"); + p.reference_speed_points = + declare_parameter>("on_transition.reference_speed_points"); + p.lon_acc_lim = declare_parameter>("on_transition.lon_acc_lim"); + p.lon_jerk_lim = declare_parameter>("on_transition.lon_jerk_lim"); + p.lat_acc_lim = declare_parameter>("on_transition.lat_acc_lim"); + p.lat_jerk_lim = declare_parameter>("on_transition.lat_jerk_lim"); + p.actual_steer_diff_lim = + declare_parameter>("on_transition.actual_steer_diff_lim"); filter_on_transition_.setParam(p); } @@ -503,9 +514,14 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont const auto ego_is_stopped = vehicle_stop_checker_->isVehicleStopped(stop_check_duration_); const auto input_cmd_is_stopping = in.longitudinal.acceleration < 0.0; + filter_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); + filter_on_transition_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); + + IsFilterActivated is_filter_activated; + // Apply transition_filter when transiting from MANUAL to AUTO. if (mode.is_in_transition) { - filter_on_transition_.filterAll(dt, current_steer_, out); + filter_on_transition_.filterAll(dt, current_steer_, out, is_filter_activated); } else { // When ego is stopped and the input command is not stopping, // use the higher of actual vehicle longitudinal state @@ -524,7 +540,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont : current_status_cmd.longitudinal.speed; filter_.setPrevCmd(prev_cmd); } - filter_.filterAll(dt, current_steer_, out); + filter_.filterAll(dt, current_steer_, out, is_filter_activated); } // set prev value for both to keep consistency over switching: @@ -547,6 +563,9 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont filter_.setPrevCmd(prev_values); filter_on_transition_.setPrevCmd(prev_values); + is_filter_activated.stamp = now(); + is_filter_activated_pub_->publish(is_filter_activated); + return out; } diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index b94240ce495af..d516ecac29ca2 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -64,6 +65,7 @@ using tier4_external_api_msgs::msg::Heartbeat; using tier4_external_api_msgs::srv::SetEmergency; using tier4_system_msgs::msg::MrmBehaviorStatus; using tier4_vehicle_msgs::msg::VehicleEmergencyStamped; +using vehicle_cmd_gate::msg::IsFilterActivated; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; @@ -99,6 +101,7 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Publisher::SharedPtr gate_mode_pub_; rclcpp::Publisher::SharedPtr engage_pub_; rclcpp::Publisher::SharedPtr operation_mode_pub_; + rclcpp::Publisher::SharedPtr is_filter_activated_pub_; // Subscription rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp new file mode 100644 index 0000000000000..6384fbfb22f60 --- /dev/null +++ b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -0,0 +1,400 @@ +// Copyright 2021 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 "../../src/vehicle_cmd_gate.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +#include + +#include +#include +#include +#include +#include + +#define ASSERT_LT_NEAR(x, y, alpha) ASSERT_LT(x, y * alpha) +#define ASSERT_GT_NEAR(x, y, alpha) ASSERT_GT(x, y * alpha) + +#define PRINT_VALUES(...) print_values(0, #__VA_ARGS__, __VA_ARGS__) +template +void print_values([[maybe_unused]] int i, [[maybe_unused]] T name) +{ + std::cerr << std::endl; +} +template +void print_values(int i, const T1 & name, const T2 & a, const T3 &... b) +{ + for (; name[i] != ',' && name[i] != '\0'; i++) std::cerr << name[i]; + + std::ostringstream oss; + oss << std::setprecision(4) << std::setw(9) << a; + std::cerr << ":" << oss.str() << " "; + print_values(i + 1, name, b...); +} + +// global params +const std::vector reference_speed_points = {5., 10., 15., 20.}; +const std::vector lon_acc_lim = {1.5, 1.0, 0.8, 0.6}; +const std::vector lon_jerk_lim = {1.4, 0.9, 0.7, 0.5}; +const std::vector lat_acc_lim = {2.0, 1.6, 1.2, 0.8}; +const std::vector lat_jerk_lim = {1.7, 1.3, 0.9, 0.6}; +const std::vector actual_steer_diff_lim = {0.5, 0.4, 0.2, 0.1}; +const double wheelbase = 2.89; + +using vehicle_cmd_gate::VehicleCmdGate; + +using autoware_adapi_v1_msgs::msg::MrmState; +using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_vehicle_msgs::msg::GearCommand; +using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; +using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using nav_msgs::msg::Odometry; +using tier4_control_msgs::msg::GateMode; +using tier4_external_api_msgs::msg::Emergency; +using tier4_external_api_msgs::msg::Heartbeat; +using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; + +class PubSubNode : public rclcpp::Node +{ +public: + PubSubNode() : Node{"test_vehicle_cmd_gate_filter_pubsub"} + { + sub_cmd_ = create_subscription( + "output/control_cmd", rclcpp::QoS{1}, + [this](const AckermannControlCommand::ConstSharedPtr msg) { + cmd_history_.push_back(msg); + cmd_received_times_.push_back(now()); + checkFilter(); + }); + + rclcpp::QoS qos{1}; + qos.transient_local(); + + pub_external_emergency_stop_heartbeat_ = + create_publisher("input/external_emergency_stop_heartbeat", qos); + pub_engage_ = create_publisher("input/engage", qos); + pub_gate_mode_ = create_publisher("input/gate_mode", qos); + pub_odom_ = create_publisher("/localization/kinematic_state", qos); + pub_acc_ = create_publisher("input/acceleration", qos); + pub_steer_ = create_publisher("input/steering", qos); + pub_operation_mode_ = create_publisher("input/operation_mode", qos); + pub_mrm_state_ = create_publisher("input/mrm_state", qos); + + pub_auto_control_cmd_ = + create_publisher("input/auto/control_cmd", qos); + pub_auto_turn_indicator_cmd_ = + create_publisher("input/auto/turn_indicators_cmd", qos); + pub_auto_hazard_light_cmd_ = + create_publisher("input/auto/hazard_lights_cmd", qos); + pub_auto_gear_cmd_ = create_publisher("input/auto/gear_cmd", qos); + } + + rclcpp::Subscription::SharedPtr sub_cmd_; + + rclcpp::Publisher::SharedPtr pub_external_emergency_stop_heartbeat_; + rclcpp::Publisher::SharedPtr pub_engage_; + rclcpp::Publisher::SharedPtr pub_gate_mode_; + rclcpp::Publisher::SharedPtr pub_odom_; + rclcpp::Publisher::SharedPtr pub_acc_; + rclcpp::Publisher::SharedPtr pub_steer_; + rclcpp::Publisher::SharedPtr pub_operation_mode_; + rclcpp::Publisher::SharedPtr pub_mrm_state_; + rclcpp::Publisher::SharedPtr pub_auto_control_cmd_; + rclcpp::Publisher::SharedPtr pub_auto_turn_indicator_cmd_; + rclcpp::Publisher::SharedPtr pub_auto_hazard_light_cmd_; + rclcpp::Publisher::SharedPtr pub_auto_gear_cmd_; + + std::vector cmd_history_; + std::vector raw_cmd_history_; + std::vector cmd_received_times_; + + // publish except for the control_cmd + void publishDefaultTopicsNoSpin() + { + { + Heartbeat msg; + msg.stamp = now(); + pub_external_emergency_stop_heartbeat_->publish(msg); + } + { + EngageMsg msg; + msg.stamp = now(); + msg.engage = true; + pub_engage_->publish(msg); + } + { + GateMode msg; + msg.data = GateMode::AUTO; + pub_gate_mode_->publish(msg); + } + { + Odometry msg; // initialized for zero pose and twist + msg.header.frame_id = "baselink"; + msg.header.stamp = now(); + msg.pose.pose.orientation.w = 1.0; + msg.twist.twist.linear.x = 0.0; + if (!cmd_history_.empty()) { // ego moves as commanded. + msg.twist.twist.linear.x = + cmd_history_.back()->longitudinal.speed; // ego moves as commanded. + } else { + } + pub_odom_->publish(msg); + } + { + AccelWithCovarianceStamped msg; + msg.header.frame_id = "baselink"; + msg.header.stamp = now(); + msg.accel.accel.linear.x = 0.0; + if (!cmd_history_.empty()) { // ego moves as commanded. + msg.accel.accel.linear.x = cmd_history_.back()->longitudinal.acceleration; + } + pub_acc_->publish(msg); + } + { + SteeringReport msg; + msg.stamp = now(); + msg.steering_tire_angle = 0.0; + if (!cmd_history_.empty()) { // ego moves as commanded. + msg.steering_tire_angle = cmd_history_.back()->lateral.steering_tire_angle; + } + pub_steer_->publish(msg); + } + { + OperationModeState msg; + msg.stamp = now(); + msg.mode = OperationModeState::AUTONOMOUS; + msg.is_autoware_control_enabled = true; + pub_operation_mode_->publish(msg); + } + { + MrmState msg; + msg.stamp = now(); + msg.state = MrmState::NORMAL; + msg.behavior = MrmState::NONE; + pub_mrm_state_->publish(msg); + } + { + TurnIndicatorsCommand msg; + msg.stamp = now(); + msg.command = TurnIndicatorsCommand::DISABLE; + pub_auto_turn_indicator_cmd_->publish(msg); + } + { + HazardLightsCommand msg; + msg.stamp = now(); + msg.command = HazardLightsCommand::DISABLE; + pub_auto_hazard_light_cmd_->publish(msg); + } + { + GearCommand msg; + msg.stamp = now(); + msg.command = GearCommand::DRIVE; + pub_auto_gear_cmd_->publish(msg); + } + } + + void publishControlCommand(AckermannControlCommand msg) + { + msg.stamp = now(); + pub_auto_control_cmd_->publish(msg); + raw_cmd_history_.push_back(std::make_shared(msg)); + } + + void checkFilter() + { + if (cmd_history_.size() != cmd_received_times_.size()) { + throw std::logic_error("cmd history and received times must have same size. Check code."); + } + + if (cmd_history_.size() == 1) return; + + const size_t i_curr = cmd_history_.size() - 1; + const size_t i_prev = cmd_history_.size() - 2; + const auto cmd_curr = cmd_history_.at(i_curr); + const auto cmd_prev = cmd_history_.at(i_prev); + + const auto max_lon_acc_lim = *std::max_element(lon_acc_lim.begin(), lon_acc_lim.end()); + const auto max_lon_jerk_lim = *std::max_element(lon_jerk_lim.begin(), lon_jerk_lim.end()); + const auto max_lat_acc_lim = *std::max_element(lat_acc_lim.begin(), lat_acc_lim.end()); + const auto max_lat_jerk_lim = *std::max_element(lat_jerk_lim.begin(), lat_jerk_lim.end()); + + const auto dt = (cmd_received_times_.at(i_curr) - cmd_received_times_.at(i_prev)).seconds(); + const auto lon_vel = cmd_curr->longitudinal.speed; + const auto lon_acc = cmd_curr->longitudinal.acceleration; + const auto lon_jerk = (lon_acc - cmd_prev->longitudinal.acceleration) / dt; + const auto lat_acc = + lon_vel * lon_vel * std::tan(cmd_curr->lateral.steering_tire_angle) / wheelbase; + const auto prev_lon_vel = cmd_prev->longitudinal.speed; + const auto prev_lat_acc = + prev_lon_vel * prev_lon_vel * std::tan(cmd_prev->lateral.steering_tire_angle) / wheelbase; + const auto lat_jerk = (lat_acc - prev_lat_acc) / dt; + + /* debug print */ + // const auto steer = cmd_curr->lateral.steering_tire_angle; + // PRINT_VALUES( + // dt, lon_vel, lon_acc, lon_jerk, lat_acc, lat_jerk, steer, max_lon_acc_lim, + // max_lon_jerk_lim, max_lat_acc_lim, max_lat_jerk_lim); + + // Output command must be smaller than maximum limit. + // TODO(Horibe): check for each velocity range. + constexpr auto threshold_scale = 1.1; + if (std::abs(lon_vel) > 0.01) { + ASSERT_LT_NEAR(std::abs(lon_acc), max_lon_acc_lim, threshold_scale); + ASSERT_LT_NEAR(std::abs(lon_jerk), max_lon_jerk_lim, threshold_scale); + ASSERT_LT_NEAR(std::abs(lat_acc), max_lat_acc_lim, threshold_scale); + ASSERT_LT_NEAR(std::abs(lat_jerk), max_lat_jerk_lim, threshold_scale); + } + } +}; + +struct CmdParam +{ + double max; + double freq; + double bias; + CmdParam() {} + CmdParam(double m, double f, double b) : max(m), freq(f), bias(b) {} +}; + +struct CmdParams +{ + CmdParam steering; + CmdParam velocity; + CmdParam acceleration; + CmdParams() {} + CmdParams(CmdParam s, CmdParam v, CmdParam a) : steering(s), velocity(v), acceleration(a) {} +}; + +class ControlCmdGenerator +{ +public: + CmdParams p_; // used for sin wave command generation + + using Clock = std::chrono::high_resolution_clock; + std::chrono::time_point start_time_{Clock::now()}; + + // generate ControlCommand with sin wave format. + // TODO(Horibe): implement steering_rate and jerk command. + AckermannControlCommand calcSinWaveCommand(bool reset_clock = false) + { + if (reset_clock) { + start_time_ = Clock::now(); + } + + const auto dt_ns = + std::chrono::duration_cast(Clock::now() - start_time_); + const auto dt_s = dt_ns.count() / 1e9; + + const auto sinWave = [&](auto amp, auto freq, auto bias) { + return amp * std::sin(2.0 * M_PI * freq * dt_s + bias); + }; + + AckermannControlCommand cmd; + cmd.lateral.steering_tire_angle = sinWave(p_.steering.max, p_.steering.freq, p_.steering.bias); + cmd.longitudinal.speed = + sinWave(p_.velocity.max, p_.velocity.freq, p_.velocity.bias) + p_.velocity.max; + cmd.longitudinal.acceleration = + sinWave(p_.acceleration.max, p_.acceleration.freq, p_.acceleration.bias); + + return cmd; + } +}; + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto vehicle_cmd_gate_dir = + ament_index_cpp::get_package_share_directory("vehicle_cmd_gate"); + const auto vehicle_info_util_dir = + ament_index_cpp::get_package_share_directory("vehicle_info_util"); + + node_options.arguments( + {"--ros-args", "--params-file", vehicle_cmd_gate_dir + "/config/vehicle_cmd_gate.param.yaml", + "--ros-args", "--params-file", vehicle_info_util_dir + "/config/vehicle_info.param.yaml"}); + + const auto override = [&](const auto s, const std::vector v) { + node_options.append_parameter_override>(s, v); + }; + + node_options.append_parameter_override("wheel_base", wheelbase); + override("nominal.reference_speed_points", reference_speed_points); + override("nominal.reference_speed_points", reference_speed_points); + override("nominal.lon_acc_lim", lon_acc_lim); + override("nominal.lon_jerk_lim", lon_jerk_lim); + override("nominal.lat_acc_lim", lat_acc_lim); + override("nominal.lat_jerk_lim", lat_jerk_lim); + override("nominal.actual_steer_diff_lim", actual_steer_diff_lim); + + return std::make_shared(node_options); +} + +class TestFixture : public ::testing::TestWithParam +{ +protected: + void SetUp() override + { + vehicle_cmd_gate_node_ = generateNode(); + cmd_generator_.p_ = GetParam(); + } + + void TearDown() override + { + // rclcpp::shutdown(); + } + + PubSubNode pub_sub_node_; + std::shared_ptr vehicle_cmd_gate_node_; + ControlCmdGenerator cmd_generator_; +}; + +TEST_P(TestFixture, CheckFilterForSinCmd) +{ + [[maybe_unused]] auto a = std::system("ros2 node list"); + [[maybe_unused]] auto b = std::system("ros2 node info /test_vehicle_cmd_gate_filter_pubsub"); + [[maybe_unused]] auto c = std::system("ros2 node info /vehicle_cmd_gate"); + + for (size_t i = 0; i < 100; ++i) { + const bool reset_clock = (i == 0); + const auto cmd = cmd_generator_.calcSinWaveCommand(reset_clock); + pub_sub_node_.publishControlCommand(cmd); + pub_sub_node_.publishDefaultTopicsNoSpin(); + for (int i = 0; i < 20; ++i) { + rclcpp::spin_some(pub_sub_node_.get_node_base_interface()); + rclcpp::spin_some(vehicle_cmd_gate_node_->get_node_base_interface()); + } + std::this_thread::sleep_for(std::chrono::milliseconds{10LL}); + } + + std::cerr << "received cmd num = " << pub_sub_node_.cmd_received_times_.size() << std::endl; +}; + +// High frequency, large value +CmdParams p1 = {/*steer*/ {10, 1, 0}, /*velocity*/ {10, 1.2, 0}, /*acc*/ {5, 1.5, 2}}; +INSTANTIATE_TEST_SUITE_P(TestParam1, TestFixture, ::testing::Values(p1)); + +// High frequency, normal value +CmdParams p2 = {/*steer*/ {1.5, 2, 1}, /*velocity*/ {5, 1.0, 0}, /*acc*/ {2.0, 3.0, 2}}; +INSTANTIATE_TEST_SUITE_P(TestParam2, TestFixture, ::testing::Values(p2)); + +// High frequency, small value +CmdParams p3 = {/*steer*/ {1.5, 3, 2}, /*velocity*/ {2, 3, 0}, /*acc*/ {0.5, 3, 2}}; +INSTANTIATE_TEST_SUITE_P(TestParam3, TestFixture, ::testing::Values(p3)); + +// Low frequency +CmdParams p4 = {/*steer*/ {10, 0.1, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}}; +INSTANTIATE_TEST_SUITE_P(TestParam4, TestFixture, ::testing::Values(p4)); diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index 5fbfec237b047..7ce8580120652 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -25,19 +25,25 @@ #define ASSERT_GT_NEAR(x, y) ASSERT_GT(x, y - THRESHOLD) using autoware_auto_control_msgs::msg::AckermannControlCommand; +using vehicle_cmd_gate::LimitArray; constexpr double NOMINAL_INTERVAL = 1.0; void setFilterParams( - vehicle_cmd_gate::VehicleCmdFilter & f, double v, double a, double j, double lat_a, double lat_j, - double wheelbase) + vehicle_cmd_gate::VehicleCmdFilter & f, double v, LimitArray speed_points, LimitArray a, + LimitArray j, LimitArray lat_a, LimitArray lat_j, LimitArray steer_diff, const double wheelbase) { - f.setVelLim(v); - f.setLonAccLim(a); - f.setLonJerkLim(j); - f.setLatAccLim(lat_a); - f.setLatJerkLim(lat_j); - f.setWheelBase(wheelbase); + vehicle_cmd_gate::VehicleCmdFilterParam p; + p.vel_lim = v; + p.wheel_base = wheelbase; + p.reference_speed_points = speed_points; + p.lat_acc_lim = lat_a; + p.lat_jerk_lim = lat_j; + p.lon_acc_lim = a; + p.lon_jerk_lim = j; + p.actual_steer_diff_lim = steer_diff; + + f.setParam(p); } AckermannControlCommand genCmd(double s, double sr, double v, double a) @@ -56,15 +62,29 @@ double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase) return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } -void test_all( - double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, +double calcLatJerk( + const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, + const double wheelbase, const double dt) +{ + const auto prev_v = prev_cmd.longitudinal.speed; + const auto prev = prev_v * prev_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; + + const auto curr_v = cmd.longitudinal.speed; + const auto curr = curr_v * curr_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; + + return (curr - prev) / dt; +} + +void test_1d_limit( + double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, double STEER_DIFF, const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) { const double WHEELBASE = 3.0; const double DT = 0.1; // [s] vehicle_cmd_gate::VehicleCmdFilter filter; - setFilterParams(filter, V_LIM, A_LIM, J_LIM, LAT_A_LIM, LAT_J_LIM, WHEELBASE); + setFilterParams( + filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, WHEELBASE); filter.setPrevCmd(prev_cmd); // velocity filter @@ -164,6 +184,23 @@ void test_all( filtered_cmd.lateral.steering_tire_angle, raw_cmd.lateral.steering_tire_angle, THRESHOLD); } } + + // steer diff + { + const auto current_steering = 0.1; + auto filtered_cmd = raw_cmd; + filter.limitActualSteerDiff(current_steering, filtered_cmd); + const auto filtered_steer_diff = filtered_cmd.lateral.steering_tire_angle - current_steering; + const auto raw_steer_diff = raw_cmd.lateral.steering_tire_angle - current_steering; + // check if the filtered value does not exceed the limit. + ASSERT_LT_NEAR(std::abs(filtered_steer_diff), STEER_DIFF); + + // check if the undesired filter is not applied. + if (std::abs(raw_steer_diff) < STEER_DIFF) { + ASSERT_NEAR( + filtered_cmd.lateral.steering_tire_angle, raw_cmd.lateral.steering_tire_angle, THRESHOLD); + } + } } TEST(VehicleCmdFilter, VehicleCmdFilter) @@ -173,6 +210,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) const std::vector j_arr = {0.0, 0.1, 1.0}; const std::vector lat_a_arr = {0.01, 1.0, 100.0}; const std::vector lat_j_arr = {0.01, 1.0, 100.0}; + const std::vector steer_diff_arr = {0.01, 1.0, 100.0}; const std::vector prev_cmd_arr = { genCmd(0.0, 0.0, 0.0, 0.0), genCmd(1.0, 1.0, 1.0, 1.0)}; @@ -187,7 +225,9 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) for (const auto & lj : lat_j_arr) { for (const auto & prev_cmd : prev_cmd_arr) { for (const auto & raw_cmd : raw_cmd_arr) { - test_all(v, a, j, la, lj, prev_cmd, raw_cmd); + for (const auto & steer_diff : steer_diff_arr) { + test_1d_limit(v, a, j, la, lj, steer_diff, prev_cmd, raw_cmd); + } } } } @@ -196,3 +236,228 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) } } } + +TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) +{ + constexpr double WHEELBASE = 2.8; + vehicle_cmd_gate::VehicleCmdFilter filter; + + vehicle_cmd_gate::VehicleCmdFilterParam p; + p.wheel_base = WHEELBASE; + p.vel_lim = 20.0; + p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + p.lon_acc_lim = std::vector{0.3, 0.4, 0.5}; + p.lon_jerk_lim = std::vector{0.4, 0.4, 0.7}; + p.lat_acc_lim = std::vector{0.1, 0.2, 0.3}; + p.lat_jerk_lim = std::vector{0.9, 0.7, 0.1}; + p.actual_steer_diff_lim = std::vector{0.1, 0.3, 0.2}; + filter.setParam(p); + + const auto DT = 0.033; + + const auto orig_cmd = []() { + AckermannControlCommand cmd; + cmd.lateral.steering_tire_angle = 0.5; + cmd.lateral.steering_tire_rotation_rate = 0.5; + cmd.longitudinal.speed = 30.0; + cmd.longitudinal.acceleration = 10.0; + cmd.longitudinal.jerk = 10.0; + return cmd; + }(); + + const auto set_speed_and_reset_prev = [&](const auto & current_vel) { + filter.setCurrentSpeed(current_vel); + }; + + const auto _limitLongitudinalWithVel = [&](const auto & in) { + auto out = in; + filter.limitLongitudinalWithVel(out); + return out; + }; + const auto _limitLongitudinalWithAcc = [&](const auto & in) { + auto out = in; + filter.limitLongitudinalWithAcc(DT, out); + return out; + }; + const auto _limitLongitudinalWithJerk = [&](const auto & in) { + auto out = in; + filter.limitLongitudinalWithJerk(DT, out); + return out; + }; + const auto _limitLateralWithLatAcc = [&](const auto & in) { + auto out = in; + filter.limitLateralWithLatAcc(DT, out); + return out; + }; + const auto _limitLateralWithLatJerk = [&](const auto & in) { + auto out = in; + filter.limitLateralWithLatJerk(DT, out); + return out; + }; + const auto _limitActualSteerDiff = [&](const auto & in) { + auto out = in; + const auto prev_steering = 0.0; + filter.limitActualSteerDiff(prev_steering, out); + return out; + }; + + constexpr double ep = 1.0e-5; + + // vel lim + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.speed, 20.0, ep); + } + + // longitudinal acc lim + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.3, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.3, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.35, ep); + + set_speed_and_reset_prev(5.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.4 + 0.1 / 6.0, ep); + + set_speed_and_reset_prev(8.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.4 + 0.4 / 6.0, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.5, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.5, ep); + } + + // longitudinal jerk lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.lon_jerk_lim = std::vector{0.4, 0.4, 0.7}; + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, 0.4, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * 0.4, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, 0.4, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * 0.4, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, 0.4, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * 0.4, ep); + + set_speed_and_reset_prev(5.0); + const auto expect_v5 = 0.4 + 0.3 / 6.0; + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, expect_v5, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * expect_v5, ep); + + set_speed_and_reset_prev(8.0); + const auto expect_v8 = 0.4 + 1.2 / 6.0; + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, expect_v8, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * expect_v8, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, 0.7, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * 0.7, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, 0.7, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * 0.7, ep); + } + + // lateral acc lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.lat_acc_lim = std::vector{0.1, 0.2, 0.3}; + const auto _calcLatAcc = [&](const auto & cmd) { return calcLatAcc(cmd, WHEELBASE); }; + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.15, ep); + + set_speed_and_reset_prev(5.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.2 + 0.1 / 6.0, ep); + + set_speed_and_reset_prev(8.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.2 + 0.4 / 6.0, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.3, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.3, ep); + } + + // lateral jerk lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.lat_jerk_lim = std::vector{0.9, 0.7, 0.1}; + const auto _calcLatJerk = [&](const auto & cmd) { + return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT); + }; + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.9, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.9, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.9, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.9, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.8, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.8, ep); + + set_speed_and_reset_prev(5.0); + const auto expect_v5 = 0.7 - 0.6 * (1.0 / 6.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), expect_v5, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * expect_v5, ep); + + set_speed_and_reset_prev(8.0); + const auto expect_v8 = 0.7 - 0.6 * (4.0 / 6.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), expect_v8, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * expect_v8, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep); + } + + // steering diff lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.actual_steer_diff_lim = std::vector{0.1, 0.3, 0.2}; + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), 0.1, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), 0.1, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), 0.2, ep); + + set_speed_and_reset_prev(5.0); + const auto expect_v5 = 0.3 - 0.1 / 6.0; + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), expect_v5, ep); + + set_speed_and_reset_prev(8.0); + const auto expect_v8 = 0.3 - 0.4 / 6.0; + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), expect_v8, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), 0.2, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), 0.2, ep); + } +} diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index a673eb08c222f..116055dd9e87e 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -306,6 +306,12 @@ def launch_setup(context, *args, **kwargs): target_container="/control/control_container", ) + glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + # set container to run all required components in the same process container = ComposableNodeContainer( name="control_container", @@ -319,6 +325,7 @@ def launch_setup(context, *args, **kwargs): shift_decider_component, vehicle_cmd_gate_component, operation_mode_transition_manager_component, + glog_component, ], ) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 86ede0d4be47d..5b6134dc45584 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -11,6 +11,13 @@ + + + + + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index fc1b55ee52eac..59e8038e49188 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -64,6 +64,12 @@ def launch_setup(context, *args, **kwargs): with open(LaunchConfiguration("behavior_path_planner_param_path").perform(context), "r") as f: behavior_path_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + behavior_path_planner_component = ComposableNode( package="behavior_path_planner", plugin="behavior_path_planner::BehaviorPathPlannerNode", @@ -211,6 +217,7 @@ def launch_setup(context, *args, **kwargs): composable_node_descriptions=[ behavior_path_planner_component, behavior_velocity_planner_component, + glog_component, ], output="screen", ) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index 34ad4659b5ece..d8c5d7825f19d 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -309,6 +309,16 @@ def launch_setup(context, *args, **kwargs): condition=IfCondition(LaunchConfiguration("use_surround_obstacle_check")), ) + glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + glog_component_loader = LoadComposableNodes( + composable_node_descriptions=[glog_component], + target_container=container, + ) + group = GroupAction( [ container, @@ -319,6 +329,7 @@ def launch_setup(context, *args, **kwargs): obstacle_cruise_planner_loader, obstacle_cruise_planner_relay_loader, surround_obstacle_checker_loader, + glog_component_loader, ] ) return [group] diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 4de0f91c53c6a..39b95286bb6cc 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -60,6 +60,7 @@ external_cmd_selector external_velocity_limit_selector freespace_planner + glog_component mission_planner motion_velocity_smoother obstacle_avoidance_planner diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp index 6a127c70bb9a0..57f2302fd5c5a 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp @@ -54,7 +54,9 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st { tier4_autoware_utils::StopWatch stop_watch; line_segment_detector_->detect(gray_image, lines); - line_segment_detector_->drawSegments(gray_image, lines); + if (lines.size().width != 0) { + line_segment_detector_->drawSegments(gray_image, lines); + } RCLCPP_INFO_STREAM(this->get_logger(), "lsd: " << stop_watch.toc() << "[ms]"); } diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 4c64eb1eaecae..816cfc418f016 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -144,7 +144,7 @@ The node projects lan/lon coordinates into arbitrary coordinates defined in `/ma The node supports the following three types of coordinate systems: - MGRS -- UTM +- LocalCartesianUTM - local ### How to run diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index bb52e17c342ea..c443318a146ac 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -98,7 +98,7 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( if (errors.empty()) { return map; } - } else if (lanelet2_map_projector_type == "UTM") { + } else if (lanelet2_map_projector_type == "LocalCartesianUTM") { lanelet::GPSPoint position{map_origin_lat, map_origin_lon}; lanelet::Origin origin{position}; lanelet::projection::UtmProjector projector{origin}; diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index a53cdddf93b5b..8658ba172fde2 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -39,7 +39,7 @@ if(BUILD_TESTING) TIMEOUT "30" ) add_launch_test( - test/test_node_load_utm_from_yaml.test.py + test/test_node_load_local_cartesian_utm_from_yaml.test.py TIMEOUT "30" ) install(DIRECTORY diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index bbf15f34929da..63095a72e835e 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -38,13 +38,13 @@ type: "MGRS" mgrs_grid: "54SUE" ``` -### Using UTM +### Using LocalCartesianUTM -If you want to use UTM, please specify the map origin as well. +If you want to use local cartesian UTM, please specify the map origin as well. ```yaml # map_projector_info.yaml -type: "UTM" +type: "LocalCartesianUTM" map_origin: latitude: 35.6092 longitude: 139.7303 diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 6d1459c628e35..2bd8a9cfa243a 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -30,14 +30,14 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi msg.type = data["type"].as(); if (msg.type == "MGRS") { msg.mgrs_grid = data["mgrs_grid"].as(); - } else if (msg.type == "UTM") { + } else if (msg.type == "LocalCartesianUTM") { msg.map_origin.latitude = data["map_origin"]["latitude"].as(); msg.map_origin.longitude = data["map_origin"]["longitude"].as(); } else if (msg.type == "local") { ; // do nothing } else { throw std::runtime_error( - "Invalid map projector type. Currently supported types: MGRS, UTM, and local"); + "Invalid map projector type. Currently supported types: MGRS, LocalCartesianUTM, and local"); } return msg; } diff --git a/map/map_projection_loader/test/data/projection_info_utm.yaml b/map/map_projection_loader/test/data/projection_info_local_cartesian_utm.yaml similarity index 69% rename from map/map_projection_loader/test/data/projection_info_utm.yaml rename to map/map_projection_loader/test/data/projection_info_local_cartesian_utm.yaml index 002383c97a474..c7500d37bb35a 100644 --- a/map/map_projection_loader/test/data/projection_info_utm.yaml +++ b/map/map_projection_loader/test/data/projection_info_local_cartesian_utm.yaml @@ -1,4 +1,4 @@ -type: UTM +type: LocalCartesianUTM map_origin: latitude: 35.6762 longitude: 139.6503 diff --git a/map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py similarity index 98% rename from map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py rename to map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py index 8d863033adcb8..920f5e3be11a1 100644 --- a/map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py @@ -33,7 +33,7 @@ logger = get_logger(__name__) -YAML_FILE_PATH = "test/data/projection_info_utm.yaml" +YAML_FILE_PATH = "test/data/projection_info_local_cartesian_utm.yaml" @pytest.mark.launch_test diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index edd5e6d7a835f..2b0fff35729d0 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -55,7 +55,7 @@ BicycleTracker::BicycleTracker( float q_stddev_y = 0.6; // [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(25); // [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(15); // [rad/(s*s)] float r_stddev_x = 0.6; // [m] float r_stddev_y = 0.4; // [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 7cb2772f006c3..73916742f17bb 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -54,7 +54,7 @@ BigVehicleTracker::BigVehicleTracker( float q_stddev_y = 1.5; // [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // [rad/(s*s)] float r_stddev_x = 1.5; // [m] float r_stddev_y = 0.5; // [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] @@ -150,7 +150,7 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.1; // under steered if smaller than 0.5 + double point_ratio = 0.2; // under steered if smaller than 0.5 lf_ = bounding_box_.length * point_ratio; lr_ = bounding_box_.length * (1.0 - point_ratio); } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 12a84a7c7929c..27c2a7b821eb6 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -54,7 +54,7 @@ NormalVehicleTracker::NormalVehicleTracker( float q_stddev_y = 1.0; // object coordinate [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // map coordinate[rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // object coordinate [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(20); // object coordinate [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // object coordinate [rad/(s*s)] float r_stddev_x = 1.0; // object coordinate [m] float r_stddev_y = 0.3; // object coordinate [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index 741d333142c9d..f2cd6203b3685 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -25,8 +25,16 @@ bool transformPointcloud( const std::string & target_frame, sensor_msgs::msg::PointCloud2 & output) { geometry_msgs::msg::TransformStamped tf_stamped; - tf_stamped = tf2.lookupTransform( - target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); + // lookup transform + try { + tf_stamped = tf2.lookupTransform( + target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN( + rclcpp::get_logger("probabilistic_occupancy_grid_map"), "Failed to lookup transform: %s", + ex.what()); + return false; + } // transform pointcloud Eigen::Matrix4f tf_matrix = tf2::transformToEigen(tf_stamped.transform).matrix().cast(); pcl_ros::transformPointCloud(tf_matrix, input, output); diff --git a/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py b/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py index 6b0150dc3fa7b..093811d4cc213 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py +++ b/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py @@ -238,6 +238,29 @@ def test_null_input(self, proc_info): self.assertIn("occupancy_grid_map_node", nodes) self.assertEqual(len(self.msg_buffer), 1) + def test_null_input2(self, proc_info): + """Test null input. + + input: null pointcloud without even frame_id + output: null ogm + """ + # wait for the node to be ready + time.sleep(3) + input_points = [] + pub_raw, pub_obstacle, sub = self.create_pub_sub() + # publish input pointcloud + pt_msg = get_pointcloud_msg(input_points) + pt_msg.header.frame_id = "" + pub_raw.publish(pt_msg) + pub_obstacle.publish(pt_msg) + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=3.0) + + # check if process is successfully terminated + nodes = self.node.get_node_names() + self.assertIn("occupancy_grid_map_node", nodes) + self.assertEqual(len(self.msg_buffer), 0) + @launch_testing.post_shutdown_test() class TestProcessOutput(unittest.TestCase): diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index e06393495fa1b..9ecc1a63b55b0 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -143,6 +143,14 @@ time_horizon: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] safety_check_hysteresis_factor: 2.0 # [-] + # rss parameters + expected_front_deceleration: -1.0 # [m/ss] + expected_rear_deceleration: -1.0 # [m/ss] + rear_vehicle_reaction_time: 2.0 # [s] + rear_vehicle_safety_time_margin: 1.0 # [s] + lateral_distance_max_threshold: 2.0 # [m] + longitudinal_distance_min_threshold: 3.0 # [m] + longitudinal_velocity_delta_time: 0.8 # [s] # For avoidance maneuver avoidance: diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 02dc13ffd84da..95c60612bfdb7 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -25,19 +25,6 @@ visualize_maximum_drivable_area: true - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 # [s] - - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lane_following: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 745fd3877fec6..925e70d9084d6 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -26,6 +26,18 @@ min_longitudinal_acc: -1.0 max_longitudinal_acc: 1.0 + # safety check + safety_check: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + expected_front_deceleration_for_abort: -1.0 + expected_rear_deceleration_for_abort: -2.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index e7d017477b177..d6b99dca70618 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -141,20 +141,6 @@ struct BehaviorPathPlannerParameters // maximum drivable area visualization bool visualize_maximum_drivable_area; - - // collision check - double lateral_distance_max_threshold; - double longitudinal_distance_min_threshold; - double longitudinal_velocity_delta_time; - - double expected_front_deceleration; // brake parameter under normal lane change - double expected_rear_deceleration; // brake parameter under normal lane change - - double expected_front_deceleration_for_abort; // hard brake parameter for abort - double expected_rear_deceleration_for_abort; // hard brake parameter for abort - - double rear_vehicle_reaction_time; - double rear_vehicle_safety_time_margin; }; #endif // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 9a6a12427d783..5d13507f5206c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -140,7 +140,7 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const double front_decel, const double rear_decel, + const utils::path_safety_checker::RSSparams & rss_params, std::unordered_map & debug_data) const; LaneChangeTargetObjectIndices filterObject( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index ef956fb7950dd..1f47b804e185b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include @@ -296,6 +297,9 @@ struct AvoidanceParameters // parameters depend on object class std::unordered_map object_parameters; + // rss parameters + utils::path_safety_checker::RSSparams rss_params; + // clip left and right bounds for objects bool enable_bound_clipping{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 721c5b00f6e33..0556a780467c0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -76,6 +76,10 @@ struct LaneChangeParameters bool check_motorcycle{true}; // check object motorbike bool check_pedestrian{true}; // check object pedestrian + // safety check + utils::path_safety_checker::RSSparams rss_params; + utils::path_safety_checker::RSSparams rss_params_for_abort; + // abort LaneChangeCancelParameters cancel; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 519f589f561ee..c4a6a86861e6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -123,6 +123,8 @@ struct RSSparams double longitudinal_distance_min_threshold{ 0.0}; ///< Minimum threshold for longitudinal distance. double longitudinal_velocity_delta_time{0.0}; ///< Delta time for longitudinal velocity. + double front_vehicle_deceleration; ///< brake parameter + double rear_vehicle_deceleration; ///< brake parameter }; /** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 8606f2cd2397d..6d9df7677ead5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -65,8 +65,7 @@ Polygon2d createExtendedPolygon( double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, - const double front_object_deceleration, const double rear_object_deceleration, - const BehaviorPathPlannerParameters & params); + const RSSparams & rss_params); double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, @@ -98,8 +97,8 @@ bool checkCollision( const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, - const double rear_object_deceleration, CollisionCheckDebug & debug); + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + CollisionCheckDebug & debug); /** * @brief Check collision between ego path footprints with extra longitudinal stopping margin and diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp index d68985ec27350..608b918e839fd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp @@ -19,6 +19,7 @@ #include +#include #include namespace behavior_path_planner @@ -27,6 +28,8 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; struct PullOutPath { std::vector partial_paths{}; + // accelerate with constant acceleration to the target velocity + std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; Pose end_pose{}; }; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 3e2ccd23d58e7..3023357bd0a96 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -391,23 +391,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold"); - p.longitudinal_distance_min_threshold = - declare_parameter("longitudinal_distance_min_threshold"); - p.longitudinal_velocity_delta_time = - declare_parameter("longitudinal_velocity_delta_time"); - - p.expected_front_deceleration = declare_parameter("expected_front_deceleration"); - p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration"); - - p.expected_front_deceleration_for_abort = - declare_parameter("expected_front_deceleration_for_abort"); - p.expected_rear_deceleration_for_abort = - declare_parameter("expected_rear_deceleration_for_abort"); - - p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time"); - p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin"); - if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( get_logger(), "Lane change buffer must be more than 1 meter. Modifying the buffer."); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 3550b2035248a..b0fda03418069 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1879,8 +1879,8 @@ bool AvoidanceModule::isSafePath( for (const auto & obj_path : obj_predicted_paths) { CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( - shifted_path.path, ego_predicted_path, object, obj_path, p, - p.expected_front_deceleration, p.expected_rear_deceleration, collision)) { + shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, + collision)) { return false; } } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 606869eb4007f..c13c237673625 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -131,7 +131,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.object_last_seen_threshold = get_parameter(node, ns + "object_last_seen_threshold"); } - // safety check + // safety check general params { std::string ns = "avoidance.safety_check."; p.enable_safety_check = get_parameter(node, ns + "enable"); @@ -149,6 +149,25 @@ AvoidanceModuleManager::AvoidanceModuleManager( get_parameter(node, ns + "safety_check_hysteresis_factor"); } + // safety check rss params + { + std::string ns = "avoidance.safety_check."; + p.rss_params.longitudinal_distance_min_threshold = + get_parameter(node, ns + "longitudinal_distance_min_threshold"); + p.rss_params.longitudinal_velocity_delta_time = + get_parameter(node, ns + "longitudinal_velocity_delta_time"); + p.rss_params.front_vehicle_deceleration = + get_parameter(node, ns + "expected_front_deceleration"); + p.rss_params.rear_vehicle_deceleration = + get_parameter(node, ns + "expected_rear_deceleration"); + p.rss_params.rear_vehicle_reaction_time = + get_parameter(node, ns + "rear_vehicle_reaction_time"); + p.rss_params.rear_vehicle_safety_time_margin = + get_parameter(node, ns + "rear_vehicle_safety_time_margin"); + p.rss_params.lateral_distance_max_threshold = + get_parameter(node, ns + "lateral_distance_max_threshold"); + } + // avoidance maneuver (lateral) { std::string ns = "avoidance.avoidance.lateral."; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 5dc2a1ee9837c..f392ef6a26e22 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -85,6 +85,36 @@ LaneChangeModuleManager::LaneChangeModuleManager( get_parameter(node, parameter("check_objects_on_other_lanes")); p.use_all_predicted_path = get_parameter(node, parameter("use_all_predicted_path")); + p.rss_params.longitudinal_distance_min_threshold = + get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_velocity_delta_time = + get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); + p.rss_params.front_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_front_deceleration")); + p.rss_params.rear_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_rear_deceleration")); + p.rss_params.rear_vehicle_reaction_time = + get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); + p.rss_params.rear_vehicle_safety_time_margin = + get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); + p.rss_params.lateral_distance_max_threshold = + get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); + + p.rss_params_for_abort.longitudinal_distance_min_threshold = + get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); + p.rss_params_for_abort.longitudinal_velocity_delta_time = + get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); + p.rss_params_for_abort.front_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_front_deceleration_for_abort")); + p.rss_params_for_abort.rear_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_rear_deceleration_for_abort")); + p.rss_params_for_abort.rear_vehicle_reaction_time = + get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); + p.rss_params_for_abort.rear_vehicle_safety_time_margin = + get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); + p.rss_params_for_abort.lateral_distance_max_threshold = + get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); + // target object { std::string ns = "lane_change.target_object."; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index dad925c6b2335..8fdb79a6271d1 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1045,8 +1045,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, common_parameters.expected_front_deceleration, - common_parameters.expected_rear_deceleration, object_debug_); + *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); if (is_safe) { return true; @@ -1059,7 +1058,6 @@ bool NormalLaneChange::getLaneChangePaths( PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { - const auto & common_parameters = getCommonParam(); const auto & path = status_.lane_change_path; const auto & current_lanes = status_.current_lanes; const auto & target_lanes = status_.target_lanes; @@ -1068,8 +1066,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; const auto safety_status = isLaneChangePathSafe( - path, target_objects, common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, debug_data); + path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); { // only for debug purpose object_debug_.clear(); @@ -1326,7 +1323,7 @@ bool NormalLaneChange::getAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const double front_decel, const double rear_decel, + const utils::path_safety_checker::RSSparams & rss_params, std::unordered_map & debug_data) const { PathSafetyStatus path_safety_status; @@ -1388,7 +1385,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { if (!utils::path_safety_checker::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, front_decel, rear_decel, + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, current_debug_data.second)) { path_safety_status.is_safe = false; updateDebugInfo(current_debug_data, path_safety_status.is_safe); diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 4dd2e7a13a94d..2dc2401571d9a 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -106,6 +106,7 @@ void StartPlannerModule::processOnExit() bool StartPlannerModule::isExecutionRequested() const { + // TODO(Sugahara): if required lateral shift distance is small, don't engage this module. // Execute when current pose is near route start pose const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 34ba607938385..c6a68e108e7e1 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -135,8 +135,7 @@ Polygon2d createExtendedPolygon( double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, - const double front_object_deceleration, const double rear_object_deceleration, - const BehaviorPathPlannerParameters & params) + const RSSparams & rss_params) { const auto stoppingDistance = [](const auto vehicle_velocity, const auto vehicle_accel) { // compensate if user accidentally set the deceleration to some positive value @@ -145,23 +144,23 @@ double calcRssDistance( }; const double & reaction_time = - params.rear_vehicle_reaction_time + params.rear_vehicle_safety_time_margin; + rss_params.rear_vehicle_reaction_time + rss_params.rear_vehicle_safety_time_margin; const double front_object_stop_length = - stoppingDistance(front_object_velocity, front_object_deceleration); + stoppingDistance(front_object_velocity, rss_params.front_vehicle_deceleration); const double rear_object_stop_length = rear_object_velocity * reaction_time + - stoppingDistance(rear_object_velocity, rear_object_deceleration); + stoppingDistance(rear_object_velocity, rss_params.rear_vehicle_deceleration); return rear_object_stop_length - front_object_stop_length; } double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, - const BehaviorPathPlannerParameters & params) + const RSSparams & rss_params) { - const double & lon_threshold = params.longitudinal_distance_min_threshold; + const double & lon_threshold = rss_params.longitudinal_distance_min_threshold; const auto max_vel = std::max(front_object_velocity, rear_object_velocity); - return params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; + return rss_params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; } boost::optional calcInterpolatedPoseWithVelocity( @@ -219,8 +218,8 @@ bool checkCollision( const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, - const double rear_object_deceleration, CollisionCheckDebug & debug) + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + CollisionCheckDebug & debug) { debug.lerped_path.reserve(target_object_path.path.size()); @@ -267,16 +266,15 @@ bool checkCollision( : std::make_pair(ego_velocity, object_velocity); // compute rss dist - const auto rss_dist = calcRssDistance( - front_object_velocity, rear_object_velocity, front_object_deceleration, - rear_object_deceleration, common_parameters); + const auto rss_dist = + calcRssDistance(front_object_velocity, rear_object_velocity, rss_parameters); // minimum longitudinal length const auto min_lon_length = - calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, common_parameters); + calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, rss_parameters); const auto & lon_offset = std::max(rss_dist, min_lon_length); - const auto & lat_margin = common_parameters.lateral_distance_max_threshold; + const auto & lat_margin = rss_parameters.lateral_distance_max_threshold; const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 721ffd3064840..45d36d51d69b4 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -80,15 +80,48 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p parameters_.collision_check_margin)) { return {}; } + const double velocity = parallel_parking_parameters_.forward_parking_velocity; if (parameters_.divide_pull_out_path) { output.partial_paths = planner_.getPaths(); + /* + Calculate the acceleration required to reach the forward parking velocity at the center of + the front path, assuming constant acceleration and deceleration. + v v + | | + | /\ | /\ + | / \ | / \ + | / \ | / \ + | / \ | / \ + |/________\_____ x |/________\______ t + 0 a_l/2 a_l 0 t_c 2*t_c + Notes: + a_l represents "arc_length_on_path_front" + t_c represents "time_to_center" + */ // insert stop velocity to first arc path end output.partial_paths.front().points.back().point.longitudinal_velocity_mps = 0.0; + const double arc_length_on_first_arc_path = + motion_utils::calcArcLength(output.partial_paths.front().points); + const double time_to_center = arc_length_on_first_arc_path / (2 * velocity); + const double average_velocity = arc_length_on_first_arc_path / (time_to_center * 2); + const double average_acceleration = average_velocity / (time_to_center * 2); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(average_velocity, average_acceleration)); + const double arc_length_on_second_arc_path = + motion_utils::calcArcLength(planner_.getArcPaths().at(1).points); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(velocity, velocity * velocity / (2 * arc_length_on_second_arc_path))); } else { - auto partial_paths = planner_.getPaths(); + const auto partial_paths = planner_.getPaths(); const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); + + // Calculate the acceleration required to reach the forward parking velocity at the center of + // the path, assuming constant acceleration and deceleration. + const double arc_length_on_path = motion_utils::calcArcLength(combined_path.points); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(velocity, velocity * velocity / 2 * arc_length_on_path)); } output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 08dcc0254deb1..7352821e140be 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -163,9 +163,12 @@ std::vector ShiftPullOut::calcPullOutPaths( // non_shifted_path for when shift length or pull out distance is too short const PullOutPath non_shifted_path = std::invoke([&]() { PullOutPath non_shifted_path{}; + // In non_shifted_path, to minimize safety checks, 0 is assigned to prevent the predicted_path + // of the ego vehicle from becoming too large. non_shifted_path.partial_paths.push_back(road_lane_reference_path); non_shifted_path.start_pose = start_pose; non_shifted_path.end_pose = start_pose; + non_shifted_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(0, 0)); return non_shifted_path; }); @@ -283,6 +286,8 @@ std::vector ShiftPullOut::calcPullOutPaths( candidate_path.partial_paths.push_back(shifted_path.path); candidate_path.start_pose = shift_line.start; candidate_path.end_pose = shift_line.end; + candidate_path.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(terminal_velocity, longitudinal_acc)); candidate_paths.push_back(candidate_path); } diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index dbbab9f040cfa..6da5ff97b9bf8 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1223,6 +1223,7 @@ boost::optional getOverlappedLaneletId(const std::vector boost::geometry::intersection( lanelet.polygon2d().basicPolygon(), target_lanelet.polygon2d().basicPolygon(), intersections); + // if only one point intersects, it is assumed not to be overlapped if (intersections.size() > 1) { return true; @@ -1265,35 +1266,69 @@ std::vector cutOverlappedLanes( std::vector shorten_lanes{lanes.begin(), lanes.begin() + *overlapped_lanelet_idx}; const auto shorten_lanelets = utils::transformToLanelets(shorten_lanes); - // create removed lanelets - std::vector removed_lane_ids; - for (size_t i = *overlapped_lanelet_idx; i < lanes.size(); ++i) { - const auto target_lanelets = utils::transformToLanelets(lanes.at(i)); - for (const auto & target_lanelet : target_lanelets) { - // if target lane is inside of the shorten lanelets, we do not remove it - if (checkHasSameLane(shorten_lanelets, target_lanelet)) { - continue; + const auto original_points = path.points; + + path.points.clear(); + + const auto has_same_id_lane = [](const auto & lanelet, const auto & p) { + return std::any_of(p.lane_ids.begin(), p.lane_ids.end(), [&lanelet](const auto id) { + return lanelet.id() == id; + }); + }; + + const auto has_same_id_lanes = [&has_same_id_lane](const auto & lanelets, const auto & p) { + return std::any_of( + lanelets.begin(), lanelets.end(), + [&has_same_id_lane, &p](const auto & lanelet) { return has_same_id_lane(lanelet, p); }); + }; + + // Step1. find first path point within drivable lanes + size_t start_point_idx = std::numeric_limits::max(); + for (const auto & lanes : shorten_lanes) { + for (size_t i = 0; i < original_points.size(); ++i) { + // check right lane + if (has_same_id_lane(lanes.right_lane, original_points.at(i))) { + start_point_idx = std::min(start_point_idx, i); + } + + // check left lane + if (has_same_id_lane(lanes.left_lane, original_points.at(i))) { + start_point_idx = std::min(start_point_idx, i); + } + + // check middle lanes + if (has_same_id_lanes(lanes.middle_lanes, original_points.at(i))) { + start_point_idx = std::min(start_point_idx, i); } - removed_lane_ids.push_back(target_lanelet.id()); } } - const auto is_same_lane_id = [&removed_lane_ids](const auto & point) { - const auto & lane_ids = point.lane_ids; - for (const auto & lane_id : lane_ids) { - const auto is_same_id = [&lane_id](const auto id) { return lane_id == id; }; + // Step2. pick up only path points within drivable lanes + for (const auto & lanes : shorten_lanes) { + for (size_t i = start_point_idx; i < original_points.size(); ++i) { + // check right lane + if (has_same_id_lane(lanes.right_lane, original_points.at(i))) { + path.points.push_back(original_points.at(i)); + continue; + } - if (std::any_of(removed_lane_ids.begin(), removed_lane_ids.end(), is_same_id)) { - return true; + // check left lane + if (has_same_id_lane(lanes.left_lane, original_points.at(i))) { + path.points.push_back(original_points.at(i)); + continue; } - } - return false; - }; - const auto points_with_overlapped_id = - std::remove_if(path.points.begin(), path.points.end(), is_same_lane_id); + // check middle lanes + if (has_same_id_lanes(lanes.middle_lanes, original_points.at(i))) { + path.points.push_back(original_points.at(i)); + continue; + } + + start_point_idx = i; + break; + } + } - path.points.erase(points_with_overlapped_id, path.points.end()); return shorten_lanes; } @@ -1606,6 +1641,10 @@ std::vector calcBound( const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + const auto is_clockwise_polygon = + boost::geometry::is_valid(lanelet::utils::to2D(polygon.basicPolygon())); + const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; + const auto start_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { return p.id() == bound.front().id(); }); @@ -1621,7 +1660,8 @@ std::vector calcBound( // extract line strings between start_idx and end_idx. const size_t start_idx = std::distance(polygon.begin(), start_itr); const size_t end_idx = std::distance(polygon.begin(), end_itr); - for (const auto & point : extract_bound_from_polygon(polygon, start_idx, end_idx, is_left)) { + for (const auto & point : + extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration)) { output_points.push_back(point); } diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index e24f3274179fe..e1b74636f5ed1 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -178,18 +178,20 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, calcRssDistance) { using behavior_path_planner::utils::path_safety_checker::calcRssDistance; + using behavior_path_planner::utils::path_safety_checker::RSSparams; { const double front_vel = 5.0; const double front_decel = -2.0; const double rear_vel = 10.0; const double rear_decel = -1.0; - BehaviorPathPlannerParameters params; + RSSparams params; params.rear_vehicle_reaction_time = 1.0; params.rear_vehicle_safety_time_margin = 1.0; params.longitudinal_distance_min_threshold = 3.0; + params.rear_vehicle_deceleration = rear_decel; + params.front_vehicle_deceleration = front_decel; - EXPECT_NEAR( - calcRssDistance(front_vel, rear_vel, front_decel, rear_decel, params), 63.75, epsilon); + EXPECT_NEAR(calcRssDistance(front_vel, rear_vel, params), 63.75, epsilon); } } diff --git a/planning/freespace_planner/config/freespace_planner.param.yaml b/planning/freespace_planner/config/freespace_planner.param.yaml index 75c813266a83d..8692aa293cbf7 100644 --- a/planning/freespace_planner/config/freespace_planner.param.yaml +++ b/planning/freespace_planner/config/freespace_planner.param.yaml @@ -33,3 +33,11 @@ only_behind_solutions: false use_back: true distance_heuristic_weight: 1.0 + + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 0.1 diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp index 6f1810fabec88..3dced3eb1d01d 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp @@ -118,9 +118,9 @@ class AstarSearch : public AbstractPlanningAlgorithm : AstarSearch( planner_common_param, collision_vehicle_shape, AstarParam{ - node.declare_parameter("astar.only_behind_solutions", false), - node.declare_parameter("astar.use_back", true), - node.declare_parameter("astar.distance_heuristic_weight", 1.0)}) + node.declare_parameter("astar.only_behind_solutions"), + node.declare_parameter("astar.use_back"), + node.declare_parameter("astar.distance_heuristic_weight")}) { } diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp index 07f73281e7176..ac21f7333df37 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp @@ -47,11 +47,11 @@ class RRTStar : public AbstractPlanningAlgorithm : RRTStar( planner_common_param, original_vehicle_shape, RRTStarParam{ - node.declare_parameter("rrtstar.enable_update", true), - node.declare_parameter("rrtstar.use_informed_sampling", true), - node.declare_parameter("rrtstar.max_planning_time", 150.0), - node.declare_parameter("rrtstar.neighbor_radius", 8.0), - node.declare_parameter("rrtstar.margin", 0.1)}) + node.declare_parameter("rrtstar.enable_update"), + node.declare_parameter("rrtstar.use_informed_sampling"), + node.declare_parameter("rrtstar.max_planning_time"), + node.declare_parameter("rrtstar.neighbor_radius"), + node.declare_parameter("rrtstar.margin")}) { } diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 37d3ab4984501..e2012fab43ba4 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -14,6 +14,8 @@ min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] + hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s] + hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m] nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index b6a242609dfff..12ebadf770996 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -165,6 +165,11 @@ struct LongitudinalInfo safe_distance_margin = node.declare_parameter("common.safe_distance_margin"); terminal_safe_distance_margin = node.declare_parameter("common.terminal_safe_distance_margin"); + + hold_stop_velocity_threshold = + node.declare_parameter("common.hold_stop_velocity_threshold"); + hold_stop_distance_threshold = + node.declare_parameter("common.hold_stop_distance_threshold"); } void onParam(const std::vector & parameters) @@ -188,6 +193,11 @@ struct LongitudinalInfo parameters, "common.safe_distance_margin", safe_distance_margin); tier4_autoware_utils::updateParam( parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin); + + tier4_autoware_utils::updateParam( + parameters, "common.hold_stop_velocity_threshold", hold_stop_velocity_threshold); + tier4_autoware_utils::updateParam( + parameters, "common.hold_stop_distance_threshold", hold_stop_distance_threshold); } // common parameter @@ -208,6 +218,10 @@ struct LongitudinalInfo // distance margin double safe_distance_margin; double terminal_safe_distance_margin; + + // hold stop + double hold_stop_velocity_threshold; + double hold_stop_distance_threshold; }; struct DebugData diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 8aeef15c8b97c..9be6ec10e952e 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -254,6 +254,11 @@ class PlannerInterface SlowDownParam slow_down_param_; std::vector prev_slow_down_output_; + // previous trajectory and distance to stop + // NOTE: Previous trajectory is memorized to deal with nearest index search for overlapping or + // crossing lanes. + std::optional, double>> prev_stop_distance_info_{ + std::nullopt}; }; #endif // OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 1aaa897986bcd..4cf6bf9e23806 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -228,6 +228,7 @@ std::vector PlannerInterface::generateStopTrajectory( motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + prev_stop_distance_info_ = std::nullopt; return planner_data.traj_points; } @@ -244,6 +245,7 @@ std::vector PlannerInterface::generateStopTrajectory( motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + prev_stop_distance_info_ = std::nullopt; return planner_data.traj_points; } @@ -315,9 +317,36 @@ std::vector PlannerInterface::generateStopTrajectory( } // Generate Output Trajectory + const double zero_vel_dist = [&]() { + const double current_zero_vel_dist = + std::max(0.0, closest_obstacle_dist - abs_ego_offset - feasible_margin_from_obstacle); + + // Hold previous stop distance if necessary + if ( + std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && + prev_stop_distance_info_) { + // NOTE: We assume that the current trajectory's front point is ahead of the previous + // trajectory's front point. + const size_t traj_front_point_prev_seg_idx = + motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_stop_distance_info_->first, planner_data.traj_points.front().pose); + const double diff_dist_front_points = motion_utils::calcSignedArcLength( + prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, + traj_front_point_prev_seg_idx); + + const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; + if ( + std::abs(prev_zero_vel_dist - current_zero_vel_dist) < + longitudinal_info_.hold_stop_distance_threshold) { + return prev_zero_vel_dist; + } + } + + return current_zero_vel_dist; + }(); + + // Insert stop point auto output_traj_points = planner_data.traj_points; - const double zero_vel_dist = - std::max(0.0, closest_obstacle_dist - abs_ego_offset - feasible_margin_from_obstacle); const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points); if (zero_vel_idx) { // virtual wall marker for stop obstacle @@ -338,6 +367,8 @@ std::vector PlannerInterface::generateStopTrajectory( const auto stop_speed_exceeded_msg = createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); + + prev_stop_distance_info_ = std::make_pair(output_traj_points, zero_vel_dist); } stop_planning_debug_info_.set( diff --git a/sensing/geo_pos_conv/CHANGELOG.rst b/sensing/geo_pos_conv/CHANGELOG.rst deleted file mode 100644 index 12d311b5aaa5d..0000000000000 --- a/sensing/geo_pos_conv/CHANGELOG.rst +++ /dev/null @@ -1,203 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package geo_pos_conv -^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.0 (2020-10-03) ------------------- -* Convert package to ROS 2 - -1.11.0 (2019-03-21) -------------------- -* add constructor (`#1913 `_) -* Fix license notice in corresponding package.xml -* Contributors: YamatoAndo, amc-nu - -1.10.0 (2019-01-17) -------------------- -* Switch to Apache 2 license (develop branch) (`#1741 `_) - * Switch to Apache 2 - * Replace BSD-3 license header with Apache 2 and reassign copyright to the - Autoware Foundation. - * Update license on Python files - * Update copyright years - * Add #ifndef/define _POINTS_IMAGE_H\_ - * Updated license comment -* Contributors: Esteve Fernandez - -1.9.1 (2018-11-06) ------------------- - -1.9.0 (2018-10-31) ------------------- - -1.8.0 (2018-08-31) ------------------- - -1.7.0 (2018-05-18) ------------------- -* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst -* [fix] Fixes for all packages and dependencies (`#1240 `_) - * Initial Cleanup - * fixed also for indigo - * kf cjeck - * Fix road wizard - * Added travis ci - * Trigger CI - * Fixes to cv_tracker and lidar_tracker cmake - * Fix kitti player dependencies - * Removed unnecessary dependencies - * messages fixing for can - * Update build script travis - * Travis Path - * Travis Paths fix - * Travis test - * Eigen checks - * removed unnecessary dependencies - * Eigen Detection - * Job number reduced - * Eigen3 more fixes - * More Eigen3 - * Even more Eigen - * find package cmake modules included - * More fixes to cmake modules - * Removed non ros dependency - * Enable industrial_ci for indidog and kinetic - * Wrong install command - * fix rviz_plugin install - * FastVirtualScan fix - * Fix Qt5 Fastvirtualscan - * Fixed qt5 system dependencies for rosdep - * NDT TKU Fix catkin not pacakged - * More in detail dependencies fixes for more packages - * GLEW library for ORB - * Ignore OrbLocalizer - * Ignore Version checker - * Fix for driveworks interface - * driveworks not catkinpackagedd - * Missing catkin for driveworks - * libdpm opencv not catkin packaged - * catkin lib gnss not included in obj_db - * Points2Polygon fix - * More missing dependencies - * image viewer not packaged - * Fixed SSH2 detection, added viewers for all distros - * Fix gnss localizer incorrect dependency config - * Fixes to multiple packages dependencies - * gnss plib and package - * More fixes to gnss - * gnss dependencies for gnss_loclaizer - * Missing gnss dependency for gnss on localizer - * More fixes for dependencies - Replaced gnss for autoware_gnss_library - * gnss more fixes - * fixes to more dependencies - * header dependency - * Debug message - * more debug messages changed back to gnss - * debud messages - * gnss test - * gnss install command - * Several fixes for OpenPlanner and its lbiraries - * Fixes to ROSInterface - * More fixes to robotsdk and rosinterface - * robotsdk calibration fix - * Fixes to rosinterface robotsdk libraries and its nodes - * Fixes to Qt5 missing dependencies in robotsdk - * glviewer missing dependencies - * Missing qt specific config cmake for robotsdk - * disable cv_tracker - * Fix to open planner un needed dependendecies - * Fixes for libraries indecision maker - * Fixes to libraries decision_maker installation - * Gazebo on Kinetic - * Added Missing library - * * Removed Gazebo and synchonization packages - * Renames vmap in lane_planner - * Added installation commands for missing pakcages - * Fixes to lane_planner - * Added NDT TKU Glut extra dependencies - * ndt localizer/lib fast pcl fixes - re enable cv_tracker - * Fix kf_lib - * Keep industrial_ci - * Fixes for dpm library - * Fusion lib fixed - * dpm and fusion header should match exported project name - * Fixes to dpm_ocv ndt_localizer and pcl_omp - * no fast_pcl anymore - * fixes to libdpm and its package - * CI test - * test with native travis ci - * missing update for apt - * Fixes to pcl_omp installation and headers - * Final fixes for tests, modified README - * * Fixes to README - * Enable industrial_ci - * re enable native travis tests -* Contributors: Abraham Monrroy, Kosuke Murakami - -1.6.3 (2018-03-06) ------------------- - -1.6.2 (2018-02-27) ------------------- -* Update CHANGELOG -* Contributors: Yusuke FUJII - -1.6.1 (2018-01-20) ------------------- -* update CHANGELOG -* Contributors: Yusuke FUJII - -1.6.0 (2017-12-11) ------------------- -* Prepare release for 1.6.0 -* support all plane(1-19) in geo_pos_conv -* Contributors: Yamato ANDO, yukikitsukawa - -1.5.1 (2017-09-25) ------------------- -* Release/1.5.1 (`#816 `_) - * fix a build error by gcc version - * fix build error for older indigo version - * update changelog for v1.5.1 - * 1.5.1 -* Contributors: Yusuke FUJII - -1.5.0 (2017-09-21) ------------------- -* Update changelog -* install target -* Contributors: Dejan Pangercic, Yusuke FUJII - -1.4.0 (2017-08-04) ------------------- -* version number must equal current release number so we can start releasing in the future -* added changelogs -* Contributors: Dejan Pangercic - -1.3.1 (2017-07-16) ------------------- - -1.3.0 (2017-07-14) ------------------- - -1.2.0 (2017-06-07) ------------------- - -1.1.2 (2017-02-27 23:10) ------------------------- - -1.1.1 (2017-02-27 22:25) ------------------------- - -1.1.0 (2017-02-24) ------------------- - -1.0.1 (2017-01-14) ------------------- - -1.0.0 (2016-12-22) ------------------- -* Initial commit for public release -* Contributors: Shinpei Kato diff --git a/sensing/geo_pos_conv/CMakeLists.txt b/sensing/geo_pos_conv/CMakeLists.txt deleted file mode 100644 index 3c3fad0fadf5c..0000000000000 --- a/sensing/geo_pos_conv/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(geo_pos_conv) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(geo_pos_conv SHARED - src/geo_pos_conv.cpp -) - -ament_auto_package() diff --git a/sensing/geo_pos_conv/README.md b/sensing/geo_pos_conv/README.md deleted file mode 100644 index c75a642bdefb5..0000000000000 --- a/sensing/geo_pos_conv/README.md +++ /dev/null @@ -1,21 +0,0 @@ -# geo_pos_conv - -## Purpose - -The `geo_pos_conv` is a library to calculate the conversion between **x, y positions on the plane rectangular coordinate** and **latitude/longitude on the earth**. - -## Inner-workings / Algorithms - -## Inputs / Outputs - -## Parameters - -## Assumptions / Known limits - -## (Optional) Error detection and handling - -## (Optional) Performance characterization - -## (Optional) References/External links - -## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/geo_pos_conv/include/geo_pos_conv/geo_pos_conv.hpp b/sensing/geo_pos_conv/include/geo_pos_conv/geo_pos_conv.hpp deleted file mode 100644 index 68c4b408efd2d..0000000000000 --- a/sensing/geo_pos_conv/include/geo_pos_conv/geo_pos_conv.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// 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 GEO_POS_CONV__GEO_POS_CONV_HPP_ -#define GEO_POS_CONV__GEO_POS_CONV_HPP_ - -#include - -class geo_pos_conv -{ -private: - double m_x; // m - double m_y; // m - double m_z; // m - - double m_lat; // latitude - double m_lon; // longitude - double m_h; - - double m_PLato; // plane lat - double m_PLo; // plane lon - -public: - geo_pos_conv(); - double x() const; - double y() const; - double z() const; - - void set_plane(double lat, double lon); - void set_plane(int num); - void set_xyz(double cx, double cy, double cz); - - // set llh in radians - void set_llh(double lat, double lon, double h); - - // set llh in nmea degrees - void set_llh_nmea_degrees(double latd, double lond, double h); - - void llh_to_xyz(double lat, double lon, double ele); - - void conv_llh2xyz(void); - void conv_xyz2llh(void); -}; - -#endif // GEO_POS_CONV__GEO_POS_CONV_HPP_ diff --git a/sensing/geo_pos_conv/src/geo_pos_conv.cpp b/sensing/geo_pos_conv/src/geo_pos_conv.cpp deleted file mode 100644 index 5fc489d46980f..0000000000000 --- a/sensing/geo_pos_conv/src/geo_pos_conv.cpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright 2020 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. -/* - * Copyright 2015-2019 Autoware Foundation. 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. - */ - -#include "geo_pos_conv/geo_pos_conv.hpp" - -geo_pos_conv::geo_pos_conv() -: m_x(0), m_y(0), m_z(0), m_lat(0), m_lon(0), m_h(0), m_PLato(0), m_PLo(0) -{ -} - -double geo_pos_conv::x() const -{ - return m_x; -} - -double geo_pos_conv::y() const -{ - return m_y; -} - -double geo_pos_conv::z() const -{ - return m_z; -} - -void geo_pos_conv::set_plane(double lat, double lon) -{ - m_PLato = lat; - m_PLo = lon; -} - -void geo_pos_conv::set_plane(int num) -{ - int lon_deg, lon_min, lat_deg, - lat_min; // longitude and latitude of origin of each plane in Japan - if (num == 1) { - lon_deg = 33; - lon_min = 0; - lat_deg = 129; - lat_min = 30; - } else if (num == 2) { - lon_deg = 33; - lon_min = 0; - lat_deg = 131; - lat_min = 0; - } else if (num == 3) { - lon_deg = 36; - lon_min = 0; - lat_deg = 132; - lat_min = 10; - } else if (num == 4) { - lon_deg = 33; - lon_min = 0; - lat_deg = 133; - lat_min = 30; - } else if (num == 5) { - lon_deg = 36; - lon_min = 0; - lat_deg = 134; - lat_min = 20; - } else if (num == 6) { - lon_deg = 36; - lon_min = 0; - lat_deg = 136; - lat_min = 0; - } else if (num == 7) { - lon_deg = 36; - lon_min = 0; - lat_deg = 137; - lat_min = 10; - } else if (num == 8) { - lon_deg = 36; - lon_min = 0; - lat_deg = 138; - lat_min = 30; - } else if (num == 9) { - lon_deg = 36; - lon_min = 0; - lat_deg = 139; - lat_min = 50; - } else if (num == 10) { - lon_deg = 40; - lon_min = 0; - lat_deg = 140; - lat_min = 50; - } else if (num == 11) { - lon_deg = 44; - lon_min = 0; - lat_deg = 140; - lat_min = 15; - } else if (num == 12) { - lon_deg = 44; - lon_min = 0; - lat_deg = 142; - lat_min = 15; - } else if (num == 13) { - lon_deg = 44; - lon_min = 0; - lat_deg = 144; - lat_min = 15; - } else if (num == 14) { - lon_deg = 26; - lon_min = 0; - lat_deg = 142; - lat_min = 0; - } else if (num == 15) { - lon_deg = 26; - lon_min = 0; - lat_deg = 127; - lat_min = 30; - } else if (num == 16) { - lon_deg = 26; - lon_min = 0; - lat_deg = 124; - lat_min = 0; - } else if (num == 17) { - lon_deg = 26; - lon_min = 0; - lat_deg = 131; - lat_min = 0; - } else if (num == 18) { - lon_deg = 20; - lon_min = 0; - lat_deg = 136; - lat_min = 0; - } else if (num == 19) { - lon_deg = 26; - lon_min = 0; - lat_deg = 154; - lat_min = 0; - } else { - lon_deg = 0; - lon_min = 0; - lat_deg = 0; - lat_min = 0; - } - - // swap longitude and latitude - m_PLo = M_PI * (lat_deg + lat_min / 60.0) / 180.0; - m_PLato = M_PI * (lon_deg + lon_min / 60.0) / 180; -} - -void geo_pos_conv::set_xyz(double cx, double cy, double cz) -{ - m_x = cx; - m_y = cy; - m_z = cz; - conv_xyz2llh(); -} - -void geo_pos_conv::set_llh_nmea_degrees(double latd, double lond, double h) -{ - double lat, lad, lod, lon; - // 1234.56 -> 12'34.56 -> 12+ 34.56/60 - - lad = floor(latd / 100.); - lat = latd - lad * 100.; - lod = floor(lond / 100.); - lon = lond - lod * 100.; - - // Changing Longitude and Latitude to Radians - m_lat = (lad + lat / 60.0) * M_PI / 180; - m_lon = (lod + lon / 60.0) * M_PI / 180; - m_h = h; - - conv_llh2xyz(); -} - -void geo_pos_conv::llh_to_xyz(double lat, double lon, double ele) -{ - m_lat = lat * M_PI / 180; - m_lon = lon * M_PI / 180; - m_h = ele; - - conv_llh2xyz(); -} - -void geo_pos_conv::conv_llh2xyz(void) -{ - double PS; // - double PSo; // - double PDL; // - double Pt; // - double PN; // - double PW; // - - double PB1, PB2, PB3, PB4, PB5, PB6, PB7, PB8, PB9; - double PA, PB, PC, PD, PE, PF, PG, PH, PI; - double Pe; // - double Pet; // - double Pnn; // - double AW, FW, Pmo; - - Pmo = 0.9999; - - /*WGS84 Parameters*/ - AW = 6378137.0; // Semi-major Axis - FW = 1.0 / 298.257222101; // 298.257223563 //Geometrical flattening - - Pe = sqrt(2.0 * FW - pow(FW, 2)); - Pet = sqrt(pow(Pe, 2) / (1.0 - pow(Pe, 2))); - - PA = 1.0 + 3.0 / 4.0 * pow(Pe, 2) + 45.0 / 64.0 * pow(Pe, 4) + 175.0 / 256.0 * pow(Pe, 6) + - 11025.0 / 16384.0 * pow(Pe, 8) + 43659.0 / 65536.0 * pow(Pe, 10) + - 693693.0 / 1048576.0 * pow(Pe, 12) + 19324305.0 / 29360128.0 * pow(Pe, 14) + - 4927697775.0 / 7516192768.0 * pow(Pe, 16); - - PB = 3.0 / 4.0 * pow(Pe, 2) + 15.0 / 16.0 * pow(Pe, 4) + 525.0 / 512.0 * pow(Pe, 6) + - 2205.0 / 2048.0 * pow(Pe, 8) + 72765.0 / 65536.0 * pow(Pe, 10) + - 297297.0 / 262144.0 * pow(Pe, 12) + 135270135.0 / 117440512.0 * pow(Pe, 14) + - 547521975.0 / 469762048.0 * pow(Pe, 16); - - PC = 15.0 / 64.0 * pow(Pe, 4) + 105.0 / 256.0 * pow(Pe, 6) + 2205.0 / 4096.0 * pow(Pe, 8) + - 10395.0 / 16384.0 * pow(Pe, 10) + 1486485.0 / 2097152.0 * pow(Pe, 12) + - 45090045.0 / 58720256.0 * pow(Pe, 14) + 766530765.0 / 939524096.0 * pow(Pe, 16); - - PD = 35.0 / 512.0 * pow(Pe, 6) + 315.0 / 2048.0 * pow(Pe, 8) + 31185.0 / 131072.0 * pow(Pe, 10) + - 165165.0 / 524288.0 * pow(Pe, 12) + 45090045.0 / 117440512.0 * pow(Pe, 14) + - 209053845.0 / 469762048.0 * pow(Pe, 16); - - PE = 315.0 / 16384.0 * pow(Pe, 8) + 3465.0 / 65536.0 * pow(Pe, 10) + - 99099.0 / 1048576.0 * pow(Pe, 12) + 4099095.0 / 29360128.0 * pow(Pe, 14) + - 348423075.0 / 1879048192.0 * pow(Pe, 16); - - PF = 693.0 / 131072.0 * pow(Pe, 10) + 9009.0 / 524288.0 * pow(Pe, 12) + - 4099095.0 / 117440512.0 * pow(Pe, 14) + 26801775.0 / 469762048.0 * pow(Pe, 16); - - PG = 3003.0 / 2097152.0 * pow(Pe, 12) + 315315.0 / 58720256.0 * pow(Pe, 14) + - 11486475.0 / 939524096.0 * pow(Pe, 16); - - PH = 45045.0 / 117440512.0 * pow(Pe, 14) + 765765.0 / 469762048.0 * pow(Pe, 16); - - PI = 765765.0 / 7516192768.0 * pow(Pe, 16); - - PB1 = AW * (1.0 - pow(Pe, 2)) * PA; - PB2 = AW * (1.0 - pow(Pe, 2)) * PB / -2.0; - PB3 = AW * (1.0 - pow(Pe, 2)) * PC / 4.0; - PB4 = AW * (1.0 - pow(Pe, 2)) * PD / -6.0; - PB5 = AW * (1.0 - pow(Pe, 2)) * PE / 8.0; - PB6 = AW * (1.0 - pow(Pe, 2)) * PF / -10.0; - PB7 = AW * (1.0 - pow(Pe, 2)) * PG / 12.0; - PB8 = AW * (1.0 - pow(Pe, 2)) * PH / -14.0; - PB9 = AW * (1.0 - pow(Pe, 2)) * PI / 16.0; - - PS = PB1 * m_lat + PB2 * sin(2.0 * m_lat) + PB3 * sin(4.0 * m_lat) + PB4 * sin(6.0 * m_lat) + - PB5 * sin(8.0 * m_lat) + PB6 * sin(10.0 * m_lat) + PB7 * sin(12.0 * m_lat) + - PB8 * sin(14.0 * m_lat) + PB9 * sin(16.0 * m_lat); - - PSo = PB1 * m_PLato + PB2 * sin(2.0 * m_PLato) + PB3 * sin(4.0 * m_PLato) + - PB4 * sin(6.0 * m_PLato) + PB5 * sin(8.0 * m_PLato) + PB6 * sin(10.0 * m_PLato) + - PB7 * sin(12.0 * m_PLato) + PB8 * sin(14.0 * m_PLato) + PB9 * sin(16.0 * m_PLato); - - PDL = m_lon - m_PLo; - Pt = tan(m_lat); - PW = sqrt(1.0 - pow(Pe, 2) * pow(sin(m_lat), 2)); - PN = AW / PW; - Pnn = sqrt(pow(Pet, 2) * pow(cos(m_lat), 2)); - - m_x = ((PS - PSo) + (1.0 / 2.0) * PN * pow(cos(m_lat), 2.0) * Pt * pow(PDL, 2.0) + - (1.0 / 24.0) * PN * pow(cos(m_lat), 4) * Pt * - (5.0 - pow(Pt, 2) + 9.0 * pow(Pnn, 2) + 4.0 * pow(Pnn, 4)) * pow(PDL, 4) - - (1.0 / 720.0) * PN * pow(cos(m_lat), 6) * Pt * - (-61.0 + 58.0 * pow(Pt, 2) - pow(Pt, 4) - 270.0 * pow(Pnn, 2) + - 330.0 * pow(Pt, 2) * pow(Pnn, 2)) * - pow(PDL, 6) - - (1.0 / 40320.0) * PN * pow(cos(m_lat), 8) * Pt * - (-1385.0 + 3111 * pow(Pt, 2) - 543 * pow(Pt, 4) + pow(Pt, 6)) * pow(PDL, 8)) * - Pmo; - - m_y = (PN * cos(m_lat) * PDL - - 1.0 / 6.0 * PN * pow(cos(m_lat), 3) * (-1 + pow(Pt, 2) - pow(Pnn, 2)) * pow(PDL, 3) - - 1.0 / 120.0 * PN * pow(cos(m_lat), 5) * - (-5.0 + 18.0 * pow(Pt, 2) - pow(Pt, 4) - 14.0 * pow(Pnn, 2) + - 58.0 * pow(Pt, 2) * pow(Pnn, 2)) * - pow(PDL, 5) - - 1.0 / 5040.0 * PN * pow(cos(m_lat), 7) * - (-61.0 + 479.0 * pow(Pt, 2) - 179.0 * pow(Pt, 4) + pow(Pt, 6)) * pow(PDL, 7)) * - Pmo; - - m_z = m_h; -} - -void geo_pos_conv::conv_xyz2llh(void) -{ - // n/a -} diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index d37152297c61d..6eef2cc243ef5 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -27,15 +27,14 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates ### Core Parameters -| Name | Type | Default Value | Description | -| ---------------------- | ------ | ---------------- | -------------------------------------------------------------------------------------------------------------------------- | -| `base_frame` | string | "base_link" | frame id | -| `gnss_frame` | string | "gnss" | frame id | -| `gnss_base_frame` | string | "gnss_base_link" | frame id | -| `map_frame` | string | "map" | frame id | -| `coordinate_system` | int | "4" | coordinate system enumeration; 0: UTM, 1: MGRS, 2: Plane, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System | -| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems. | -| `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | +| Name | Type | Default Value | Description | +| ---------------------- | ------ | ---------------- | -------------------------------------------------------------------------------------------------------- | +| `base_frame` | string | "base_link" | frame id | +| `gnss_frame` | string | "gnss" | frame id | +| `gnss_base_frame` | string | "gnss_base_link" | frame id | +| `map_frame` | string | "map" | frame id | +| `coordinate_system` | int | "4" | coordinate system enumeration; 1: MGRS, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System | +| `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | ## Assumptions / Known limits diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 3e4122893ecb6..5c1f621626aa2 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include @@ -62,7 +61,6 @@ GNSSStat NavSatFix2LocalCartesianWGS84( sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger) { GNSSStat local_cartesian; - local_cartesian.coordinate_system = CoordinateSystem::LOCAL_CARTESIAN_WGS84; try { GeographicLib::LocalCartesian localCartesian_origin( @@ -84,7 +82,6 @@ GNSSStat NavSatFix2UTM( int height_system) { GNSSStat utm; - utm.coordinate_system = CoordinateSystem::UTM; try { GeographicLib::UTMUPS::Forward( @@ -108,11 +105,9 @@ GNSSStat NavSatFix2LocalCartesianUTM( sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system) { GNSSStat utm_local; - utm_local.coordinate_system = CoordinateSystem::UTM; try { // origin of the local coordinate system in global frame GNSSStat utm_origin; - utm_origin.coordinate_system = CoordinateSystem::UTM; GeographicLib::UTMUPS::Forward( nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone, utm_origin.east_north_up, utm_origin.x, utm_origin.y); @@ -151,7 +146,6 @@ GNSSStat UTM2MGRS( constexpr int GZD_ID_size = 5; // size of header like "53SPU" GNSSStat mgrs = utm; - mgrs.coordinate_system = CoordinateSystem::MGRS; try { std::string mgrs_code; GeographicLib::MGRS::Forward( @@ -183,20 +177,6 @@ GNSSStat NavSatFix2MGRS( return mgrs; } -GNSSStat NavSatFix2PLANE( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const int & plane_zone, - const rclcpp::Logger & logger) -{ - GNSSStat plane; - plane.coordinate_system = CoordinateSystem::PLANE; - geo_pos_conv geo; - geo.set_plane(plane_zone); - geo.llh_to_xyz(nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude); - plane.x = geo.y(); - plane.y = geo.x(); - plane.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger); - return plane; -} } // namespace gnss_poser #endif // GNSS_POSER__CONVERT_HPP_ diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 2b729a9e374f7..4baaec2dfe80e 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -100,8 +100,6 @@ class GNSSPoser : public rclcpp::Node boost::circular_buffer position_buffer_; - int plane_zone_; - autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr msg_gnss_ins_orientation_stamped_; int height_system_; diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp index 46fdc6eeff9ee..fa17bf0e6e232 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp @@ -18,19 +18,12 @@ namespace gnss_poser { -enum class CoordinateSystem { - UTM = 0, - MGRS = 1, - PLANE = 2, - LOCAL_CARTESIAN_WGS84 = 3, - LOCAL_CARTESIAN_UTM = 4 -}; +enum class CoordinateSystem { MGRS = 1, LOCAL_CARTESIAN_WGS84 = 3, LOCAL_CARTESIAN_UTM = 4 }; struct GNSSStat { GNSSStat() - : coordinate_system(CoordinateSystem::MGRS), - east_north_up(true), + : east_north_up(true), zone(0), mgrs_zone(""), x(0), @@ -42,7 +35,6 @@ struct GNSSStat { } - CoordinateSystem coordinate_system; bool east_north_up; int zone; std::string mgrs_zone; diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index e7dc06864ce84..3883a492358d6 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -13,10 +13,9 @@ - + - @@ -34,7 +33,6 @@ - diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 4887494614a81..1488f329edecb 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -14,7 +14,6 @@ libboost-dev autoware_sensing_msgs - geo_pos_conv geographiclib geometry_msgs rclcpp diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 8fc7d9c1599f5..7d244ce188eee 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -32,7 +32,6 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) gnss_base_frame_(declare_parameter("gnss_base_frame", "gnss_base_link")), map_frame_(declare_parameter("map_frame", "map")), use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation", true)), - plane_zone_(declare_parameter("plane_zone", 9)), msg_gnss_ins_orientation_stamped_( std::make_shared()), height_system_(declare_parameter("height_system", 1)), @@ -191,16 +190,12 @@ GNSSStat GNSSPoser::convert( int height_system) { GNSSStat gnss_stat; - if (coordinate_system == CoordinateSystem::UTM) { - gnss_stat = NavSatFix2UTM(nav_sat_fix_msg, this->get_logger(), height_system); - } else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) { + if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) { gnss_stat = NavSatFix2LocalCartesianUTM( nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger(), height_system); } else if (coordinate_system == CoordinateSystem::MGRS) { gnss_stat = NavSatFix2MGRS( nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), height_system); - } else if (coordinate_system == CoordinateSystem::PLANE) { - gnss_stat = NavSatFix2PLANE(nav_sat_fix_msg, plane_zone_, this->get_logger()); } else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_WGS84) { gnss_stat = NavSatFix2LocalCartesianWGS84(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger()); diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 42ca694235a00..eedc8e6bcb573 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -22,13 +22,12 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | -| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | +| Name | Type | Default Value | Description | +| ------------------------- | ------- | ------------- | ----------- | +| `distance_ratio` | double | 1.03 | | +| `object_length_threshold` | double | 0.1 | | +| `num_points_threshold` | int | 4 | | +| `max_rings_num` | uint_16 | 128 | | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index ab7d4e0012304..f655a9245ca6d 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -46,7 +46,6 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter double object_length_threshold_; int num_points_threshold_; uint16_t max_rings_num_; - size_t max_points_num_per_ring_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -54,21 +53,6 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter /** \brief Parameter service callback */ rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - bool isCluster( - const PointCloud2ConstPtr & input, std::pair data_idx_both_ends, int walk_size) - { - if (walk_size > num_points_threshold_) return true; - - auto first_point = reinterpret_cast(&input->data[data_idx_both_ends.first]); - auto last_point = reinterpret_cast(&input->data[data_idx_both_ends.second]); - - const auto x = first_point->x - last_point->x; - const auto y = first_point->y - last_point->y; - const auto z = first_point->z - last_point->z; - - return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_; - } - public: PCL_MAKE_ALIGNED_OPERATOR_NEW explicit RingOutlierFilterComponent(const rclcpp::NodeOptions & options); diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index a65d14c0a1194..e3090f34d6854 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -31,6 +31,7 @@ pcl_msgs pcl_ros point_cloud_msg_wrapper + range-v3 rclcpp rclcpp_components sensor_msgs diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index d2570b9c4d786..e277b221a090d 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -14,10 +14,18 @@ #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" +#include "pointcloud_preprocessor/utility/utilities.hpp" + +#include +#include +#include + #include #include +#include #include + namespace pointcloud_preprocessor { RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options) @@ -40,8 +48,6 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions static_cast(declare_parameter("object_length_threshold", 0.1)); num_points_threshold_ = static_cast(declare_parameter("num_points_threshold", 4)); max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); - max_points_num_per_ring_ = - static_cast(declare_parameter("max_points_num_per_ring", 4000)); } using std::placeholders::_1; @@ -61,122 +67,252 @@ void RingOutlierFilterComponent::faster_filter( } stop_watch_ptr_->toc("processing_time", true); - output.point_step = sizeof(PointXYZI); - output.data.resize(output.point_step * input->width); - size_t output_size = 0; + // The ring_outlier_filter specifies the expected input point cloud format, + // however, we want to verify the input is correct and make failures explicit. + auto getFieldOffsetSafely = [=]( + const std::string & field_name, + const pcl::PCLPointField::PointFieldTypes expected_type) -> size_t { + const auto field_index = pcl::getFieldIndex(*input, field_name); + if (field_index == -1) { + RCLCPP_ERROR(get_logger(), "Field %s not found in input point cloud", field_name.c_str()); + return -1UL; + } - const auto ring_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; - const auto azimuth_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).offset; - const auto distance_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; - const auto intensity_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).offset; - - std::vector> ring2indices; - ring2indices.reserve(max_rings_num_); - - for (uint16_t i = 0; i < max_rings_num_; i++) { - ring2indices.push_back(std::vector()); - ring2indices.back().reserve(max_points_num_per_ring_); - } + auto field = (*input).fields.at(field_index); + if (field.datatype != expected_type) { + RCLCPP_ERROR( + get_logger(), "Field %s has unexpected type %d (expected %d)", field_name.c_str(), + field.datatype, expected_type); + return -1UL; + } - for (size_t data_idx = 0; data_idx < input->data.size(); data_idx += input->point_step) { - const uint16_t ring = *reinterpret_cast(&input->data[data_idx + ring_offset]); - ring2indices[ring].push_back(data_idx); + return static_cast(field.offset); + }; + + // as per the specification of this node, these fields must be present in the input + const auto ring_offset = getFieldOffsetSafely("ring", pcl::PCLPointField::UINT16); + const auto azimuth_offset = getFieldOffsetSafely("azimuth", pcl::PCLPointField::FLOAT32); + const auto distance_offset = getFieldOffsetSafely("distance", pcl::PCLPointField::FLOAT32); + const auto intensity_offset = getFieldOffsetSafely("intensity", pcl::PCLPointField::FLOAT32); + + if ( + ring_offset == -1UL || azimuth_offset == -1UL || distance_offset == -1UL || + intensity_offset == -1UL) { + RCLCPP_ERROR(get_logger(), "One or more required fields are missing in input point cloud"); + return; } - // walk range: [walk_first_idx, walk_last_idx] - int walk_first_idx = 0; - int walk_last_idx = -1; + // The initial implementation of ring outlier filter looked like this: + // 1. Iterate over the input cloud and group point indices by ring + // 2. For each ring: + // 2.1. iterate over the ring points, and group points belonging to the same "walk" + // 2.2. when a walk is closed, copy indexed points to the output cloud if the walk is long + // enough. + // + // Because LIDAR data is naturally "azimuth-major order" and not "ring-major order", such + // implementation is not cache friendly at all, and has negative impact on all the other nodes. + // + // To tackle this issue, the algorithm has been rewritten so that points would be accessed in + // order. To do so, all rings are being processing simultaneously instead of separately. The + // overall logic is still the same. + + // ad-hoc struct to store finished walks information (for isCluster()) + struct WalkInfo + { + size_t id; + int num_points; + float first_point_distance; + float first_point_azimuth; + float last_point_distance; + float last_point_azimuth; + }; + + // ad-hoc struct to keep track of each ring current walk + struct RingWalkInfo + { + WalkInfo first_walk; + WalkInfo current_walk; + }; + + // helper functions + + // check if walk is a valid cluster + const float object_length_threshold2 = object_length_threshold_ * object_length_threshold_; + auto isCluster = [=](const WalkInfo & walk_info) { + // A cluster is a walk which has many points or is long enough + if (walk_info.num_points > num_points_threshold_) return true; + + // Using the law of cosines, the distance between 2 points can be written as: + // |p2-p1|^2 = d1^2 + d2^2 - 2*d1*d2*cos(a) + // where d1 and d2 are the distance attribute of p1 and p2, and 'a' the azimuth diff between + // the 2 points. + const float dist2 = + walk_info.first_point_distance * walk_info.first_point_distance + + walk_info.last_point_distance * walk_info.last_point_distance - + 2 * walk_info.first_point_distance * walk_info.last_point_distance * + std::cos((walk_info.last_point_azimuth - walk_info.first_point_azimuth) * (M_PI / 18000.0)); + return dist2 > object_length_threshold2; + }; + + // check if 2 points belong to the same walk + auto isSameWalk = + [=](float curr_distance, float curr_azimuth, float prev_distance, float prev_azimuth) { + float azimuth_diff = curr_azimuth - prev_azimuth; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + return std::max(curr_distance, prev_distance) < + std::min(curr_distance, prev_distance) * distance_ratio_ && + azimuth_diff < 100.f; + }; + + // tmp vectors to keep track of walk/ring state while processing points in order (cache efficient) + std::vector rings; // info for each LiDAR ring + std::vector points_walk_id; // for each input point, the walk index associated with it + std::vector + walks_cluster_status; // for each generated walk, stores whether it is a cluster + + size_t latest_walk_id = -1UL; // ID given to the latest walk created + + // initialize each ring with two empty walks (first and current walk) + rings.resize(max_rings_num_, RingWalkInfo{{-1UL, 0, 0, 0, 0, 0}, {-1UL, 0, 0, 0, 0, 0}}); + // points are initially associated to no walk (-1UL) + points_walk_id.resize(input->width * input->height, -1UL); + walks_cluster_status.reserve( + max_rings_num_ * 2); // In the worst case, this could grow to the number of input points + + int invalid_ring_count = 0; + + // Build walks and classify points + for (const auto & [raw_p, point_walk_id] : + ranges::views::zip(input->data | ranges::views::chunk(input->point_step), points_walk_id)) { + uint16_t ring_idx{}; + float curr_azimuth{}; + float curr_distance{}; + std::memcpy(&ring_idx, &raw_p.data()[ring_offset], sizeof(ring_idx)); + std::memcpy(&curr_azimuth, &raw_p.data()[azimuth_offset], sizeof(curr_azimuth)); + std::memcpy(&curr_distance, &raw_p.data()[distance_offset], sizeof(curr_distance)); + + if (ring_idx >= max_rings_num_) { + // Either the data is corrupted or max_rings_num_ is not set correctly + // Note: point_walk_id == -1 so the point will be filtered out + ++invalid_ring_count; + continue; + } - for (const auto & indices : ring2indices) { - if (indices.size() < 2) continue; + auto & ring = rings[ring_idx]; + if (ring.current_walk.id == -1UL) { + // first walk ever for this ring. It is both the first and current walk of the ring. + ring.first_walk = + WalkInfo{++latest_walk_id, 1, curr_distance, curr_azimuth, curr_distance, curr_azimuth}; + ring.current_walk = ring.first_walk; + point_walk_id = latest_walk_id; + continue; + } - walk_first_idx = 0; - walk_last_idx = -1; + auto & walk = ring.current_walk; + if (isSameWalk( + curr_distance, curr_azimuth, walk.last_point_distance, walk.last_point_azimuth)) { + // current point is part of previous walk + walk.num_points += 1; + walk.last_point_distance = curr_distance; + walk.last_point_azimuth = curr_azimuth; + point_walk_id = walk.id; + } else { + // previous walk is finished, start a new one + + // check and store whether the previous walks is a cluster + if (walk.id >= walks_cluster_status.size()) walks_cluster_status.resize(walk.id + 1, false); + walks_cluster_status.at(walk.id) = isCluster(walk); + + ring.current_walk = + WalkInfo{++latest_walk_id, 1, curr_distance, curr_azimuth, curr_distance, curr_azimuth}; + point_walk_id = latest_walk_id; + } + } - for (size_t idx = 0U; idx < indices.size() - 1; ++idx) { - const size_t & current_data_idx = indices[idx]; - const size_t & next_data_idx = indices[idx + 1]; - walk_last_idx = idx; + // So far, we have processed ring points as if rings were not circular. Of course, the last and + // first points of a ring could totally be part of the same walk. When such thing happens, we need + // to merge the two walks + for (auto & ring : rings) { + if (ring.current_walk.id == -1UL) { + continue; + } - // if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08) + const auto & walk = ring.current_walk; + if (walk.id >= walks_cluster_status.size()) walks_cluster_status.resize(walk.id + 1, false); + walks_cluster_status.at(walk.id) = isCluster(walk); - const float & current_azimuth = - *reinterpret_cast(&input->data[current_data_idx + azimuth_offset]); - const float & next_azimuth = - *reinterpret_cast(&input->data[next_data_idx + azimuth_offset]); - float azimuth_diff = next_azimuth - current_azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + if (ring.first_walk.id == ring.current_walk.id) { + continue; + } - const float & current_distance = - *reinterpret_cast(&input->data[current_data_idx + distance_offset]); - const float & next_distance = - *reinterpret_cast(&input->data[next_data_idx + distance_offset]); + auto & first_walk = ring.first_walk; + auto & last_walk = ring.current_walk; + + // check if the two walks are connected + if (isSameWalk( + first_walk.first_point_distance, first_walk.first_point_azimuth, + last_walk.last_point_distance, last_walk.last_point_azimuth)) { + // merge + auto combined_num_points = first_walk.num_points + last_walk.num_points; + first_walk.first_point_distance = last_walk.first_point_distance; + first_walk.first_point_azimuth = last_walk.first_point_azimuth; + first_walk.num_points = combined_num_points; + last_walk.last_point_distance = first_walk.last_point_distance; + last_walk.last_point_azimuth = first_walk.last_point_azimuth; + last_walk.num_points = combined_num_points; + + walks_cluster_status.at(first_walk.id) = isCluster(first_walk); + walks_cluster_status.at(last_walk.id) = isCluster(last_walk); + } + } - if ( - std::max(current_distance, next_distance) < - std::min(current_distance, next_distance) * distance_ratio_ && - azimuth_diff < 100.f) { - continue; // Determined to be included in the same walk + // finally copy points + output.point_step = sizeof(PointXYZI); + output.data.resize(output.point_step * input->width * input->height); + size_t output_size = 0; + if (transform_info.need_transform) { + for (const auto & [raw_p, point_walk_id] : ranges::views::zip( + input->data | ranges::views::chunk(input->point_step), points_walk_id)) { + // Filter out points on invalid rings and points not in a cluster + if (point_walk_id == -1UL || !walks_cluster_status.at(point_walk_id)) { + continue; } - if (isCluster( - input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), - walk_last_idx - walk_first_idx + 1)) { - for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto output_ptr = reinterpret_cast(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); - - if (transform_info.need_transform) { - Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); - p = transform_info.eigen_transform * p; - output_ptr->x = p[0]; - output_ptr->y = p[1]; - output_ptr->z = p[2]; - } else { - *output_ptr = *input_ptr; - } - const float & intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - output_ptr->intensity = intensity; - - output_size += output.point_step; - } + // assume binary representation of input point is compatible with PointXYZ* + PointXYZI out_point; + std::memcpy(&out_point, raw_p.data(), sizeof(PointXYZI)); + + Eigen::Vector4f p(out_point.x, out_point.y, out_point.z, 1); + p = transform_info.eigen_transform * p; + out_point.x = p[0]; + out_point.y = p[1]; + out_point.z = p[2]; + // FIXME(VRichardJP) tmp fix because input data does not have the same layout than PointXYZI + std::memcpy( + &out_point.intensity, &raw_p.data()[intensity_offset], sizeof(out_point.intensity)); + + std::memcpy(&output.data[output_size], &out_point, sizeof(PointXYZI)); + output_size += sizeof(PointXYZI); + } + } else { + for (const auto & [raw_p, point_walk_id] : ranges::views::zip( + input->data | ranges::views::chunk(input->point_step), points_walk_id)) { + // Filter out points on invalid rings and points not in a cluster + if (point_walk_id == -1UL || !walks_cluster_status.at(point_walk_id)) { + continue; } - walk_first_idx = idx + 1; - } + PointXYZI out_point; + std::memcpy(&out_point, raw_p.data(), sizeof(PointXYZI)); + // FIXME(VRichardJP) tmp fix because input data does not have the same layout than PointXYZI + std::memcpy( + &out_point.intensity, &raw_p.data()[intensity_offset], sizeof(out_point.intensity)); - if (walk_first_idx > walk_last_idx) continue; - - if (isCluster( - input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), - walk_last_idx - walk_first_idx + 1)) { - for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto output_ptr = reinterpret_cast(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); - - if (transform_info.need_transform) { - Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); - p = transform_info.eigen_transform * p; - output_ptr->x = p[0]; - output_ptr->y = p[1]; - output_ptr->z = p[2]; - } else { - *output_ptr = *input_ptr; - } - const float & intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - output_ptr->intensity = intensity; - - output_size += output.point_step; - } + std::memcpy(&output.data[output_size], &out_point, sizeof(PointXYZI)); + + output_size += sizeof(PointXYZI); } } - output.data.resize(output_size); // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform @@ -196,6 +332,12 @@ void RingOutlierFilterComponent::faster_filter( sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); + if (invalid_ring_count > 0) { + RCLCPP_WARN( + get_logger(), "%d points had ring index over max_rings_num (%d) and have been ignored.", + invalid_ring_count, max_rings_num_); + } + // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index d22e065d37c5f..61d5b2dde6103 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -154,7 +154,7 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; - } else if (map_projector_info_->type == "UTM") { + } else if (map_projector_info_->type == "LocalCartesianUTM") { lanelet::GPSPoint position{ map_projector_info_->map_origin.latitude, map_projector_info_->map_origin.longitude}; lanelet::Origin origin{position};