From c638c004e6b60b8580e45e4466cf8c896e5a1e4c Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Sat, 9 Apr 2022 07:09:58 +0000 Subject: [PATCH 01/37] chore: sync files (#648) * chore: sync files Signed-off-by: GitHub * Revert "chore: sync files" This reverts commit b24f530b48306e16aa285f80a629ce5c5a9ccda7. Signed-off-by: Kenji Miyake * sync codecov.yaml Signed-off-by: Kenji Miyake Co-authored-by: kenji-miyake Co-authored-by: Kenji Miyake --- .github/sync-files.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 1cb1b399efe4d..494ab9300f025 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -35,6 +35,7 @@ - source: .github/workflows/build-and-test-self-hosted.yaml - source: .github/workflows/check-build-depends.yaml - source: .github/workflows/clang-tidy-pr-comments.yaml + - source: codecov.yaml - repository: autowarefoundation/autoware-documentation files: From 2f595b211f28921672f03128702ae9801654ab47 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Sat, 9 Apr 2022 16:21:24 +0900 Subject: [PATCH 02/37] feat(multi_object_tracker): add pass through tracker (#655) * add pass through tracker * remove unnecessary output * add parameter file * remove unnecessary code --- .../multi_object_tracker/CMakeLists.txt | 1 + .../config/default_tracker.param.yaml | 8 ++ .../config/simulation_tracker.param.yaml | 8 ++ .../multi_object_tracker_core.hpp | 3 + .../tracker/model/pass_through_tracker.hpp | 47 ++++++++ .../multi_object_tracker/tracker/tracker.hpp | 1 + .../launch/multi_object_tracker.launch.xml | 2 + .../src/multi_object_tracker_core.cpp | 32 +++++- .../tracker/model/pass_through_tracker.cpp | 105 ++++++++++++++++++ 9 files changed, 203 insertions(+), 4 deletions(-) create mode 100644 perception/multi_object_tracker/config/default_tracker.param.yaml create mode 100644 perception/multi_object_tracker/config/simulation_tracker.param.yaml create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp create mode 100644 perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index c944af87e4bc5..046e68dec0c0e 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -38,6 +38,7 @@ set(MULTI_OBJECT_TRACKER_SRC src/tracker/model/pedestrian_tracker.cpp src/tracker/model/pedestrian_and_bicycle_tracker.cpp src/tracker/model/unknown_tracker.cpp + src/tracker/model/pass_through_tracker.cpp src/data_association/data_association.cpp ) diff --git a/perception/multi_object_tracker/config/default_tracker.param.yaml b/perception/multi_object_tracker/config/default_tracker.param.yaml new file mode 100644 index 0000000000000..495df3a30d96e --- /dev/null +++ b/perception/multi_object_tracker/config/default_tracker.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + car_tracker: "multi_vehicle_tracker" + truck_tracker: "multi_vehicle_tracker" + bus_tracker: "multi_vehicle_tracker" + pedestrian_tracker: "pedestrian_and_bicycle_tracker" + bicycle_tracker: "pedestrian_and_bicycle_tracker" + motorcycle_tracker: "pedestrian_and_bicycle_tracker" diff --git a/perception/multi_object_tracker/config/simulation_tracker.param.yaml b/perception/multi_object_tracker/config/simulation_tracker.param.yaml new file mode 100644 index 0000000000000..325cfacc9a3a3 --- /dev/null +++ b/perception/multi_object_tracker/config/simulation_tracker.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + car_tracker: "pass_through_tracker" + truck_tracker: "pass_through_tracker" + bus_tracker: "pass_through_tracker" + pedestrian_tracker: "pass_through_tracker" + bicycle_tracker: "pass_through_tracker" + motorcycle_tracker: "pass_through_tracker" diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index d79180e4b2dd5..48e6bd2b0ec23 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -55,6 +56,8 @@ class MultiObjectTracker : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; + std::map tracker_map_; + void onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp new file mode 100644 index 0000000000000..8c5033e2c3900 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 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. +// +// +// Author: v1.0 Yutaka Shimizu +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ + +#include "tracker_base.hpp" + +#include + +class PassThroughTracker : public Tracker +{ +private: + autoware_auto_perception_msgs::msg::DetectedObject object_; + autoware_auto_perception_msgs::msg::DetectedObject prev_observed_object_; + rclcpp::Logger logger_; + rclcpp::Time last_update_time_; + +public: + PassThroughTracker( + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool predict(const rclcpp::Time & time) override; + bool measure( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time) override; + bool getTrackedObject( + const rclcpp::Time & time, + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + virtual ~PassThroughTracker() {} +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp index 75560750f07b6..a8bc58e254fc2 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp @@ -23,6 +23,7 @@ #include "model/big_vehicle_tracker.hpp" #include "model/multiple_vehicle_tracker.hpp" #include "model/normal_vehicle_tracker.hpp" +#include "model/pass_through_tracker.hpp" #include "model/pedestrian_and_bicycle_tracker.hpp" #include "model/pedestrian_tracker.hpp" #include "model/tracker_base.hpp" diff --git a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml index 7b1a2cb31538a..db0e18e408a8b 100644 --- a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -4,6 +4,7 @@ + @@ -11,6 +12,7 @@ + diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index d5c2946843365..c8b89ec3c518d 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -196,6 +196,20 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); + // tracker map + tracker_map_.insert( + std::make_pair(Label::CAR, this->declare_parameter("car_tracker"))); + tracker_map_.insert( + std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); + tracker_map_.insert( + std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); + tracker_map_.insert( + std::make_pair(Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); + tracker_map_.insert( + std::make_pair(Label::BICYCLE, this->declare_parameter("bicycle_tracker"))); + tracker_map_.insert( + std::make_pair(Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); + data_association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, min_iou_matrix); @@ -265,12 +279,22 @@ std::shared_ptr MultiObjectTracker::createNewTracker( const rclcpp::Time & time) const { const std::uint8_t label = utils::getHighestProbLabel(object.classification); - if (label == Label::CAR || label == Label::TRUCK || label == Label::BUS) { + const auto tracker = tracker_map_.at(label); + + if (tracker == "bicycle_tracker") { + return std::make_shared(time, object); + } else if (tracker == "big_vehicle_tracker") { + return std::make_shared(time, object); + } else if (tracker == "multi_vehicle_tracker") { return std::make_shared(time, object); - } else if (label == Label::PEDESTRIAN) { - return std::make_shared(time, object); - } else if (label == Label::BICYCLE || label == Label::MOTORCYCLE) { + } else if (tracker == "normal_vehicle_tracker") { + return std::make_shared(time, object); + } else if (tracker == "pass_through_tracker") { + return std::make_shared(time, object); + } else if (tracker == "pedestrian_and_bicycle_tracker") { return std::make_shared(time, object); + } else if (tracker == "pedestrian_tracker") { + return std::make_shared(time, object); } else { return std::make_shared(time, object); } diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp new file mode 100644 index 0000000000000..8838e98ef373f --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -0,0 +1,105 @@ +// Copyright 2022 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. +// +// +// Author: v1.0 Yutaka Shimizu +// + +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include "multi_object_tracker/tracker/model/pass_through_tracker.hpp" +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + +PassThroughTracker::PassThroughTracker( + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) +: Tracker(time, object.classification), + logger_(rclcpp::get_logger("PassThroughTracker")), + last_update_time_(time) +{ + object_ = object; + prev_observed_object_ = object; +} + +bool PassThroughTracker::predict(const rclcpp::Time & time) +{ + if (0.5 /*500msec*/ < std::fabs((time - last_update_time_).seconds())) { + RCLCPP_WARN( + logger_, "There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); + } + + return true; +} + +bool PassThroughTracker::measure( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time) +{ + prev_observed_object_ = object_; + object_ = object; + + // Update Velocity if the observed object does not have twist information + const double dt = (time - last_update_time_).seconds(); + if (!object_.kinematics.has_twist && dt > 1e-6) { + const double dx = object_.kinematics.pose_with_covariance.pose.position.x - + prev_observed_object_.kinematics.pose_with_covariance.pose.position.x; + const double dy = object_.kinematics.pose_with_covariance.pose.position.y - + prev_observed_object_.kinematics.pose_with_covariance.pose.position.y; + object_.kinematics.twist_with_covariance.twist.linear.x = std::hypot(dx, dy) / dt; + } + last_update_time_ = time; + + return true; +} + +bool PassThroughTracker::getTrackedObject( + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const +{ + object = utils::toTrackedObject(object_); + object.object_id = getUUID(); + object.classification = getClassification(); + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Z_Z] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] = 0.0; + + // twist covariance + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = 0.0; + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] = 0.0; + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Z_Z] = 0.0; + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = 0.0; + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = 0.0; + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] = 0.0; + + const double dt = (time - last_update_time_).seconds(); + if (0.5 /*500msec*/ < dt) { + RCLCPP_WARN( + logger_, "There is a large gap between last updated time and current time. (%f)", + (time - last_update_time_).seconds()); + } + + return true; +} From 718a040ebcd73bd399ac00b1653a2baaf2296b03 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 11 Apr 2022 11:25:15 +0900 Subject: [PATCH 03/37] fix(simple_planning_simulator): fix bug in function to apply noise (#665) Signed-off-by: satoshi-ota --- .../simple_planning_simulator_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 96264dce277fe..f9927114fc35c 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -383,7 +383,7 @@ void SimplePlanningSimulator::add_measurement_noise( odom.pose.pose.position.x += (*n.pos_dist_)(*n.rand_engine_); odom.pose.pose.position.y += (*n.pos_dist_)(*n.rand_engine_); const auto velocity_noise = (*n.vel_dist_)(*n.rand_engine_); - odom.twist.twist.linear.x = velocity_noise; + odom.twist.twist.linear.x += velocity_noise; float32_t yaw = motion::motion_common::to_angle(odom.pose.pose.orientation); yaw += static_cast((*n.rpy_dist_)(*n.rand_engine_)); odom.pose.pose.orientation = motion::motion_common::from_angle(yaw); From 06901958aea5ae18322ec8c7ae750d40a6702553 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 11 Apr 2022 12:53:39 +0900 Subject: [PATCH 04/37] fix(obstacle_avoidance_planner): fix bug of lerp when points' size = 1 (#664) Signed-off-by: Takayuki Murooka --- .../include/obstacle_avoidance_planner/node.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 6f38883aa4790..d93cc890054a5 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -51,6 +51,10 @@ template double lerpTwistX( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { + if (points.size() == 1) { + return points.at(0).longitudinal_velocity_mps; + } + constexpr double epsilon = 1e-6; const double closest_to_target_dist = @@ -70,6 +74,10 @@ template double lerpPoseZ( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { + if (points.size() == 1) { + return points.at(0).pose.position.z; + } + constexpr double epsilon = 1e-6; const double closest_to_target_dist = From 49cc906418b15994b7facb881f3c133a9d8eb3a1 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 11 Apr 2022 15:58:38 +0900 Subject: [PATCH 05/37] fix(autoware_state_panel): fix message type for /api/autoware/get/engage (#666) * fix(autoware_state_panel): fix message type for /api/autoware/get/engage Signed-off-by: h-ohta * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp | 4 ++-- common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index b0cad0282cf9a..78681106b3d61 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -109,7 +109,7 @@ void AutowareStatePanel::onInitialize() sub_gear_ = raw_node_->create_subscription( "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1)); - sub_engage_ = raw_node_->create_subscription( + sub_engage_ = raw_node_->create_subscription( "/api/autoware/get/engage", 10, std::bind(&AutowareStatePanel::onEngageStatus, this, _1)); client_engage_ = raw_node_->create_client( @@ -209,7 +209,7 @@ void AutowareStatePanel::onShift( } void AutowareStatePanel::onEngageStatus( - const tier4_external_api_msgs::msg::EngageStatus::ConstSharedPtr msg) + const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg) { current_engage_ = msg->engage; engage_status_label_ptr_->setText(QString::fromStdString(Bool2String(current_engage_))); diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index af562dd055f7c..45cd290e5f4a3 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -23,10 +23,10 @@ #include #include +#include #include #include #include -#include #include namespace rviz_plugins @@ -48,7 +48,7 @@ public Q_SLOTS: const tier4_control_msgs::msg::ExternalCommandSelectorMode::ConstSharedPtr msg); void onAutowareState(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg); void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); - void onEngageStatus(const tier4_external_api_msgs::msg::EngageStatus::ConstSharedPtr msg); + void onEngageStatus(const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; rclcpp::Subscription::SharedPtr sub_gate_mode_; @@ -57,7 +57,7 @@ public Q_SLOTS: rclcpp::Subscription::SharedPtr sub_autoware_state_; rclcpp::Subscription::SharedPtr sub_gear_; - rclcpp::Subscription::SharedPtr sub_engage_; + rclcpp::Subscription::SharedPtr sub_engage_; rclcpp::Client::SharedPtr client_engage_; From 973cc80a0e2d6307822b5a718eeecbe9d842e37f Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 11 Apr 2022 16:31:22 +0900 Subject: [PATCH 06/37] fix(control_launch): change default mpc param to improve performance (#667) Signed-off-by: Takamasa Horibe --- .../config/trajectory_follower/lateral_controller.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml index 88736ec29fa3a..f8a129af4d2a4 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -44,8 +44,8 @@ # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s] - steer_lim_deg: 20.0 # steering angle limit [deg] + vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] + steer_lim_deg: 40.0 # steering angle limit [deg] steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s] acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] From 6dbbaeeeac30dd67fe2fdc61cf18ba2919faa9e0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 11 Apr 2022 17:28:51 +0900 Subject: [PATCH 07/37] feat(tier4_traffic_light_rviz_plugin): selectable light shape, status and confidence (#663) Signed-off-by: satoshi-ota --- .../src/traffic_light_publish_panel.cpp | 161 +++++++++++++----- .../src/traffic_light_publish_panel.hpp | 3 + 2 files changed, 121 insertions(+), 43 deletions(-) diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp index cdb735f76845f..3ffc1aef30744 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp @@ -43,11 +43,25 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm traffic_light_id_input_->setRange(0, 999999); traffic_light_id_input_->setValue(0); + // Traffic Light Confidence + traffic_light_confidence_input_ = new QDoubleSpinBox(); + traffic_light_confidence_input_->setRange(0.0, 1.0); + traffic_light_confidence_input_->setSingleStep(0.1); + traffic_light_confidence_input_->setValue(1.0); + // Traffic Light Color light_color_combo_ = new QComboBox(); - light_color_combo_->addItems( - {"RED", "AMBER", "GREEN", "WHITE", "LEFT_ARROW", "RIGHT_ARROW", "UP_ARROW", "DOWN_ARROW", - "DOWN_LEFT_ARROW", "DOWN_RIGHT_ARROW", "FLASHING", "UNKNOWN"}); + light_color_combo_->addItems({"RED", "AMBER", "GREEN", "WHITE", "UNKNOWN"}); + + // Traffic Light Shape + light_shape_combo_ = new QComboBox(); + light_shape_combo_->addItems( + {"CIRCLE", "LEFT_ARROW", "RIGHT_ARROW", "UP_ARROW", "DOWN_ARROW", "DOWN_LEFT_ARROW", + "DOWN_RIGHT_ARROW", "CROSS", "UNKNOWN"}); + + // Traffic Light Status + light_status_combo_ = new QComboBox(); + light_status_combo_->addItems({"SOLID_ON", "SOLID_OFF", "FLASHING", "UNKNOWN"}); // Set Traffic Signals Button set_button_ = new QPushButton("SET"); @@ -64,8 +78,8 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm horizontal_header->setSectionResizeMode(QHeaderView::Stretch); traffic_table_ = new QTableWidget(); - traffic_table_->setColumnCount(2); - traffic_table_->setHorizontalHeaderLabels({"ID", "Status"}); + traffic_table_->setColumnCount(5); + traffic_table_->setHorizontalHeaderLabels({"ID", "Color", "Shape", "Status", "Confidence"}); traffic_table_->setVerticalHeader(vertical_header); traffic_table_->setHorizontalHeader(horizontal_header); @@ -77,34 +91,48 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm auto * h_layout_1 = new QHBoxLayout; h_layout_1->addWidget(new QLabel("Rate: ")); h_layout_1->addWidget(publishing_rate_input_); - h_layout_1->addWidget(new QLabel("Traffic Light ID: ")); + h_layout_1->addWidget(new QLabel("ID: ")); h_layout_1->addWidget(traffic_light_id_input_); + h_layout_1->addWidget(new QLabel("Confidence: ")); + h_layout_1->addWidget(traffic_light_confidence_input_); auto * h_layout_2 = new QHBoxLayout; - h_layout_2->addWidget(new QLabel("Traffic Light Status: ")); - h_layout_2->addWidget(light_color_combo_); + h_layout_2->addWidget(new QLabel("Traffic Light Color: "), 40); + h_layout_2->addWidget(light_color_combo_, 60); + + auto * h_layout_3 = new QHBoxLayout; + h_layout_3->addWidget(new QLabel("Traffic Light Shape: "), 40); + h_layout_3->addWidget(light_shape_combo_, 60); + + auto * h_layout_4 = new QHBoxLayout; + h_layout_4->addWidget(new QLabel("Traffic Light Status: "), 40); + h_layout_4->addWidget(light_status_combo_, 60); auto * v_layout = new QVBoxLayout; v_layout->addLayout(h_layout_1); v_layout->addLayout(h_layout_2); + v_layout->addLayout(h_layout_3); + v_layout->addLayout(h_layout_4); v_layout->addWidget(set_button_); v_layout->addWidget(reset_button_); v_layout->addWidget(publish_button_); - auto * h_layout_3 = new QHBoxLayout; - h_layout_3->addLayout(v_layout); - h_layout_3->addWidget(traffic_table_); + auto * h_layout_5 = new QHBoxLayout; + h_layout_5->addLayout(v_layout); + h_layout_5->addWidget(traffic_table_); - setLayout(h_layout_3); + setLayout(h_layout_5); } void TrafficLightPublishPanel::onSetTrafficLightState() { const auto traffic_light_id = traffic_light_id_input_->value(); const auto color = light_color_combo_->currentText(); + const auto shape = light_shape_combo_->currentText(); + const auto status = light_status_combo_->currentText(); TrafficLight traffic_light; - traffic_light.confidence = 1.0; + traffic_light.confidence = traffic_light_confidence_input_->value(); if (color == "RED") { traffic_light.color = TrafficLight::RED; @@ -114,24 +142,38 @@ void TrafficLightPublishPanel::onSetTrafficLightState() traffic_light.color = TrafficLight::GREEN; } else if (color == "WHITE") { traffic_light.color = TrafficLight::WHITE; - } else if (color == "LEFT_ARROW") { - traffic_light.color = TrafficLight::LEFT_ARROW; - } else if (color == "RIGHT_ARROW") { - traffic_light.color = TrafficLight::RIGHT_ARROW; - } else if (color == "UP_ARROW") { - traffic_light.color = TrafficLight::UP_ARROW; - } else if (color == "DOWN_ARROW") { - traffic_light.color = TrafficLight::DOWN_ARROW; - } else if (color == "DOWN_LEFT_ARROW") { - traffic_light.color = TrafficLight::DOWN_LEFT_ARROW; - } else if (color == "DOWN_RIGHT_ARROW") { - traffic_light.color = TrafficLight::DOWN_RIGHT_ARROW; - } else if (color == "FLASHING") { - traffic_light.color = TrafficLight::FLASHING; } else if (color == "UNKNOWN") { traffic_light.color = TrafficLight::UNKNOWN; } + if (shape == "CIRCLE") { + traffic_light.shape = TrafficLight::CIRCLE; + } else if (shape == "LEFT_ARROW") { + traffic_light.shape = TrafficLight::LEFT_ARROW; + } else if (shape == "RIGHT_ARROW") { + traffic_light.shape = TrafficLight::RIGHT_ARROW; + } else if (shape == "UP_ARROW") { + traffic_light.shape = TrafficLight::UP_ARROW; + } else if (shape == "DOWN_ARROW") { + traffic_light.shape = TrafficLight::DOWN_ARROW; + } else if (shape == "DOWN_LEFT_ARROW") { + traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW; + } else if (shape == "DOWN_RIGHT_ARROW") { + traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW; + } else if (shape == "UNKNOWN") { + traffic_light.shape = TrafficLight::UNKNOWN; + } + + if (status == "SOLID_OFF") { + traffic_light.status = TrafficLight::SOLID_OFF; + } else if (status == "SOLID_ON") { + traffic_light.status = TrafficLight::SOLID_ON; + } else if (status == "FLASHING") { + traffic_light.status = TrafficLight::FLASHING; + } else if (status == "UNKNOWN") { + traffic_light.status = TrafficLight::UNKNOWN; + } + TrafficSignal traffic_signal; traffic_signal.lights.push_back(traffic_light); traffic_signal.map_primitive_id = traffic_light_id; @@ -234,44 +276,77 @@ void TrafficLightPublishPanel::onTimer() color_label->setText("WHITE"); color_label->setStyleSheet("background-color: #FFFFFF;"); break; + case TrafficLight::UNKNOWN: + color_label->setText("UNKNOWN"); + color_label->setStyleSheet("background-color: #808080;"); + break; + default: + break; + } + + auto shape_label = new QLabel(); + shape_label->setAlignment(Qt::AlignCenter); + + switch (light.shape) { + case TrafficLight::CIRCLE: + shape_label->setText("CIRCLE"); + break; case TrafficLight::LEFT_ARROW: - color_label->setText("LEFT_ARROW"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("LEFT_ARROW"); break; case TrafficLight::RIGHT_ARROW: - color_label->setText("RIGHT_ARROW"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("RIGHT_ARROW"); break; case TrafficLight::UP_ARROW: - color_label->setText("UP_ARROW"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("UP_ARROW"); break; case TrafficLight::DOWN_ARROW: - color_label->setText("DOWN_ARROW"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("DOWN_ARROW"); break; case TrafficLight::DOWN_LEFT_ARROW: - color_label->setText("DOWN_LEFT_ARROW"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("DOWN_LEFT_ARROW"); break; case TrafficLight::DOWN_RIGHT_ARROW: - color_label->setText("DOWN_RIGHT_ARROW"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("DOWN_RIGHT_ARROW"); break; case TrafficLight::FLASHING: - color_label->setText("FLASHING"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("FLASHING"); break; case TrafficLight::UNKNOWN: - color_label->setText("UNKNOWN"); - color_label->setStyleSheet("background-color: #808080;"); + shape_label->setText("UNKNOWN"); break; default: break; } + auto status_label = new QLabel(); + status_label->setAlignment(Qt::AlignCenter); + + switch (light.status) { + case TrafficLight::SOLID_OFF: + status_label->setText("SOLID_OFF"); + break; + case TrafficLight::SOLID_ON: + status_label->setText("SOLID_ON"); + break; + case TrafficLight::FLASHING: + status_label->setText("FLASHING"); + break; + case TrafficLight::UNKNOWN: + status_label->setText("UNKNOWN"); + break; + default: + break; + } + + auto confidence_label = new QLabel(QString::number(light.confidence)); + confidence_label->setAlignment(Qt::AlignCenter); + traffic_table_->setCellWidget(i, 0, id_label); traffic_table_->setCellWidget(i, 1, color_label); + traffic_table_->setCellWidget(i, 2, shape_label); + traffic_table_->setCellWidget(i, 3, status_label); + traffic_table_->setCellWidget(i, 4, confidence_label); } } diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp index 7eb81fa2f3ceb..d63f0f7d4d14a 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp @@ -60,7 +60,10 @@ public Q_SLOTS: QSpinBox * publishing_rate_input_; QSpinBox * traffic_light_id_input_; + QDoubleSpinBox * traffic_light_confidence_input_; QComboBox * light_color_combo_; + QComboBox * light_shape_combo_; + QComboBox * light_status_combo_; QPushButton * set_button_; QPushButton * reset_button_; QPushButton * publish_button_; From c555767cea9b1d75217597f6cd3a0b2faefe9439 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 11 Apr 2022 17:07:05 +0000 Subject: [PATCH 08/37] ci(pre-commit): autoupdate (#671) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/pre-commit/pre-commit-hooks: v4.1.0 → v4.2.0](https://github.com/pre-commit/pre-commit-hooks/compare/v4.1.0...v4.2.0) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index fa2480a48ee2e..733a842865802 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,7 +4,7 @@ ci: repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.1.0 + rev: v4.2.0 hooks: - id: check-json - id: check-merge-conflict From 36165da8b010dc403984a47ad4df7a16d3efdf9d Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 12 Apr 2022 11:05:46 +0900 Subject: [PATCH 09/37] feat(behavior_path_planner): stop lane_driving planners in non-lane-driving scenario (#668) * stop lanedriving in parking scenario Signed-off-by: Takamasa Horibe * use skip_first Signed-off-by: Takamasa Horibe * add scenario remap in launch Signed-off-by: Takamasa Horibe * replace warn to info Signed-off-by: Takamasa Horibe --- .../behavior_planning.launch.py | 1 + .../behavior_path_planner_node.hpp | 4 ++++ .../launch/behavior_path_planner.launch.xml | 1 + .../src/behavior_path_planner_node.cpp | 18 ++++++++++++++++-- 4 files changed, 22 insertions(+), 2 deletions(-) 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 f81d631db5040..2c1b0dd58841d 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 @@ -138,6 +138,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/vector_map", LaunchConfiguration("map_topic_name")), ("~/input/perception", "/perception/object_recognition/objects"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/scenario", "/planning/scenario_planning/scenario"), ( "~/input/external_approval", "/planning/scenario_planning/lane_driving/behavior_planning/" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index e5c1b89454f4e..5d6a374f970eb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include @@ -68,6 +69,7 @@ using nav_msgs::msg::Odometry; using route_handler::RouteHandler; using tier4_planning_msgs::msg::PathChangeModule; using tier4_planning_msgs::msg::PathChangeModuleArray; +using tier4_planning_msgs::msg::Scenario; using visualization_msgs::msg::MarkerArray; class BehaviorPathPlannerNode : public rclcpp::Node @@ -79,6 +81,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr route_subscriber_; rclcpp::Subscription::SharedPtr vector_map_subscriber_; rclcpp::Subscription::SharedPtr velocity_subscriber_; + rclcpp::Subscription::SharedPtr scenario_subscriber_; rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr external_approval_subscriber_; rclcpp::Subscription::SharedPtr force_approval_subscriber_; @@ -94,6 +97,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr planner_data_; std::shared_ptr bt_manager_; tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; + Scenario::SharedPtr current_scenario_{nullptr}; std::string prev_ready_module_name_ = "NONE"; diff --git a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml index 63f9ba16292a5..22d5283b1b0e3 100644 --- a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml +++ b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml @@ -11,6 +11,7 @@ + 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 bd392853bcb23..596b90b15266e 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -51,6 +51,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1)); perception_subscriber_ = create_subscription( "~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1)); + scenario_subscriber_ = create_subscription( + "~/input/scenario", 1, [this](const Scenario::SharedPtr msg) { current_scenario_ = msg; }); external_approval_subscriber_ = create_subscription( "~/input/external_approval", 1, std::bind(&BehaviorPathPlannerNode::onExternalApproval, this, _1)); @@ -445,8 +447,15 @@ BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam() void BehaviorPathPlannerNode::waitForData() { // wait until mandatory data is ready + while (!current_scenario_ && rclcpp::ok()) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for scenario topic"); + rclcpp::spin_some(this->get_node_base_interface()); + rclcpp::Rate(100).sleep(); + } + while (!planner_data_->route_handler->isHandlerReady() && rclcpp::ok()) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for route to be ready"); + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting for route to be ready"); rclcpp::spin_some(this->get_node_base_interface()); rclcpp::Rate(100).sleep(); } @@ -455,7 +464,7 @@ void BehaviorPathPlannerNode::waitForData() if (planner_data_->dynamic_object && planner_data_->self_odometry) { break; } - RCLCPP_WARN_THROTTLE( + RCLCPP_INFO_SKIPFIRST_THROTTLE( get_logger(), *get_clock(), 5000, "waiting for vehicle pose, vehicle_velocity, and obstacles"); rclcpp::spin_some(this->get_node_base_interface()); @@ -470,6 +479,11 @@ void BehaviorPathPlannerNode::run() { RCLCPP_DEBUG(get_logger(), "----- BehaviorPathPlannerNode start -----"); + // behavior_path_planner runs only in LANE DRIVING scenario. + if (current_scenario_->current_scenario != Scenario::LANEDRIVING) { + return; + } + // update planner data updateCurrentPose(); From dc682e6bebe7edac7507519f4d8e54194a40bd1a Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 12 Apr 2022 15:27:51 +0900 Subject: [PATCH 10/37] fix(route): set transient_local for route subscriber (#674) Signed-off-by: Takamasa Horibe --- .../behavior_path_planner/src/behavior_path_planner_node.cpp | 5 +++-- .../src/freespace_planner/freespace_planner_node.cpp | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) 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 596b90b15266e..57bf8e364c9af 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -60,11 +60,12 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/force_approval", 1, std::bind(&BehaviorPathPlannerNode::onForceApproval, this, _1)); // route_handler + auto qos_transient_local = rclcpp::QoS{1}.transient_local(); vector_map_subscriber_ = create_subscription( - "~/input/vector_map", rclcpp::QoS{1}.transient_local(), + "~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1)); route_subscriber_ = create_subscription( - "~/input/route", 1, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1)); + "~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1)); // publisher path_publisher_ = create_publisher("~/output/path", 1); diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index 478e069539cbe..e3ad37c1ef87f 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -233,7 +233,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti // Subscribers { route_sub_ = create_subscription( - "~/input/route", 1, std::bind(&FreespacePlannerNode::onRoute, this, _1)); + "~/input/route", rclcpp::QoS{1}.transient_local(), + std::bind(&FreespacePlannerNode::onRoute, this, _1)); occupancy_grid_sub_ = create_subscription( "~/input/occupancy_grid", 1, std::bind(&FreespacePlannerNode::onOccupancyGrid, this, _1)); scenario_sub_ = create_subscription( From f08233a9470df78047484e8a7e50d677589d08ea Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 12 Apr 2022 19:28:02 +0900 Subject: [PATCH 11/37] fix(trajectory_follower_nodes): switch to isolated gtests (#661) Signed-off-by: Maxime CLEMENT --- control/trajectory_follower_nodes/CMakeLists.txt | 2 +- control/trajectory_follower_nodes/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_nodes/CMakeLists.txt index 32e4c5d42523b..4ec4da305ca35 100644 --- a/control/trajectory_follower_nodes/CMakeLists.txt +++ b/control/trajectory_follower_nodes/CMakeLists.txt @@ -68,7 +68,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() # Unit tests set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_nodes) - ament_add_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} + ament_add_ros_isolated_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} test/trajectory_follower_test_utils.hpp test/test_latlon_muxer_node.cpp test/test_lateral_controller_node.cpp diff --git a/control/trajectory_follower_nodes/package.xml b/control/trajectory_follower_nodes/package.xml index 6f22e6bddff2f..215ebaea5187b 100644 --- a/control/trajectory_follower_nodes/package.xml +++ b/control/trajectory_follower_nodes/package.xml @@ -21,7 +21,7 @@ ros2launch - ament_cmake_gtest + ament_cmake_ros ament_index_python From e212d14bc2d846cd74382351fca3feebf1911c8b Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 12 Apr 2022 20:24:18 +0900 Subject: [PATCH 12/37] fix(map_based_prediction): add config to INSTALL_TO_SHARE (#681) --- perception/map_based_prediction/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt index 1e290e43a6330..044c451160cc4 100644 --- a/perception/map_based_prediction/CMakeLists.txt +++ b/perception/map_based_prediction/CMakeLists.txt @@ -34,5 +34,6 @@ endif() ament_auto_package( INSTALL_TO_SHARE + config launch ) From c2d8091c3ddc1e6b736f14b46293a950f5c384df Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Tue, 12 Apr 2022 22:04:30 +0900 Subject: [PATCH 13/37] feat(tier4_autoware_api_launch): add tier4_autoware_api_launch package (#658) * feat(tier4_autoware_api_launch): add tier4_autoware_api_launch package Signed-off-by: mitsudome-r * ci(pre-commit): autofix * fix(tier4_autoware_api_launch): fix the command in the README instructions Signed-off-by: mitsudome-r Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../tier4_autoware_api_launch/CMakeLists.txt | 12 ++++ launch/tier4_autoware_api_launch/README.md | 24 ++++++++ .../launch/autoware_api.launch.xml | 14 +++++ .../include/external_api_adaptor.launch.py | 57 +++++++++++++++++++ .../include/internal_api_adaptor.launch.py | 51 +++++++++++++++++ .../include/internal_api_relay.launch.xml | 26 +++++++++ launch/tier4_autoware_api_launch/package.xml | 25 ++++++++ 7 files changed, 209 insertions(+) create mode 100644 launch/tier4_autoware_api_launch/CMakeLists.txt create mode 100644 launch/tier4_autoware_api_launch/README.md create mode 100644 launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml create mode 100644 launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py create mode 100644 launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py create mode 100644 launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml create mode 100644 launch/tier4_autoware_api_launch/package.xml diff --git a/launch/tier4_autoware_api_launch/CMakeLists.txt b/launch/tier4_autoware_api_launch/CMakeLists.txt new file mode 100644 index 0000000000000..95c4157c0122a --- /dev/null +++ b/launch/tier4_autoware_api_launch/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.5) +project(tier4_autoware_api_launch) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/launch/tier4_autoware_api_launch/README.md b/launch/tier4_autoware_api_launch/README.md new file mode 100644 index 0000000000000..e2ea11b7de4c1 --- /dev/null +++ b/launch/tier4_autoware_api_launch/README.md @@ -0,0 +1,24 @@ +# tier4_autoware_api_launch + +## Description + +This package contains launch files that run nodes to convert Autoware internal topics into consistent API used by external software (e.g., fleet management system, simulator). + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +You can include as follows in `*.launch.xml` to use `control.launch.py`. + +```xml + + + + +``` + +## Notes + +For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS2 (similar to Nodelet in ROS1 ) diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml new file mode 100644 index 0000000000000..3d9113f436eca --- /dev/null +++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py b/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py new file mode 100644 index 0000000000000..4f1295588a834 --- /dev/null +++ b/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py @@ -0,0 +1,57 @@ +# 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. + +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def _create_api_node(node_name, class_name, **kwargs): + return ComposableNode( + namespace="external", + name=node_name, + package="autoware_iv_external_api_adaptor", + plugin="external_api::" + class_name, + **kwargs + ) + + +def generate_launch_description(): + components = [ + _create_api_node("cpu_usage", "CpuUsage"), + _create_api_node("diagnostics", "Diagnostics"), + _create_api_node("door", "Door"), + _create_api_node("emergency", "Emergency"), + _create_api_node("engage", "Engage"), + _create_api_node("fail_safe_state", "FailSafeState"), + _create_api_node("initial_pose", "InitialPose"), + _create_api_node("map", "Map"), + _create_api_node("operator", "Operator"), + _create_api_node("metadata_packages", "MetadataPackages"), + _create_api_node("route", "Route"), + _create_api_node("service", "Service"), + _create_api_node("start", "Start"), + _create_api_node("vehicle_status", "VehicleStatus"), + _create_api_node("velocity", "Velocity"), + _create_api_node("version", "Version"), + ] + container = ComposableNodeContainer( + namespace="external", + name="autoware_iv_adaptor", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=components, + output="screen", + ) + return launch.LaunchDescription([container]) diff --git a/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py b/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py new file mode 100644 index 0000000000000..4b8513f544588 --- /dev/null +++ b/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py @@ -0,0 +1,51 @@ +# 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. + +import launch +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def _create_api_node(node_name, class_name, **kwargs): + return ComposableNode( + namespace="internal", + name=node_name, + package="autoware_iv_internal_api_adaptor", + plugin="internal_api::" + class_name, + **kwargs + ) + + +def generate_launch_description(): + param_initial_pose = { + "init_simulator_pose": LaunchConfiguration("init_simulator_pose"), + "init_localization_pose": LaunchConfiguration("init_localization_pose"), + } + components = [ + _create_api_node("initial_pose", "InitialPose", parameters=[param_initial_pose]), + _create_api_node("iv_msgs", "IVMsgs"), + _create_api_node("operator", "Operator"), + _create_api_node("route", "Route"), + _create_api_node("velocity", "Velocity"), + ] + container = ComposableNodeContainer( + namespace="internal", + name="autoware_iv_adaptor", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=components, + output="screen", + ) + return launch.LaunchDescription([container]) diff --git a/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml b/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml new file mode 100644 index 0000000000000..810e6a566bd3a --- /dev/null +++ b/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml new file mode 100644 index 0000000000000..a455e71ffd0bb --- /dev/null +++ b/launch/tier4_autoware_api_launch/package.xml @@ -0,0 +1,25 @@ + + + + tier4_autoware_api_launch + 0.0.0 + The tier4_autoware_api_launch package + Takagi, Isamu + Ryohsuke, Mitsudome + Apache License 2.0 + + ament_cmake_auto + + autoware_iv_external_api_adaptor + autoware_iv_internal_api_adaptor + awapi_awiv_adapter + path_distance_calculator + topic_tools + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + From 7681249713b7c25459afcf0b1f52fb82d11c2c5b Mon Sep 17 00:00:00 2001 From: Berkay Date: Tue, 12 Apr 2022 17:32:08 +0300 Subject: [PATCH 14/37] feat(joy_controller): adding genesis joystick to joy controller (#683) * feat(joy_controller): add genesis p65 converter Signed-off-by: Berkay * feat(joy_controller): fix steering direction Signed-off-by: Berkay * feat(joy_controller): changed map Signed-off-by: Berkay * ci(pre-commit): autofix Co-authored-by: Berkay Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- control/joy_controller/README.md | 21 +++++ .../joy_converter/p65_joy_converter.hpp | 81 +++++++++++++++++++ .../launch/joy_controller.launch.xml | 2 +- .../joy_controller/joy_controller_node.cpp | 5 +- 4 files changed, 107 insertions(+), 2 deletions(-) create mode 100644 control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index a494a9e6fb330..4df53e52e3cc1 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -41,3 +41,24 @@ | `max_forward_velocity` | double | absolute max velocity to go forward | | `max_backward_velocity` | double | absolute max velocity to go backward | | `backward_accel_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) | + +## P65 Joystick Key Map + +| Acceleration | R2 | +| -------------------- | --------------------- | +| Brake | L2 | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | L1 | +| Turn Signal Right | R1 | +| Clear Turn Signal | A | +| Gate Mode | B | +| Emergency Stop | Select | +| Clear Emergency Stop | Start | +| Autoware Engage | X | +| Autoware Disengage | Y | +| Vehicle Engage | PS | +| Vehicle Disengage | Right Trigger | diff --git a/control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp b/control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp new file mode 100644 index 0000000000000..e4cb822846fef --- /dev/null +++ b/control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp @@ -0,0 +1,81 @@ +// Copyright 2022 Leo Drive Teknoloji A.Ş., 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 JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ +#define JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ + +#include "joy_controller/joy_converter/joy_converter_base.hpp" + +#include + +namespace joy_controller +{ +class P65JoyConverter : public JoyConverterBase +{ +public: + explicit P65JoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} + + float accel() const { return std::max(0.0f, -((R2() - 1.0f) / 2.0f)); } + + float brake() const { return std::max(0.0f, -((L2() - 1.0f) / 2.0f)); } + + float steer() const { return LStickLeftRight(); } + + bool shift_up() const { return CursorUpDown() == 1.0f; } + bool shift_down() const { return CursorUpDown() == -1.0f; } + bool shift_drive() const { return CursorLeftRight() == 1.0f; } + bool shift_reverse() const { return CursorLeftRight() == -1.0f; } + + bool turn_signal_left() const { return L1(); } + bool turn_signal_right() const { return R1(); } + bool clear_turn_signal() const { return A(); } + + bool gate_mode() const { return B(); } + + bool emergency_stop() const { return Select(); } + bool clear_emergency_stop() const { return Start(); } + + bool autoware_engage() const { return X(); } + bool autoware_disengage() const { return Y(); } + + bool vehicle_engage() const { return PS(); } + bool vehicle_disengage() const { return RTrigger(); } + +private: + float LStickLeftRight() const { return j_.axes.at(0); } + float LStickUpDown() const { return j_.axes.at(1); } + float RStickLeftRight() const { return j_.axes.at(3); } + float RStickUpDown() const { return j_.axes.at(4); } + float CursorLeftRight() const { return j_.axes.at(6); } + float CursorUpDown() const { return j_.axes.at(7); } + float L2() const { return j_.axes.at(2); } + float R2() const { return j_.axes.at(5); } + + bool A() const { return j_.buttons.at(0); } + bool B() const { return j_.buttons.at(1); } + bool X() const { return j_.buttons.at(2); } + bool Y() const { return j_.buttons.at(3); } + bool L1() const { return j_.buttons.at(4); } + bool R1() const { return j_.buttons.at(5); } + bool Select() const { return j_.buttons.at(6); } + bool Start() const { return j_.buttons.at(7); } + bool PS() const { return j_.buttons.at(8); } + bool LTrigger() const { return j_.buttons.at(9); } + bool RTrigger() const { return j_.buttons.at(10); } + + const sensor_msgs::msg::Joy j_; +}; +} // namespace joy_controller + +#endif // JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/joy_controller/launch/joy_controller.launch.xml index 8642e830ba24a..b2d4035411b0c 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/joy_controller/launch/joy_controller.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index 5d61be904f845..0e0ebf03fe0ed 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -15,6 +15,7 @@ #include "joy_controller/joy_controller.hpp" #include "joy_controller/joy_converter/ds4_joy_converter.hpp" #include "joy_controller/joy_converter/g29_joy_converter.hpp" +#include "joy_controller/joy_converter/p65_joy_converter.hpp" #include @@ -151,8 +152,10 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt last_joy_received_time_ = msg->header.stamp; if (joy_type_ == "G29") { joy_ = std::make_shared(*msg); - } else { + } else if (joy_type_ == "DS4") { joy_ = std::make_shared(*msg); + } else { + joy_ = std::make_shared(*msg); } if (joy_->shift_up() || joy_->shift_down() || joy_->shift_drive() || joy_->shift_reverse()) { From e11cee2fcb02b4f80cdbbfe9f346f8325805037f Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 22 Mar 2022 16:53:42 +0900 Subject: [PATCH 15/37] Initial commit with barebone SafeVelocityAdjustorNode --- .../safe_velocity_adjustor/CMakeLists.txt | 36 ++++++ planning/safe_velocity_adjustor/README.md | 33 ++++++ ...parent_safety_velocity_adjustor.param.yaml | 4 + .../safe_velocity_adjustor_node.hpp | 103 ++++++++++++++++++ .../motion_velocity_smoother.launch.xml | 28 +++++ planning/safe_velocity_adjustor/package.xml | 26 +++++ .../src/safe_velocity_adjustor_node.cpp | 22 ++++ 7 files changed, 252 insertions(+) create mode 100644 planning/safe_velocity_adjustor/CMakeLists.txt create mode 100644 planning/safe_velocity_adjustor/README.md create mode 100644 planning/safe_velocity_adjustor/config/apparent_safety_velocity_adjustor.param.yaml create mode 100644 planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp create mode 100644 planning/safe_velocity_adjustor/launch/motion_velocity_smoother.launch.xml create mode 100644 planning/safe_velocity_adjustor/package.xml create mode 100644 planning/safe_velocity_adjustor/src/safe_velocity_adjustor_node.cpp diff --git a/planning/safe_velocity_adjustor/CMakeLists.txt b/planning/safe_velocity_adjustor/CMakeLists.txt new file mode 100644 index 0000000000000..db2ec93aa899c --- /dev/null +++ b/planning/safe_velocity_adjustor/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.5) +project(safe_velocity_adjustor) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Boost REQUIRED) + +ament_auto_add_library(safe_velocity_adjustor_node SHARED + DIRECTORY src +) + +rclcpp_components_register_node(safe_velocity_adjustor_node + PLUGIN "safe_velocity_adjustor::SafeVelocityAdjustorNode" + EXECUTABLE safe_velocity_adjustor +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/planning/safe_velocity_adjustor/README.md b/planning/safe_velocity_adjustor/README.md new file mode 100644 index 0000000000000..f51efbf696c08 --- /dev/null +++ b/planning/safe_velocity_adjustor/README.md @@ -0,0 +1,33 @@ +# Safe Velocity Adjustor + +## Purpose + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------- | ---------------------------------------- | -------------------- | +| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | + +### Output + +| Name | Type | Description | +| ---------------------------------------------- | ---------------------------------------- | ---------------------------------------------------------- | +| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory | +| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | +| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | + +## Parameters + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/planning/safe_velocity_adjustor/config/apparent_safety_velocity_adjustor.param.yaml b/planning/safe_velocity_adjustor/config/apparent_safety_velocity_adjustor.param.yaml new file mode 100644 index 0000000000000..a3f65f5754506 --- /dev/null +++ b/planning/safe_velocity_adjustor/config/apparent_safety_velocity_adjustor.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + time_safety_buffer: 0.5 # [s] required time without collision in forward simulation + dist_safety_buffer: 1.5 # [m] distance from obstacles to consider a collision diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp new file mode 100644 index 0000000000000..2073a9f6e25ca --- /dev/null +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -0,0 +1,103 @@ +// Copyright 2022 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 SAFE_VELOCITY_ADJUSTOR__SAFE_VELOCITY_ADJUSTOR_NODE_HPP_ +#define SAFE_VELOCITY_ADJUSTOR__SAFE_VELOCITY_ADJUSTOR_NODE_HPP_ + +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace safe_velocity_adjustor +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; + +class SafeVelocityAdjustorNode : public rclcpp::Node +{ +public: + explicit SafeVelocityAdjustorNode(const rclcpp::NodeOptions & node_options) + : rclcpp::Node("safe_velocity_adjustor", node_options) + { + // TODO(Maxime CLEMENT): declare/get ROS parameters + time_safety_buffer_ = 0.5; + dist_safety_buffer_ = 1.5; + } + +private: + rclcpp::Publisher::SharedPtr + pub_trajectory_; //!< @brief publisher for output trajectory + rclcpp::Subscription::SharedPtr + sub_trajectory_; //!< @brief subscriber for reference trajectory + + // parameters + double time_safety_buffer_; + double dist_safety_buffer_; + + // parameter update + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + + // topic callback + void onTrajectory(const Trajectory::ConstSharedPtr msg) + { + Trajectory safe_trajectory = *msg; + for (auto & trajectory_point : safe_trajectory.points) { + const auto closest_collision_point = distanceToClosestCollision(trajectory_point); + if (closest_collision_point) + trajectory_point.longitudinal_velocity_mps = + calculateSafeVelocity(trajectory_point, *closest_collision_point); + } + safe_trajectory.header.stamp = now(); + pub_trajectory_->publish(safe_trajectory); + publishDebug(*msg, safe_trajectory); + } + + // publish methods + std::optional distanceToClosestCollision(const TrajectoryPoint & trajectory_point) + { + (void)trajectory_point; + return {}; + } + + double calculateSafeVelocity( + const TrajectoryPoint & trajectory_point, const double & dist_to_collision) + { + return std::min( + trajectory_point.longitudinal_velocity_mps, + static_cast( + dist_to_collision / time_safety_buffer_)); + } + + void publishDebug( + const Trajectory & original_trajectory, const Trajectory & safe_trajectory) const + { + (void)original_trajectory; + (void)safe_trajectory; + } + + // debug +}; +} // namespace safe_velocity_adjustor + +#endif // SAFE_VELOCITY_ADJUSTOR__SAFE_VELOCITY_ADJUSTOR_NODE_HPP_ diff --git a/planning/safe_velocity_adjustor/launch/motion_velocity_smoother.launch.xml b/planning/safe_velocity_adjustor/launch/motion_velocity_smoother.launch.xml new file mode 100644 index 0000000000000..9c5768fccfad3 --- /dev/null +++ b/planning/safe_velocity_adjustor/launch/motion_velocity_smoother.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/safe_velocity_adjustor/package.xml b/planning/safe_velocity_adjustor/package.xml new file mode 100644 index 0000000000000..0e42fca69d5be --- /dev/null +++ b/planning/safe_velocity_adjustor/package.xml @@ -0,0 +1,26 @@ + + + safe_velocity_adjustor + 0.1.0 + Package to adjust velocities of a trajectory in order for the ride to feel safe + + Maxime CLEMENT + + Apache License 2.0 + + ament_cmake_auto + eigen3_cmake_module + + autoware_auto_planning_msgs + libboost-dev + rclcpp + rclcpp_components + tier4_planning_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/safe_velocity_adjustor/src/safe_velocity_adjustor_node.cpp b/planning/safe_velocity_adjustor/src/safe_velocity_adjustor_node.cpp new file mode 100644 index 0000000000000..294868d4de608 --- /dev/null +++ b/planning/safe_velocity_adjustor/src/safe_velocity_adjustor_node.cpp @@ -0,0 +1,22 @@ +// Copyright 2022 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 "safe_velocity_adjustor/safe_velocity_adjustor_node.hpp" + +namespace safe_velocity_adjustor +{ +} // namespace safe_velocity_adjustor + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(safe_velocity_adjustor::SafeVelocityAdjustorNode) From db62c4c13f5e6ffe8d15bfe8c95a43dd185ddb01 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 22 Mar 2022 22:17:00 +0900 Subject: [PATCH 16/37] Add debug topics, launch file, and config file --- ...default_safe_velocity_adjustor.param.yaml} | 0 .../safe_velocity_adjustor_node.hpp | 68 ++++++++++++++++--- .../motion_velocity_smoother.launch.xml | 28 -------- .../launch/safe_velocity_adjustor.xml | 20 ++++++ 4 files changed, 78 insertions(+), 38 deletions(-) rename planning/safe_velocity_adjustor/config/{apparent_safety_velocity_adjustor.param.yaml => default_safe_velocity_adjustor.param.yaml} (100%) delete mode 100644 planning/safe_velocity_adjustor/launch/motion_velocity_smoother.launch.xml create mode 100644 planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.xml diff --git a/planning/safe_velocity_adjustor/config/apparent_safety_velocity_adjustor.param.yaml b/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml similarity index 100% rename from planning/safe_velocity_adjustor/config/apparent_safety_velocity_adjustor.param.yaml rename to planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index 2073a9f6e25ca..cb7114d4468a7 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -19,8 +19,15 @@ #include #include +#include +#include +#include +#include +#include + +#include +#include -#include #include #include #include @@ -31,6 +38,7 @@ namespace safe_velocity_adjustor using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using Float = decltype(TrajectoryPoint::longitudinal_velocity_mps); class SafeVelocityAdjustorNode : public rclcpp::Node { @@ -41,17 +49,28 @@ class SafeVelocityAdjustorNode : public rclcpp::Node // TODO(Maxime CLEMENT): declare/get ROS parameters time_safety_buffer_ = 0.5; dist_safety_buffer_ = 1.5; + + pub_trajectory_ = create_publisher("~output/trajectory", 1); + pub_debug_markers_ = + create_publisher("~output/debug_markers", 1); + sub_trajectory_ = create_subscription( + "~input/trajectory", 1, [this](const Trajectory::ConstSharedPtr msg) { onTrajectory(msg); }); + + // set_param_res_ = add_on_set_parameters_callback([this](const auto & + // params){onParameter(params);}); } private: rclcpp::Publisher::SharedPtr pub_trajectory_; //!< @brief publisher for output trajectory + rclcpp::Publisher::SharedPtr + pub_debug_markers_; //!< @brief publisher for debug markers rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief subscriber for reference trajectory // parameters - double time_safety_buffer_; - double dist_safety_buffer_; + Float time_safety_buffer_; + Float dist_safety_buffer_; // parameter update OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -74,26 +93,55 @@ class SafeVelocityAdjustorNode : public rclcpp::Node } // publish methods - std::optional distanceToClosestCollision(const TrajectoryPoint & trajectory_point) + std::optional distanceToClosestCollision(const TrajectoryPoint & trajectory_point) { (void)trajectory_point; return {}; } - double calculateSafeVelocity( - const TrajectoryPoint & trajectory_point, const double & dist_to_collision) + Float calculateSafeVelocity( + const TrajectoryPoint & trajectory_point, const Float & dist_to_collision) const { return std::min( trajectory_point.longitudinal_velocity_mps, - static_cast( - dist_to_collision / time_safety_buffer_)); + static_cast(dist_to_collision / time_safety_buffer_)); + } + + static visualization_msgs::msg::Marker makeEnvelopeMarker( + const Trajectory & trajectory, const Float time_safety_buffer, const Float dist_safety_buffer) + { + visualization_msgs::msg::Marker envelope; + envelope.header = trajectory.header; + envelope.type = visualization_msgs::msg::Marker::LINE_STRIP; + for (const auto & point : trajectory.points) { + const auto heading = tf2::getYaw(point.pose.orientation); + auto p = point.pose.position; + p.x += static_cast( + point.longitudinal_velocity_mps * std::cos(heading) * time_safety_buffer) + + dist_safety_buffer; + p.y += static_cast( + point.longitudinal_velocity_mps * std::sin(heading) * time_safety_buffer) + + dist_safety_buffer; + envelope.points.push_back(p); + } + return envelope; } void publishDebug( const Trajectory & original_trajectory, const Trajectory & safe_trajectory) const { - (void)original_trajectory; - (void)safe_trajectory; + visualization_msgs::msg::MarkerArray debug_markers; + auto unsafe_envelope = + makeEnvelopeMarker(original_trajectory, time_safety_buffer_, dist_safety_buffer_); + unsafe_envelope.color.r = 100; + unsafe_envelope.ns = "unsafe"; + debug_markers.markers.push_back(unsafe_envelope); + auto safe_envelope = + makeEnvelopeMarker(safe_trajectory, time_safety_buffer_, dist_safety_buffer_); + safe_envelope.color.g = 100; + safe_envelope.ns = "safe"; + debug_markers.markers.push_back(safe_envelope); + pub_debug_markers_->publish(debug_markers); } // debug diff --git a/planning/safe_velocity_adjustor/launch/motion_velocity_smoother.launch.xml b/planning/safe_velocity_adjustor/launch/motion_velocity_smoother.launch.xml deleted file mode 100644 index 9c5768fccfad3..0000000000000 --- a/planning/safe_velocity_adjustor/launch/motion_velocity_smoother.launch.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.xml b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.xml new file mode 100644 index 0000000000000..1a944c4132485 --- /dev/null +++ b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + From a094c81c9962d48377e2048ac9a6009c2fd2b39d Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 23 Mar 2022 17:36:49 +0900 Subject: [PATCH 17/37] Fix debug markers --- .../safe_velocity_adjustor_node.hpp | 43 ++++++++++++++----- ....xml => safe_velocity_adjustor.launch.xml} | 8 ++-- planning/safe_velocity_adjustor/package.xml | 1 + 3 files changed, 37 insertions(+), 15 deletions(-) rename planning/safe_velocity_adjustor/launch/{safe_velocity_adjustor.xml => safe_velocity_adjustor.launch.xml} (75%) diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index cb7114d4468a7..89ac0244d1dff 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -15,6 +15,7 @@ #ifndef SAFE_VELOCITY_ADJUSTOR__SAFE_VELOCITY_ADJUSTOR_NODE_HPP_ #define SAFE_VELOCITY_ADJUSTOR__SAFE_VELOCITY_ADJUSTOR_NODE_HPP_ +#include #include #include @@ -46,18 +47,17 @@ class SafeVelocityAdjustorNode : public rclcpp::Node explicit SafeVelocityAdjustorNode(const rclcpp::NodeOptions & node_options) : rclcpp::Node("safe_velocity_adjustor", node_options) { - // TODO(Maxime CLEMENT): declare/get ROS parameters - time_safety_buffer_ = 0.5; - dist_safety_buffer_ = 1.5; + time_safety_buffer_ = static_cast(declare_parameter("time_safety_buffer")); + dist_safety_buffer_ = static_cast(declare_parameter("dist_safety_buffer")); - pub_trajectory_ = create_publisher("~output/trajectory", 1); + pub_trajectory_ = create_publisher("~/output/trajectory", 1); pub_debug_markers_ = - create_publisher("~output/debug_markers", 1); + create_publisher("~/output/debug_markers", 1); sub_trajectory_ = create_subscription( - "~input/trajectory", 1, [this](const Trajectory::ConstSharedPtr msg) { onTrajectory(msg); }); + "~/input/trajectory", 1, [this](const Trajectory::ConstSharedPtr msg) { onTrajectory(msg); }); - // set_param_res_ = add_on_set_parameters_callback([this](const auto & - // params){onParameter(params);}); + set_param_res_ = + add_on_set_parameters_callback([this](const auto & params) { return onParameter(params); }); } private: @@ -75,7 +75,25 @@ class SafeVelocityAdjustorNode : public rclcpp::Node // parameter update OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onParameter( - const std::vector & parameters); + const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + for (const auto & parameter : parameters) { + if (parameter.get_name() == "time_safety_buffer") { + time_safety_buffer_ = static_cast(parameter.as_double()); + } else if (parameter.get_name() == "dist_safety_buffer") { + time_safety_buffer_ = static_cast(parameter.as_double()); + result.successful = true; + } else { + RCLCPP_WARN(get_logger(), "Unknown parameter %s", parameter.get_name().c_str()); + } + } + return result; + } + + // cached inputs + Trajectory::ConstSharedPtr prev_input_trajectory_; // topic callback void onTrajectory(const Trajectory::ConstSharedPtr msg) @@ -90,6 +108,7 @@ class SafeVelocityAdjustorNode : public rclcpp::Node safe_trajectory.header.stamp = now(); pub_trajectory_->publish(safe_trajectory); publishDebug(*msg, safe_trajectory); + prev_input_trajectory_ = msg; } // publish methods @@ -113,6 +132,8 @@ class SafeVelocityAdjustorNode : public rclcpp::Node visualization_msgs::msg::Marker envelope; envelope.header = trajectory.header; envelope.type = visualization_msgs::msg::Marker::LINE_STRIP; + envelope.scale.x = 0.1; + envelope.color.a = 1.0; for (const auto & point : trajectory.points) { const auto heading = tf2::getYaw(point.pose.orientation); auto p = point.pose.position; @@ -133,12 +154,12 @@ class SafeVelocityAdjustorNode : public rclcpp::Node visualization_msgs::msg::MarkerArray debug_markers; auto unsafe_envelope = makeEnvelopeMarker(original_trajectory, time_safety_buffer_, dist_safety_buffer_); - unsafe_envelope.color.r = 100; + unsafe_envelope.color.r = 1.0; unsafe_envelope.ns = "unsafe"; debug_markers.markers.push_back(unsafe_envelope); auto safe_envelope = makeEnvelopeMarker(safe_trajectory, time_safety_buffer_, dist_safety_buffer_); - safe_envelope.color.g = 100; + safe_envelope.color.g = 1.0; safe_envelope.ns = "safe"; debug_markers.markers.push_back(safe_envelope); pub_debug_markers_->publish(debug_markers); diff --git a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.xml b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml similarity index 75% rename from planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.xml rename to planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml index 1a944c4132485..2e46bbc31fe14 100644 --- a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.xml +++ b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml @@ -1,17 +1,17 @@ - - + + - + - + diff --git a/planning/safe_velocity_adjustor/package.xml b/planning/safe_velocity_adjustor/package.xml index 0e42fca69d5be..e0b235993f234 100644 --- a/planning/safe_velocity_adjustor/package.xml +++ b/planning/safe_velocity_adjustor/package.xml @@ -16,6 +16,7 @@ rclcpp rclcpp_components tier4_planning_msgs + visualization_msgs ament_lint_auto autoware_lint_common From 424adbc65325329738c32b712a5aea0342038116 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 23 Mar 2022 17:48:02 +0900 Subject: [PATCH 18/37] Fix dynamic parameters --- .../safe_velocity_adjustor/safe_velocity_adjustor_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index 89ac0244d1dff..9f4b4700ee109 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -83,7 +83,7 @@ class SafeVelocityAdjustorNode : public rclcpp::Node if (parameter.get_name() == "time_safety_buffer") { time_safety_buffer_ = static_cast(parameter.as_double()); } else if (parameter.get_name() == "dist_safety_buffer") { - time_safety_buffer_ = static_cast(parameter.as_double()); + dist_safety_buffer_ = static_cast(parameter.as_double()); result.successful = true; } else { RCLCPP_WARN(get_logger(), "Unknown parameter %s", parameter.get_name().c_str()); From 6c58e64e69133aafeaea7a2003d21c2c16def370 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 25 Mar 2022 13:56:15 +0900 Subject: [PATCH 19/37] Add proper collision detection and debug footprint Implements Proposal 1. Calculation of the adjusted velocity still needs to be improved --- .../collision_distance.hpp | 89 ++++++++++++++ .../pointcloud_processing.hpp | 82 +++++++++++++ .../safe_velocity_adjustor_node.hpp | 113 +++++++++++++----- .../launch/safe_velocity_adjustor.launch.xml | 2 + planning/safe_velocity_adjustor/package.xml | 4 + 5 files changed, 257 insertions(+), 33 deletions(-) create mode 100644 planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp create mode 100644 planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp new file mode 100644 index 0000000000000..1ed27a4a5f654 --- /dev/null +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp @@ -0,0 +1,89 @@ +// Copyright 2022 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 SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ +#define SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace safe_velocity_adjustor +{ +namespace bg = boost::geometry; +using point_t = bg::model::d2::point_xy; +using points_t = bg::model::linestring; +using polygon_t = bg::model::polygon; +using multipolygon_t = bg::model::multi_polygon; + +inline polygon_t forwardSimulatedFootprint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & trajectory_point, + const double time_safety_buffer, const double dist_safety_buffer, const double vehicle_width) +{ + const auto heading = tf2::getYaw(trajectory_point.pose.orientation); + const auto velocity = trajectory_point.longitudinal_velocity_mps; + const auto from = point_t{trajectory_point.pose.position.x, trajectory_point.pose.position.y}; + const auto to = point_t{ + from.x() + std::cos(heading) * (velocity * time_safety_buffer + dist_safety_buffer), + from.y() + std::sin(heading) * (velocity * time_safety_buffer + dist_safety_buffer)}; + multipolygon_t footprint; + namespace strategy = bg::strategy::buffer; + bg::buffer( + points_t{from, to}, footprint, strategy::distance_symmetric(vehicle_width / 2), + strategy::side_straight(), strategy::join_miter(), strategy::end_flat(), + strategy::point_square()); + return footprint[0]; +} + +inline std::optional distanceToClosestCollision( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & trajectory_point, + const polygon_t & footprint, const pcl::PointCloud & obstacle_points) +{ + const auto traj_point = + point_t{trajectory_point.pose.position.x, trajectory_point.pose.position.y}; + auto min_dist = std::numeric_limits::infinity(); + for (const auto & obstacle_point : obstacle_points) { + const auto obs_point = point_t{obstacle_point.x, obstacle_point.y}; + if (bg::within(obs_point, footprint)) { + const auto hypot_length = bg::distance(obs_point, traj_point); + // TODO(Maxime CLEMENT): more precise distance requires distance from central segment to the + // coll point const auto coll_dist = std::sqrt(hypot_length * hypot_length - + min_dist = std::min(min_dist, hypot_length); + } + } + return (min_dist != std::numeric_limits::infinity() ? min_dist : std::optional()); +} +} // namespace safe_velocity_adjustor + +#endif // SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp new file mode 100644 index 0000000000000..05f204332089a --- /dev/null +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp @@ -0,0 +1,82 @@ +// Copyright 2022 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 SAFE_VELOCITY_ADJUSTOR__POINTCLOUD_PROCESSING_HPP_ +#define SAFE_VELOCITY_ADJUSTOR__POINTCLOUD_PROCESSING_HPP_ + +#include "tier4_autoware_utils/ros/transform_listener.hpp" + +#include +#include + +#include +#include +#include + +#include +#include + +namespace safe_velocity_adjustor +{ + +pcl::PointCloud getTransformedPointCloud( + const sensor_msgs::msg::PointCloud2 & pointcloud_msg, + const geometry_msgs::msg::Transform & transform) +{ + const Eigen::Matrix4f transform_matrix = tf2::transformToEigen(transform).matrix().cast(); + + sensor_msgs::msg::PointCloud2 transformed_msg; + pcl_ros::transformPointCloud(transform_matrix, pointcloud_msg, transformed_msg); + + pcl::PointCloud transformed_pointcloud; + pcl::fromROSMsg(transformed_msg, transformed_pointcloud); + + return transformed_pointcloud; +} + +pcl::PointCloud filterPointCloudByTrajectory( + const pcl::PointCloud & pointcloud, + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double radius) +{ + pcl::PointCloud filtered_pointcloud; + for (const auto & point : pointcloud.points) { + for (const auto & trajectory_point : trajectory.points) { + const double dx = trajectory_point.pose.position.x - point.x; + const double dy = trajectory_point.pose.position.y - point.y; + if (std::hypot(dx, dy) < radius) { + filtered_pointcloud.points.push_back(point); + break; + } + } + } + return filtered_pointcloud; +} + +pcl::PointCloud transformAndFilterPointCloud( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const sensor_msgs::msg::PointCloud2 & pointcloud, + tier4_autoware_utils::TransformListener & transform_listener, const double filter_range) +{ + // TODO(Maxime CLEMENT): we may need to remove dynamic obstacles from the point cloud + const auto & header = pointcloud.header; + const auto transform = transform_listener.getTransform( + trajectory.header.frame_id, header.frame_id, header.stamp, + rclcpp::Duration::from_nanoseconds(0)); + const auto obstacle_pointcloud = getTransformedPointCloud(pointcloud, transform->transform); + return filterPointCloudByTrajectory(obstacle_pointcloud, trajectory, filter_range); +} + +} // namespace safe_velocity_adjustor + +#endif // SAFE_VELOCITY_ADJUSTOR__POINTCLOUD_PROCESSING_HPP_ diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index 9f4b4700ee109..f265e7cdde5d0 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -15,15 +15,18 @@ #ifndef SAFE_VELOCITY_ADJUSTOR__SAFE_VELOCITY_ADJUSTOR_NODE_HPP_ #define SAFE_VELOCITY_ADJUSTOR__SAFE_VELOCITY_ADJUSTOR_NODE_HPP_ +#include "safe_velocity_adjustor/collision_distance.hpp" +#include "safe_velocity_adjustor/pointcloud_processing.hpp" + #include +#include #include +#include #include #include #include -#include -#include -#include +#include #include #include @@ -38,6 +41,7 @@ namespace safe_velocity_adjustor { using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using sensor_msgs::msg::PointCloud2; using TrajectoryPoints = std::vector; using Float = decltype(TrajectoryPoint::longitudinal_velocity_mps); @@ -45,34 +49,43 @@ class SafeVelocityAdjustorNode : public rclcpp::Node { public: explicit SafeVelocityAdjustorNode(const rclcpp::NodeOptions & node_options) - : rclcpp::Node("safe_velocity_adjustor", node_options) + : rclcpp::Node("safe_velocity_adjustor", node_options), transform_listener_(this) { time_safety_buffer_ = static_cast(declare_parameter("time_safety_buffer")); dist_safety_buffer_ = static_cast(declare_parameter("dist_safety_buffer")); + sub_trajectory_ = create_subscription( + "~/input/trajectory", 1, [this](const Trajectory::ConstSharedPtr msg) { onTrajectory(msg); }); + sub_obstacle_pointcloud_ = create_subscription( + "~/input/obstacle_pointcloud", rclcpp::QoS(1).best_effort().transient_local(), + [this](const PointCloud2::ConstSharedPtr msg) { obstacle_pointcloud_ptr_ = msg; }); + pub_trajectory_ = create_publisher("~/output/trajectory", 1); pub_debug_markers_ = create_publisher("~/output/debug_markers", 1); - sub_trajectory_ = create_subscription( - "~/input/trajectory", 1, [this](const Trajectory::ConstSharedPtr msg) { onTrajectory(msg); }); set_param_res_ = add_on_set_parameters_callback([this](const auto & params) { return onParameter(params); }); } private: + tier4_autoware_utils::TransformListener transform_listener_; rclcpp::Publisher::SharedPtr pub_trajectory_; //!< @brief publisher for output trajectory rclcpp::Publisher::SharedPtr pub_debug_markers_; //!< @brief publisher for debug markers rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief subscriber for reference trajectory + rclcpp::Subscription::SharedPtr + sub_obstacle_pointcloud_; //!< @brief subscriber for obstacle pointcloud + + // cached inputs + PointCloud2::ConstSharedPtr obstacle_pointcloud_ptr_; // parameters Float time_safety_buffer_; Float dist_safety_buffer_; - // parameter update OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onParameter( const std::vector & parameters) @@ -92,30 +105,47 @@ class SafeVelocityAdjustorNode : public rclcpp::Node return result; } - // cached inputs - Trajectory::ConstSharedPtr prev_input_trajectory_; - - // topic callback void onTrajectory(const Trajectory::ConstSharedPtr msg) { + if (!obstacle_pointcloud_ptr_) { + RCLCPP_WARN(get_logger(), "Obstable pointcloud not yet received"); + return; + } + std::vector> footprints; Trajectory safe_trajectory = *msg; + const auto max_vel = std::max_element( + safe_trajectory.points.cbegin(), safe_trajectory.points.cend(), + [](const auto & p1, const auto & p2) { + return p1.longitudinal_velocity_mps < p2.longitudinal_velocity_mps; + }) + ->longitudinal_velocity_mps; + const auto filtered_obstacle_pointcloud = transformAndFilterPointCloud( + safe_trajectory, *obstacle_pointcloud_ptr_, transform_listener_, + max_vel * time_safety_buffer_ + dist_safety_buffer_); + for (auto & trajectory_point : safe_trajectory.points) { - const auto closest_collision_point = distanceToClosestCollision(trajectory_point); - if (closest_collision_point) + // TODO(Maxime CLEMENT): get vehicle width from vehicle parameters + const auto forward_simulated_footprint = + forwardSimulatedFootprint(trajectory_point, time_safety_buffer_, dist_safety_buffer_, 2.0); + std::vector footprint_points; + for (const auto footprint_point : forward_simulated_footprint.outer()) { + geometry_msgs::msg::Point p; + p.x = footprint_point.x(); + p.y = footprint_point.y(); + p.z = trajectory_point.pose.position.z; + footprint_points.push_back(p); + } + footprints.push_back(footprint_points); + const auto dist_to_collision = distanceToClosestCollision( + trajectory_point, forward_simulated_footprint, filtered_obstacle_pointcloud); + if (dist_to_collision) { trajectory_point.longitudinal_velocity_mps = - calculateSafeVelocity(trajectory_point, *closest_collision_point); + calculateSafeVelocity(trajectory_point, static_cast(*dist_to_collision)); + } } safe_trajectory.header.stamp = now(); pub_trajectory_->publish(safe_trajectory); - publishDebug(*msg, safe_trajectory); - prev_input_trajectory_ = msg; - } - - // publish methods - std::optional distanceToClosestCollision(const TrajectoryPoint & trajectory_point) - { - (void)trajectory_point; - return {}; + publishDebug(*msg, safe_trajectory, footprints); } Float calculateSafeVelocity( @@ -149,19 +179,36 @@ class SafeVelocityAdjustorNode : public rclcpp::Node } void publishDebug( - const Trajectory & original_trajectory, const Trajectory & safe_trajectory) const + const Trajectory & original_trajectory, const Trajectory & adjusted_trajectory, + const std::vector> & footprints) const { visualization_msgs::msg::MarkerArray debug_markers; - auto unsafe_envelope = + auto original_envelope = makeEnvelopeMarker(original_trajectory, time_safety_buffer_, dist_safety_buffer_); - unsafe_envelope.color.r = 1.0; - unsafe_envelope.ns = "unsafe"; - debug_markers.markers.push_back(unsafe_envelope); - auto safe_envelope = - makeEnvelopeMarker(safe_trajectory, time_safety_buffer_, dist_safety_buffer_); - safe_envelope.color.g = 1.0; - safe_envelope.ns = "safe"; - debug_markers.markers.push_back(safe_envelope); + original_envelope.color.r = 1.0; + original_envelope.ns = "original"; + debug_markers.markers.push_back(original_envelope); + auto adjusted_envelope = + makeEnvelopeMarker(adjusted_trajectory, time_safety_buffer_, dist_safety_buffer_); + adjusted_envelope.color.g = 1.0; + adjusted_envelope.ns = "adjusted"; + debug_markers.markers.push_back(adjusted_envelope); + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = now(); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.ns = "original_footprints"; + for (const auto & footprint : footprints) { + marker.id++; + marker.scale.x = 0.1; + marker.color.a = 1.0; + marker.color.b = 1.0; + marker.color.g = 1.0; + marker.points = footprint; + debug_markers.markers.push_back(marker); + } + pub_debug_markers_->publish(debug_markers); } diff --git a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml index 2e46bbc31fe14..cbf1e8951903f 100644 --- a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml +++ b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml @@ -1,6 +1,7 @@ + @@ -13,6 +14,7 @@ + diff --git a/planning/safe_velocity_adjustor/package.xml b/planning/safe_velocity_adjustor/package.xml index e0b235993f234..e6497b9142df7 100644 --- a/planning/safe_velocity_adjustor/package.xml +++ b/planning/safe_velocity_adjustor/package.xml @@ -12,9 +12,13 @@ eigen3_cmake_module autoware_auto_planning_msgs + eigen libboost-dev + pcl_ros rclcpp rclcpp_components + sensor_msgs + tier4_autoware_utils tier4_planning_msgs visualization_msgs From 809e7944743cf1be815e2f1823d0591b5e4f6889 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 25 Mar 2022 15:53:32 +0900 Subject: [PATCH 20/37] Add script to compare the original and adjusted velocity profiles --- .../script/trajectory_visualizer.py | 111 ++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100755 planning/safe_velocity_adjustor/script/trajectory_visualizer.py diff --git a/planning/safe_velocity_adjustor/script/trajectory_visualizer.py b/planning/safe_velocity_adjustor/script/trajectory_visualizer.py new file mode 100755 index 0000000000000..1cf8cb1b97d31 --- /dev/null +++ b/planning/safe_velocity_adjustor/script/trajectory_visualizer.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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. + +from autoware_auto_planning_msgs.msg import Trajectory +import matplotlib.pyplot as plt +import numpy as np +import rclpy +from rclpy.node import Node + + +class TrajectoryVisualizer(Node): + def __init__(self): + + super().__init__("trajectory_visualizer") + + self.fig = plt.figure() + + self.sub_original_traj = self.create_subscription( + Trajectory, + "/planning/scenario_planning/scenario_selector/trajectory", + self.plotOriginalTrajectory, + 1, + ) + self.sub_adjusted_traj = self.create_subscription( + Trajectory, + "/planning/scenario_planning/safe_velocity_adjustor/trajectory", + self.plotAdjustedTrajectory, + 1, + ) + + self.initPlotTrajectoryVelocity() + + def plotOriginalTrajectory(self, original): + x = self.CalcArcLength(original) + y = self.ToVelList(original) + self.im1.set_data(x, y) + self.ax1.relim() + self.ax1.autoscale_view(True, True, True) + plt.draw() + plt.pause(0.01) + + def plotAdjustedTrajectory(self, adjusted): + x = self.CalcArcLength(adjusted) + y = self.ToVelList(adjusted) + self.im2.set_data(x, y) + self.ax1.relim() + self.ax1.autoscale_view(True, True, True) + plt.draw() + plt.pause(0.01) + + def initPlotTrajectoryVelocity(self): + self.ax1 = plt.subplot(1, 1, 1) # row, col, index( 0: + s_arr.append(s_sum) + + for i in range(1, len(traj.points)): + p0 = traj.points[i - 1] + p1 = traj.points[i] + dx = p1.pose.position.x - p0.pose.position.x + dy = p1.pose.position.y - p0.pose.position.y + ds = np.sqrt(dx**2 + dy**2) + s_sum += ds + s_arr.append(s_sum) + return s_arr + + def ToVelList(self, traj): + v_list = [] + for p in traj.points: + v_list.append(p.longitudinal_velocity_mps) + return v_list + + +def main(args=None): + try: + rclpy.init(args=args) + node = TrajectoryVisualizer() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() From e553b413f3f9939b84e6987ea36f2efaa1631cd9 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 28 Mar 2022 10:43:07 +0900 Subject: [PATCH 21/37] Fix calculation of distance to obstacle --- .../collision_distance.hpp | 28 +++++++----- .../safe_velocity_adjustor_node.hpp | 45 +++++-------------- 2 files changed, 30 insertions(+), 43 deletions(-) diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp index 1ed27a4a5f654..e13d269a4c511 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp @@ -43,24 +43,29 @@ namespace safe_velocity_adjustor { namespace bg = boost::geometry; using point_t = bg::model::d2::point_xy; -using points_t = bg::model::linestring; +using linestring_t = bg::model::linestring; using polygon_t = bg::model::polygon; using multipolygon_t = bg::model::multi_polygon; -inline polygon_t forwardSimulatedFootprint( +inline linestring_t forwardSimulatedVector( const autoware_auto_planning_msgs::msg::TrajectoryPoint & trajectory_point, - const double time_safety_buffer, const double dist_safety_buffer, const double vehicle_width) + const double time_safety_buffer, const double dist_safety_buffer, const double vehicle_length) { const auto heading = tf2::getYaw(trajectory_point.pose.orientation); const auto velocity = trajectory_point.longitudinal_velocity_mps; + const auto length = velocity * time_safety_buffer + dist_safety_buffer + vehicle_length / 2; const auto from = point_t{trajectory_point.pose.position.x, trajectory_point.pose.position.y}; - const auto to = point_t{ - from.x() + std::cos(heading) * (velocity * time_safety_buffer + dist_safety_buffer), - from.y() + std::sin(heading) * (velocity * time_safety_buffer + dist_safety_buffer)}; + const auto to = + point_t{from.x() + std::cos(heading) * length, from.y() + std::sin(heading) * length}; + return linestring_t{from, to}; +} + +inline polygon_t forwardSimulatedFootprint(const linestring_t & vector, const double vehicle_width) +{ multipolygon_t footprint; namespace strategy = bg::strategy::buffer; bg::buffer( - points_t{from, to}, footprint, strategy::distance_symmetric(vehicle_width / 2), + vector, footprint, strategy::distance_symmetric(vehicle_width / 2), strategy::side_straight(), strategy::join_miter(), strategy::end_flat(), strategy::point_square()); return footprint[0]; @@ -76,10 +81,13 @@ inline std::optional distanceToClosestCollision( for (const auto & obstacle_point : obstacle_points) { const auto obs_point = point_t{obstacle_point.x, obstacle_point.y}; if (bg::within(obs_point, footprint)) { + const auto footprint_heading = tf2::getYaw(trajectory_point.pose.orientation); + const auto collision_heading = + std::atan2(obs_point.y() - traj_point.y(), obs_point.x() - traj_point.x()); + const auto angle = footprint_heading - collision_heading; const auto hypot_length = bg::distance(obs_point, traj_point); - // TODO(Maxime CLEMENT): more precise distance requires distance from central segment to the - // coll point const auto coll_dist = std::sqrt(hypot_length * hypot_length - - min_dist = std::min(min_dist, hypot_length); + const auto dist = std::abs(std::cos(angle)) * hypot_length; + min_dist = std::min(min_dist, dist); } } return (min_dist != std::numeric_limits::infinity() ? min_dist : std::optional()); diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index f265e7cdde5d0..c4bf60f076f57 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -85,6 +85,9 @@ class SafeVelocityAdjustorNode : public rclcpp::Node // parameters Float time_safety_buffer_; Float dist_safety_buffer_; + // TODO(Maxime CLEMENT): get vehicle width and length from vehicle parameters + Float vehicle_width_ = 2.0; + Float vehicle_length_ = 4.0; OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onParameter( @@ -124,28 +127,22 @@ class SafeVelocityAdjustorNode : public rclcpp::Node max_vel * time_safety_buffer_ + dist_safety_buffer_); for (auto & trajectory_point : safe_trajectory.points) { - // TODO(Maxime CLEMENT): get vehicle width from vehicle parameters + const auto forward_simulated_vector = forwardSimulatedVector( + trajectory_point, time_safety_buffer_, dist_safety_buffer_, vehicle_length_); const auto forward_simulated_footprint = - forwardSimulatedFootprint(trajectory_point, time_safety_buffer_, dist_safety_buffer_, 2.0); - std::vector footprint_points; - for (const auto footprint_point : forward_simulated_footprint.outer()) { - geometry_msgs::msg::Point p; - p.x = footprint_point.x(); - p.y = footprint_point.y(); - p.z = trajectory_point.pose.position.z; - footprint_points.push_back(p); - } - footprints.push_back(footprint_points); + forwardSimulatedFootprint(forward_simulated_vector, vehicle_width_); const auto dist_to_collision = distanceToClosestCollision( trajectory_point, forward_simulated_footprint, filtered_obstacle_pointcloud); if (dist_to_collision) { - trajectory_point.longitudinal_velocity_mps = - calculateSafeVelocity(trajectory_point, static_cast(*dist_to_collision)); + trajectory_point.longitudinal_velocity_mps = calculateSafeVelocity( + trajectory_point, + std::max( + {}, static_cast(*dist_to_collision - vehicle_length_ - dist_safety_buffer_))); } } safe_trajectory.header.stamp = now(); pub_trajectory_->publish(safe_trajectory); - publishDebug(*msg, safe_trajectory, footprints); + publishDebug(*msg, safe_trajectory); } Float calculateSafeVelocity( @@ -179,8 +176,7 @@ class SafeVelocityAdjustorNode : public rclcpp::Node } void publishDebug( - const Trajectory & original_trajectory, const Trajectory & adjusted_trajectory, - const std::vector> & footprints) const + const Trajectory & original_trajectory, const Trajectory & adjusted_trajectory) const { visualization_msgs::msg::MarkerArray debug_markers; auto original_envelope = @@ -194,25 +190,8 @@ class SafeVelocityAdjustorNode : public rclcpp::Node adjusted_envelope.ns = "adjusted"; debug_markers.markers.push_back(adjusted_envelope); - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = now(); - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.ns = "original_footprints"; - for (const auto & footprint : footprints) { - marker.id++; - marker.scale.x = 0.1; - marker.color.a = 1.0; - marker.color.b = 1.0; - marker.color.g = 1.0; - marker.points = footprint; - debug_markers.markers.push_back(marker); - } - pub_debug_markers_->publish(debug_markers); } - - // debug }; } // namespace safe_velocity_adjustor From 7dd6112a1f401644cb56bd76f46a4997555c2595 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 28 Mar 2022 11:16:12 +0900 Subject: [PATCH 22/37] Add test for calculation collision distance --- .../safe_velocity_adjustor/CMakeLists.txt | 7 + planning/safe_velocity_adjustor/package.xml | 1 + .../test/test_collision_distance.cpp | 122 ++++++++++++++++++ 3 files changed, 130 insertions(+) create mode 100644 planning/safe_velocity_adjustor/test/test_collision_distance.cpp diff --git a/planning/safe_velocity_adjustor/CMakeLists.txt b/planning/safe_velocity_adjustor/CMakeLists.txt index db2ec93aa899c..908d7179b48cc 100644 --- a/planning/safe_velocity_adjustor/CMakeLists.txt +++ b/planning/safe_velocity_adjustor/CMakeLists.txt @@ -27,6 +27,13 @@ rclcpp_components_register_node(safe_velocity_adjustor_node if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_${PROJECT_NAME} + test/test_collision_distance.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + safe_velocity_adjustor_node + ) endif() ament_auto_package( diff --git a/planning/safe_velocity_adjustor/package.xml b/planning/safe_velocity_adjustor/package.xml index e6497b9142df7..9fe22e89dedd0 100644 --- a/planning/safe_velocity_adjustor/package.xml +++ b/planning/safe_velocity_adjustor/package.xml @@ -22,6 +22,7 @@ tier4_planning_msgs visualization_msgs + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/planning/safe_velocity_adjustor/test/test_collision_distance.cpp b/planning/safe_velocity_adjustor/test/test_collision_distance.cpp new file mode 100644 index 0000000000000..b37081a06c19d --- /dev/null +++ b/planning/safe_velocity_adjustor/test/test_collision_distance.cpp @@ -0,0 +1,122 @@ +// Copyright 2022 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 "safe_velocity_adjustor/collision_distance.hpp" + +#include +#include + +#include + +TEST(TestCollisionDistance, distanceToClosestCollision) +{ + using safe_velocity_adjustor::distanceToClosestCollision; + using safe_velocity_adjustor::point_t; + using safe_velocity_adjustor::polygon_t; + autoware_auto_planning_msgs::msg::TrajectoryPoint trajectory_point; + trajectory_point.pose.position.x = 0; + trajectory_point.pose.position.y = 0; + trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); + polygon_t footprint; + footprint.outer() = {point_t{0, 1}, point_t{5, 1}, point_t{5, -1}, point_t{0, -1}, point_t{0, 1}}; + pcl::PointCloud obstacle_points; + obstacle_points.points.emplace_back(6, 2, 0); // outside of the footprint + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_FALSE(dist.has_value()); + } + obstacle_points.points.emplace_back(4, 0, 0); // distance of 4 + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_TRUE(dist.has_value()); + EXPECT_DOUBLE_EQ(*dist, 4.0); + } + obstacle_points.points.emplace_back( + 4.5, 0, 0); // distance of 4.5, does not change the minimum distance + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_TRUE(dist.has_value()); + EXPECT_DOUBLE_EQ(*dist, 4.0); + } + obstacle_points.points.emplace_back(2.0, 0.5, 0); // new minumum distance of 2.0 + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_TRUE(dist.has_value()); + EXPECT_DOUBLE_EQ(*dist, 2.0); + } + obstacle_points.points.emplace_back(1.5, -0.75, 0); // new minumum distance of 1.5 + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_TRUE(dist.has_value()); + EXPECT_DOUBLE_EQ(*dist, 1.5); + } + + // Change footprint heading + trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(M_PI_2); + footprint.outer() = { + point_t{-1, 0}, point_t{-1, 5}, point_t{1, 5}, point_t{1, 0}, point_t{-1, 0}}; + obstacle_points.clear(); + obstacle_points.emplace_back(-2, 3, 0); // outside of the footprint + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_FALSE(dist.has_value()); + } + obstacle_points.points.emplace_back(-0.5, 4, 0); // new minumum distance of 4 + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_TRUE(dist.has_value()); + EXPECT_DOUBLE_EQ(*dist, 4); + } + obstacle_points.points.emplace_back(0, 4, 0); // no change in the minimum distance + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_TRUE(dist.has_value()); + EXPECT_DOUBLE_EQ(*dist, 4); + } + obstacle_points.points.emplace_back(0.5, 0.5, 0); // change the minimum distance + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_TRUE(dist.has_value()); + EXPECT_DOUBLE_EQ(*dist, 0.5); + } + + // Change footprint heading + trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(M_PI_4); + footprint.outer() = { + point_t{-0.5, 0.5}, point_t{2, 3}, point_t{3, 2}, point_t{0.5, -0.5}, point_t{-0.5, 0.5}}; + obstacle_points.clear(); + obstacle_points.emplace_back(3, 3, 0); // outside of the footprint + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_FALSE(dist.has_value()); + } + obstacle_points.points.emplace_back(2.25, 1.75, 0); // new minumum distance of 4 + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_TRUE(dist.has_value()); + EXPECT_DOUBLE_EQ(*dist, std::sqrt(8)); + } + obstacle_points.points.emplace_back(2, 2, 0); // no change in the minimum distance + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_TRUE(dist.has_value()); + EXPECT_DOUBLE_EQ(*dist, std::sqrt(8)); + } + obstacle_points.points.emplace_back(0.25, 0.75, 0); // change the minimum distance + { + const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + ASSERT_TRUE(dist.has_value()); + EXPECT_DOUBLE_EQ(*dist, std::sqrt(0.5)); + } +} From 8a9483872afeb0bd470ddead823d8f8724f8d44d Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 29 Mar 2022 16:45:03 +0900 Subject: [PATCH 23/37] Add launch file to test the safe_velocity_adjustor with a bag --- .../safe_velocity_adjustor/CMakeLists.txt | 1 + .../launch/safe_velocity_adjustor.launch.xml | 5 +- .../safe_velocity_adjustor_bag.launch.xml | 65 +++++++++++++++++++ ...y_visualizer.py => velocity_visualizer.py} | 0 4 files changed, 70 insertions(+), 1 deletion(-) create mode 100644 planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.launch.xml rename planning/safe_velocity_adjustor/script/{trajectory_visualizer.py => velocity_visualizer.py} (100%) diff --git a/planning/safe_velocity_adjustor/CMakeLists.txt b/planning/safe_velocity_adjustor/CMakeLists.txt index 908d7179b48cc..183943be5eb4a 100644 --- a/planning/safe_velocity_adjustor/CMakeLists.txt +++ b/planning/safe_velocity_adjustor/CMakeLists.txt @@ -40,4 +40,5 @@ ament_auto_package( INSTALL_TO_SHARE launch config + script ) diff --git a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml index cbf1e8951903f..a3ebbbe0c8d2e 100644 --- a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml +++ b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml @@ -10,7 +10,10 @@ - + + + diff --git a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.launch.xml b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.launch.xml new file mode 100644 index 0000000000000..aa8f7f144c5ce --- /dev/null +++ b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.launch.xml @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/safe_velocity_adjustor/script/trajectory_visualizer.py b/planning/safe_velocity_adjustor/script/velocity_visualizer.py similarity index 100% rename from planning/safe_velocity_adjustor/script/trajectory_visualizer.py rename to planning/safe_velocity_adjustor/script/velocity_visualizer.py From 8a1bf8a1eef099719f911c98de7f9763acd17ce0 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 29 Mar 2022 19:02:01 +0900 Subject: [PATCH 24/37] Cleanup code and add tests for forwardSimulatedVector --- .../collision_distance.hpp | 15 ++-- .../pointcloud_processing.hpp | 19 +++-- .../safe_velocity_adjustor_node.hpp | 24 +++--- .../test/test_collision_distance.cpp | 73 ++++++++++++++++++- 4 files changed, 103 insertions(+), 28 deletions(-) diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp index e13d269a4c511..0e20df065fc75 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp @@ -20,11 +20,9 @@ #include #include #include -#include #include #include #include -#include #include #include #include @@ -32,7 +30,8 @@ #include #include -#include +#include +#include #include #include @@ -47,19 +46,22 @@ using linestring_t = bg::model::linestring; using polygon_t = bg::model::polygon; using multipolygon_t = bg::model::multi_polygon; +/// @brief generate a segment to where the vehicle body would be after some duration assuming a +/// constant velocity and heading inline linestring_t forwardSimulatedVector( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & trajectory_point, - const double time_safety_buffer, const double dist_safety_buffer, const double vehicle_length) + const autoware_auto_planning_msgs::msg::TrajectoryPoint & trajectory_point, const double duration, + const double extra_distance) { const auto heading = tf2::getYaw(trajectory_point.pose.orientation); const auto velocity = trajectory_point.longitudinal_velocity_mps; - const auto length = velocity * time_safety_buffer + dist_safety_buffer + vehicle_length / 2; + const auto length = velocity * duration + extra_distance; const auto from = point_t{trajectory_point.pose.position.x, trajectory_point.pose.position.y}; const auto to = point_t{from.x() + std::cos(heading) * length, from.y() + std::sin(heading) * length}; return linestring_t{from, to}; } +/// @brief generate a footprint from a segment and a vehicle width inline polygon_t forwardSimulatedFootprint(const linestring_t & vector, const double vehicle_width) { multipolygon_t footprint; @@ -71,6 +73,7 @@ inline polygon_t forwardSimulatedFootprint(const linestring_t & vector, const do return footprint[0]; } +/// @brief calculate the distance to the closest obstacle point colliding with the footprint inline std::optional distanceToClosestCollision( const autoware_auto_planning_msgs::msg::TrajectoryPoint & trajectory_point, const polygon_t & footprint, const pcl::PointCloud & obstacle_points) diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp index 05f204332089a..ee10588bde34e 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp @@ -30,7 +30,8 @@ namespace safe_velocity_adjustor { -pcl::PointCloud getTransformedPointCloud( +/// @brief return the pointcloud msg transformed and converted to pcl format +inline pcl::PointCloud getTransformedPointCloud( const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const geometry_msgs::msg::Transform & transform) { @@ -45,13 +46,16 @@ pcl::PointCloud getTransformedPointCloud( return transformed_pointcloud; } -pcl::PointCloud filterPointCloudByTrajectory( +/// @brief returns the pointcloud with only points within a given distance to the trajectory +inline pcl::PointCloud filterPointCloudByTrajectory( const pcl::PointCloud & pointcloud, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double radius) + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double duration, + const double distance) { pcl::PointCloud filtered_pointcloud; for (const auto & point : pointcloud.points) { for (const auto & trajectory_point : trajectory.points) { + const auto radius = trajectory_point.longitudinal_velocity_mps * duration + distance; const double dx = trajectory_point.pose.position.x - point.x; const double dy = trajectory_point.pose.position.y - point.y; if (std::hypot(dx, dy) < radius) { @@ -63,10 +67,13 @@ pcl::PointCloud filterPointCloudByTrajectory( return filtered_pointcloud; } -pcl::PointCloud transformAndFilterPointCloud( +/// @brief returns the pointcloud transformed to the trajectory frame and in PCL format with only +/// points that are within range of the trajectory +inline pcl::PointCloud transformAndFilterPointCloud( const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const sensor_msgs::msg::PointCloud2 & pointcloud, - tier4_autoware_utils::TransformListener & transform_listener, const double filter_range) + tier4_autoware_utils::TransformListener & transform_listener, const double duration, + const double distance) { // TODO(Maxime CLEMENT): we may need to remove dynamic obstacles from the point cloud const auto & header = pointcloud.header; @@ -74,7 +81,7 @@ pcl::PointCloud transformAndFilterPointCloud( trajectory.header.frame_id, header.frame_id, header.stamp, rclcpp::Duration::from_nanoseconds(0)); const auto obstacle_pointcloud = getTransformedPointCloud(pointcloud, transform->transform); - return filterPointCloudByTrajectory(obstacle_pointcloud, trajectory, filter_range); + return filterPointCloudByTrajectory(obstacle_pointcloud, trajectory, duration, distance); } } // namespace safe_velocity_adjustor diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index c4bf60f076f57..96d226b35c252 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -25,13 +25,13 @@ #include #include -#include #include #include #include #include +#include #include #include #include @@ -57,7 +57,7 @@ class SafeVelocityAdjustorNode : public rclcpp::Node sub_trajectory_ = create_subscription( "~/input/trajectory", 1, [this](const Trajectory::ConstSharedPtr msg) { onTrajectory(msg); }); sub_obstacle_pointcloud_ = create_subscription( - "~/input/obstacle_pointcloud", rclcpp::QoS(1).best_effort().transient_local(), + "~/input/obstacle_pointcloud", rclcpp::QoS(1).best_effort(), [this](const PointCloud2::ConstSharedPtr msg) { obstacle_pointcloud_ptr_ = msg; }); pub_trajectory_ = create_publisher("~/output/trajectory", 1); @@ -114,21 +114,15 @@ class SafeVelocityAdjustorNode : public rclcpp::Node RCLCPP_WARN(get_logger(), "Obstable pointcloud not yet received"); return; } - std::vector> footprints; Trajectory safe_trajectory = *msg; - const auto max_vel = std::max_element( - safe_trajectory.points.cbegin(), safe_trajectory.points.cend(), - [](const auto & p1, const auto & p2) { - return p1.longitudinal_velocity_mps < p2.longitudinal_velocity_mps; - }) - ->longitudinal_velocity_mps; + const auto extra_vehicle_length = vehicle_length_ / 2 + dist_safety_buffer_; const auto filtered_obstacle_pointcloud = transformAndFilterPointCloud( - safe_trajectory, *obstacle_pointcloud_ptr_, transform_listener_, - max_vel * time_safety_buffer_ + dist_safety_buffer_); + safe_trajectory, *obstacle_pointcloud_ptr_, transform_listener_, time_safety_buffer_, + extra_vehicle_length); for (auto & trajectory_point : safe_trajectory.points) { - const auto forward_simulated_vector = forwardSimulatedVector( - trajectory_point, time_safety_buffer_, dist_safety_buffer_, vehicle_length_); + const auto forward_simulated_vector = + forwardSimulatedVector(trajectory_point, time_safety_buffer_, extra_vehicle_length); const auto forward_simulated_footprint = forwardSimulatedFootprint(forward_simulated_vector, vehicle_width_); const auto dist_to_collision = distanceToClosestCollision( @@ -136,10 +130,10 @@ class SafeVelocityAdjustorNode : public rclcpp::Node if (dist_to_collision) { trajectory_point.longitudinal_velocity_mps = calculateSafeVelocity( trajectory_point, - std::max( - {}, static_cast(*dist_to_collision - vehicle_length_ - dist_safety_buffer_))); + std::max({}, static_cast(*dist_to_collision - extra_vehicle_length))); } } + safe_trajectory.header.stamp = now(); pub_trajectory_->publish(safe_trajectory); publishDebug(*msg, safe_trajectory); diff --git a/planning/safe_velocity_adjustor/test/test_collision_distance.cpp b/planning/safe_velocity_adjustor/test/test_collision_distance.cpp index b37081a06c19d..c42e030323392 100644 --- a/planning/safe_velocity_adjustor/test/test_collision_distance.cpp +++ b/planning/safe_velocity_adjustor/test/test_collision_distance.cpp @@ -13,12 +13,83 @@ // limitations under the License. #include "safe_velocity_adjustor/collision_distance.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include -#include + +#include #include +TEST(TestCollisionDistance, forwardSimulatedVector) +{ + using safe_velocity_adjustor::forwardSimulatedVector; + using safe_velocity_adjustor::linestring_t; + autoware_auto_planning_msgs::msg::TrajectoryPoint trajectory_point; + + trajectory_point.pose.position.x = 0.0; + trajectory_point.pose.position.y = 0.0; + + auto duration = 0.0; + auto extra_dist = 0.0; + + const auto check_vector = [&](const auto vector_length) { + trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); + auto vector = forwardSimulatedVector(trajectory_point, duration, extra_dist); + EXPECT_DOUBLE_EQ(vector[0].x(), trajectory_point.pose.position.x); + EXPECT_DOUBLE_EQ(vector[0].y(), trajectory_point.pose.position.y); + EXPECT_DOUBLE_EQ(vector[1].x(), vector_length); + EXPECT_DOUBLE_EQ(vector[1].y(), 0.0); + trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(M_PI_2); + vector = forwardSimulatedVector(trajectory_point, duration, extra_dist); + EXPECT_DOUBLE_EQ(vector[0].x(), trajectory_point.pose.position.x); + EXPECT_DOUBLE_EQ(vector[0].y(), trajectory_point.pose.position.y); + EXPECT_NEAR(vector[1].x(), 0.0, 1e-9); + EXPECT_DOUBLE_EQ(vector[1].y(), vector_length); + trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(M_PI_4); + vector = forwardSimulatedVector(trajectory_point, duration, extra_dist); + EXPECT_DOUBLE_EQ(vector[0].x(), trajectory_point.pose.position.x); + EXPECT_DOUBLE_EQ(vector[0].y(), trajectory_point.pose.position.y); + EXPECT_DOUBLE_EQ(vector[1].x(), std::sqrt(0.5) * vector_length); + EXPECT_DOUBLE_EQ(vector[1].y(), std::sqrt(0.5) * vector_length); + trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(-M_PI_2); + vector = forwardSimulatedVector(trajectory_point, duration, extra_dist); + EXPECT_DOUBLE_EQ(vector[0].x(), trajectory_point.pose.position.x); + EXPECT_DOUBLE_EQ(vector[0].y(), trajectory_point.pose.position.y); + EXPECT_NEAR(vector[1].x(), 0.0, 1e-9); + EXPECT_DOUBLE_EQ(vector[1].y(), -vector_length); + }; + + // 0 velocity: whatever the duration the vector length is always = to extra_dist + trajectory_point.longitudinal_velocity_mps = 0.0; + + duration = 0.0; + extra_dist = 0.0; + check_vector(extra_dist); + + duration = 5.0; + extra_dist = 2.0; + check_vector(extra_dist); + + duration = -5.0; + extra_dist = 3.5; + check_vector(extra_dist); + + // set non-zero velocities + trajectory_point.longitudinal_velocity_mps = 1.0; + + duration = 1.0; + extra_dist = 0.0; + check_vector(1.0 + extra_dist); + + duration = 5.0; + extra_dist = 2.0; + check_vector(5.0 + extra_dist); + + duration = -5.0; + extra_dist = 3.5; + check_vector(-5.0 + extra_dist); +} TEST(TestCollisionDistance, distanceToClosestCollision) { using safe_velocity_adjustor::distanceToClosestCollision; From 4b7151c1036ae82160dae3c45ba58ea9ddac714b Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 30 Mar 2022 11:01:48 +0900 Subject: [PATCH 25/37] Simplify collision detection by not using a footprint polygon --- .../collision_distance.hpp | 57 ++++--------- .../safe_velocity_adjustor_node.hpp | 4 +- .../test/test_collision_distance.cpp | 84 +++++++++---------- 3 files changed, 57 insertions(+), 88 deletions(-) diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp index 0e20df065fc75..5efacc43b1666 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp @@ -18,17 +18,10 @@ #include #include -#include #include -#include +#include #include -#include -#include -#include -#include -#include -#include -#include +#include #include #include @@ -42,13 +35,11 @@ namespace safe_velocity_adjustor { namespace bg = boost::geometry; using point_t = bg::model::d2::point_xy; -using linestring_t = bg::model::linestring; -using polygon_t = bg::model::polygon; -using multipolygon_t = bg::model::multi_polygon; +using segment_t = bg::model::segment; /// @brief generate a segment to where the vehicle body would be after some duration assuming a /// constant velocity and heading -inline linestring_t forwardSimulatedVector( +inline segment_t forwardSimulatedVector( const autoware_auto_planning_msgs::msg::TrajectoryPoint & trajectory_point, const double duration, const double extra_distance) { @@ -58,42 +49,30 @@ inline linestring_t forwardSimulatedVector( const auto from = point_t{trajectory_point.pose.position.x, trajectory_point.pose.position.y}; const auto to = point_t{from.x() + std::cos(heading) * length, from.y() + std::sin(heading) * length}; - return linestring_t{from, to}; -} - -/// @brief generate a footprint from a segment and a vehicle width -inline polygon_t forwardSimulatedFootprint(const linestring_t & vector, const double vehicle_width) -{ - multipolygon_t footprint; - namespace strategy = bg::strategy::buffer; - bg::buffer( - vector, footprint, strategy::distance_symmetric(vehicle_width / 2), - strategy::side_straight(), strategy::join_miter(), strategy::end_flat(), - strategy::point_square()); - return footprint[0]; + return segment_t{from, to}; } /// @brief calculate the distance to the closest obstacle point colliding with the footprint inline std::optional distanceToClosestCollision( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & trajectory_point, - const polygon_t & footprint, const pcl::PointCloud & obstacle_points) + const segment_t & vector, const double vehicle_width, + const pcl::PointCloud & obstacle_points) { - const auto traj_point = - point_t{trajectory_point.pose.position.x, trajectory_point.pose.position.y}; + const auto traj_heading = + std::atan2(vector.second.y() - vector.first.y(), vector.second.x() - vector.first.x()); auto min_dist = std::numeric_limits::infinity(); for (const auto & obstacle_point : obstacle_points) { const auto obs_point = point_t{obstacle_point.x, obstacle_point.y}; - if (bg::within(obs_point, footprint)) { - const auto footprint_heading = tf2::getYaw(trajectory_point.pose.orientation); - const auto collision_heading = - std::atan2(obs_point.y() - traj_point.y(), obs_point.x() - traj_point.x()); - const auto angle = footprint_heading - collision_heading; - const auto hypot_length = bg::distance(obs_point, traj_point); - const auto dist = std::abs(std::cos(angle)) * hypot_length; - min_dist = std::min(min_dist, dist); + const auto collision_heading = + std::atan2(obs_point.y() - vector.first.y(), obs_point.x() - vector.first.x()); + const auto angle = traj_heading - collision_heading; + const auto hypot_length = bg::distance(obs_point, vector.first); + const auto long_dist = std::abs(std::cos(angle)) * hypot_length; + const auto lat_dist = std::sqrt(hypot_length * hypot_length - long_dist * long_dist); + if (lat_dist <= vehicle_width / 2) { + min_dist = std::min(min_dist, long_dist); } } - return (min_dist != std::numeric_limits::infinity() ? min_dist : std::optional()); + return (min_dist <= bg::length(vector) ? min_dist : std::optional()); } } // namespace safe_velocity_adjustor diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index 96d226b35c252..a045525ae4859 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -123,10 +123,8 @@ class SafeVelocityAdjustorNode : public rclcpp::Node for (auto & trajectory_point : safe_trajectory.points) { const auto forward_simulated_vector = forwardSimulatedVector(trajectory_point, time_safety_buffer_, extra_vehicle_length); - const auto forward_simulated_footprint = - forwardSimulatedFootprint(forward_simulated_vector, vehicle_width_); const auto dist_to_collision = distanceToClosestCollision( - trajectory_point, forward_simulated_footprint, filtered_obstacle_pointcloud); + forward_simulated_vector, vehicle_width_, filtered_obstacle_pointcloud); if (dist_to_collision) { trajectory_point.longitudinal_velocity_mps = calculateSafeVelocity( trajectory_point, diff --git a/planning/safe_velocity_adjustor/test/test_collision_distance.cpp b/planning/safe_velocity_adjustor/test/test_collision_distance.cpp index c42e030323392..587b7477c019c 100644 --- a/planning/safe_velocity_adjustor/test/test_collision_distance.cpp +++ b/planning/safe_velocity_adjustor/test/test_collision_distance.cpp @@ -24,7 +24,7 @@ TEST(TestCollisionDistance, forwardSimulatedVector) { using safe_velocity_adjustor::forwardSimulatedVector; - using safe_velocity_adjustor::linestring_t; + using safe_velocity_adjustor::segment_t; autoware_auto_planning_msgs::msg::TrajectoryPoint trajectory_point; trajectory_point.pose.position.x = 0.0; @@ -36,28 +36,28 @@ TEST(TestCollisionDistance, forwardSimulatedVector) const auto check_vector = [&](const auto vector_length) { trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); auto vector = forwardSimulatedVector(trajectory_point, duration, extra_dist); - EXPECT_DOUBLE_EQ(vector[0].x(), trajectory_point.pose.position.x); - EXPECT_DOUBLE_EQ(vector[0].y(), trajectory_point.pose.position.y); - EXPECT_DOUBLE_EQ(vector[1].x(), vector_length); - EXPECT_DOUBLE_EQ(vector[1].y(), 0.0); + EXPECT_DOUBLE_EQ(vector.first.x(), trajectory_point.pose.position.x); + EXPECT_DOUBLE_EQ(vector.first.y(), trajectory_point.pose.position.y); + EXPECT_DOUBLE_EQ(vector.second.x(), vector_length); + EXPECT_DOUBLE_EQ(vector.second.y(), 0.0); trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(M_PI_2); vector = forwardSimulatedVector(trajectory_point, duration, extra_dist); - EXPECT_DOUBLE_EQ(vector[0].x(), trajectory_point.pose.position.x); - EXPECT_DOUBLE_EQ(vector[0].y(), trajectory_point.pose.position.y); - EXPECT_NEAR(vector[1].x(), 0.0, 1e-9); - EXPECT_DOUBLE_EQ(vector[1].y(), vector_length); + EXPECT_DOUBLE_EQ(vector.first.x(), trajectory_point.pose.position.x); + EXPECT_DOUBLE_EQ(vector.first.y(), trajectory_point.pose.position.y); + EXPECT_NEAR(vector.second.x(), 0.0, 1e-9); + EXPECT_DOUBLE_EQ(vector.second.y(), vector_length); trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(M_PI_4); vector = forwardSimulatedVector(trajectory_point, duration, extra_dist); - EXPECT_DOUBLE_EQ(vector[0].x(), trajectory_point.pose.position.x); - EXPECT_DOUBLE_EQ(vector[0].y(), trajectory_point.pose.position.y); - EXPECT_DOUBLE_EQ(vector[1].x(), std::sqrt(0.5) * vector_length); - EXPECT_DOUBLE_EQ(vector[1].y(), std::sqrt(0.5) * vector_length); + EXPECT_DOUBLE_EQ(vector.first.x(), trajectory_point.pose.position.x); + EXPECT_DOUBLE_EQ(vector.first.y(), trajectory_point.pose.position.y); + EXPECT_DOUBLE_EQ(vector.second.x(), std::sqrt(0.5) * vector_length); + EXPECT_DOUBLE_EQ(vector.second.y(), std::sqrt(0.5) * vector_length); trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(-M_PI_2); vector = forwardSimulatedVector(trajectory_point, duration, extra_dist); - EXPECT_DOUBLE_EQ(vector[0].x(), trajectory_point.pose.position.x); - EXPECT_DOUBLE_EQ(vector[0].y(), trajectory_point.pose.position.y); - EXPECT_NEAR(vector[1].x(), 0.0, 1e-9); - EXPECT_DOUBLE_EQ(vector[1].y(), -vector_length); + EXPECT_DOUBLE_EQ(vector.first.x(), trajectory_point.pose.position.x); + EXPECT_DOUBLE_EQ(vector.first.y(), trajectory_point.pose.position.y); + EXPECT_NEAR(vector.second.x(), 0.0, 1e-9); + EXPECT_DOUBLE_EQ(vector.second.y(), -vector_length); }; // 0 velocity: whatever the duration the vector length is always = to extra_dist @@ -94,99 +94,91 @@ TEST(TestCollisionDistance, distanceToClosestCollision) { using safe_velocity_adjustor::distanceToClosestCollision; using safe_velocity_adjustor::point_t; - using safe_velocity_adjustor::polygon_t; - autoware_auto_planning_msgs::msg::TrajectoryPoint trajectory_point; - trajectory_point.pose.position.x = 0; - trajectory_point.pose.position.y = 0; - trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); - polygon_t footprint; - footprint.outer() = {point_t{0, 1}, point_t{5, 1}, point_t{5, -1}, point_t{0, -1}, point_t{0, 1}}; + using safe_velocity_adjustor::segment_t; + segment_t vector = {point_t{0, 0}, point_t{5, 0}}; + const auto vehicle_width = 2.0; pcl::PointCloud obstacle_points; obstacle_points.points.emplace_back(6, 2, 0); // outside of the footprint { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_FALSE(dist.has_value()); } obstacle_points.points.emplace_back(4, 0, 0); // distance of 4 { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_TRUE(dist.has_value()); EXPECT_DOUBLE_EQ(*dist, 4.0); } obstacle_points.points.emplace_back( 4.5, 0, 0); // distance of 4.5, does not change the minimum distance { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_TRUE(dist.has_value()); EXPECT_DOUBLE_EQ(*dist, 4.0); } obstacle_points.points.emplace_back(2.0, 0.5, 0); // new minumum distance of 2.0 { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_TRUE(dist.has_value()); EXPECT_DOUBLE_EQ(*dist, 2.0); } obstacle_points.points.emplace_back(1.5, -0.75, 0); // new minumum distance of 1.5 { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_TRUE(dist.has_value()); EXPECT_DOUBLE_EQ(*dist, 1.5); } - // Change footprint heading - trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(M_PI_2); - footprint.outer() = { - point_t{-1, 0}, point_t{-1, 5}, point_t{1, 5}, point_t{1, 0}, point_t{-1, 0}}; + // Change vector heading + vector = {point_t{0, 0}, point_t{0, 5}}; obstacle_points.clear(); obstacle_points.emplace_back(-2, 3, 0); // outside of the footprint { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_FALSE(dist.has_value()); } obstacle_points.points.emplace_back(-0.5, 4, 0); // new minumum distance of 4 { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_TRUE(dist.has_value()); EXPECT_DOUBLE_EQ(*dist, 4); } obstacle_points.points.emplace_back(0, 4, 0); // no change in the minimum distance { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_TRUE(dist.has_value()); EXPECT_DOUBLE_EQ(*dist, 4); } obstacle_points.points.emplace_back(0.5, 0.5, 0); // change the minimum distance { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_TRUE(dist.has_value()); EXPECT_DOUBLE_EQ(*dist, 0.5); } - // Change footprint heading - trajectory_point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(M_PI_4); - footprint.outer() = { - point_t{-0.5, 0.5}, point_t{2, 3}, point_t{3, 2}, point_t{0.5, -0.5}, point_t{-0.5, 0.5}}; + // Change vector + vector = {point_t{0, 0}, point_t{2.5, 2.5}}; obstacle_points.clear(); obstacle_points.emplace_back(3, 3, 0); // outside of the footprint { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_FALSE(dist.has_value()); } - obstacle_points.points.emplace_back(2.25, 1.75, 0); // new minumum distance of 4 + obstacle_points.points.emplace_back(2.25, 1.75, 0); // new minumum distance { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_TRUE(dist.has_value()); EXPECT_DOUBLE_EQ(*dist, std::sqrt(8)); } obstacle_points.points.emplace_back(2, 2, 0); // no change in the minimum distance { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_TRUE(dist.has_value()); EXPECT_DOUBLE_EQ(*dist, std::sqrt(8)); } obstacle_points.points.emplace_back(0.25, 0.75, 0); // change the minimum distance { - const auto dist = distanceToClosestCollision(trajectory_point, footprint, obstacle_points); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); ASSERT_TRUE(dist.has_value()); EXPECT_DOUBLE_EQ(*dist, std::sqrt(0.5)); } From d17ccc2ecc2d0c0c2820fcef101bbe2de5925de2 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Thu, 31 Mar 2022 18:47:19 +0900 Subject: [PATCH 26/37] Add filtering of the dynamic objects from the pointcloud --- .../collision_distance.hpp | 51 + .../pointcloud_processing.hpp | 22 +- .../safe_velocity_adjustor_node.hpp | 36 +- .../launch/safe_velocity_adjustor.launch.xml | 2 + .../safe_velocity_adjustor_bag.launch.xml | 2 +- .../launch/safe_velocity_adjustor_bag.rviz | 1574 +++++++++++++++++ planning/safe_velocity_adjustor/package.xml | 1 + 7 files changed, 1681 insertions(+), 7 deletions(-) create mode 100644 planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.rviz diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp index 5efacc43b1666..dc15d9d9d095a 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp @@ -15,11 +15,14 @@ #ifndef SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ #define SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ +#include #include +#include #include #include #include +#include #include #include @@ -30,11 +33,13 @@ #include #include #include +#include namespace safe_velocity_adjustor { namespace bg = boost::geometry; using point_t = bg::model::d2::point_xy; +using polygon_t = bg::model::polygon; using segment_t = bg::model::segment; /// @brief generate a segment to where the vehicle body would be after some duration assuming a @@ -74,6 +79,52 @@ inline std::optional distanceToClosestCollision( } return (min_dist <= bg::length(vector) ? min_dist : std::optional()); } + +inline polygon_t createObjPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size) +{ + // (objects.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions); + // rename + const double x = pose.position.x; + const double y = pose.position.y; + const double h = size.x; + const double w = size.y; + const double yaw = tf2::getYaw(pose.orientation); + + // create base polygon + polygon_t obj_poly; + boost::geometry::exterior_ring(obj_poly) = boost::assign::list_of(h / 2.0, w / 2.0)( + -h / 2.0, w / 2.0)(-h / 2.0, -w / 2.0)(h / 2.0, -w / 2.0)(h / 2.0, w / 2.0); + + // rotate polygon(yaw) + boost::geometry::strategy::transform::rotate_transformer + rotate(-yaw); // anti-clockwise -> :clockwise rotation + polygon_t rotate_obj_poly; + boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); + + // translate polygon(x, y) + boost::geometry::strategy::transform::translate_transformer translate(x, y); + polygon_t translate_obj_poly; + boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); + return translate_obj_poly; +} + +inline std::vector createObjPolygons( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects) +{ + std::vector polygons; + for (const auto & object : objects.objects) + polygons.push_back(createObjPolygon( + object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions)); + return polygons; +} + +inline bool inPolygons(const point_t & point, const std::vector & polygons) +{ + for (const auto & polygon : polygons) + if (bg::distance(point, polygon) < 0.5) return true; + return false; +} } // namespace safe_velocity_adjustor #endif // SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp index ee10588bde34e..0d7905d789335 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp @@ -15,15 +15,18 @@ #ifndef SAFE_VELOCITY_ADJUSTOR__POINTCLOUD_PROCESSING_HPP_ #define SAFE_VELOCITY_ADJUSTOR__POINTCLOUD_PROCESSING_HPP_ +#include "safe_velocity_adjustor/collision_distance.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" #include #include +#include #include #include #include +#include #include #include @@ -67,21 +70,36 @@ inline pcl::PointCloud filterPointCloudByTrajectory( return filtered_pointcloud; } +pcl::PointCloud filterPointCloudByObjects( + const pcl::PointCloud & pointcloud, + const autoware_auto_perception_msgs::msg::PredictedObjects & objects) +{ + pcl::PointCloud filtered_pointcloud; + const auto object_polygons = createObjPolygons(objects); + for (const auto & point : pointcloud.points) { + if (!inPolygons({point.x, point.y}, object_polygons)) + filtered_pointcloud.points.push_back(point); + } + return filtered_pointcloud; +} + /// @brief returns the pointcloud transformed to the trajectory frame and in PCL format with only /// points that are within range of the trajectory inline pcl::PointCloud transformAndFilterPointCloud( const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const sensor_msgs::msg::PointCloud2 & pointcloud, + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, tier4_autoware_utils::TransformListener & transform_listener, const double duration, const double distance) { - // TODO(Maxime CLEMENT): we may need to remove dynamic obstacles from the point cloud const auto & header = pointcloud.header; const auto transform = transform_listener.getTransform( trajectory.header.frame_id, header.frame_id, header.stamp, rclcpp::Duration::from_nanoseconds(0)); const auto obstacle_pointcloud = getTransformedPointCloud(pointcloud, transform->transform); - return filterPointCloudByTrajectory(obstacle_pointcloud, trajectory, duration, distance); + const auto pointcloud_filtered_by_traj = + filterPointCloudByTrajectory(obstacle_pointcloud, trajectory, duration, distance); + return filterPointCloudByObjects(pointcloud_filtered_by_traj, objects); } } // namespace safe_velocity_adjustor diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index a045525ae4859..d4231d88b7506 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -19,15 +19,23 @@ #include "safe_velocity_adjustor/pointcloud_processing.hpp" #include +#include +#include #include #include #include +#include +#include +#include #include #include +#include #include #include +#include +#include #include #include @@ -39,6 +47,7 @@ namespace safe_velocity_adjustor { +using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using sensor_msgs::msg::PointCloud2; @@ -59,10 +68,14 @@ class SafeVelocityAdjustorNode : public rclcpp::Node sub_obstacle_pointcloud_ = create_subscription( "~/input/obstacle_pointcloud", rclcpp::QoS(1).best_effort(), [this](const PointCloud2::ConstSharedPtr msg) { obstacle_pointcloud_ptr_ = msg; }); + sub_objects_ = create_subscription( + "~/input/dynamic_obstacles", 1, + [this](const PredictedObjects::ConstSharedPtr msg) { dynamic_obstacles_ptr_ = msg; }); pub_trajectory_ = create_publisher("~/output/trajectory", 1); pub_debug_markers_ = create_publisher("~/output/debug_markers", 1); + pub_debug_pointcloud_ = create_publisher("~/output/pointcloud", 1); set_param_res_ = add_on_set_parameters_callback([this](const auto & params) { return onParameter(params); }); @@ -74,13 +87,17 @@ class SafeVelocityAdjustorNode : public rclcpp::Node pub_trajectory_; //!< @brief publisher for output trajectory rclcpp::Publisher::SharedPtr pub_debug_markers_; //!< @brief publisher for debug markers + rclcpp::Publisher::SharedPtr + pub_debug_pointcloud_; //!< @brief publisher for filtered pointcloud rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief subscriber for reference trajectory rclcpp::Subscription::SharedPtr sub_obstacle_pointcloud_; //!< @brief subscriber for obstacle pointcloud + rclcpp::Subscription::SharedPtr sub_objects_; // cached inputs PointCloud2::ConstSharedPtr obstacle_pointcloud_ptr_; + PredictedObjects::ConstSharedPtr dynamic_obstacles_ptr_; // parameters Float time_safety_buffer_; @@ -110,15 +127,21 @@ class SafeVelocityAdjustorNode : public rclcpp::Node void onTrajectory(const Trajectory::ConstSharedPtr msg) { - if (!obstacle_pointcloud_ptr_) { - RCLCPP_WARN(get_logger(), "Obstable pointcloud not yet received"); + if (!obstacle_pointcloud_ptr_ || !dynamic_obstacles_ptr_) { + constexpr auto one_sec = rcutils_duration_value_t(10); + if (!obstacle_pointcloud_ptr_) + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), one_sec, "Obstable pointcloud not yet received"); + if (!dynamic_obstacles_ptr_) + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), one_sec, "Dynamic obstable not yet received"); return; } Trajectory safe_trajectory = *msg; const auto extra_vehicle_length = vehicle_length_ / 2 + dist_safety_buffer_; const auto filtered_obstacle_pointcloud = transformAndFilterPointCloud( - safe_trajectory, *obstacle_pointcloud_ptr_, transform_listener_, time_safety_buffer_, - extra_vehicle_length); + safe_trajectory, *obstacle_pointcloud_ptr_, *dynamic_obstacles_ptr_, transform_listener_, + time_safety_buffer_, extra_vehicle_length); for (auto & trajectory_point : safe_trajectory.points) { const auto forward_simulated_vector = @@ -135,6 +158,11 @@ class SafeVelocityAdjustorNode : public rclcpp::Node safe_trajectory.header.stamp = now(); pub_trajectory_->publish(safe_trajectory); publishDebug(*msg, safe_trajectory); + PointCloud2 ros_pointcloud; + pcl::toROSMsg(filtered_obstacle_pointcloud, ros_pointcloud); + ros_pointcloud.header.stamp = now(); + ros_pointcloud.header.frame_id = msg->header.frame_id; + pub_debug_pointcloud_->publish(ros_pointcloud); } Float calculateSafeVelocity( diff --git a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml index a3ebbbe0c8d2e..30dfe1cd50310 100644 --- a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml +++ b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml @@ -2,6 +2,7 @@ + @@ -18,6 +19,7 @@ + diff --git a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.launch.xml b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.launch.xml index aa8f7f144c5ce..b5ab923f8c0ea 100644 --- a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.launch.xml +++ b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.launch.xml @@ -57,7 +57,7 @@ - + diff --git a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.rviz b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.rviz new file mode 100644 index 0000000000000..791fb66a7f60d --- /dev/null +++ b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.rviz @@ -0,0 +1,1574 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Map1 + - /Map1/PointCloudMap1 + - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 + - /Perception1 + - /Planning1 + - /Planning1/ScenarioPlanning1 + - /MarkerArray1 + - /FilteredPointCloud1 + - /FilteredPointCloud1/Status1 + Splitter Ratio: 0.557669460773468 + Tree Height: 289 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: ~ + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel + Name: InitialPoseButtonPanel + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: FilteredPointCloud +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/SteeringAngle + Enabled: true + Left: 128 + Length: 256 + Name: SteeringAngle + Scale: 17 + Text Color: 25; 255; 240 + Top: 128 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/steering_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Class: rviz_plugins/ConsoleMeter + Enabled: true + Left: 512 + Length: 256 + Name: ConsoleMeter + Text Color: 25; 255; 240 + Top: 128 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Alpha: 0.9990000128746033 + Class: rviz_plugins/VelocityHistory + Color Border Vel Max: 3 + Constant Color: + Color: 255; 255; 255 + Value: true + Enabled: true + Name: VelocityHistory + Scale: 0.30000001192092896 + Timeout: 10 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera0/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera0/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera1/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera1/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera2/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera2/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera3/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera3/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera4/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera4/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera5/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera5/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + gnss_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + livox_front_left: + Alpha: 1 + Show Axes: false + Show Trail: false + livox_front_left_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + livox_front_right: + Alpha: 1 + Show Axes: false + Show Trail: false + livox_front_right_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_kit_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + tamagawa/imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + traffic_light_left_camera/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + traffic_light_left_camera/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + traffic_light_right_camera/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + traffic_light_right_camera/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + velodyne_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_left_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_rear: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_rear_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_right_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_top_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: VehicleModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_plugins/PolarGridDisplay + Color: 255; 255; 255 + Delta Range: 10 + Enabled: true + Max Alpha: 0.5 + Max Range: 100 + Max Wave Alpha: 0.5 + Min Alpha: 0.009999999776482582 + Min Wave Alpha: 0.009999999776482582 + Name: PolarGridDisplay + Reference Frame: base_link + Value: true + Wave Color: 255; 255; 255 + Wave Velocity: 40 + - Class: rviz_plugins/MaxVelocity + Enabled: true + Left: 595 + Length: 96 + Name: MaxVelocity + Text Color: 255; 255; 255 + Top: 280 + Topic: /planning/scenario_planning/current_max_velocity + Value: true + Value Scale: 0.25 + - Class: rviz_plugins/TurnSignal + Enabled: true + Height: 256 + Left: 196 + Name: TurnSignal + Top: 350 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/turn_indicators_status + Value: true + Width: 512 + Enabled: true + Name: Vehicle + Enabled: true + Name: System + - Class: rviz_common/Group + Displays: + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 28.71826171875 + Min Value: -7.4224700927734375 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 237 + Min Color: 211; 215; 207 + Min Intensity: 0 + Name: PointCloudMap + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/pointcloud_map + Use Fixed Frame: true + Use rainbow: false + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Lanelet2VectorMap + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/vector_map_marker + Value: true + Enabled: true + Name: Map + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.4000000059604645 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5 + Min Value: -1 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ConcatenatePointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/lidar/concatenated/pointcloud + Use Fixed Frame: false + Use rainbow: true + Value: true + - Alpha: 0.9990000128746033 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: MeasurementRange + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/lidar/crop_box_filter/crop_box_polygon + Value: false + Enabled: true + Name: LiDAR + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 233; 185; 110 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.20000000298023224 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.699999988079071 + Head Radius: 1.2000000476837158 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.5 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/gnss/pose_with_covariance + Value: true + Enabled: false + Name: GNSS + Enabled: false + Name: Sensing + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 170; 255 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovInitial + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/initial_pose_with_covariance + Value: false + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 255; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovAligned + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose_with_covariance + Value: false + - Buffer Size: 200 + Class: rviz_plugins::PoseHistory + Enabled: false + Line: + Alpha: 0.9990000128746033 + Color: 170; 255; 127 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Initial + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.019999999552965164 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/util/downsample/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 85; 255; 127 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Aligned + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.019999999552965164 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/points_aligned + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MonteCarloInitialPose + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/monte_carlo_initial_pose_marker + Value: true + Enabled: true + Name: NDT + - Class: rviz_common/Group + Displays: + - Buffer Size: 1000 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 0; 255; 255 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_twist_fusion_filter/pose + Value: true + Enabled: true + Name: EKF + Enabled: false + Name: Localization + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 15 + Min Value: -2 + Value: false + Axis: Z + Channel Name: z + Class: rviz_default_plugins/PointCloud2 + Color: 200; 200; 200 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 15 + Min Color: 0; 0; 0 + Min Intensity: -5 + Name: NoGroundPointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/obstacle_segmentation/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Segmentation + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/DetectedObjects + Display 3d polygon: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: DetectedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /perception/object_recognition/detection/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Enabled: false + Name: Detection + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/TrackedObjects + Display 3d polygon: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: TrackedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /perception/object_recognition/tracking/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Enabled: false + Name: Tracking + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/PredictedObjects + Display 3d polygon: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: PredictedObjects + Namespaces: + label: true + path: true + path confidence: true + position covariance: true + shape: true + twist: true + uuid: true + velocity: true + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /perception/object_recognition/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Enabled: true + Name: Prediction + Enabled: true + Name: ObjectRecognition + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RecognitionResultOnImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /perception/traffic_light_recognition/debug/rois + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MapBasedDetectionResult + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers + Value: true + Enabled: false + Name: TrafficLight + - Class: rviz_common/Group + Displays: + - Alpha: 0.5 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /perception/occupancy_grid_map/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /perception/occupancy_grid_map/map_updates + Use Timestamp: false + Value: true + Enabled: false + Name: OccupancyGrid + Enabled: true + Name: Perception + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: RouteArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/route_marker + Value: true + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.30000001192092896 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.5 + Name: GoalPose + Shaft Length: 3 + Shaft Radius: 0.20000000298023224 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/echo_back_goal_pose + Value: true + Enabled: false + Name: MissionPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: ScenarioTrajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/trajectory + Value: true + View Path: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: Path + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/path + Value: true + View Path: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Velocity: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Arrow + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Crosswalk + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Intersection + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MergeFromPrivate + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/merge_from_private + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Blind Spot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: TrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualTrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: StopLine + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: DetectionArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: OcclusionSpot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: NoStoppingArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area + Value: true + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/path_candidate + Value: true + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: true + Value: true + Width: 2 + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + Enabled: true + Name: BehaviorPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/trajectory + Value: false + View Path: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ObstacleStop + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: SurroundObstacleCheck + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ObstacleAvoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ObstacleAvoidanceWall + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/wall_marker + Value: true + Enabled: true + Name: MotionPlanning + Enabled: false + Name: LaneDriving + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Costmap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates + Use Timestamp: false + Value: false + - Alpha: 0.9990000128746033 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PartialPoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (3D) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array + Value: true + - Alpha: 0.9990000128746033 + Arrow Length: 0.5 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 0; 255 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array + Value: true + Enabled: false + Name: Parking + Enabled: true + Name: ScenarioPlanning + Enabled: true + Name: Planning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: Predicted Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/lateral/predicted_trajectory + Value: true + View Path: + Alpha: 1 + Color: 255; 255; 255 + Constant Color: true + Value: true + Width: 0.05000000074505806 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/MPC + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/mpc_follower/debug/markers + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/PurePursuit + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/pure_pursuit_node_exe/debug/markers + Value: false + Enabled: false + Name: Control + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + adjusted: true + original: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/safe_velocity_adjustor/debug_markers + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 103; 103; 103 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: FilteredPointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /safe_velocity_adjustor/output/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 10; 10; 10 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/goal + - Class: rviz_plugins/PedestrianInitialPoseTool + Interactive: false + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 1 + Z std deviation: 0.029999999329447746 + - Class: rviz_plugins/CarInitialPoseTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Theta std deviation: 0.0872664600610733 + Velocity: 3 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Class: rviz_plugins/MissionCheckpointTool + Pose Topic: /planning/mission_planning/checkpoint + Theta std deviation: 0.2617993950843811 + X std deviation: 0.5 + Y std deviation: 0.5 + Z position: 0 + - Class: rviz_plugins/DeleteAllObjectsTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 1.4263962507247925 + Target Frame: base_link + Value: TopDownOrtho (rviz_default_plugins) + X: 7.768266201019287 + Y: 6.952993869781494 + Saved: + - Class: rviz_default_plugins/ThirdPersonFollower + Distance: 18 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 0.20000000298023224 + Target Frame: base_link + Value: ThirdPersonFollower (rviz) + Yaw: 3.141592025756836 + - Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: TopDownOrtho + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 +Window Geometry: + Displays: + collapsed: false + Height: 1023 + Hide Left Dock: false + Hide Right Dock: false + InitialPoseButtonPanel: + collapsed: false + QMainWindow State: 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 + RecognitionResultOnImage: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 0 diff --git a/planning/safe_velocity_adjustor/package.xml b/planning/safe_velocity_adjustor/package.xml index 9fe22e89dedd0..e3d556f465704 100644 --- a/planning/safe_velocity_adjustor/package.xml +++ b/planning/safe_velocity_adjustor/package.xml @@ -11,6 +11,7 @@ ament_cmake_auto eigen3_cmake_module + autoware_auto_perception_msgs autoware_auto_planning_msgs eigen libboost-dev From 946481ff2af15acf37317d9108b5f4e38d46985c Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Thu, 31 Mar 2022 22:08:14 +0900 Subject: [PATCH 27/37] [DEBUG] Print runtimes of expensive functions --- .../collision_distance.hpp | 2 ++ .../pointcloud_processing.hpp | 11 ++++++++++- .../safe_velocity_adjustor_node.hpp | 18 ++++++++++++++++++ 3 files changed, 30 insertions(+), 1 deletion(-) diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp index dc15d9d9d095a..c88b261a2a224 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp @@ -15,6 +15,8 @@ #ifndef SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ #define SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ +#include + #include #include diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp index 0d7905d789335..b3076d724c2f5 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -92,14 +93,22 @@ inline pcl::PointCloud transformAndFilterPointCloud( tier4_autoware_utils::TransformListener & transform_listener, const double duration, const double distance) { + tier4_autoware_utils::StopWatch stopwatch; const auto & header = pointcloud.header; const auto transform = transform_listener.getTransform( trajectory.header.frame_id, header.frame_id, header.stamp, rclcpp::Duration::from_nanoseconds(0)); const auto obstacle_pointcloud = getTransformedPointCloud(pointcloud, transform->transform); + std::cerr << "* pointcloud transform = " << stopwatch.toc() << " s\n"; + stopwatch.tic("filterByTraj"); const auto pointcloud_filtered_by_traj = filterPointCloudByTrajectory(obstacle_pointcloud, trajectory, duration, distance); - return filterPointCloudByObjects(pointcloud_filtered_by_traj, objects); + std::cerr << "* pointcloud filter traj = " << stopwatch.toc("filterByTraj") << " s\n"; + stopwatch.tic("filterByObj"); + auto final_pointcloud = filterPointCloudByObjects(pointcloud_filtered_by_traj, objects); + std::cerr << "* pointcloud filter obj = " << stopwatch.toc("filterByObj") << " s\n"; + std::cerr << "** pointcloud total = " << stopwatch.toc() << " s\n"; + return final_pointcloud; } } // namespace safe_velocity_adjustor diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index d4231d88b7506..92d55fb280dbb 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -137,23 +138,40 @@ class SafeVelocityAdjustorNode : public rclcpp::Node get_logger(), *get_clock(), one_sec, "Dynamic obstable not yet received"); return; } + + double pointcloud_d{}; + double vector_d{}; + double dist_d{}; + double vel_d{}; + tier4_autoware_utils::StopWatch stopwatch; Trajectory safe_trajectory = *msg; const auto extra_vehicle_length = vehicle_length_ / 2 + dist_safety_buffer_; + stopwatch.tic("pointcloud_d"); const auto filtered_obstacle_pointcloud = transformAndFilterPointCloud( safe_trajectory, *obstacle_pointcloud_ptr_, *dynamic_obstacles_ptr_, transform_listener_, time_safety_buffer_, extra_vehicle_length); + pointcloud_d += stopwatch.toc("pointcloud_d"); for (auto & trajectory_point : safe_trajectory.points) { + stopwatch.tic("vector_d"); const auto forward_simulated_vector = forwardSimulatedVector(trajectory_point, time_safety_buffer_, extra_vehicle_length); + vector_d += stopwatch.toc("vector_d"); + stopwatch.tic("dist_d"); const auto dist_to_collision = distanceToClosestCollision( forward_simulated_vector, vehicle_width_, filtered_obstacle_pointcloud); + dist_d += stopwatch.toc("dist_d"); if (dist_to_collision) { + stopwatch.tic("vel_d"); trajectory_point.longitudinal_velocity_mps = calculateSafeVelocity( trajectory_point, std::max({}, static_cast(*dist_to_collision - extra_vehicle_length))); + vel_d += stopwatch.toc("vel_d"); } } + RCLCPP_WARN(get_logger(), "pointcloud = %2.2fs", pointcloud_d); + RCLCPP_WARN(get_logger(), "dist = %2.2fs", dist_d); + RCLCPP_WARN(get_logger(), "*** Total = %2.2fs", stopwatch.toc()); safe_trajectory.header.stamp = now(); pub_trajectory_->publish(safe_trajectory); From 9e705bf708abb586effd47b126a725511cfd7bd6 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 1 Apr 2022 17:08:25 +0900 Subject: [PATCH 28/37] Add trajectory downsampling to boost performance + improve debug markers --- .../default_safe_velocity_adjustor.param.yaml | 3 +- .../pointcloud_processing.hpp | 1 + .../safe_velocity_adjustor_node.hpp | 55 +++++++++------- .../launch/safe_velocity_adjustor_bag.rviz | 65 ++++++++----------- 4 files changed, 63 insertions(+), 61 deletions(-) diff --git a/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml b/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml index a3f65f5754506..63ecce55c2f34 100644 --- a/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml +++ b/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml @@ -1,4 +1,5 @@ /**: ros__parameters: - time_safety_buffer: 0.5 # [s] required time without collision in forward simulation + time_safety_buffer: 1.5 # [s] required time without collision in forward simulation dist_safety_buffer: 1.5 # [m] distance from obstacles to consider a collision + downsample_factor: 10 # factor by which to downsample the input trajectory for calculation diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp index b3076d724c2f5..7e205fab72a60 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp @@ -109,6 +109,7 @@ inline pcl::PointCloud transformAndFilterPointCloud( std::cerr << "* pointcloud filter obj = " << stopwatch.toc("filterByObj") << " s\n"; std::cerr << "** pointcloud total = " << stopwatch.toc() << " s\n"; return final_pointcloud; + // return pointcloud_filtered_by_traj; } } // namespace safe_velocity_adjustor diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index 92d55fb280dbb..cd1d7df7d86a6 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -18,7 +18,6 @@ #include "safe_velocity_adjustor/collision_distance.hpp" #include "safe_velocity_adjustor/pointcloud_processing.hpp" -#include #include #include #include @@ -26,13 +25,12 @@ #include #include -#include -#include #include #include #include -#include +#include #include +#include #include #include @@ -63,6 +61,7 @@ class SafeVelocityAdjustorNode : public rclcpp::Node { time_safety_buffer_ = static_cast(declare_parameter("time_safety_buffer")); dist_safety_buffer_ = static_cast(declare_parameter("dist_safety_buffer")); + downsample_factor_ = static_cast(declare_parameter("downsample_factor")); sub_trajectory_ = create_subscription( "~/input/trajectory", 1, [this](const Trajectory::ConstSharedPtr msg) { onTrajectory(msg); }); @@ -103,6 +102,7 @@ class SafeVelocityAdjustorNode : public rclcpp::Node // parameters Float time_safety_buffer_; Float dist_safety_buffer_; + int downsample_factor_; // TODO(Maxime CLEMENT): get vehicle width and length from vehicle parameters Float vehicle_width_ = 2.0; Float vehicle_length_ = 4.0; @@ -119,6 +119,9 @@ class SafeVelocityAdjustorNode : public rclcpp::Node } else if (parameter.get_name() == "dist_safety_buffer") { dist_safety_buffer_ = static_cast(parameter.as_double()); result.successful = true; + } else if (parameter.get_name() == "downsample_factor") { + downsample_factor_ = static_cast(parameter.as_int()); + result.successful = true; } else { RCLCPP_WARN(get_logger(), "Unknown parameter %s", parameter.get_name().c_str()); } @@ -139,6 +142,13 @@ class SafeVelocityAdjustorNode : public rclcpp::Node return; } + // Downsample trajectory + Trajectory in_traj; + in_traj.header = msg->header; + in_traj.points.reserve(msg->points.size() / downsample_factor_); + for (size_t i = 0; i < msg->points.size(); i += downsample_factor_) + in_traj.points.push_back(msg->points[i]); + double pointcloud_d{}; double vector_d{}; double dist_d{}; @@ -148,11 +158,11 @@ class SafeVelocityAdjustorNode : public rclcpp::Node const auto extra_vehicle_length = vehicle_length_ / 2 + dist_safety_buffer_; stopwatch.tic("pointcloud_d"); const auto filtered_obstacle_pointcloud = transformAndFilterPointCloud( - safe_trajectory, *obstacle_pointcloud_ptr_, *dynamic_obstacles_ptr_, transform_listener_, + in_traj, *obstacle_pointcloud_ptr_, *dynamic_obstacles_ptr_, transform_listener_, time_safety_buffer_, extra_vehicle_length); pointcloud_d += stopwatch.toc("pointcloud_d"); - for (auto & trajectory_point : safe_trajectory.points) { + for (auto & trajectory_point : in_traj.points) { stopwatch.tic("vector_d"); const auto forward_simulated_vector = forwardSimulatedVector(trajectory_point, time_safety_buffer_, extra_vehicle_length); @@ -169,9 +179,13 @@ class SafeVelocityAdjustorNode : public rclcpp::Node vel_d += stopwatch.toc("vel_d"); } } + + for (size_t i = 0; i < safe_trajectory.points.size(); ++i) { + safe_trajectory.points[i].longitudinal_velocity_mps = + in_traj.points[i / downsample_factor_].longitudinal_velocity_mps; + } RCLCPP_WARN(get_logger(), "pointcloud = %2.2fs", pointcloud_d); RCLCPP_WARN(get_logger(), "dist = %2.2fs", dist_d); - RCLCPP_WARN(get_logger(), "*** Total = %2.2fs", stopwatch.toc()); safe_trajectory.header.stamp = now(); pub_trajectory_->publish(safe_trajectory); @@ -179,8 +193,10 @@ class SafeVelocityAdjustorNode : public rclcpp::Node PointCloud2 ros_pointcloud; pcl::toROSMsg(filtered_obstacle_pointcloud, ros_pointcloud); ros_pointcloud.header.stamp = now(); - ros_pointcloud.header.frame_id = msg->header.frame_id; + ros_pointcloud.header.frame_id = in_traj.header.frame_id; pub_debug_pointcloud_->publish(ros_pointcloud); + + RCLCPP_WARN(get_logger(), "*** Total = %2.2fs", stopwatch.toc()); } Float calculateSafeVelocity( @@ -191,8 +207,7 @@ class SafeVelocityAdjustorNode : public rclcpp::Node static_cast(dist_to_collision / time_safety_buffer_)); } - static visualization_msgs::msg::Marker makeEnvelopeMarker( - const Trajectory & trajectory, const Float time_safety_buffer, const Float dist_safety_buffer) + visualization_msgs::msg::Marker makeEnvelopeMarker(const Trajectory & trajectory) const { visualization_msgs::msg::Marker envelope; envelope.header = trajectory.header; @@ -200,14 +215,12 @@ class SafeVelocityAdjustorNode : public rclcpp::Node envelope.scale.x = 0.1; envelope.color.a = 1.0; for (const auto & point : trajectory.points) { - const auto heading = tf2::getYaw(point.pose.orientation); - auto p = point.pose.position; - p.x += static_cast( - point.longitudinal_velocity_mps * std::cos(heading) * time_safety_buffer) + - dist_safety_buffer; - p.y += static_cast( - point.longitudinal_velocity_mps * std::sin(heading) * time_safety_buffer) + - dist_safety_buffer; + const auto vector = forwardSimulatedVector( + point, time_safety_buffer_, dist_safety_buffer_ + vehicle_length_ / 2); + geometry_msgs::msg::Point p; + p.x = vector.second.x(); + p.y = vector.second.y(); + p.z = point.pose.position.z; envelope.points.push_back(p); } return envelope; @@ -217,13 +230,11 @@ class SafeVelocityAdjustorNode : public rclcpp::Node const Trajectory & original_trajectory, const Trajectory & adjusted_trajectory) const { visualization_msgs::msg::MarkerArray debug_markers; - auto original_envelope = - makeEnvelopeMarker(original_trajectory, time_safety_buffer_, dist_safety_buffer_); + auto original_envelope = makeEnvelopeMarker(original_trajectory); original_envelope.color.r = 1.0; original_envelope.ns = "original"; debug_markers.markers.push_back(original_envelope); - auto adjusted_envelope = - makeEnvelopeMarker(adjusted_trajectory, time_safety_buffer_, dist_safety_buffer_); + auto adjusted_envelope = makeEnvelopeMarker(adjusted_trajectory); adjusted_envelope.color.g = 1.0; adjusted_envelope.ns = "adjusted"; debug_markers.markers.push_back(adjusted_envelope); diff --git a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.rviz b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.rviz index 791fb66a7f60d..5a272cda5134b 100644 --- a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.rviz +++ b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.rviz @@ -4,15 +4,11 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Map1 - - /Map1/PointCloudMap1 + - /System1/Vehicle1 - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - /Perception1 - - /Planning1 - - /Planning1/ScenarioPlanning1 - - /MarkerArray1 - - /FilteredPointCloud1 - - /FilteredPointCloud1/Status1 + - /Perception1/ObjectRecognition1 + - /Perception1/ObjectRecognition1/Prediction1 Splitter Ratio: 0.557669460773468 Tree Height: 289 - Class: rviz_common/Selection @@ -73,7 +69,7 @@ Visualization Manager: - Class: rviz_common/Group Displays: - Class: rviz_plugins/SteeringAngle - Enabled: true + Enabled: false Left: 128 Length: 256 Name: SteeringAngle @@ -86,11 +82,11 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /vehicle/status/steering_status - Value: true + Value: false Value Scale: 0.14999249577522278 Value height offset: 0 - Class: rviz_plugins/ConsoleMeter - Enabled: true + Enabled: false Left: 512 Length: 256 Name: ConsoleMeter @@ -102,7 +98,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /vehicle/status/velocity_status - Value: true + Value: false Value Scale: 0.14999249577522278 Value height offset: 0 - Alpha: 0.9990000128746033 @@ -297,7 +293,7 @@ Visualization Manager: - Class: rviz_plugins/PolarGridDisplay Color: 255; 255; 255 Delta Range: 10 - Enabled: true + Enabled: false Max Alpha: 0.5 Max Range: 100 Max Wave Alpha: 0.5 @@ -305,21 +301,21 @@ Visualization Manager: Min Wave Alpha: 0.009999999776482582 Name: PolarGridDisplay Reference Frame: base_link - Value: true + Value: false Wave Color: 255; 255; 255 Wave Velocity: 40 - Class: rviz_plugins/MaxVelocity - Enabled: true + Enabled: false Left: 595 Length: 96 Name: MaxVelocity Text Color: 255; 255; 255 Top: 280 Topic: /planning/scenario_planning/current_max_velocity - Value: true + Value: false Value Scale: 0.25 - Class: rviz_plugins/TurnSignal - Enabled: true + Enabled: false Height: 256 Left: 196 Name: TurnSignal @@ -330,7 +326,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /vehicle/status/turn_indicators_status - Value: true + Value: false Width: 512 Enabled: true Name: Vehicle @@ -827,27 +823,20 @@ Visualization Manager: Color: 119; 11; 32 Class: autoware_auto_perception_rviz_plugin/PredictedObjects Display 3d polygon: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true + Display Label: false + Display PoseWithCovariance: false + Display Predicted Path Confidence: false + Display Predicted Paths: false + Display Twist: false + Display UUID: false + Display Velocity: false Enabled: true MOTORCYCLE: Alpha: 0.9990000128746033 Color: 119; 11; 32 Name: PredictedObjects Namespaces: - label: true - path: true - path confidence: true - position covariance: true - shape: true - twist: true - uuid: true - velocity: true + {} PEDESTRIAN: Alpha: 0.9990000128746033 Color: 255; 192; 203 @@ -971,7 +960,7 @@ Visualization Manager: Displays: - Class: rviz_plugins/Trajectory Color Border Vel Max: 3 - Enabled: true + Enabled: false Name: ScenarioTrajectory Topic: Depth: 5 @@ -980,7 +969,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /planning/scenario_planning/trajectory - Value: true + Value: false View Path: Alpha: 0.9990000128746033 Color: 0; 0; 0 @@ -1332,7 +1321,7 @@ Visualization Manager: Name: Parking Enabled: true Name: ScenarioPlanning - Enabled: true + Enabled: false Name: Planning - Class: rviz_common/Group Displays: @@ -1509,11 +1498,11 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 1.4263962507247925 + Scale: 12.543999671936035 Target Frame: base_link Value: TopDownOrtho (rviz_default_plugins) - X: 7.768266201019287 - Y: 6.952993869781494 + X: 0 + Y: 0 Saved: - Class: rviz_default_plugins/ThirdPersonFollower Distance: 18 From 9252d9e1f0c9c99721c1f38a985255d060453a98 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 5 Apr 2022 13:47:22 +0900 Subject: [PATCH 29/37] Modify velocity only from ego pose + distance parameter --- .../default_safe_velocity_adjustor.param.yaml | 1 + .../safe_velocity_adjustor_node.hpp | 63 ++++++++++++++----- 2 files changed, 47 insertions(+), 17 deletions(-) diff --git a/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml b/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml index 63ecce55c2f34..f444f7193e339 100644 --- a/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml +++ b/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml @@ -2,4 +2,5 @@ ros__parameters: time_safety_buffer: 1.5 # [s] required time without collision in forward simulation dist_safety_buffer: 1.5 # [m] distance from obstacles to consider a collision + start_distance: 2.0 # [m] distance ahead of ego from which to start modifying the trajectory downsample_factor: 10 # factor by which to downsample the input trajectory for calculation diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index cd1d7df7d86a6..05540e157dcc9 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -17,18 +17,20 @@ #include "safe_velocity_adjustor/collision_distance.hpp" #include "safe_velocity_adjustor/pointcloud_processing.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include #include #include +#include #include #include +#include #include #include #include -#include #include #include #include @@ -57,10 +59,11 @@ class SafeVelocityAdjustorNode : public rclcpp::Node { public: explicit SafeVelocityAdjustorNode(const rclcpp::NodeOptions & node_options) - : rclcpp::Node("safe_velocity_adjustor", node_options), transform_listener_(this) + : rclcpp::Node("safe_velocity_adjustor", node_options) { time_safety_buffer_ = static_cast(declare_parameter("time_safety_buffer")); dist_safety_buffer_ = static_cast(declare_parameter("dist_safety_buffer")); + start_distance_ = static_cast(declare_parameter("start_distance")); downsample_factor_ = static_cast(declare_parameter("downsample_factor")); sub_trajectory_ = create_subscription( @@ -79,10 +82,13 @@ class SafeVelocityAdjustorNode : public rclcpp::Node set_param_res_ = add_on_set_parameters_callback([this](const auto & params) { return onParameter(params); }); + + self_pose_listener_.waitForFirstPose(); } private: - tier4_autoware_utils::TransformListener transform_listener_; + tier4_autoware_utils::TransformListener transform_listener_{this}; + tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; rclcpp::Publisher::SharedPtr pub_trajectory_; //!< @brief publisher for output trajectory rclcpp::Publisher::SharedPtr @@ -102,6 +108,7 @@ class SafeVelocityAdjustorNode : public rclcpp::Node // parameters Float time_safety_buffer_; Float dist_safety_buffer_; + Float start_distance_; int downsample_factor_; // TODO(Maxime CLEMENT): get vehicle width and length from vehicle parameters Float vehicle_width_ = 2.0; @@ -122,6 +129,9 @@ class SafeVelocityAdjustorNode : public rclcpp::Node } else if (parameter.get_name() == "downsample_factor") { downsample_factor_ = static_cast(parameter.as_int()); result.successful = true; + } else if (parameter.get_name() == "start_distance") { + start_distance_ = static_cast(parameter.as_double()); + result.successful = true; } else { RCLCPP_WARN(get_logger(), "Unknown parameter %s", parameter.get_name().c_str()); } @@ -131,23 +141,29 @@ class SafeVelocityAdjustorNode : public rclcpp::Node void onTrajectory(const Trajectory::ConstSharedPtr msg) { - if (!obstacle_pointcloud_ptr_ || !dynamic_obstacles_ptr_) { - constexpr auto one_sec = rcutils_duration_value_t(10); + const auto ego_idx = tier4_autoware_utils::findNearestIndex( + msg->points, self_pose_listener_.getCurrentPose()->pose); + if (!obstacle_pointcloud_ptr_ || !dynamic_obstacles_ptr_ || !ego_idx) { + constexpr auto one_sec = rcutils_duration_value_t(1000); if (!obstacle_pointcloud_ptr_) RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), one_sec, "Obstable pointcloud not yet received"); if (!dynamic_obstacles_ptr_) RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), one_sec, "Dynamic obstable not yet received"); + if (!ego_idx) + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), one_sec, "Cannot calculate ego index on the trajectory"); return; } - + const auto start_idx = calculateStartIndex(*msg, *ego_idx, start_distance_); // Downsample trajectory - Trajectory in_traj; - in_traj.header = msg->header; - in_traj.points.reserve(msg->points.size() / downsample_factor_); - for (size_t i = 0; i < msg->points.size(); i += downsample_factor_) - in_traj.points.push_back(msg->points[i]); + const size_t downsample_step = downsample_factor_; + Trajectory downsampled_traj; + downsampled_traj.header = msg->header; + downsampled_traj.points.reserve(msg->points.size() / downsample_step); + for (size_t i = start_idx; i < msg->points.size(); i += downsample_step) + downsampled_traj.points.push_back(msg->points[i]); double pointcloud_d{}; double vector_d{}; @@ -158,11 +174,11 @@ class SafeVelocityAdjustorNode : public rclcpp::Node const auto extra_vehicle_length = vehicle_length_ / 2 + dist_safety_buffer_; stopwatch.tic("pointcloud_d"); const auto filtered_obstacle_pointcloud = transformAndFilterPointCloud( - in_traj, *obstacle_pointcloud_ptr_, *dynamic_obstacles_ptr_, transform_listener_, + downsampled_traj, *obstacle_pointcloud_ptr_, *dynamic_obstacles_ptr_, transform_listener_, time_safety_buffer_, extra_vehicle_length); pointcloud_d += stopwatch.toc("pointcloud_d"); - for (auto & trajectory_point : in_traj.points) { + for (auto & trajectory_point : downsampled_traj.points) { stopwatch.tic("vector_d"); const auto forward_simulated_vector = forwardSimulatedVector(trajectory_point, time_safety_buffer_, extra_vehicle_length); @@ -180,9 +196,9 @@ class SafeVelocityAdjustorNode : public rclcpp::Node } } - for (size_t i = 0; i < safe_trajectory.points.size(); ++i) { - safe_trajectory.points[i].longitudinal_velocity_mps = - in_traj.points[i / downsample_factor_].longitudinal_velocity_mps; + for (size_t i = 0; i < downsampled_traj.points.size(); ++i) { + safe_trajectory.points[start_idx + i * downsample_step].longitudinal_velocity_mps = + downsampled_traj.points[i].longitudinal_velocity_mps; } RCLCPP_WARN(get_logger(), "pointcloud = %2.2fs", pointcloud_d); RCLCPP_WARN(get_logger(), "dist = %2.2fs", dist_d); @@ -193,7 +209,7 @@ class SafeVelocityAdjustorNode : public rclcpp::Node PointCloud2 ros_pointcloud; pcl::toROSMsg(filtered_obstacle_pointcloud, ros_pointcloud); ros_pointcloud.header.stamp = now(); - ros_pointcloud.header.frame_id = in_traj.header.frame_id; + ros_pointcloud.header.frame_id = downsampled_traj.header.frame_id; pub_debug_pointcloud_->publish(ros_pointcloud); RCLCPP_WARN(get_logger(), "*** Total = %2.2fs", stopwatch.toc()); @@ -241,6 +257,19 @@ class SafeVelocityAdjustorNode : public rclcpp::Node pub_debug_markers_->publish(debug_markers); } + + static size_t calculateStartIndex( + const Trajectory & trajectory, const size_t ego_idx, const Float start_distance) + { + auto dist = 0.0; + auto idx = ego_idx; + while (idx + 1 < trajectory.points.size() && dist < start_distance) { + dist += + tier4_autoware_utils::calcDistance2d(trajectory.points[idx], trajectory.points[idx + 1]); + ++idx; + } + return idx; + } }; } // namespace safe_velocity_adjustor From 707e8e1574e714286c6c02dcd5cfce6b00c08755 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 6 Apr 2022 16:17:32 +0900 Subject: [PATCH 30/37] Add 1st Eigen version of distanceToClosestCollision + benchmark --- .../collision_distance.hpp | 44 ++++++++++++++ .../test/test_collision_distance.cpp | 60 ++++++++++++++++++- 2 files changed, 102 insertions(+), 2 deletions(-) diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp index c88b261a2a224..2f91bbecd3fb2 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp @@ -15,6 +15,7 @@ #ifndef SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ #define SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ +#include #include #include @@ -28,6 +29,7 @@ #include #include +#include #include #include #include @@ -59,6 +61,48 @@ inline segment_t forwardSimulatedVector( return segment_t{from, to}; } +static double a{}; +static double b{}; +static double c{}; +static double d{}; +static double e{}; +static double f{}; +/// @brief calculate the distance to the closest obstacle point colliding with the footprint +inline std::optional distanceToClosestCollision_Eigen( + const segment_t & vector, const double vehicle_width, + const pcl::PointCloud & obstacle_points) +{ + tier4_autoware_utils::StopWatch watch; + watch.tic("a"); + Eigen::ArrayX2d dist_array(obstacle_points.size(), 2); + const auto dist_matrix = obstacle_points.getMatrixXfMap(3, 4, 0).cast(); + dist_array.col(0) = dist_matrix.row(0).array() - vector.first.x(); + dist_array.col(1) = dist_matrix.row(1).array() - vector.first.y(); + a += watch.toc("a"); + watch.tic("b"); + auto collision_headings = Eigen::ArrayXd(dist_array.rows()); + for (auto row_idx = 0; row_idx < dist_array.rows(); ++row_idx) + collision_headings(row_idx) = std::atan2(dist_array(row_idx, 1), dist_array(row_idx, 0)); + b += watch.toc("b"); + watch.tic("c"); + const auto traj_heading = + std::atan2(vector.second.y() - vector.first.y(), vector.second.x() - vector.first.x()); + c += watch.toc("c"); + watch.tic("d"); + const auto angles = traj_heading - collision_headings; + const auto hypot_lengths = + (dist_array.col(0).array().square() + dist_array.col(1).array().square()).sqrt(); + d += watch.toc("d"); + watch.tic("e"); + const auto long_dists = angles.cos().abs() * hypot_lengths; + const auto lat_dists = (hypot_lengths.square() - long_dists.square()).sqrt(); + e += watch.toc("e"); + watch.tic("f"); + const auto min_dist = + (long_dists + bg::length(vector) * (lat_dists > vehicle_width / 2).cast()).minCoeff(); + f += watch.toc("f"); + return (min_dist <= bg::length(vector) ? min_dist : std::optional()); +} /// @brief calculate the distance to the closest obstacle point colliding with the footprint inline std::optional distanceToClosestCollision( const segment_t & vector, const double vehicle_width, diff --git a/planning/safe_velocity_adjustor/test/test_collision_distance.cpp b/planning/safe_velocity_adjustor/test/test_collision_distance.cpp index 587b7477c019c..625ccf1c8fc7d 100644 --- a/planning/safe_velocity_adjustor/test/test_collision_distance.cpp +++ b/planning/safe_velocity_adjustor/test/test_collision_distance.cpp @@ -14,12 +14,14 @@ #include "safe_velocity_adjustor/collision_distance.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" - -#include +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include +#include +#include +#include TEST(TestCollisionDistance, forwardSimulatedVector) { @@ -183,3 +185,57 @@ TEST(TestCollisionDistance, distanceToClosestCollision) EXPECT_DOUBLE_EQ(*dist, std::sqrt(0.5)); } } + +TEST(TestCollisionDistance, distanceToClosestCollisionBench) +{ + using pcl::common::CloudGenerator; + using pcl::common::UniformGenerator; + using safe_velocity_adjustor::distanceToClosestCollision; + using safe_velocity_adjustor::distanceToClosestCollision_Eigen; + safe_velocity_adjustor::segment_t vector = { + safe_velocity_adjustor::point_t{0, 0}, safe_velocity_adjustor::point_t{5, 0}}; + const auto vehicle_width = 2.0; + tier4_autoware_utils::StopWatch watch; + + pcl::PointCloud obstacle_points; + constexpr auto size = 10000; + constexpr auto min = -5.0; + constexpr auto max = 15.0; + CloudGenerator > generator; + double gen{}; + double baseline{}; + double eigen{}; + for (auto i = 0; i < 100; ++i) { + watch.tic("gen"); + obstacle_points.clear(); + UniformGenerator::Parameters x_params(min, max, i); + generator.setParametersForX(x_params); + UniformGenerator::Parameters y_params(min, max, i); + generator.setParametersForY(y_params); + UniformGenerator::Parameters z_params(min, max, i); + generator.setParametersForZ(z_params); + generator.fill(size, 1, obstacle_points); + gen += watch.toc("gen"); + watch.tic("baseline"); + const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); + baseline += watch.toc("baseline"); + watch.tic("eigen"); + const auto dist_eigen = + distanceToClosestCollision_Eigen(vector, vehicle_width, obstacle_points); + eigen += watch.toc("eigen"); + if (dist && dist_eigen) + EXPECT_EQ(*dist, *dist_eigen); + else + EXPECT_EQ(dist.has_value(), dist_eigen.has_value()); + } + std::cerr << "Cloud gen: " << gen << std::endl; + std::cerr << "Baseline : " << baseline << std::endl; + std::cerr << "Eigen: " << eigen << std::endl; + + std::cerr << "\na: " << safe_velocity_adjustor::a << std::endl; + std::cerr << "b: " << safe_velocity_adjustor::b << std::endl; + std::cerr << "c: " << safe_velocity_adjustor::c << std::endl; + std::cerr << "d: " << safe_velocity_adjustor::d << std::endl; + std::cerr << "e: " << safe_velocity_adjustor::e << std::endl; + std::cerr << "f: " << safe_velocity_adjustor::f << std::endl; +} From eb26b9a1897f483fde2f105c805d4920c75f1629 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 11 Apr 2022 18:18:32 +0900 Subject: [PATCH 31/37] Switch to using contours from occupancy grid for collision checking Filtering of dynamic objects is not great --- .../safe_velocity_adjustor/CMakeLists.txt | 1 + .../default_safe_velocity_adjustor.param.yaml | 6 +- .../collision_distance.hpp | 49 +++- .../occupancy_grid_utils.hpp | 102 ++++++++ .../pointcloud_processing.hpp | 117 --------- .../safe_velocity_adjustor_node.hpp | 230 +++++++++++------- .../launch/safe_velocity_adjustor.launch.xml | 4 +- .../launch/safe_velocity_adjustor_bag.rviz | 48 +--- planning/safe_velocity_adjustor/package.xml | 3 + .../test/test_collision_distance.cpp | 2 +- .../test/test_occupancy_grid_utils.cpp | 45 ++++ 11 files changed, 350 insertions(+), 257 deletions(-) create mode 100644 planning/safe_velocity_adjustor/include/safe_velocity_adjustor/occupancy_grid_utils.hpp delete mode 100644 planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp create mode 100644 planning/safe_velocity_adjustor/test/test_occupancy_grid_utils.cpp diff --git a/planning/safe_velocity_adjustor/CMakeLists.txt b/planning/safe_velocity_adjustor/CMakeLists.txt index 183943be5eb4a..bbf87b5dbb76d 100644 --- a/planning/safe_velocity_adjustor/CMakeLists.txt +++ b/planning/safe_velocity_adjustor/CMakeLists.txt @@ -30,6 +30,7 @@ if(BUILD_TESTING) ament_add_gtest(test_${PROJECT_NAME} test/test_collision_distance.cpp + test/test_occupancy_grid_utils.cpp ) target_link_libraries(test_${PROJECT_NAME} safe_velocity_adjustor_node diff --git a/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml b/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml index f444f7193e339..932a81af05c7b 100644 --- a/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml +++ b/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml @@ -2,5 +2,7 @@ ros__parameters: time_safety_buffer: 1.5 # [s] required time without collision in forward simulation dist_safety_buffer: 1.5 # [m] distance from obstacles to consider a collision - start_distance: 2.0 # [m] distance ahead of ego from which to start modifying the trajectory - downsample_factor: 10 # factor by which to downsample the input trajectory for calculation + min_adjusted_velocity: 1.5 # [m/s] minimum velocity that the module can set + start_distance: 0.0 # [m] distance ahead of ego from which to start modifying the trajectory + downsample_factor: 1 # factor by which to downsample the input trajectory for calculation + occupancy_grid_obstacle_threshold: 60 # occupancy grid values higher than this are considered to be obstacles diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp index 2f91bbecd3fb2..f76a499380192 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp @@ -23,10 +23,14 @@ #include #include +#include #include #include #include #include +#include +#include +#include #include #include @@ -44,7 +48,10 @@ namespace safe_velocity_adjustor namespace bg = boost::geometry; using point_t = bg::model::d2::point_xy; using polygon_t = bg::model::polygon; +using multipolygon_t = bg::model::multi_polygon; using segment_t = bg::model::segment; +using linestring_t = bg::model::linestring; +using multilinestring_t = bg::model::multi_linestring; /// @brief generate a segment to where the vehicle body would be after some duration assuming a /// constant velocity and heading @@ -61,6 +68,18 @@ inline segment_t forwardSimulatedVector( return segment_t{from, to}; } +/// @brief generate a footprint from a segment and a vehicle width +inline polygon_t forwardSimulatedFootprint(const segment_t & vector, const double vehicle_width) +{ + multipolygon_t footprint; + namespace strategy = bg::strategy::buffer; + bg::buffer( + linestring_t{vector.first, vector.second}, footprint, + strategy::distance_symmetric(vehicle_width / 2), strategy::side_straight(), + strategy::join_miter(), strategy::end_flat(), strategy::point_square()); + return footprint[0]; +} + static double a{}; static double b{}; static double c{}; @@ -126,6 +145,25 @@ inline std::optional distanceToClosestCollision( return (min_dist <= bg::length(vector) ? min_dist : std::optional()); } +inline std::optional distanceToClosestCollision( + const segment_t & vector, const polygon_t & footprint, const multilinestring_t & obstacles) +{ + double min_dist = std::numeric_limits::max(); + multilinestring_t intersection_lines; + for (const auto & obstacle : obstacles) { + if (bg::intersection(footprint, obstacle, intersection_lines)) { + for (const auto & intersection_line : intersection_lines) { + for (const auto & point : intersection_line) { + min_dist = std::min(min_dist, bg::distance(vector.first, point)); + } + } + } + } + std::optional distance; + if (min_dist != std::numeric_limits::max()) distance = min_dist; + return distance; +} + inline polygon_t createObjPolygon( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size) { @@ -155,22 +193,15 @@ inline polygon_t createObjPolygon( return translate_obj_poly; } -inline std::vector createObjPolygons( +inline multipolygon_t createObjPolygons( const autoware_auto_perception_msgs::msg::PredictedObjects & objects) { - std::vector polygons; + multipolygon_t polygons; for (const auto & object : objects.objects) polygons.push_back(createObjPolygon( object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions)); return polygons; } - -inline bool inPolygons(const point_t & point, const std::vector & polygons) -{ - for (const auto & polygon : polygons) - if (bg::distance(point, polygon) < 0.5) return true; - return false; -} } // namespace safe_velocity_adjustor #endif // SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/occupancy_grid_utils.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/occupancy_grid_utils.hpp new file mode 100644 index 0000000000000..86d50aa90a786 --- /dev/null +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/occupancy_grid_utils.hpp @@ -0,0 +1,102 @@ +// Copyright 2022 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 SAFE_VELOCITY_ADJUSTOR__OCCUPANCY_GRID_UTILS_HPP_ +#define SAFE_VELOCITY_ADJUSTOR__OCCUPANCY_GRID_UTILS_HPP_ + +#include "grid_map_core/Polygon.hpp" +#include "grid_map_core/TypeDefs.hpp" +#include "grid_map_core/iterators/GridMapIterator.hpp" +#include "safe_velocity_adjustor/collision_distance.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +namespace safe_velocity_adjustor +{ + +inline void maskPolygons(grid_map::GridMap & grid_map, const multipolygon_t & polygons) +{ + for (const auto & polygon : polygons) { + grid_map::Polygon grid_map_poly; + for (const auto & point : polygon.outer()) { + grid_map_poly.addVertex(grid_map::Position(point.x(), point.y())); + } + for (grid_map::PolygonIterator iterator(grid_map, grid_map_poly); !iterator.isPastEnd(); + ++iterator) { + grid_map.at("layer", *iterator) = 0; + } + } +} + +inline void threshold(grid_map::GridMap & grid_map, const float occupied_threshold) +{ + for (grid_map::GridMapIterator iter(grid_map); !iter.isPastEnd(); ++iter) { + auto & val = grid_map.at("layer", *iter); + if (val < occupied_threshold) { + val = 0.0; + } else { + val = 127; + } + } +} + +inline void denoise(cv::Mat & cv_image, const int num_iter = 2) +{ + cv::dilate(cv_image, cv_image, cv::Mat(), cv::Point(-1, -1), num_iter); + cv::erode(cv_image, cv_image, cv::Mat(), cv::Point(-1, -1), num_iter); +} + +inline multilinestring_t extractStaticObstaclePolygons( + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + const multipolygon_t & dynamic_obstacle_polygons, const int8_t occupied_threshold) +{ + cv::Mat cv_image; + grid_map::GridMap grid_map; + grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); + maskPolygons(grid_map, dynamic_obstacle_polygons); + threshold(grid_map, occupied_threshold); + grid_map::GridMapCvConverter::toImage(grid_map, "layer", CV_8UC1, cv_image); + denoise(cv_image); + std::vector> contours; + cv::findContours(cv_image, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE); + multilinestring_t polygons; + const auto & info = occupancy_grid.info; + for (const auto & contour : contours) { + linestring_t polygon; + for (const auto & point : contour) { + polygon.emplace_back( + (info.width - 1.0 - point.y) * info.resolution + info.origin.position.x, + (info.height - 1.0 - point.x) * info.resolution + info.origin.position.y); + } + polygons.push_back(polygon); + } + return polygons; +} +} // namespace safe_velocity_adjustor + +#endif // SAFE_VELOCITY_ADJUSTOR__OCCUPANCY_GRID_UTILS_HPP_ diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp deleted file mode 100644 index 7e205fab72a60..0000000000000 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/pointcloud_processing.hpp +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright 2022 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 SAFE_VELOCITY_ADJUSTOR__POINTCLOUD_PROCESSING_HPP_ -#define SAFE_VELOCITY_ADJUSTOR__POINTCLOUD_PROCESSING_HPP_ - -#include "safe_velocity_adjustor/collision_distance.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -namespace safe_velocity_adjustor -{ - -/// @brief return the pointcloud msg transformed and converted to pcl format -inline pcl::PointCloud getTransformedPointCloud( - const sensor_msgs::msg::PointCloud2 & pointcloud_msg, - const geometry_msgs::msg::Transform & transform) -{ - const Eigen::Matrix4f transform_matrix = tf2::transformToEigen(transform).matrix().cast(); - - sensor_msgs::msg::PointCloud2 transformed_msg; - pcl_ros::transformPointCloud(transform_matrix, pointcloud_msg, transformed_msg); - - pcl::PointCloud transformed_pointcloud; - pcl::fromROSMsg(transformed_msg, transformed_pointcloud); - - return transformed_pointcloud; -} - -/// @brief returns the pointcloud with only points within a given distance to the trajectory -inline pcl::PointCloud filterPointCloudByTrajectory( - const pcl::PointCloud & pointcloud, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double duration, - const double distance) -{ - pcl::PointCloud filtered_pointcloud; - for (const auto & point : pointcloud.points) { - for (const auto & trajectory_point : trajectory.points) { - const auto radius = trajectory_point.longitudinal_velocity_mps * duration + distance; - const double dx = trajectory_point.pose.position.x - point.x; - const double dy = trajectory_point.pose.position.y - point.y; - if (std::hypot(dx, dy) < radius) { - filtered_pointcloud.points.push_back(point); - break; - } - } - } - return filtered_pointcloud; -} - -pcl::PointCloud filterPointCloudByObjects( - const pcl::PointCloud & pointcloud, - const autoware_auto_perception_msgs::msg::PredictedObjects & objects) -{ - pcl::PointCloud filtered_pointcloud; - const auto object_polygons = createObjPolygons(objects); - for (const auto & point : pointcloud.points) { - if (!inPolygons({point.x, point.y}, object_polygons)) - filtered_pointcloud.points.push_back(point); - } - return filtered_pointcloud; -} - -/// @brief returns the pointcloud transformed to the trajectory frame and in PCL format with only -/// points that are within range of the trajectory -inline pcl::PointCloud transformAndFilterPointCloud( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, - const sensor_msgs::msg::PointCloud2 & pointcloud, - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, - tier4_autoware_utils::TransformListener & transform_listener, const double duration, - const double distance) -{ - tier4_autoware_utils::StopWatch stopwatch; - const auto & header = pointcloud.header; - const auto transform = transform_listener.getTransform( - trajectory.header.frame_id, header.frame_id, header.stamp, - rclcpp::Duration::from_nanoseconds(0)); - const auto obstacle_pointcloud = getTransformedPointCloud(pointcloud, transform->transform); - std::cerr << "* pointcloud transform = " << stopwatch.toc() << " s\n"; - stopwatch.tic("filterByTraj"); - const auto pointcloud_filtered_by_traj = - filterPointCloudByTrajectory(obstacle_pointcloud, trajectory, duration, distance); - std::cerr << "* pointcloud filter traj = " << stopwatch.toc("filterByTraj") << " s\n"; - stopwatch.tic("filterByObj"); - auto final_pointcloud = filterPointCloudByObjects(pointcloud_filtered_by_traj, objects); - std::cerr << "* pointcloud filter obj = " << stopwatch.toc("filterByObj") << " s\n"; - std::cerr << "** pointcloud total = " << stopwatch.toc() << " s\n"; - return final_pointcloud; - // return pointcloud_filtered_by_traj; -} - -} // namespace safe_velocity_adjustor - -#endif // SAFE_VELOCITY_ADJUSTOR__POINTCLOUD_PROCESSING_HPP_ diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index 05540e157dcc9..e61433e3e4d1b 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -16,9 +16,10 @@ #define SAFE_VELOCITY_ADJUSTOR__SAFE_VELOCITY_ADJUSTOR_NODE_HPP_ #include "safe_velocity_adjustor/collision_distance.hpp" -#include "safe_velocity_adjustor/pointcloud_processing.hpp" +#include "safe_velocity_adjustor/occupancy_grid_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" +#include #include #include #include @@ -31,7 +32,12 @@ #include #include #include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -41,6 +47,7 @@ #include #include +#include #include #include #include @@ -51,7 +58,7 @@ namespace safe_velocity_adjustor using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using sensor_msgs::msg::PointCloud2; +using nav_msgs::msg::OccupancyGrid; using TrajectoryPoints = std::vector; using Float = decltype(TrajectoryPoint::longitudinal_velocity_mps); @@ -61,16 +68,11 @@ class SafeVelocityAdjustorNode : public rclcpp::Node explicit SafeVelocityAdjustorNode(const rclcpp::NodeOptions & node_options) : rclcpp::Node("safe_velocity_adjustor", node_options) { - time_safety_buffer_ = static_cast(declare_parameter("time_safety_buffer")); - dist_safety_buffer_ = static_cast(declare_parameter("dist_safety_buffer")); - start_distance_ = static_cast(declare_parameter("start_distance")); - downsample_factor_ = static_cast(declare_parameter("downsample_factor")); - sub_trajectory_ = create_subscription( "~/input/trajectory", 1, [this](const Trajectory::ConstSharedPtr msg) { onTrajectory(msg); }); - sub_obstacle_pointcloud_ = create_subscription( - "~/input/obstacle_pointcloud", rclcpp::QoS(1).best_effort(), - [this](const PointCloud2::ConstSharedPtr msg) { obstacle_pointcloud_ptr_ = msg; }); + sub_occupancy_grid_ = create_subscription( + "~/input/occupancy_grid", 1, + [this](const OccupancyGrid::ConstSharedPtr msg) { occupancy_grid_ptr_ = msg; }); sub_objects_ = create_subscription( "~/input/dynamic_obstacles", 1, [this](const PredictedObjects::ConstSharedPtr msg) { dynamic_obstacles_ptr_ = msg; }); @@ -78,7 +80,9 @@ class SafeVelocityAdjustorNode : public rclcpp::Node pub_trajectory_ = create_publisher("~/output/trajectory", 1); pub_debug_markers_ = create_publisher("~/output/debug_markers", 1); - pub_debug_pointcloud_ = create_publisher("~/output/pointcloud", 1); + pub_debug_polygons_ = + create_publisher("~/output/debug_polygons", 1); + pub_debug_occupancy_grid_ = create_publisher("~/output/occupancy_grid", 1); set_param_res_ = add_on_set_parameters_callback([this](const auto & params) { return onParameter(params); }); @@ -93,23 +97,29 @@ class SafeVelocityAdjustorNode : public rclcpp::Node pub_trajectory_; //!< @brief publisher for output trajectory rclcpp::Publisher::SharedPtr pub_debug_markers_; //!< @brief publisher for debug markers - rclcpp::Publisher::SharedPtr - pub_debug_pointcloud_; //!< @brief publisher for filtered pointcloud + rclcpp::Publisher::SharedPtr + pub_debug_occupancy_grid_; //!< @brief publisher for filtered occupancy grid + rclcpp::Publisher::SharedPtr + pub_debug_polygons_; //!< @brief publisher for filtered occupancy grid rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief subscriber for reference trajectory - rclcpp::Subscription::SharedPtr - sub_obstacle_pointcloud_; //!< @brief subscriber for obstacle pointcloud - rclcpp::Subscription::SharedPtr sub_objects_; + rclcpp::Subscription::SharedPtr + sub_objects_; //!< @brief subscribe for dynamic objects + rclcpp::Subscription::SharedPtr + sub_occupancy_grid_; //!< @brief subscriber for occupancy grid // cached inputs - PointCloud2::ConstSharedPtr obstacle_pointcloud_ptr_; PredictedObjects::ConstSharedPtr dynamic_obstacles_ptr_; + OccupancyGrid::ConstSharedPtr occupancy_grid_ptr_; // parameters - Float time_safety_buffer_; - Float dist_safety_buffer_; - Float start_distance_; - int downsample_factor_; + Float time_safety_buffer_ = static_cast(declare_parameter("time_safety_buffer")); + Float dist_safety_buffer_ = static_cast(declare_parameter("dist_safety_buffer")); + Float start_distance_ = static_cast(declare_parameter("start_distance")); + int downsample_factor_ = static_cast(declare_parameter("downsample_factor")); + int8_t occupancy_grid_obstacle_threshold_ = + static_cast(declare_parameter("occupancy_grid_obstacle_threshold")); + // TODO(Maxime CLEMENT): get vehicle width and length from vehicle parameters Float vehicle_width_ = 2.0; Float vehicle_length_ = 4.0; @@ -126,11 +136,14 @@ class SafeVelocityAdjustorNode : public rclcpp::Node } else if (parameter.get_name() == "dist_safety_buffer") { dist_safety_buffer_ = static_cast(parameter.as_double()); result.successful = true; + } else if (parameter.get_name() == "start_distance") { + start_distance_ = static_cast(parameter.as_double()); + result.successful = true; } else if (parameter.get_name() == "downsample_factor") { downsample_factor_ = static_cast(parameter.as_int()); result.successful = true; - } else if (parameter.get_name() == "start_distance") { - start_distance_ = static_cast(parameter.as_double()); + } else if (parameter.get_name() == "occupancy_grid_obstacle_threshold") { + occupancy_grid_obstacle_threshold_ = static_cast(parameter.as_int()); result.successful = true; } else { RCLCPP_WARN(get_logger(), "Unknown parameter %s", parameter.get_name().c_str()); @@ -141,78 +154,96 @@ class SafeVelocityAdjustorNode : public rclcpp::Node void onTrajectory(const Trajectory::ConstSharedPtr msg) { - const auto ego_idx = tier4_autoware_utils::findNearestIndex( - msg->points, self_pose_listener_.getCurrentPose()->pose); - if (!obstacle_pointcloud_ptr_ || !dynamic_obstacles_ptr_ || !ego_idx) { - constexpr auto one_sec = rcutils_duration_value_t(1000); - if (!obstacle_pointcloud_ptr_) - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), one_sec, "Obstable pointcloud not yet received"); - if (!dynamic_obstacles_ptr_) - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), one_sec, "Dynamic obstable not yet received"); - if (!ego_idx) - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), one_sec, "Cannot calculate ego index on the trajectory"); - return; - } - const auto start_idx = calculateStartIndex(*msg, *ego_idx, start_distance_); - // Downsample trajectory - const size_t downsample_step = downsample_factor_; - Trajectory downsampled_traj; - downsampled_traj.header = msg->header; - downsampled_traj.points.reserve(msg->points.size() / downsample_step); - for (size_t i = start_idx; i < msg->points.size(); i += downsample_step) - downsampled_traj.points.push_back(msg->points[i]); + try { + const auto ego_idx = tier4_autoware_utils::findNearestIndex( + msg->points, self_pose_listener_.getCurrentPose()->pose); + if (!occupancy_grid_ptr_ || !dynamic_obstacles_ptr_ || !ego_idx) { + constexpr auto one_sec = rcutils_duration_value_t(1000); + if (!occupancy_grid_ptr_) + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), one_sec, "Occupancy grid not yet received"); + if (!dynamic_obstacles_ptr_) + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), one_sec, "Dynamic obstable not yet received"); + if (!ego_idx) + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), one_sec, "Cannot calculate ego index on the trajectory"); + return; + } + const auto start_idx = calculateStartIndex(*msg, *ego_idx, start_distance_); + // Downsample trajectory + const size_t downsample_step = downsample_factor_; + Trajectory downsampled_traj; + downsampled_traj.header = msg->header; + downsampled_traj.points.reserve(msg->points.size() / downsample_step); + for (size_t i = start_idx; i < msg->points.size(); i += downsample_step) + downsampled_traj.points.push_back(msg->points[i]); + + const auto dynamic_obstacle_polygons = createObjPolygons(*dynamic_obstacles_ptr_); - double pointcloud_d{}; - double vector_d{}; - double dist_d{}; - double vel_d{}; - tier4_autoware_utils::StopWatch stopwatch; - Trajectory safe_trajectory = *msg; - const auto extra_vehicle_length = vehicle_length_ / 2 + dist_safety_buffer_; - stopwatch.tic("pointcloud_d"); - const auto filtered_obstacle_pointcloud = transformAndFilterPointCloud( - downsampled_traj, *obstacle_pointcloud_ptr_, *dynamic_obstacles_ptr_, transform_listener_, - time_safety_buffer_, extra_vehicle_length); - pointcloud_d += stopwatch.toc("pointcloud_d"); + double footprint_d{}; + double dist_poly_d{}; + tier4_autoware_utils::StopWatch stopwatch; - for (auto & trajectory_point : downsampled_traj.points) { - stopwatch.tic("vector_d"); - const auto forward_simulated_vector = - forwardSimulatedVector(trajectory_point, time_safety_buffer_, extra_vehicle_length); - vector_d += stopwatch.toc("vector_d"); - stopwatch.tic("dist_d"); - const auto dist_to_collision = distanceToClosestCollision( - forward_simulated_vector, vehicle_width_, filtered_obstacle_pointcloud); - dist_d += stopwatch.toc("dist_d"); - if (dist_to_collision) { - stopwatch.tic("vel_d"); - trajectory_point.longitudinal_velocity_mps = calculateSafeVelocity( - trajectory_point, - std::max({}, static_cast(*dist_to_collision - extra_vehicle_length))); - vel_d += stopwatch.toc("vel_d"); + // Obstacle Polygon from Occupancy Grid + stopwatch.tic("obs_poly_d"); + nav_msgs::msg::OccupancyGrid debug_occ_grid; + const auto obstacle_polygons = extractStaticObstaclePolygons( + *occupancy_grid_ptr_, dynamic_obstacle_polygons, occupancy_grid_obstacle_threshold_); + pub_debug_occupancy_grid_->publish(debug_occ_grid); + RCLCPP_WARN(get_logger(), "obstacle_polygons = %2.2fs", stopwatch.toc("obs_poly_d")); + visualization_msgs::msg::MarkerArray polygon_markers; + static auto max_id = 0; + auto id = 0; + for (const auto & poly : obstacle_polygons) { + auto marker = makePolygonMarker(poly, id++); + marker.header.frame_id = occupancy_grid_ptr_->header.frame_id; + marker.header.stamp = now(); + polygon_markers.markers.push_back(marker); } - } + max_id = std::max(id, max_id); + while (id <= max_id) { + visualization_msgs::msg::Marker marker; + marker.action = visualization_msgs::msg::Marker::DELETE; + marker.ns = "obstacle_polygons"; + marker.id = id++; + polygon_markers.markers.push_back(marker); + } + pub_debug_polygons_->publish(polygon_markers); - for (size_t i = 0; i < downsampled_traj.points.size(); ++i) { - safe_trajectory.points[start_idx + i * downsample_step].longitudinal_velocity_mps = - downsampled_traj.points[i].longitudinal_velocity_mps; - } - RCLCPP_WARN(get_logger(), "pointcloud = %2.2fs", pointcloud_d); - RCLCPP_WARN(get_logger(), "dist = %2.2fs", dist_d); + Trajectory safe_trajectory = *msg; + const auto extra_vehicle_length = vehicle_length_ / 2 + dist_safety_buffer_; + for (auto & trajectory_point : downsampled_traj.points) { + const auto forward_simulated_vector = + forwardSimulatedVector(trajectory_point, time_safety_buffer_, extra_vehicle_length); + stopwatch.tic("footprint_d"); + const auto footprint = forwardSimulatedFootprint(forward_simulated_vector, vehicle_width_); + footprint_d += stopwatch.toc("footprint_d"); + stopwatch.tic("dist_poly_d"); + const auto dist_to_collision = + distanceToClosestCollision(forward_simulated_vector, footprint, obstacle_polygons); + dist_poly_d += stopwatch.toc("dist_poly_d"); + if (dist_to_collision) { + trajectory_point.longitudinal_velocity_mps = calculateSafeVelocity( + trajectory_point, + std::max({}, static_cast(*dist_to_collision - extra_vehicle_length))); + } + } - safe_trajectory.header.stamp = now(); - pub_trajectory_->publish(safe_trajectory); - publishDebug(*msg, safe_trajectory); - PointCloud2 ros_pointcloud; - pcl::toROSMsg(filtered_obstacle_pointcloud, ros_pointcloud); - ros_pointcloud.header.stamp = now(); - ros_pointcloud.header.frame_id = downsampled_traj.header.frame_id; - pub_debug_pointcloud_->publish(ros_pointcloud); + for (size_t i = 0; i < downsampled_traj.points.size(); ++i) { + safe_trajectory.points[start_idx + i * downsample_step].longitudinal_velocity_mps = + downsampled_traj.points[i].longitudinal_velocity_mps; + } + RCLCPP_WARN(get_logger(), "footprint = %2.2fs", footprint_d); + RCLCPP_WARN(get_logger(), "dist_poly = %2.2fs", dist_poly_d); - RCLCPP_WARN(get_logger(), "*** Total = %2.2fs", stopwatch.toc()); + safe_trajectory.header.stamp = now(); + pub_trajectory_->publish(safe_trajectory); + publishDebug(*msg, safe_trajectory); + RCLCPP_WARN(get_logger(), "*** Total = %2.2fs", stopwatch.toc()); + } catch (const std::exception & e) { + RCLCPP_WARN(get_logger(), "%s", e.what()); + } } Float calculateSafeVelocity( @@ -223,6 +254,25 @@ class SafeVelocityAdjustorNode : public rclcpp::Node static_cast(dist_to_collision / time_safety_buffer_)); } + static visualization_msgs::msg::Marker makePolygonMarker( + const linestring_t & polygon, const int id) + { + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.scale.x = 0.1; + marker.color.b = 1.0; + marker.color.a = 1.0; + marker.id = id; + marker.ns = "obstacle_polygons"; + for (const auto & point : polygon) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + p.z = 100; + marker.points.push_back(p); + } + return marker; + } visualization_msgs::msg::Marker makeEnvelopeMarker(const Trajectory & trajectory) const { visualization_msgs::msg::Marker envelope; diff --git a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml index 30dfe1cd50310..464ed64c1287f 100644 --- a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml +++ b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml @@ -1,8 +1,8 @@ - + @@ -17,8 +17,8 @@ launch-prefix="$(var gdb-launch-prefix)"> + - diff --git a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.rviz b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.rviz index 5a272cda5134b..4e46027f43af3 100644 --- a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.rviz +++ b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor_bag.rviz @@ -7,7 +7,6 @@ Panels: - /System1/Vehicle1 - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - /Perception1 - - /Perception1/ObjectRecognition1 - /Perception1/ObjectRecognition1/Prediction1 Splitter Ratio: 0.557669460773468 Tree Height: 289 @@ -28,7 +27,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: FilteredPointCloud + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -914,7 +913,7 @@ Visualization Manager: Value: /perception/occupancy_grid_map/map_updates Use Timestamp: false Value: true - Enabled: false + Enabled: true Name: OccupancyGrid Enabled: true Name: Perception @@ -1380,10 +1379,9 @@ Visualization Manager: Name: Control - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: MarkerArray + Name: Envelope Namespaces: - adjusted: true - original: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -1391,39 +1389,17 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/safe_velocity_adjustor/debug_markers Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 103; 103; 103 - Color Transformer: FlatColor - Decay Time: 0 + - Class: rviz_default_plugins/MarkerArray Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: FilteredPointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points + Name: Polygons + Namespaces: + {} Topic: Depth: 5 Durability Policy: Volatile - Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /safe_velocity_adjustor/output/pointcloud - Use Fixed Frame: true - Use rainbow: true + Value: /safe_velocity_adjustor/output/debug_polygons Value: true Enabled: true Global Options: @@ -1498,11 +1474,11 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 12.543999671936035 + Scale: 8.44149112701416 Target Frame: base_link Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 + X: 6.502593994140625 + Y: -9.435478210449219 Saved: - Class: rviz_default_plugins/ThirdPersonFollower Distance: 18 diff --git a/planning/safe_velocity_adjustor/package.xml b/planning/safe_velocity_adjustor/package.xml index e3d556f465704..99659c30be13b 100644 --- a/planning/safe_velocity_adjustor/package.xml +++ b/planning/safe_velocity_adjustor/package.xml @@ -14,7 +14,10 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs eigen + grid_map_msgs + grid_map_ros libboost-dev + nav_msgs pcl_ros rclcpp rclcpp_components diff --git a/planning/safe_velocity_adjustor/test/test_collision_distance.cpp b/planning/safe_velocity_adjustor/test/test_collision_distance.cpp index 625ccf1c8fc7d..234db63f694ae 100644 --- a/planning/safe_velocity_adjustor/test/test_collision_distance.cpp +++ b/planning/safe_velocity_adjustor/test/test_collision_distance.cpp @@ -16,7 +16,7 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include +#include #include #include diff --git a/planning/safe_velocity_adjustor/test/test_occupancy_grid_utils.cpp b/planning/safe_velocity_adjustor/test/test_occupancy_grid_utils.cpp new file mode 100644 index 0000000000000..b7df535bb1286 --- /dev/null +++ b/planning/safe_velocity_adjustor/test/test_occupancy_grid_utils.cpp @@ -0,0 +1,45 @@ +// Copyright 2022 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 "safe_velocity_adjustor/collision_distance.hpp" +#include "safe_velocity_adjustor/occupancy_grid_utils.hpp" + +#include + +TEST(TestOccupancyGridUtils, maskPolygons) {} + +TEST(TestOccupancyGridUtils, extractStaticObstaclePolygons) +{ + using safe_velocity_adjustor::extractStaticObstaclePolygons; + constexpr int8_t occupied_threshold = 10; + nav_msgs::msg::OccupancyGrid occupancy_grid; + occupancy_grid.info.height = 5; + occupancy_grid.info.width = 5; + occupancy_grid.data = + std::vector(occupancy_grid.info.height * occupancy_grid.info.width); + occupancy_grid.info.resolution = 1.0; + + auto polygons = extractStaticObstaclePolygons(occupancy_grid, {}, occupied_threshold); + EXPECT_TRUE(polygons.empty()); + + occupancy_grid.data.at(5) = 10; + polygons = extractStaticObstaclePolygons(occupancy_grid, {}, occupied_threshold); + EXPECT_EQ(polygons.size(), 1ul); + + for (auto i = 1; i < 4; ++i) + for (auto j = 1; j < 4; ++j) occupancy_grid.data[j + i * occupancy_grid.info.width] = 10; + polygons = extractStaticObstaclePolygons(occupancy_grid, {}, occupied_threshold); + EXPECT_EQ(polygons.size(), 1ul); + std::cerr << boost::geometry::wkt(polygons) << std::endl; +} From 80df66ce3105a0f50fd3a802597ab76cddad621c Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 12 Apr 2022 12:38:49 +0900 Subject: [PATCH 32/37] Add buffer around dynamic obstacles to avoid false obstacle detection --- .../default_safe_velocity_adjustor.param.yaml | 1 + .../safe_velocity_adjustor/collision_distance.hpp | 11 ++++++----- .../safe_velocity_adjustor_node.hpp | 14 ++++++++------ 3 files changed, 15 insertions(+), 11 deletions(-) diff --git a/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml b/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml index 932a81af05c7b..b47ba72e5db70 100644 --- a/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml +++ b/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml @@ -6,3 +6,4 @@ start_distance: 0.0 # [m] distance ahead of ego from which to start modifying the trajectory downsample_factor: 1 # factor by which to downsample the input trajectory for calculation occupancy_grid_obstacle_threshold: 60 # occupancy grid values higher than this are considered to be obstacles + dynamic_obstacles_buffer: 1.0 # [m] extra distance around dynamic obstacles used to mask the occupancy grid and avoid false obstacle detection diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp index f76a499380192..00bdfa3e66c28 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp @@ -165,14 +165,15 @@ inline std::optional distanceToClosestCollision( } inline polygon_t createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size) + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size, + const double buffer) { // (objects.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions); // rename const double x = pose.position.x; const double y = pose.position.y; - const double h = size.x; - const double w = size.y; + const double h = size.x + buffer; + const double w = size.y + buffer; const double yaw = tf2::getYaw(pose.orientation); // create base polygon @@ -194,12 +195,12 @@ inline polygon_t createObjPolygon( } inline multipolygon_t createObjPolygons( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects) + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer) { multipolygon_t polygons; for (const auto & object : objects.objects) polygons.push_back(createObjPolygon( - object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions)); + object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions, buffer)); return polygons; } } // namespace safe_velocity_adjustor diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index e61433e3e4d1b..c9fbf128333ad 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -119,6 +119,8 @@ class SafeVelocityAdjustorNode : public rclcpp::Node int downsample_factor_ = static_cast(declare_parameter("downsample_factor")); int8_t occupancy_grid_obstacle_threshold_ = static_cast(declare_parameter("occupancy_grid_obstacle_threshold")); + Float dynamic_obstacles_buffer_ = + static_cast(declare_parameter("dynamic_obstacles_buffer")); // TODO(Maxime CLEMENT): get vehicle width and length from vehicle parameters Float vehicle_width_ = 2.0; @@ -129,24 +131,23 @@ class SafeVelocityAdjustorNode : public rclcpp::Node const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; - result.successful = false; + result.successful = true; for (const auto & parameter : parameters) { if (parameter.get_name() == "time_safety_buffer") { time_safety_buffer_ = static_cast(parameter.as_double()); } else if (parameter.get_name() == "dist_safety_buffer") { dist_safety_buffer_ = static_cast(parameter.as_double()); - result.successful = true; } else if (parameter.get_name() == "start_distance") { start_distance_ = static_cast(parameter.as_double()); - result.successful = true; } else if (parameter.get_name() == "downsample_factor") { downsample_factor_ = static_cast(parameter.as_int()); - result.successful = true; } else if (parameter.get_name() == "occupancy_grid_obstacle_threshold") { occupancy_grid_obstacle_threshold_ = static_cast(parameter.as_int()); - result.successful = true; + } else if (parameter.get_name() == "dynamic_obstacles_buffer") { + dynamic_obstacles_buffer_ = static_cast(parameter.as_double()); } else { RCLCPP_WARN(get_logger(), "Unknown parameter %s", parameter.get_name().c_str()); + result.successful = false; } } return result; @@ -179,7 +180,8 @@ class SafeVelocityAdjustorNode : public rclcpp::Node for (size_t i = start_idx; i < msg->points.size(); i += downsample_step) downsampled_traj.points.push_back(msg->points[i]); - const auto dynamic_obstacle_polygons = createObjPolygons(*dynamic_obstacles_ptr_); + const auto dynamic_obstacle_polygons = + createObjPolygons(*dynamic_obstacles_ptr_, dynamic_obstacles_buffer_); double footprint_d{}; double dist_poly_d{}; From 0423e4c5e57af5aea5f0472222c00948a5ae1abe Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 12 Apr 2022 13:00:11 +0900 Subject: [PATCH 33/37] Add parameter to limit the adjusted velocity --- .../safe_velocity_adjustor_node.hpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index c9fbf128333ad..362e74f01d761 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -116,6 +116,8 @@ class SafeVelocityAdjustorNode : public rclcpp::Node Float time_safety_buffer_ = static_cast(declare_parameter("time_safety_buffer")); Float dist_safety_buffer_ = static_cast(declare_parameter("dist_safety_buffer")); Float start_distance_ = static_cast(declare_parameter("start_distance")); + Float min_adjusted_velocity_ = + static_cast(declare_parameter("min_adjusted_velocity")); int downsample_factor_ = static_cast(declare_parameter("downsample_factor")); int8_t occupancy_grid_obstacle_threshold_ = static_cast(declare_parameter("occupancy_grid_obstacle_threshold")); @@ -145,6 +147,8 @@ class SafeVelocityAdjustorNode : public rclcpp::Node occupancy_grid_obstacle_threshold_ = static_cast(parameter.as_int()); } else if (parameter.get_name() == "dynamic_obstacles_buffer") { dynamic_obstacles_buffer_ = static_cast(parameter.as_double()); + } else if (parameter.get_name() == "min_adjusted_velocity") { + min_adjusted_velocity_ = static_cast(parameter.as_double()); } else { RCLCPP_WARN(get_logger(), "Unknown parameter %s", parameter.get_name().c_str()); result.successful = false; @@ -227,8 +231,7 @@ class SafeVelocityAdjustorNode : public rclcpp::Node dist_poly_d += stopwatch.toc("dist_poly_d"); if (dist_to_collision) { trajectory_point.longitudinal_velocity_mps = calculateSafeVelocity( - trajectory_point, - std::max({}, static_cast(*dist_to_collision - extra_vehicle_length))); + trajectory_point, static_cast(*dist_to_collision - extra_vehicle_length)); } } @@ -253,7 +256,8 @@ class SafeVelocityAdjustorNode : public rclcpp::Node { return std::min( trajectory_point.longitudinal_velocity_mps, - static_cast(dist_to_collision / time_safety_buffer_)); + std::max( + min_adjusted_velocity_, static_cast(dist_to_collision / time_safety_buffer_))); } static visualization_msgs::msg::Marker makePolygonMarker( From d38227de2f811a9f48e75c658cbff3555c3fc131 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 12 Apr 2022 14:31:33 +0900 Subject: [PATCH 34/37] Use vehicle_info_util to get vehicle footprint --- .../collision_distance.hpp | 4 ++-- .../safe_velocity_adjustor_node.hpp | 18 +++++++++++------- .../launch/safe_velocity_adjustor.launch.xml | 6 ++---- planning/safe_velocity_adjustor/package.xml | 1 + 4 files changed, 16 insertions(+), 13 deletions(-) diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp index 00bdfa3e66c28..52ad3850ee34b 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp @@ -69,13 +69,13 @@ inline segment_t forwardSimulatedVector( } /// @brief generate a footprint from a segment and a vehicle width -inline polygon_t forwardSimulatedFootprint(const segment_t & vector, const double vehicle_width) +inline polygon_t forwardSimulatedFootprint(const segment_t & vector, const double lateral_offset) { multipolygon_t footprint; namespace strategy = bg::strategy::buffer; bg::buffer( linestring_t{vector.first, vector.second}, footprint, - strategy::distance_symmetric(vehicle_width / 2), strategy::side_straight(), + strategy::distance_symmetric(lateral_offset), strategy::side_straight(), strategy::join_miter(), strategy::end_flat(), strategy::point_square()); return footprint[0]; } diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index 362e74f01d761..30079fed85d2f 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -84,6 +85,10 @@ class SafeVelocityAdjustorNode : public rclcpp::Node create_publisher("~/output/debug_polygons", 1); pub_debug_occupancy_grid_ = create_publisher("~/output/occupancy_grid", 1); + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); + vehicle_front_offset_ = static_cast(vehicle_info.max_longitudinal_offset_m); + set_param_res_ = add_on_set_parameters_callback([this](const auto & params) { return onParameter(params); }); @@ -123,10 +128,8 @@ class SafeVelocityAdjustorNode : public rclcpp::Node static_cast(declare_parameter("occupancy_grid_obstacle_threshold")); Float dynamic_obstacles_buffer_ = static_cast(declare_parameter("dynamic_obstacles_buffer")); - - // TODO(Maxime CLEMENT): get vehicle width and length from vehicle parameters - Float vehicle_width_ = 2.0; - Float vehicle_length_ = 4.0; + Float vehicle_lateral_offset_; + Float vehicle_front_offset_; OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onParameter( @@ -218,12 +221,13 @@ class SafeVelocityAdjustorNode : public rclcpp::Node pub_debug_polygons_->publish(polygon_markers); Trajectory safe_trajectory = *msg; - const auto extra_vehicle_length = vehicle_length_ / 2 + dist_safety_buffer_; + const auto extra_vehicle_length = vehicle_front_offset_ + dist_safety_buffer_; for (auto & trajectory_point : downsampled_traj.points) { const auto forward_simulated_vector = forwardSimulatedVector(trajectory_point, time_safety_buffer_, extra_vehicle_length); stopwatch.tic("footprint_d"); - const auto footprint = forwardSimulatedFootprint(forward_simulated_vector, vehicle_width_); + const auto footprint = + forwardSimulatedFootprint(forward_simulated_vector, vehicle_lateral_offset_); footprint_d += stopwatch.toc("footprint_d"); stopwatch.tic("dist_poly_d"); const auto dist_to_collision = @@ -288,7 +292,7 @@ class SafeVelocityAdjustorNode : public rclcpp::Node envelope.color.a = 1.0; for (const auto & point : trajectory.points) { const auto vector = forwardSimulatedVector( - point, time_safety_buffer_, dist_safety_buffer_ + vehicle_length_ / 2); + point, time_safety_buffer_, dist_safety_buffer_ + vehicle_front_offset_); geometry_msgs::msg::Point p; p.x = vector.second.x(); p.y = vector.second.y(); diff --git a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml index 464ed64c1287f..3d0d6a346c958 100644 --- a/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml +++ b/planning/safe_velocity_adjustor/launch/safe_velocity_adjustor.launch.xml @@ -6,17 +6,15 @@ - - - + - + diff --git a/planning/safe_velocity_adjustor/package.xml b/planning/safe_velocity_adjustor/package.xml index 99659c30be13b..4d02537e17015 100644 --- a/planning/safe_velocity_adjustor/package.xml +++ b/planning/safe_velocity_adjustor/package.xml @@ -24,6 +24,7 @@ sensor_msgs tier4_autoware_utils tier4_planning_msgs + vehicle_info_util visualization_msgs ament_cmake_gtest From 3d74c448426b9d0e7563628c7a7a294bc909103a Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 12 Apr 2022 20:22:37 +0900 Subject: [PATCH 35/37] Calculate accurate distance to collision + add tests --- .../collision_distance.hpp | 91 +------ .../test/test_collision_distance.cpp | 228 +++++++----------- .../test/test_occupancy_grid_utils.cpp | 1 - 3 files changed, 98 insertions(+), 222 deletions(-) diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp index 52ad3850ee34b..b96c3d434adf8 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp @@ -15,27 +15,18 @@ #ifndef SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ #define SAFE_VELOCITY_ADJUSTOR__COLLISION_DISTANCE_HPP_ -#include -#include - -#include +#include #include #include #include -#include #include -#include -#include -#include +#include #include #include #include #include -#include -#include -#include #include #include @@ -80,81 +71,23 @@ inline polygon_t forwardSimulatedFootprint(const segment_t & vector, const doubl return footprint[0]; } -static double a{}; -static double b{}; -static double c{}; -static double d{}; -static double e{}; -static double f{}; -/// @brief calculate the distance to the closest obstacle point colliding with the footprint -inline std::optional distanceToClosestCollision_Eigen( - const segment_t & vector, const double vehicle_width, - const pcl::PointCloud & obstacle_points) -{ - tier4_autoware_utils::StopWatch watch; - watch.tic("a"); - Eigen::ArrayX2d dist_array(obstacle_points.size(), 2); - const auto dist_matrix = obstacle_points.getMatrixXfMap(3, 4, 0).cast(); - dist_array.col(0) = dist_matrix.row(0).array() - vector.first.x(); - dist_array.col(1) = dist_matrix.row(1).array() - vector.first.y(); - a += watch.toc("a"); - watch.tic("b"); - auto collision_headings = Eigen::ArrayXd(dist_array.rows()); - for (auto row_idx = 0; row_idx < dist_array.rows(); ++row_idx) - collision_headings(row_idx) = std::atan2(dist_array(row_idx, 1), dist_array(row_idx, 0)); - b += watch.toc("b"); - watch.tic("c"); - const auto traj_heading = - std::atan2(vector.second.y() - vector.first.y(), vector.second.x() - vector.first.x()); - c += watch.toc("c"); - watch.tic("d"); - const auto angles = traj_heading - collision_headings; - const auto hypot_lengths = - (dist_array.col(0).array().square() + dist_array.col(1).array().square()).sqrt(); - d += watch.toc("d"); - watch.tic("e"); - const auto long_dists = angles.cos().abs() * hypot_lengths; - const auto lat_dists = (hypot_lengths.square() - long_dists.square()).sqrt(); - e += watch.toc("e"); - watch.tic("f"); - const auto min_dist = - (long_dists + bg::length(vector) * (lat_dists > vehicle_width / 2).cast()).minCoeff(); - f += watch.toc("f"); - return (min_dist <= bg::length(vector) ? min_dist : std::optional()); -} -/// @brief calculate the distance to the closest obstacle point colliding with the footprint -inline std::optional distanceToClosestCollision( - const segment_t & vector, const double vehicle_width, - const pcl::PointCloud & obstacle_points) -{ - const auto traj_heading = - std::atan2(vector.second.y() - vector.first.y(), vector.second.x() - vector.first.x()); - auto min_dist = std::numeric_limits::infinity(); - for (const auto & obstacle_point : obstacle_points) { - const auto obs_point = point_t{obstacle_point.x, obstacle_point.y}; - const auto collision_heading = - std::atan2(obs_point.y() - vector.first.y(), obs_point.x() - vector.first.x()); - const auto angle = traj_heading - collision_heading; - const auto hypot_length = bg::distance(obs_point, vector.first); - const auto long_dist = std::abs(std::cos(angle)) * hypot_length; - const auto lat_dist = std::sqrt(hypot_length * hypot_length - long_dist * long_dist); - if (lat_dist <= vehicle_width / 2) { - min_dist = std::min(min_dist, long_dist); - } - } - return (min_dist <= bg::length(vector) ? min_dist : std::optional()); -} - inline std::optional distanceToClosestCollision( const segment_t & vector, const polygon_t & footprint, const multilinestring_t & obstacles) { + const auto vector_heading = + std::atan2(vector.second.y() - vector.first.y(), vector.second.x() - vector.first.x()); double min_dist = std::numeric_limits::max(); - multilinestring_t intersection_lines; for (const auto & obstacle : obstacles) { + multilinestring_t intersection_lines; if (bg::intersection(footprint, obstacle, intersection_lines)) { for (const auto & intersection_line : intersection_lines) { - for (const auto & point : intersection_line) { - min_dist = std::min(min_dist, bg::distance(vector.first, point)); + for (const auto & obs_point : intersection_line) { + const auto collision_heading = + std::atan2(obs_point.y() - vector.first.y(), obs_point.x() - vector.first.x()); + const auto angle = vector_heading - collision_heading; + const auto hypot_length = bg::distance(obs_point, vector.first); + const auto long_dist = std::abs(std::cos(angle)) * hypot_length; + min_dist = std::min(min_dist, long_dist); } } } diff --git a/planning/safe_velocity_adjustor/test/test_collision_distance.cpp b/planning/safe_velocity_adjustor/test/test_collision_distance.cpp index 234db63f694ae..9b8d601135ca8 100644 --- a/planning/safe_velocity_adjustor/test/test_collision_distance.cpp +++ b/planning/safe_velocity_adjustor/test/test_collision_distance.cpp @@ -92,150 +92,94 @@ TEST(TestCollisionDistance, forwardSimulatedVector) extra_dist = 3.5; check_vector(-5.0 + extra_dist); } + TEST(TestCollisionDistance, distanceToClosestCollision) { using safe_velocity_adjustor::distanceToClosestCollision; - using safe_velocity_adjustor::point_t; - using safe_velocity_adjustor::segment_t; - segment_t vector = {point_t{0, 0}, point_t{5, 0}}; - const auto vehicle_width = 2.0; - pcl::PointCloud obstacle_points; - obstacle_points.points.emplace_back(6, 2, 0); // outside of the footprint - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_FALSE(dist.has_value()); - } - obstacle_points.points.emplace_back(4, 0, 0); // distance of 4 - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_TRUE(dist.has_value()); - EXPECT_DOUBLE_EQ(*dist, 4.0); - } - obstacle_points.points.emplace_back( - 4.5, 0, 0); // distance of 4.5, does not change the minimum distance - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_TRUE(dist.has_value()); - EXPECT_DOUBLE_EQ(*dist, 4.0); - } - obstacle_points.points.emplace_back(2.0, 0.5, 0); // new minumum distance of 2.0 - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_TRUE(dist.has_value()); - EXPECT_DOUBLE_EQ(*dist, 2.0); - } - obstacle_points.points.emplace_back(1.5, -0.75, 0); // new minumum distance of 1.5 - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_TRUE(dist.has_value()); - EXPECT_DOUBLE_EQ(*dist, 1.5); - } - - // Change vector heading - vector = {point_t{0, 0}, point_t{0, 5}}; - obstacle_points.clear(); - obstacle_points.emplace_back(-2, 3, 0); // outside of the footprint - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_FALSE(dist.has_value()); - } - obstacle_points.points.emplace_back(-0.5, 4, 0); // new minumum distance of 4 - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_TRUE(dist.has_value()); - EXPECT_DOUBLE_EQ(*dist, 4); - } - obstacle_points.points.emplace_back(0, 4, 0); // no change in the minimum distance - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_TRUE(dist.has_value()); - EXPECT_DOUBLE_EQ(*dist, 4); - } - obstacle_points.points.emplace_back(0.5, 0.5, 0); // change the minimum distance - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_TRUE(dist.has_value()); - EXPECT_DOUBLE_EQ(*dist, 0.5); - } - - // Change vector - vector = {point_t{0, 0}, point_t{2.5, 2.5}}; - obstacle_points.clear(); - obstacle_points.emplace_back(3, 3, 0); // outside of the footprint - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_FALSE(dist.has_value()); - } - obstacle_points.points.emplace_back(2.25, 1.75, 0); // new minumum distance - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_TRUE(dist.has_value()); - EXPECT_DOUBLE_EQ(*dist, std::sqrt(8)); - } - obstacle_points.points.emplace_back(2, 2, 0); // no change in the minimum distance - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_TRUE(dist.has_value()); - EXPECT_DOUBLE_EQ(*dist, std::sqrt(8)); - } - obstacle_points.points.emplace_back(0.25, 0.75, 0); // change the minimum distance - { - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - ASSERT_TRUE(dist.has_value()); - EXPECT_DOUBLE_EQ(*dist, std::sqrt(0.5)); - } -} -TEST(TestCollisionDistance, distanceToClosestCollisionBench) -{ - using pcl::common::CloudGenerator; - using pcl::common::UniformGenerator; - using safe_velocity_adjustor::distanceToClosestCollision; - using safe_velocity_adjustor::distanceToClosestCollision_Eigen; - safe_velocity_adjustor::segment_t vector = { - safe_velocity_adjustor::point_t{0, 0}, safe_velocity_adjustor::point_t{5, 0}}; - const auto vehicle_width = 2.0; - tier4_autoware_utils::StopWatch watch; - - pcl::PointCloud obstacle_points; - constexpr auto size = 10000; - constexpr auto min = -5.0; - constexpr auto max = 15.0; - CloudGenerator > generator; - double gen{}; - double baseline{}; - double eigen{}; - for (auto i = 0; i < 100; ++i) { - watch.tic("gen"); - obstacle_points.clear(); - UniformGenerator::Parameters x_params(min, max, i); - generator.setParametersForX(x_params); - UniformGenerator::Parameters y_params(min, max, i); - generator.setParametersForY(y_params); - UniformGenerator::Parameters z_params(min, max, i); - generator.setParametersForZ(z_params); - generator.fill(size, 1, obstacle_points); - gen += watch.toc("gen"); - watch.tic("baseline"); - const auto dist = distanceToClosestCollision(vector, vehicle_width, obstacle_points); - baseline += watch.toc("baseline"); - watch.tic("eigen"); - const auto dist_eigen = - distanceToClosestCollision_Eigen(vector, vehicle_width, obstacle_points); - eigen += watch.toc("eigen"); - if (dist && dist_eigen) - EXPECT_EQ(*dist, *dist_eigen); - else - EXPECT_EQ(dist.has_value(), dist_eigen.has_value()); - } - std::cerr << "Cloud gen: " << gen << std::endl; - std::cerr << "Baseline : " << baseline << std::endl; - std::cerr << "Eigen: " << eigen << std::endl; - - std::cerr << "\na: " << safe_velocity_adjustor::a << std::endl; - std::cerr << "b: " << safe_velocity_adjustor::b << std::endl; - std::cerr << "c: " << safe_velocity_adjustor::c << std::endl; - std::cerr << "d: " << safe_velocity_adjustor::d << std::endl; - std::cerr << "e: " << safe_velocity_adjustor::e << std::endl; - std::cerr << "f: " << safe_velocity_adjustor::f << std::endl; + safe_velocity_adjustor::segment_t vector = {{0.0, 0.0}, {5.0, 0.0}}; + safe_velocity_adjustor::polygon_t footprint; + footprint.outer() = {{0.0, 1.0}, {5.0, 1.0}, {5.0, -1.0}, {0.0, -1.0}}; + boost::geometry::correct(footprint); // avoid bugs with malformed polygon + safe_velocity_adjustor::multilinestring_t obstacles; + + std::optional result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_FALSE(result.has_value()); + + obstacles.push_back({{-1.0, 0.0}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_FALSE(result.has_value()); + + obstacles.push_back({{1.0, 2.0}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_FALSE(result.has_value()); + + obstacles.push_back({{4.0, 0.0}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_TRUE(result.has_value()); + EXPECT_DOUBLE_EQ(*result, 4.0); + + obstacles.push_back({{3.0, 0.5}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_TRUE(result.has_value()); + EXPECT_DOUBLE_EQ(*result, 3.0); + + obstacles.push_back({{2.5, -0.75}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_TRUE(result.has_value()); + EXPECT_DOUBLE_EQ(*result, 2.5); + + // Change vector and footprint + vector = {{0.0, 0.0}, {5.0, 5.0}}; + footprint.outer() = {{-1.0, 1.0}, {4.0, 6.0}, {6.0, 4.0}, {1.0, -1.0}}; + boost::geometry::correct(footprint); // avoid bugs with malformed polygon + obstacles.clear(); + + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_FALSE(result.has_value()); + + obstacles.push_back({{4.0, 4.0}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_TRUE(result.has_value()); + EXPECT_DOUBLE_EQ(*result, std::sqrt(2 * 4.0 * 4.0)); + + obstacles.push_back({{1.0, 2.0}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.121, 1e-3); + + obstacles.push_back({{-2.0, 2.0}, {3.0, -1.0}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 0.354, 1e-3); + + obstacles.push_back({{-1.5, 1.5}, {0.0, 0.5}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 0.141, 1e-3); + + obstacles.push_back({{0.5, 1.0}, {0.5, -0.5}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 0.0, 1e-3); + + obstacles.clear(); + obstacles.push_back({{0.5, 1.0}, {0.5, 0.0}, {1.5, 0.0}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 0.353, 1e-3); + + // Change vector (opposite direction) + vector = {{5.0, 5.0}, {0.0, 0.0}}; + obstacles.clear(); + + obstacles.push_back({{1.0, 1.0}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_TRUE(result.has_value()); + EXPECT_DOUBLE_EQ(*result, std::sqrt(2 * 4.0 * 4.0)); + + obstacles.push_back({{4.0, 3.0}}); + result = distanceToClosestCollision(vector, footprint, obstacles); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.121, 1e-3); } diff --git a/planning/safe_velocity_adjustor/test/test_occupancy_grid_utils.cpp b/planning/safe_velocity_adjustor/test/test_occupancy_grid_utils.cpp index b7df535bb1286..e06270487dc97 100644 --- a/planning/safe_velocity_adjustor/test/test_occupancy_grid_utils.cpp +++ b/planning/safe_velocity_adjustor/test/test_occupancy_grid_utils.cpp @@ -41,5 +41,4 @@ TEST(TestOccupancyGridUtils, extractStaticObstaclePolygons) for (auto j = 1; j < 4; ++j) occupancy_grid.data[j + i * occupancy_grid.info.width] = 10; polygons = extractStaticObstaclePolygons(occupancy_grid, {}, occupied_threshold); EXPECT_EQ(polygons.size(), 1ul); - std::cerr << boost::geometry::wkt(polygons) << std::endl; } From dcef30d97ef30800895121bd3cc0d014feebad7c Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 13 Apr 2022 10:14:54 +0900 Subject: [PATCH 36/37] Add parameter for the min velocity where a dynamic obstacle is ignored --- .../default_safe_velocity_adjustor.param.yaml | 1 + .../safe_velocity_adjustor/collision_distance.hpp | 15 +++++++++++---- .../safe_velocity_adjustor_node.hpp | 8 ++++++-- 3 files changed, 18 insertions(+), 6 deletions(-) diff --git a/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml b/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml index b47ba72e5db70..d567f34190237 100644 --- a/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml +++ b/planning/safe_velocity_adjustor/config/default_safe_velocity_adjustor.param.yaml @@ -7,3 +7,4 @@ downsample_factor: 1 # factor by which to downsample the input trajectory for calculation occupancy_grid_obstacle_threshold: 60 # occupancy grid values higher than this are considered to be obstacles dynamic_obstacles_buffer: 1.0 # [m] extra distance around dynamic obstacles used to mask the occupancy grid and avoid false obstacle detection + dynamic_obstacles_min_vel: 0.5 # [m/s] velocity above which a dynamic obstacle is ignored by the module diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp index b96c3d434adf8..89f4268299d3f 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/collision_distance.hpp @@ -82,6 +82,7 @@ inline std::optional distanceToClosestCollision( if (bg::intersection(footprint, obstacle, intersection_lines)) { for (const auto & intersection_line : intersection_lines) { for (const auto & obs_point : intersection_line) { + // Calculate longitudinal distance to the collision point along the segment const auto collision_heading = std::atan2(obs_point.y() - vector.first.y(), obs_point.x() - vector.first.x()); const auto angle = vector_heading - collision_heading; @@ -128,12 +129,18 @@ inline polygon_t createObjPolygon( } inline multipolygon_t createObjPolygons( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer) + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, + const double min_velocity) { multipolygon_t polygons; - for (const auto & object : objects.objects) - polygons.push_back(createObjPolygon( - object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions, buffer)); + for (const auto & object : objects.objects) { + if ( + object.kinematics.initial_twist_with_covariance.twist.linear.x >= min_velocity || + object.kinematics.initial_twist_with_covariance.twist.linear.x <= -min_velocity) { + polygons.push_back(createObjPolygon( + object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions, buffer)); + } + } return polygons; } } // namespace safe_velocity_adjustor diff --git a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp index 30079fed85d2f..f0c062de1fbf8 100644 --- a/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp +++ b/planning/safe_velocity_adjustor/include/safe_velocity_adjustor/safe_velocity_adjustor_node.hpp @@ -128,6 +128,8 @@ class SafeVelocityAdjustorNode : public rclcpp::Node static_cast(declare_parameter("occupancy_grid_obstacle_threshold")); Float dynamic_obstacles_buffer_ = static_cast(declare_parameter("dynamic_obstacles_buffer")); + Float dynamic_obstacles_min_vel_ = + static_cast(declare_parameter("dynamic_obstacles_min_vel")); Float vehicle_lateral_offset_; Float vehicle_front_offset_; @@ -150,6 +152,8 @@ class SafeVelocityAdjustorNode : public rclcpp::Node occupancy_grid_obstacle_threshold_ = static_cast(parameter.as_int()); } else if (parameter.get_name() == "dynamic_obstacles_buffer") { dynamic_obstacles_buffer_ = static_cast(parameter.as_double()); + } else if (parameter.get_name() == "dynamic_obstacles_min_vel") { + dynamic_obstacles_min_vel_ = static_cast(parameter.as_double()); } else if (parameter.get_name() == "min_adjusted_velocity") { min_adjusted_velocity_ = static_cast(parameter.as_double()); } else { @@ -187,8 +191,8 @@ class SafeVelocityAdjustorNode : public rclcpp::Node for (size_t i = start_idx; i < msg->points.size(); i += downsample_step) downsampled_traj.points.push_back(msg->points[i]); - const auto dynamic_obstacle_polygons = - createObjPolygons(*dynamic_obstacles_ptr_, dynamic_obstacles_buffer_); + const auto dynamic_obstacle_polygons = createObjPolygons( + *dynamic_obstacles_ptr_, dynamic_obstacles_buffer_, dynamic_obstacles_min_vel_); double footprint_d{}; double dist_poly_d{}; From e380e9bac6f0186cab9a7bc50b6e03bc11863f49 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 13 Apr 2022 12:51:48 +0900 Subject: [PATCH 37/37] Add README and some pictures to explain the node inner workings --- planning/safe_velocity_adjustor/README.md | 74 ++++++++++++++++-- .../media/collision_distance.png | Bin 0 -> 27425 bytes .../media/footprint.png | Bin 0 -> 23269 bytes 3 files changed, 66 insertions(+), 8 deletions(-) create mode 100644 planning/safe_velocity_adjustor/media/collision_distance.png create mode 100644 planning/safe_velocity_adjustor/media/footprint.png diff --git a/planning/safe_velocity_adjustor/README.md b/planning/safe_velocity_adjustor/README.md index f51efbf696c08..5d0617b1d437e 100644 --- a/planning/safe_velocity_adjustor/README.md +++ b/planning/safe_velocity_adjustor/README.md @@ -2,30 +2,88 @@ ## Purpose +This node reduces the velocity of a trajectory around obstacles in order to convey a better feeling of apparent safety to the passengers. + ## Inner-workings / Algorithms +Using a parameter `time_safety_buffer`, the feeling of apparent safety is defined as +"no collision with an obstacle even if the vehicle keeps going straight for a duration of `time_safety_buffer`". + +In this node, a simple particle model is used to simulated the motion of the ego vehicle at each point of the trajectory. +A corresponding footprint polygon is constructed and checked for collision with obstacles. + +![footprint_image](./media/footprint.png) + +If a collision is found, the velocity at the trajectory point is adjusted such that the resulting footprint would no longer collide with an obstacle: +$velocity = \frac{dist\_to\_collision}{time\_safety\_buffer}$ + +To avoid reducing the velocity too much, a parameter `min_adjusted_velocity` +provides a lower bound on the modified velocity. + +![collision_distance_image](./media/collision_distance.png) + +Velocities are only modified in trajectory points past the current ego position. +Additionally, parameter `start_distance` is used to adjust how far ahead of ego to start modifying the velocities. + +### Obstacle Detection + +For efficient collision detection with a footprint, linestrings along obstacles are created from +the occupancy grid using the opencv function +[`findContour`](https://docs.opencv.org/3.4/d3/dc0/group__imgproc__shape.html#ga17ed9f5d79ae97bd4c7cf18403e1689a). + +Before being converted to an image, the dynamic obstacles are masked from occupancy grid in order +to avoid incorrectly detecting collision with vehicle moving at velocities higher than parameter `dynamic_obstacles_min_vel`. +Parameter `dynamic_obstacles_buffer` is also used to increase the size of the mask and reduce noise. + +After the occupancy grid has been converted to an image, a threshold is applied to only keep cells with a value above parameter `occupancy_grid_obstacle_threshold`. + +Contours can then be extracted from the image and corresponding linestrings are created +that can be checked for intersection with the footprint polygon using +[`boost::geometry::intersection`](https://www.boost.org/doc/libs/1_78_0/libs/geometry/doc/html/geometry/reference/algorithms/intersection/intersection_3.html). + ## Inputs / Outputs ### Input -| Name | Type | Description | -| -------------------- | ---------------------------------------- | -------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | +| Name | Type | Description | +| --------------------------- | ------------------------------------------------ | ---------------------------------------- | +| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | +| `~/input/occupancy_grid` | `nav_msgs/OccupancyGrid` | Occupancy grid with obstacle information | +| `~/input/dynamic_obstacles` | `autoware_auto_perception_msgs/PredictedObjects` | Dynamic objects | ### Output -| Name | Type | Description | -| ---------------------------------------------- | ---------------------------------------- | ---------------------------------------------------------- | -| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory | -| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | -| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| Name | Type | Description | +| ------------------------ | ---------------------------------------- | -------------------------------------------- | +| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Trajectory with adjusted velocities | +| `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | ## Parameters +| Name | Type | Description | +| ----------------------------------- | ----- | ------------------------------------------------------------------------------------------------------------------- | +| `time_safety_buffer` | float | [s] required minimum time with no collision at each point of the trajectory assuming constant heading and velocity. | +| `dist_safety_buffer` | float | [m] required distance buffer with the obstacles. | +| `min_adjusted_velocity` | float | [m/s] limit how much the node can reduce the target velocity. | +| `start_distance` | float | [m] controls from which part of the trajectory (relative to the current ego pose) the velocity is adjusted. | +| `downsample_factor` | int | trajectory downsampling factor to allow tradeoff between precision and performance. | +| `occupancy_grid_obstacle_threshold` | int | value in the occupancy grid above which a cell is considered an obstacle. | +| `dynamic_obstacles_buffer` | float | buffer around dynamic obstacles used when masking an obstacle in order to prevent noise. | +| `dynamic_obstacles_min_vel` | float | velocity above which to mask a dynamic obstacle. | + ## Assumptions / Known limits +The velocity profile produced by this node is not meant to be a realistic velocity profile +and can contain sudden jumps of velocity with no regard for acceleration and jerk. +This velocity profile should only be used as an upper bound on the actual velocity of the vehicle. + ## (Optional) Error detection and handling +The critical case for this node is when an obstacle is falsely detected very close to the trajectory such that +the corresponding safe velocity is calculated to be `0`. + +Parameter `min_adjusted_velocity` allow to prevent completely stopping the vehicle in such cases. + ## (Optional) Performance characterization ## (Optional) References/External links diff --git a/planning/safe_velocity_adjustor/media/collision_distance.png b/planning/safe_velocity_adjustor/media/collision_distance.png new file mode 100644 index 0000000000000000000000000000000000000000..5e4319b0a21ec49121ef9e9d3117c03fd363af62 GIT binary patch literal 27425 zcma&M2UHYW&@QTois7h;IiV;h2AG~aD(d8%^PuSDob!kQb3##a5ET)#7*LO5J}9Dy zIp-sSU_ikDiu!u+{BPZR*Sl-IwVGK?_wL%YYkyVsRduiCu*iS)9M-c_r%r!SDFj}p zPMzz(^MAv-fxr1~mAO-=kz-v%iOcNM=uPTQ(J=gk~k1RW8spiT#8>Bkh36Yl#^y~v0OHAc{smO zr*-Re2pk7N)xe-6qu6WYn04!pL6gaV#z!-HGyyza=(ISD z98g~<$-wYYggB)Opn(F_8ZHKH<#WkmDo;pt7-3Am2d@A}F$p4pIH07-6lT9jF2|!$ zJSt$9`Vb@w+3TcxiF^t?p!15+GOk=_5Yxm;j+bnsL*yDrz^WAcon{7ytuld-OG7BY zNpS(X)2TC?X-x23AjH`vKEADEGMa~Ju}V=&otP^WF!go}lZJPRnH(~bW+wA}a+|^v zxZLIUk`NfUl*e+Za26LE#qjB+Anr&)h8_H+_^m2CPT>-I*)${Dg7aWGFuhu%mm5@0 zw^520bNnu(3c&dl8mo|K7OVYiJ_N8)RT8mZN_V+2CX*G;#F?c=qeu02~j{d0Ylk z(vmj5_JH| z_hDg9A55#(AUs;5*++#!d}z1|je|+JUO1CUqfcYUfG!sU$@CafA zT}9V$nO>&^17*vR2&2=&vZBaBlb?-{BOGp;T1Zv{bTz^$#tH#3g{(7xa;-wZ3&o28 zg%ZjWibw$)fv8hKu`C(}pc!pigv|#IvXkj{9}GO9(HJPJ)D9(~%mJgpjbbQ`B$@?{ z#}Ftqm&&Vj_*rC=%g>g{;dZweNq|T(97X_!#8MS@Ekwld>*;>J)Xno)^gNA3!v#gM zX)Y^5g`oxLYAz|@^{ZTf03q=isDPI*5<}@sv&82YBH=KvfiJN-brhZqr6zM^aw9_^ zRAR;8675z$*Ud%wm0)all+Y37U??Gvpko5{B0BFyK{5R0_dp0jE01G$Kqd z)RORifz^xxvjBKrmz^vH$Zoeo?{N9-IGmO*3Rp=daB(;&K-1dMM1jg9L+UVafsO6bPy>wMR=NST z*&+eQ$P^|TnT9lw{C<|*XR?_XF24iL#Ta!caF_*)r?Alow;4fJSRq&_!zNYX;RY>L zNhX5-3Q%aT$f(gGB`P^6i=*{`2?%`be{#_OB_WXr2&+gVFl)3li`-#G`yo8*|Cx}Y z5jcWc;ns@ufoKGXdE0-R2oahmP42xwOAs@GkAc}$#pscCVIfc zmKoR{vYxDz>7fD^&nnS6Q8tAIsR2Yv6~s%BGCfkg*^E)3Xfy=@qO-%LWRPi)q)L-k zPLh+k!K}cvQz&Ac#YdF}z^uffDs6X5uuD;ywq!!I_-2ai-3~=TCv0@BMFsEt4(57^O#n+iOv!Ryupm&SJO=_ z0BhhY#aKTdF7QirG@8X`AnPSEE|}tRb|FhelbcaWCEST5@%?U+2=KU>Y`EB{befnf zr$lTMAjM>p-zhbVi8d3{4mEHMBA*rz!vU^MWw9G5TnG+^LsO9kH=G3^aeA(shovJcW|T6T8H2qe(-bV|X1> zA$SfRTE9)=a{1f<6G0TXF>bGe%Ru?H7N3J4hhYV7v4!FH1&kJxSLI=0h*%ukAw_Zm zK0F+WVbe7N8Urg+9^_+Gt&d2*3J!J#%GR00bSoYsfNE3rh3mgL1dacHjA z1F&F39b79QWBpjA7f&J@-YpN5fWl~|6|jz)2C7BW@` zw-b0`Nzg|ue7@hrRKuh?4ATWiyUly+Ok| zd~TFQ1fDQC6sdzImK%K-hmUM_1Yi~$MI|%Zbs8TXfLS;)DbfN%DFgvJ8>0ppuSDf1 zc(g(K$dPOO-as&fI`M9=EPxRB_>e#VE_5*H7_9`OM~aMAlT$7vVjWN{V4<^F7=xAx z1_PW@tWg$z*ywqu3~W9Agd1^|39Ls(T2AG z78#sMCi-B+DPD~f4n`F4fc$^cdkD#Db=e$xuu|y=r5#is;e>ZkeFNV>@bis6G>`4Z zz+5U9jpl}0P(~XG2PJT&Y(5qxuwXC@KTa=&=nNodC&}1muaZL0<77dqkJ1U%5Edzz zaRoAo9s>p)63xZ1Y3VpCM@}$c`9ypWyjrJLs+6M{WUE#T1{8vr$U@^pXd@FvB$yy( zAs;Dc0ak<1LxAn}?x6c9fhU;fr||g~`ZN zXgZ8WpsQE{mH|OWF+~z~01LOv^v(c=WaN54@CeCJjsr|OBoorcV)|7^KhEXbk{m1u+SZFpGk?=E%JiHH<^0!okH;L}p3A2O(16L0^TzFlq^(;)IBT!%-GA zk;v8PkX*IeF5%mxZiyJ_!pO07sTnF1BPCQWi%zg$P#hxC?k3@#ZU+X5m3qlCsKrT@ z>6{#?j3u*SX&gU?Xb~8NCKkbnGc&<51&?qm#26^nC`E~w4lzf~WPvdOC&3%_CL_R< zNG&F`O>Pk@MO+h}hSC{jBs5tRumfsPO`HYKQ`=oE3(JM}s^LBn)THIQh*p>l&*9S? zNDACQ#aNAAs87Lld1*X@#U=<6Z3+y7CE(yloyHNAf`_5K8kk<<5x7w_5JABu8-fOR z8?7FORRx|ANMJpH!;8>z43FmqFNhHiv)6&Qk_kqK6fPAb{WfPnB7~bA4)89E>tvXC zfXPY>axW8^s1OFK#^0ue!0TpGl8)TCZJ-80TGX`vCtiUmB^zIxS3)sgkeC^ z5f}#wX9!vWi$dy@mS8?5k=zajLqxQ@c{06>qjBP_c9TVF=Lg6pv>_l1PW@T z=!jq|fN0QaNmz{;N3~eB!4`l3!Z9FWWIF&=nUo*^0g=T>aH@26t5uJeXk>B{#R3>n z4jKYxaXZ*%wS*~la2RAG_^gSovywSHh@HV^`=l&UKxb9q1>iEs1Th}PVFqgpC|W>~ z2%!cX#HApS6et!0tc(I)304<3AJ7>j?Q6HWeOOKq=X3w8jHj2MqBA>zd(#OLBU#u1Vzay3_ZrGw?OP% zIM@P0kxV?hk-{hGU=F6qh-IkxHW2qDurWo21~7b;LBSHr&{!9!EzX9K;e=E*1wxmy zT`n;B;3#${Q)$za;NTP6xBP`|>V zC8*#Qp~Fw3VlY^<5e4vqbd#<2`ZX%6S*F97)gT~o0SpH#(NXZ=O`JgN5O@qM0h|GW z>y`h1lRnYtMnYtBjR7Iu!_l*OWTI)x|C#jhR+CBJ(NFBi^C*!`q3p=>aKK_V!Dw1P zpG(wpoH7=KfiXkHay7~bgVE@Cf`Wz9@r-09%7a4ki2^Ci2b0;2Ho%3p;c-+Lkr>&IBaP>X}GKeLygTTZo^*Xm0uhbcdZmiCO zV^RV}o>{9v^1VzoNiL&P#Z(zhC)6pBRM0nYD2CqZH`!%6i46`AMI;Cy1rr_->%))? z;vf-&^Klk7j}LcQK@{WUXf!2QYS2wml8a}TJI!P-5h~@9q$W7us$r_hb{^i(@!8!v zgO|^8yPZromcZ6P5T4*}lh_yr3hg1mI4+EufJXXdA{B+h2r>X23ry`U3z*VKNT`&i z(ZW0=w;ppsaKWY&$>~K)L3hD<ZG% z4#`jAf@2 zscxQ7tj1xbWD=9ZwQ9LIo&Z6RQMoca39rEVR8}JatQy!xKa~pB@_(0Q16qT2=X6FW-<5tz9Mi8x8a#eE`h_9*bPLZoa^u*tvajTYqUcZ8j^vf@ao-Q zg+?a(02N$`mw4m|HiaGxuWGZEWFupFNSjD*wL=9`s+Ss#q+7m zNW4?6W$;iEC!6O}((Mj2767*ZZ3ioRlG_GjnF(|WD=BgBMCs*vm(dAB}mx$sBgW!?dlvYx} zj%Uyy2C{@86w)ausZ${IKy>5)3J+%?`4$3}3$|atluHxg1tOSHfdVTsot;1dpU0XN zAeq7wSO_{=>|!9P)&PaeWg#snh6!p>2mqc*&5*gFAQ%BXnxca$gHhk1^~fAPJqFID zQzRZ6-YQVoIA}FS%z(P7ZhtWAX5z!osy$A#n;1Q)ZqsI?6NJs*#I@tAcE1W*Io`xf%-~d?z zUeIU^CM8?U0sR>lWN~JX52k@KU^HovKv-!chnmH;BXM-U3J|#+Laf|jMT>|Q36gJ@ z8%+u_2kh~(zz4zC3nHd104Io0TsTcE69}<-qz_N_>WE;^p5~&c)GD*a7L@9MxTO@M zgNMZ68A1n41yvI$XbQxFb+UDEfWwe^xn{Uku2ZXd5F1v=Rsb%U&mGjx>ZAiAryii7 zY#uuc%XF)x0VxseBmo|*P2_VSR1OV4*dMj)XgZ`uPIVC^0S?=wwV72Gzzs@pv7r(a z&qq;k9XdInWE+$Wn_S5gvx0$*hG6nQ?`28hO23Zbp?gd~uIXEWger~=Or>7W6t$_Q19IRrY2jsr|K3xw^a6C_NxuA@BBp*<#$XyJ@X3BeeQ z?%=`3S}Z!0lT8u%QAW1GiPM5xL+~<0N+E=e;bJXdO(qTmyF9!Amrd0Ak#3dRujMIC zaEUcsAIq6l)P8aJ1K^l@cHrK1-++_+(x;Lm=VHWezwVd@sas z=ou=f7Yc|upawFU!UwYjECd2Z02t+dpM#B-lY(J}ZB`hRpsWDe=FmC;4~#`XsJ#Ez zh5|V9zxSaqVl1{-Os7txJ5dQ(fiLO(=B@<;g+D4Ial;x0qzQTqqz)v6M-K?Uh!d<6 z|Be}S$=In|>HgBEXL=rs+O=$PhU<7#s>|E6`Rwy9N2;HlKI+weIPI+WyJnWZt0}bNm`|Ctzk=7l^ot411ogcP4f8eEX49>`yLedCJ#jg-5A%;~2t+}V7x zzq9c}Y7HZt6FMNJc=W-A7h{ID{1jl0JRK@8ohiT3TkpP>%PVnI6;}hB14Q3rU^q<9zwvNHRktysgjq%q)Dhkfd zJM*#PRCh&T?40?o&)*$<%P(4W?D6QSV~DdrjW?}5w{^oM%87|Pc9?eX!kG{A@0m7w zP{%1BRJT7i(yH};?>;%MW29rrxxDD%s`d@AV*TUq@|D;`%gx;z1-nEs^r_3v@mGsS%X3cF_I?sDw&erZOU6s+vCsS&K)&U#Oqqp-2s@v{WuCwXTeBbhrwL4 z=GFYY9Qy6}%pIJYA-p}f9uH4>_SRn6oM^aL8ucabE^TFWN$s$V9KIvb#p_tzxX2X! zKlN29rx($eH>y>YS#uo8RZ+Y%hM}>Nami(#X6^f*Lni?97xi^_?RentCFjbYp4~Jp z>BGA#52osTp&2<}K7hN`8C=-qC7m8Mzg*OZ{t79jJ0{Nl5Vf`9UEGNSJl> zb*(wSzq9$^{YjI8cu3n6w(D5I6Ej!Aiu)CCci4v4r}N#c^zvw0b=AeWr3X4}KpV65 zk)!!Iel@0T{#IDWY@^lvIA%;+; zyL4ASZDt-ezhcEEmF&vsd*{@zkI$bQo;h&dF`qAj_Lvvp>iNeW{ZnR^gf`A&&zZRL z*NhCVp=I8S`u^$-*~mHh;~f(N`}>7W0N$Fnb@;%BeiG>X9o3_b%o;W)?WpbYotMsH z`QE9#qe;<`V>SioLIvw|!p)(v9nkL?@>Xoxbn`iLQ+KTms)E?Uao8Z9mNO7xuE zrG18t>>OS9W=KPLhZ@CQL+~?)kKb`+aQ5L$`pP-i|3mYg;>{H!#R^tbYe4^Jb*s~9e?ucsmUApw~Y@$fZThG4Z@@ImeC&@_&;|@7w5DL zt{&{U_~YxWYS<3rhV?VB(O54FRih5G!8r_v=Wtz2R zvRuXU~JBks)|t6O|-Nma^-I{npR?A4RfPPt#-j)Tv_ z)hEU&`Y)X^bVo%&*w!z@{#bNa$b8R-h~_-6>0xwb_T;G7d$Tq!A+9GFe}8^P(fhD- zFxxaIirI;)F%5}ZBmP4)rEBQ->9AQ--Nv!Y*A8Sfx-xRod!9|2-CBpn;f2PLr zPwtz2^V7r=4JQ{-KP+x)FI@Q{El&J&v=%vIQi;3wucHVl5Qo|wlXH*aWaDFb(*`LPzP>hC-MFr*`QO{6 zw9HQ14oJ&oqvkitcZ-g$_?-N?!4%=#@N5J9RX1ksCQr7;n4Xz)q-*x5eI3%G`o)L9 zNY&YA`;ORD^8WBn?4|h$Km0dW`QIL!;&?{9rmj7Ah_|Qq(1qA(o)2N|zf5gykLT95 ziRaT#Kwqn7&)WWUXzNer{ihRuHwNcSr$Rv#HKSu$bPuTsiCuwhp(TwHWzY(n|)zfRZ0eyQD|otxRV zsQT)SoZqr9+0%!${y6-azQd5Yr}@sS)31y3#*>#55OuryHFWOS?Fk{HA^+~B^4f|W z4&Fb89$m)`1K2s!lady^jcz;Bw0LsM$xmP79PfqquU^M5%wXmtX(eSnCG*}5gVoMI?;m_+`IN_$vyo@hZ@wa3 z$&Wuh$=pRbJ^8uvR&znij&F1}_0X%e^rhebf~{)Xb0E9ws5oi-$j9DIhaxTw*)(dJ z>oBmN+&{W**|Veo(Ba9aQqT3Z_Dul~d^ZRiaD;VmxzyG^T7Aq5rVhXK}yCm-e<20!5{ zD=W)A4Dgj#M%`)>`|B#X{J!hpst*bCi+^8#e)V?eBcCsdk7enVbwmDrTJGIodUld1YCl@t zuVq^4!qKVM6Xukx&)tlz&PQ4|-P&>C_~SANKlNc%Il1yr^y(h=?zwZ;8Q(AMiM9q- z52+ch(DaW#+qQ(U1rR3vy1wZ3vYwf$1?QT6=t@_v6FvD^NM?Mzf6tuw`beh+%i}ZG za<-jqZ!KROlQ64K-udj}e zKXorLt6TD=-}85t7Ciqo!vD3Ze%-y)zQa@7+MV&2A?oL@`Y*RigwwVk+At(x_L=q% z&bHR~*NYS-6(a^lB?Q?+ki!f~`Lf8I^{GqA)z?pD<$YL_D|(;EbM0Py>?CGu=fSbF z2~)ebmmiPKC= zeeKn{e{RElahyC2?dzSrG)`ResEF+1ql8!H0|lR;}{G~P9I ztG;i&+SIE~+YhFk`MIJ?^ZhHYdHWPQeS+`Q&AjAQnh>9@8D@3Dle{ruGrW5>BibfA7f z=eeUHlW)YIApMNkaY>&!M_Std$fSdfTmPG(VcogorM)+b?iWVym^i?joJ*@-kKfa! zgH3J?YweuUchpaj#eXclwkp5!=Ie>Tw4OWrl>CWfF9wHttj{O?d^ThvYWl;QH=`&0 zZ+FLsisF;vZ!g*$KhiRDu5RHW`|Cj%-;1po6Xs6lTsdMG&6#XH_HuC!dlxtEalQY` zvb*$q^aHbg-Itff&8px3XO1*kSoQF;6wUhY_$z{mv28s^j93ll|MU01T9V0`N1 zJL$Behsiauhs%~RLCz!Ryj|YGuPD|~Qn#s`fm6?<3HFJQ?{Jl$^CydEx;EY{5#qqC zywAJhKhn59ytV$u%;uNM=hX58^`sHTU&X}EsVDXo1X!P+RbUD;$lW=LuWv3jlno3L zH!xG(i+enQzc6=_wjgo*zh(F0o{t98WoDKb#oW=b^UiFLBN?W-pdGv7i%2mK_gn_K z?q$r#;pExp4!7+M+25}(eeI;E$a?00g50D1kHFB46FNq>4og||3p4ro>8XUgvfYty zDl(=`9<+!xccQRSkdfY{fj@CuILJ|^;s5x+x$sa*rVP30Xu-4gjI>wp9~`FFFe7mutlJc6T7| zS&A?gnAWMN&mW=}W$WGfiajp(1a4T8g#GoyGwYtkuj6<5uO50fmR5ZzYDZ;9eB2sV zvFzNrzT~QgDF*uM^rIKVjKy6?7FUeuYY43WZ)Yf>Pa2`G$N!y~^Rw*nNl^z3W|QVU z`sjC{)2812``Yffxv>jYg4E~v>ARU7V$*E@8wqJWdi(VfTJ@-hRqUBR1)_Ztx#sav zqj{KW1QIcVo6`Q=Yokl`G|#^Fw)!plA5Gltq^eEP^kq%;o4ti?!(f_qzY;&)_%Vh! zv+vP0g>l)lAG4;qEN@%u@BZ9rYMhN-jd6dCYGfQZ{Z)qfR1xv|_phekP-v0$VJID-EJKLcXaf` zkiDY)o33O|qHn`3SAF~Nl)IttjCnQIMOSLfr)DdW)?L3!7+1w#$+iEOeUt1l;ORX)a`Y@y~jJ{`!xBs@&19$;OO{EQ@wYE;)~ON z{x2DzGMP2Gd$;`CO3e6?>$!@rH{!lCmwtX6NN`PR6eKL}RMCFEd*tQPv%d@>#Bc8( zXY22!4o4x!EjZtGFy(62lxxomlbfmjuMf4aZ;~T3>kH?{)d|;s_Z$}Nl95D z@&0+~G>zOoqt0}GNB8l=I)4Gi53`>xj7l37)=!?W z;Jd2@;jCliB8*wpo2Q`~|NZs-ZG6Ozk<5iYWe)u90}fc6419mX^n1>N>W3>BYzjPN zWAXv(*`LRrIIkyM8lcV=C;5pd-EXtr@2=T0bME)g4P)a^eZ(EBdEE-qz1*?`l;);I zzY{O>W3dU$rx*?oz}S$#&JM;|rZUE>>u9lvhFT>5H^XxSGu@x_o3 zLFY2KoRU)tre;mb1n-9@F~U9Ij@;UcUAiuQ{aFw%YH)vf%Jc|F?y?V3O9 z;La@zr$*JjT9h}2xOQ;XZ5e@magDa=PGvCuurBmlRi|7}sLp6Q^=kdJWiK}LkET&d zlTNQYwx&8CbzRW^$IoxiudU2J{(7QtPgr>VaHJ`X9)DVpl>CVpaoYdAVdvGfG5F%- zw&o=<3qM^PeD!VV**Flrh*zA_+`h=#b(fM~XNs@rcW#dP{p0y>20>T3e&p+GsOK5K z+v{i6EoIlJo}XE;LwZCr#x6+3$d6|yO}H5T0;bs*ud2R2NqnFh`u3E+g|>OByLX$6 zF#GJn1$=l#-1V8iPx*lvt~Y73?H31<_%WS&w|yvYV(b~Pr3oZIBtqTx%CqSQ65`Td zHP^elV!r_8A$$8Y4hXrKT)lBsTS>kJdfV4@`rA6`Ug2KR64LA=8fHsL`^SI!UIlfV zJU98%^3QK>;s;HUjpLUvs&3@;7_eKgd()Fezkjaa+$?H?@Rz;VTY6^G&ceVIUE>s4 zS(Utj9Uew-leddbd6W#8MQy=c0hOeZeLO;ImmJW$8Kfft{?u=JG52r)6 z9BOZCS-D(QU$fcq<*qn?u&Me%auNul4~$CwmO?FldC!5WYk3HHrR;yIxaT*X9GwEX zr13`MIo6o8Igzj#~KX1y0d>H zam@f=<-hYU^j^X4RWu4RZ*f~fUfCSwrQ*~T>pl*u`7+hrSX}Y$4s+Wp->}Y<8&UGN z(|}>;mmt?(yaFX||Lgw##QSN+eHZ8$*~i1`tLyIeigcvMFmZ*fPvJ8jCud#FoHV#O zCna+IrrU{kvaXEJ-r(*Tn7u9x^dHjJO$#KaW@w_IW9pW6?@I$qkZS0S!0xq=kC_wk z#be3qt0ki{7ybHDx+u5rfz3JB(XV)F)72p@SJ0(&irQw`_UaL)Hb!v5x~QC(T{hwxex!)6??% z74CTl>RN`g4jMkq4iks?zI{D?t0{Gx0jSJzItFz`MOJ~ zP3QptzCu?8wx>=yKcB1uTW$rI70amX!?!ZNzI&Lio_KN1z^f2qOLMO=%A*cm*oUaE zO3)K<$YtvDYg~UlNvv4S>H6+dW#o0oo3fWJwI64$++olEC>_Y!hh0$3+PX{m_ljAp z@hkc_ zicp3-m-_)AT`xKEctX^(qiMgE9jmTK4r=@Q*a~~XO1`vo$8}hjV*YU?@9zg27X!x$ z3#u(+_P(0BVir8o`L`?fSq>O+J@`wfwtai|kbj{3!&F%Ovv+O7mod*(PHQ@)EW)g= zz4V&^sTyuQB_A~*r>Mu*3(40W!@2he>dNhab4JSeoI81_z9Xj>RlIw!KmBVL`Lz_E z?fl0jRrImg)vTVmU6Ygc?b5*&hr=m-dxQDpd;|5+fVVcs8O+dL-v&sBRb+*VyCt^O zPtE=LIP%u7dt<*G6&8@eJXE7>ys`=m0taS)ezR3qtu6FxE`RQgD&1T0F6QL3q4~*k zk|t2Q_8fm|^dsfSldcBEtuH?RI`XP(qqm=9T}VqfH0(Ssux;})|LW+0H{LFJ+_Pm` ze8X3k;Ggg7(uaLvXAIj(EbW=a?|rATfFJrLSsV_AS5M2ov}#MyUL_gd>300?g)gI4 zVYVzLh4cgN_HUZCz6n!bpnL+o8I}F%bhn-3vCXQO2VHNYPp7_bp!S2tG`a>Xsjwyf z6*=RHYb);8vqhX4^G}{_-}^1IhxG9GoQRMkN#l}2i`sHtjYMU&K*F0}t`il;XB|51 zTxY0$h@G@xL-gsLii>MvW@cXz>8usR7bjm9jyuxnh`Z_8=~R7NaXJe+FHOweIyUb} z^xy8uQyzS;$q`*wyhTTC9a_n{(9T$~bZ%`*_fe44xM%*a5wW>vPt{(_CV&0#WIT~J zVAFcXw92W21Ccd(N^Bj!lvFozc3r3bJ@r?6dI6IhQf_3kwOZvdomUl7pl<_XaOGn>OW0 zAL`OUJF7lL_oN@XMO6PN*?ide!_wyb4n1+GUL8e-LWCno;NJG>(s<8S^%*EXTB3V3 z5g_9yoc_hi$6onIF@rcQM!0g1Td-|=@#KmC82Ix%%W7XoWn@RUt{E`8{fZ!CY}D{= zp;PMKgM6q)6)C~V`j4yjGH#~XQbM}k>c!|TYN2jec&qGM1Fx)NV-e2t@Qi;aBNAX+ z-VGeB9Skl%ZpmJJ{fHFR_Z;VpAFU*gVtwD^f*EV05E+}6O|DEGt_|xS67zSj-;lfe z3jK9M)4kUd*m->)nRfGbF#7eEb_Szkmi0{ArO7Q|!iMICg86(cpf8mix}XXlT|5Q* zK(OOgXLg|fBNu#&bK1rYSy_e$&`ur_w@U^Y44!Y^yIG1wC(r4%+AmHo-=2u zsJka5sL$q{MbniNfXbyZ!#mrkI)MFZZi%GNqc7jTyoEI$b57^p1xXKbV8f~s z($Y)M84=yCRPIsrO<7plH~rG!!a-k(^=E4~UM)YmK2R_!Eg=;2zJyboJZ0Z=(z~55 zjc4=$?(T++O^;hPb1?s)P`I`TF`zIdEf&7iV|2rMbsf=F{qcvRFq$B`y<$LW4r%U(zwq`8o-luBMU*)RT zD>ff?EO;^LLXW2zA17`9BqPvZ1L2CiiL-9RpKU)3Ncp9?GtN&* zJOc!8K4=hY9P5-uL z!Q{rGmgSmZ)UbjJ;wRh5eIw9BraSq#uCT0A+41fdr!%hf55IlypY&C4$KY2CF+bWf zU|?fFca9+CXZAY#`8Io|xxI4Yr?Ch1KhE^#m#MRA^2d8;{g~1}s9;>w#kI8?enzb% z&RDSLY{8wac>`ialr`Cyq_EaUC(Iir7anDt+*COtZQ!DP8^`?qaqUjc0l_3!Q{993 zJ>(^e^A0CCA|*34@US)G=HZIp;7%?g!}vW?JNZwxf^XW%{TbKtk!I+yH()Ux`ISTI z?>w~QlOuV*?C83C4B?bA4P94HB~D?awszX@$j#ZZ`fv%Bir)63`~9AGS)6ZEr-r;e z_O1$df!K3YwtUU8>RatKJ#weI4ouEJPCM4rmbhl%)S5e&Ek1eOfLPhAKJB)Qj6D)u z&6(QX(e

o8Xhq) zHD=-4{m1hFRV6Fd-7I)}cds^V_Uo{aZMV{q=I6sl%l}hO zut~D|VO~p4)6UD!W_8NT=RV%OY(u(}tMK$^4z6t8TY8a5i9mjS_wY#Su`nz#0)Fkx zf*-@)XZ6q29U?&S$NG)E&N=V@pKk$#r8B<287r^u5fPIkh_0?~HIJc{JuuwelY8~X z$lZ&6|0=1DuIEwL8WO`a+K_`wmM@NHJR=Yzrlek%L@x}b5ToKBgsMg&s4HpgiF2X# zn&++Bp^tMPMr%kHQ3>rWpJE{U6shmN?fUxG$zF{)HXqtRZo7Mblk3IEoK7j6esRLm z3q5{`Lv|VN?bA0e)0-*be=(}S_7LjPr1w3-Cnj!AZmx={%%({Nw=NWe@HE~|**&Lx zY;^ysDJ4H{eYro^ntZMwu&^Gi1)o$eFT6OhW=y}hgdU3y_P9LNu&GaWVeJ#flcBMr z6q=ZoeSRrJ{sDUf(b}ZhGZWKWI#fzLPHpb<6-1kV)%agm;F5End%cVb<(+Q*cWun^ zA-6wO=fYqy%aLo(W|fWGqu$uP{mu3bdC<(PrmSQojkn3OV(t*fcyrI0(7aEJe*c)O ztP@qu2suzZUcG2okA$&FieoVt=bgyIx=W6TA);M6)6-Kylq*J1z{oAGK=D;@AVs zctxm!WGwlsNH{I;06n9$EDxEMUOj!^uJ>SVkV^{Bx!dq!*%n#!)R5II_L>>nYe#J4 zD>V1=?^|Fga%TM0;3pUfm|maG2{UsN4pJF6OV4Bp9uyvb7P#8F64-llT*~2%wI5}; zu9I&qp6~)|=+C{}>Rm0m)VKP?-MwPez@x1T(Egua?lV7Tot7`lOk7u*-D#q1gk%5O zxRaOfp?OwDFS?V$Tfx+nW_pVY+6ygG%pWj{co> z4+YEbn|5c&P6W*-!it5SP)za9pXA8$=hcX_&-n*h^(8nM`9-jX3;O54k zjYfM2;g(02*J<0vmbFa-d4G*30I#y6`=8X^EV^}2j*sa27gnLEtAF{=%HfB{N!E@? zD%!l-6$4p2TM@EvMq6z}#0}z@zrOcrnV)>=Ww7GEG&yz4P#ChZWyvhUQCfSL50Mc2 zYhbyvV?^gKt`8fezGo>POw<^20hitmWP&3;bs1F5QM49@jiH zMFsSTIJ9nP-l#L)C-#-GyRhf`Tprp2lR-nyznZpUS$Ol(-DlLfyL#+dvTBU2{X$Lw zW-tsnCtY`|ANAw#?VFtoF2!t-M0-MS#U{jl6^^f?pjJK1Yg-rIqHcJS8MA-SEcrD* z*mtf@*!*g5E9k$<7a?8xeI~?Ky?=Z>zkS}4Ba|r@K%(5I{bKQmZC_^mcz5K|hO0pP z$T2f>WVf?s<-Mxhfch!ly^Gif&{K7FwZCtb54kq@*3KIE{n~H3h5^6A#^+oq2P>`Q zyRE6hsIh4u8mPOE{kHUn8K3@b*h}4OxVJ5(;LgE4(lc|n%m=^W@O)dDyJ7BvDVT?& z;KJ(a0Z-oC+n;|^bhYt3VgWW&!S^{i}4@4NQ1h;YnNMq}yOw%3_uo;vC$raHHmn-)esfb=E7+M~R?((nz!hHCRc1~U zZUXyTLl28FD;7uIPUo3_KaJ5CPmev5H>7p)$gE!F1?G^rw)TFB=dUje2m8J6%0TTt zS^B2-zua{Cbyi~Uq5N;TYeb<8?47K$*|U?Tx}>L<^=QZabLmd%3Z@~Puuin#pTAH! z4f76=md14IH5b-)DGjouB(+CD(dZF(Rk#Nt$olm$egsAL^Y8jbpyu#Ye<)b%N^q%;m%WDQ;7Gq8 z{>@GMzb?hE=nZVVSv=0&c&AEtJJkNE39SBa&t9SVOLsDD!s%6GOebTp=UR(_lPWR4`09exmeXStEO~|PNBIp{Tp&hPszbGTPAG&V42i5vJu$qgg+UH zUHx!Q^2p&&!2l-dT%o1Q`z9X?P2}$CQnY+=c+}3DMF9IYIY%&l*sNJkipk;XRO{gP zLyjPSUy3y4o&e@-Pl>oY=Wp9f$-HNWxL5OF%#=9mWW;qe zkkI+qe%k7XV-xqys8bFnZXNKx?-=>pwJ#=?@#qw0)caMl;@Xe9L>IO&K5aN^yq{MV z4Od3(YEU!!%EP8^vN0zbUphsYC5ubOx7D*K-GJ^hzpYpVjNzObpIz3NERuC8tthzn z`0dY&@1LEX7SS1B0XL0aGF1}RXImj+?&ol@6IRZYMJ!kwyLz>M*j&hRlHl+LP?$RM zh->QDp%o)~o?U)B`)cbJO-?~QG?P{wQ8CmAz81KA?ECr3b-6?TB`!S*e@R?BC>K7i zy!J{a-Ee1@e4;~2sJ=d+Hf&JM&gn6gdn?w15-+aLoV%XU4Oltp-X;Ds2W|T zyMDR~Uw1h#`YrSH`%`VVcG3#L8fXSH=_~N+_EGq}wW~he?bfR+a#h9(-;}75FbepJ zX8K|J?wL#`oAxt-c6VsZ(j2A#!~VoGRsT(Di*vLZm}LYU#!$q`O-}>4p_V z5Ll$Ub7@?V5)dV&J5*9wKthm^4naVr5ruaa@8^9!-(McS!NOeE%$%7y=lp+Xn3|Wp z$`J-C@b*fnft~JWy)d;cm8eP^a;s)H%dF3zKBUmwN)fDGZJhE*sDV}CiFk=f0ox)e z-224Fq`-n0F5NSriVyG> z1_x=~v++QFz6dS{9wUg{-=p+mXBqH-b`8-w3aIPNh$wn%)F)&i#O@%n_(9*nAazNv z2-x%4o8CW92PQFaut2z&C?j?H@m;8M`@@KGPu1bM`~_aLcx29IJ-qX|f`~vVSDDYS zDxSx9%5StfB^rld9v=B|=~b{-&8LO;6X8`qGU^9N2}m;|C$9*R4E;xp~G zs~^ulTp!t^CnccKh?Br%-dZO|ix!FW5(fT9-)wUjaR@`&fA3Tz5oCM4M0-_77>Eaz zHhhE63(8WcG@XK+QUw{(@9MbRG^pfKlhd6{FS0LDW4nzWYhB|G*fwsljJ2<&aM{DR z2D&j?U(PjA1QC*5g!&KDs@1)Ciqus>)FIqfSx9BRhxPCKMiiOqaLTkOxv4l$pjY{2|o9#q~uGp9_=jBSj8BA zpP7M#V_3211_4!a9b_7X*jY8CiUhe!;Ankj1 zIHLmO^5Y`!0NIYmu}nMy*uF$WF(NGxJ;%S!kUfy&ixPIBpfhRvIRX9svvq7bTU@|& zcFM4Tw0BzlsHB#z^e^Sf^o#;$ot7uFLX)yTflMN%b9$ zQtHIy~v(oqoGYvbFc_4!b)l0*Vzxd&DOaA)zR7lA((yU=9wLv z&2oj-CT&J5O=^qGktKaCXPz-ccVH|j?~|y*-3Yc-8$@n#{`sn@7appiKQSFSx*Pp( zxK@L%pL32>dfEMW+~4qN`&S+Q_1-zPGq=6EBq7%?(z9*0?Ep!5IL$ZD%sxb&-tVfH zdBB;a`-OwfZn6GRa>}M6PEAw8Fyl!BUGG4`!-RqvBFJ)uUV*G>bgBEmvl&ZHp!=lV zbDeAPaJEh=+t%osbF+MMj|?msscn1W=7sl6$*C;tH;1{CDn7oo*3Rg!!luF}Tptz> zyvqM8heyYG4sXSJzK+oaQq_iTr`WNPjP0G z>3mPQb@lR|?bZJJ<(5BC#Yl|HjWpcawmCQljyFt@4Fo8bu^A0oAg4DtjVr3h)os!0 zWecf9lQ0Q%U;eJ}tF0q^_6BGoyd~xrbTI*#6o+Hv< z9bVo{d~kkK^Wgc!oGaq+oE{x9yvr zx-LBm7U9@cR3RMtqmkg?!|dO#@nTYVPqsvQZ>H7rF9m}|H?LwqP$S71eq*Mg74s1D*~%Gu#MiQ z;q_rO06OOT89XOw2Nv2xn8djZqerqvNi6wRJwMw<4~*olUDyK6#Q+F<5E>qnpPj0Y zx72!}tu4Q(o0d12r=Ioj^UMXV-YB;24nFv7Y)!zBpPL0d!keWom3IJM=Hq{s!KeqE zEdKy6Pgw=@2E@g5nEWF#WvG!yeValv`s+v%W}Q{1L#J2ETHuV&f~Z}-OhA(c1jMu1 z;|qgH!D~rk>y0y3=_4=12>qH0f=;r3h0KX=X7U;jRW&a@)>KQ2xd5`Xa5SOnk2!(~&`S5Ph*vR+m zT>#Ao30(@v@%l!lkm;4D4opCI^6iQb@#3azBS3z~`qMNP?6 zJp7_Hnpbo8KC&doNRGeaYIfVNO%s*k3G{6u1;d*43`5=2GQ-z7AL#!O>-_CJT^v?M zowWhWGMI!Ae0Y@!5zUk624Zu6vSuR>qIET%SGzpz71YqP&U1oKVr~mG?!ULv1NK|hhMo%wDjVYsBN(J2iFL;#J7C&709o*FP^$my zgIYt@mMvtaKaY80{iTmWUC;kdT)shzvR|2LcV^{6jTSNEZ#ACh{5+XM)Tu)o@i_%r zw|SRW`MjI}I1_v0y1Rj>%8;RDiMnTNx@BKmRH?-`^{gnr{jku^X^`Qke&ZRKSkZec zb>bMUqnDauImEX5g8*})!dOr#9vRG^L_3lt_@{XN8;etC3b_N0g4v~cY}PMk&jh4I zG~EQCK1MNcN9YxM)LaTk@Z8;)c`OB5ma$&KjzHFls1|SdxplB}QB^UOeHl!khh(A6%>EXLD?5H0fDb;DiKX4{*%RXSwKPn+( zRi&W_y1M);%%2c|dnH*-=-FT@CwQL+@Lee;=%XR-06L#)(QKDqY2Kyb?KaD-9)cG%!34?6*-VIVgADbPVmo-TC>`Tkti?zI5E-S(%K5x*$Rg#hoN zR1P4G@NI;{gBeAs(p2otC++>(USg##TFh`dnG`pN0+Nu|L@ct8MZ6c|^Mg)iGdN(p zLU)#=U6~CPa<%pRfx@0NyaGmK$Yjp(xh2ewfGa$r6qPU~JSLrfQ)@WsZAFPz$j$->NIM_y#0Sj=qH3Vsw-P zazEc;MV3feAT|k^4*h~*uo}y|ypi}^iVBV2@u3|!Wgzh-z}73+6OqkNm)si6 zMV!LBf6kJ(sC3)Ljt$vgf}KLgNOX=biG`V2Q-=je3vuI*(9Q7&G!2=<9mLvVVkN+WmhUi=@QfM26r=yO;{$5E6_^bS3(K@ zcf=dN?6s)x?LWGNH#BM^-=GUmR?+7J=iET9xCn(ZO2YKME@9&O`do=Mh+fR^7DuNW zL12LgI7+q?uu)z0@>iT>c<;1~LCz!3Bp-q1=7=FFx9oJIlV>3PHEMu&l~pwpg+6aCm>c-}jlfVyPp@8E!>#4TC5ouhAQuz=PT;(Nnuq zHUrS{!jV_;fW0fBMjx=p(AU#4DF*c#RCXMBw+bXu*S+I7Ww>8q5NqRJ$pzboA73xR zV|3Rmk|mD!BQmn}%Snal4%LFXpQmk9;FUYn%VV8q+VcW;UG{L^+9SnZGONZ;{n0*;b&|+%uP{eQ~=(*X0Va`)K;_V1qSv z6YWK?1Ix=zTmJJe!63>WmR7WNGasuh{|njgEWSwHHan3}Z5wkvb>F-YI}FOG98+is z5L_Hn`1wfm`3mF*(>Su|0UAS3yk6DdKCfH!A&5LLAq6GeD5$ z@Jtr(rR)#2dn0Yte-9nzhft^SjeWnrKEwHft=6XH_^6arSYpWNP*ELT*vQ?zs|mdx%0$8UmzgH}6ualP^Y)&D}F3gJ}!#b=93bIT_IKu-=n!zy!X& z)?+ecNtbrWc^J7vs?w^n<5Ubyw^Q*txVk)*qmWj8pDMN#IGGp*Bl>7)nYc*)*HL7P+_LJZq0`s307p>XlV^jV?)og*do`GJmjO19Civ$U6JE-i1S3gS>t2& zaww;}@bSN@v6jeCAUk8-o2nrdc4RM`i;}N&YMKfy4OML+)~Ddm9#lpl8^%+}STZHv z8ot(b);KY{;${B{`>gd3YPG>J_%QIq8eT%IpE1(lf=apjN}7yEB{e;Fv=`)G za6%-HJkbY?5nAJwKT|qKRt-;z@>s-hp{Bj~)uQ%AW)5Kdgkc5d4*! z0-IfFzk%CY4s{{-<#%uVPLH@b`-`-m3y^WreD~0;P7A<4 z9d~)#36pFY->3ko#?r%?sWQVJE1qe8Y~9fZ1Mv))g9u{R)2}`SXlGvEda)JH_k?uK z0C0j|1WVNVljIEoQ66LWG&m>fJU^E|(0?l%c;-$k?OWrQBEj!vVb=5{+iy80AaB8nvS@i5}4N*1PC5}hN@tm{I{I1V#{R#+9y@){}+cy=b<5gRqJ^lkm{ zKxt7H2PV2(XlD7d%j~ut{e_2v$j=nE7^at_;=Qg9CJ3AD;!i(qX5=rf|2_QhuuvWo z-aYaC+IuTfA@sZYdm7~%eZM#HGrkKo-QirL%SZ=@UOpeYRu(0n8F0&5&)uqC5W5{o zGePYlGr06WETkK4ypRj&Dti!0eFFH!o0ffpHKAt@7SwD21PBvsF)k+a5{feoLhUaR zI)~g4i_4CY*dv7m^O9Q?BmzJ#L=Gi^pMCsIYFp0 zq=-)^n~2(8csNOQ6cRB=PNjGNM{K=m-c9)P;fIe*A%ASulktSu`~lUR^Bf<)&10<} zRtW^_uN^kKbB)7OII*hewR0kY;0Z>m8CP3m$047B=u-IA>0*E$68jl$e=?I`3^9X{ z0Ie0V?|9+f_Zr(CZOK9~&#_U3=^Ci6V<_HZBc@H)hZjqme|mtAE{BXEri+-g3xQXH z7$}-#R>-s>dRJt%Hfdm(hHA_A9pvKl$8&>MnzZ~Ax~N^IP3u8g*Qqq$SlVXXS$wA< zd=)=gG-g=6f@8Q2{CJ`Lk)@C;%NCzOuh6@GweoLR<@5h6b_PU+%f6R|wfCd20{{)Q z*^;g+$-Oq=os^xwVlf;)_U`fhR!_%6^2tI4Y&3dqbFf7-EdspqFQGHV^eW8c3Wo<7 ze1=WM2}hr7TCt*Jh$$*pl(#iWs^&AxR>!LJi=)Rea;dfK-M+RpYG(`eNgI(3tF2R| zHyR!f=k<#%{=#?2z2|BcZJO=D%pJbJ+k0!prkYX0v&6ajU^PK7$87%>ILZaqV`gbr z*h7c>Qq_rEbH3v)y&#b}N-1lZDOO9qh3~|pqwvM-Ig)`%{5`hW-WGYF>5M>3CQcHqobU&1q9k7c z64Lhm0dVPykYWhAWirWg-N$F9YY>|uN>!3Y_R%Kr;SLYn+CG%3aG@{toVesOl$`T#qo_YN zSe?^B7KWuOpzUVTtQ>6h{kLFZYV1yLCpU$VT;~`?WG#@aepCcvnW5cRgRoJ%%grVu z5lS$)b{HX836duVAhgZK;1!Y6vfs>S{uyV_`~a(mSu>W*uK)G0UuthKy@GWLhbcxY zd@HNTbw0<4qv5l=kbVCx_#TK_Mx2^cxVZ2C*+&VFiM%&G3mks~d9+D0GJx`zh&f(0 zS51HPwS&u~SKFP&tzG6^NVZHs}|MO7|E#~3`Ya0B`Li{SQhjI_Oed`*-`yN?-Tt4oDfZBBSsGw>erc z@7lI=zbvYoHLd{Ev5zx>(^Y zk|;QMy}GH&R-WE*aJ~9V^t}M4!sVrDJzCk5tQ$~X1*tH1&w;b;*98uPDG&&!!h3S7 z99DwQJ^O9nt7~Ek$=0ME>f^{WQ!6&5XmUU=<_0R8AMdb|YT6U$KjRALSfUCkLi^UB z+A`F1j?s^S|Fckb@2(3|{bF@lvb^^q{T@zUVrbHon&G22_-D5%%o6Kva7P7In}?&6TuYy zfvz(f>43m4YZx{$SE{^=esJDb5BAglVzU&MxCc zDw{9q;_=_$>EuvsE2M0NVq&vo)iqMU!Mg~6%7gT!CmXNkx3tTQFJV+dwerO{PM3eq zd+gM$JX$?BN}kNtA=Xe9Z)B;<{8TG%7=6$Ypo*2q>a@Sai6oa!uo2*psRlV0#9ID0 zOt8xX&*#YP`*B@VDAN*s{X7x3MKfz?X$CjlEh0{*Liv3O6%lp9$S2!b%4Eb120P-0 zKa0V(X?pT4`i|ROy1TEql&HMJKy|6g3yzS}jpx?)2Ik=b*?5sEvd|07SA3TU7;e~s zfqa~W%c#>C<&Q#ysNV!-{Ywg}fkK@$mG^Y`N5$gmAZMGXsy8jqM<=`?xed{!s+S2k zL6*{>Qt75w+LSPF^|^7eedtMN+&Q`#)5~R|SY~jLYrmb}XF{*l%v~M_eR?yEG>I5G z`S<3*mP#m#`qVM(Da)SE8N zQ$tELCexE}=Z5i{a$Lrvs4d3BHnwzRC&IGh&^SrLYMneVe@SH1w+#~Y-CH@8P{e}w z2I&OgtYB(9i~3N)D)PFHZ$$~Wxb2%b`nXJ-<}iNgZ}T8qxO+df!XFu`B&d=ilOX|8 zP{S8Fnzqs%e%v+X-U=DE@LI`)^!e3>Yv*0_gDBu zaWn0{PGS+)E>U+6d8ih|$fv@cN#~^CH+BlQpaQ9kiY~RF)f20<56@}_=MH&7(~`65{&-%UN2 zqI{n0Z!mur+XnVT?<$k!dn?nQW8^%Eocp2P ztL^%<<^x@5;db3aKBASjCodVy*;T=5#yfp0veljO!i^Jl*+0yDN+!r{l?6J;#2>Z5 z3rellDbfY#gdI8YNEmZ(JQFc)(AHYrj#mD=asMlVnM=FtA%RwdaBwLRtKw_U9o@qy zd2Oe0YT91^1o`l(O zI{%!sOMPV1Yvj=E!2Jsbx9(3!Q%(U)EnkI*C#LWg0rQlKJkURJR*`j^s+X~}6I-%& z{MUW^jqD8EI2df}d(^1|`Jk2Ku%1GAYi^5P8--eWoQjWW?+xp+ zU}6##l$(XXM(ZW@Y}_)z z3B}D$zT2=lD2HxgbSUTFjN>z{>qR;^i!48gdti@veaE21O-&j#uFY@AjcN9fKs&q> zU#N#I4yNROTNy*cMsWmT!!t0D_?C8$iHNT14$;Vgp7P+mz@1=NuU zta`UGXp~eqD4z>s{8kPLcXCK~-b0|U$u`-K=!Q^6{U?lj1c2&E#1@KUNPM=7hwfrSEHk>_x zMz9J(yyjSt#D*d*QQzyRcF_e^S>4%8SN*QwWvA zryqs16uVAb(I`HiYbZ12d{!B^x)F#q>3d!`Mq4kG^B2@z!LW*VdlPZhNh#<=a>O?c ztwMhu*OfZ?p(>xBhm)8I$@>0QDgFoy_C{$fX?-UzwM+OP=C{iWkMLh`c^ixm{)O$I z!G3KOoh$7i_x&Ikf<@n1_k6^YvTyBaJBD~ z8Z&~gspK)4CM|F@E;IxmH;OmPoMk|b$z|v@xR}GKqq>D>iT3UD;3dbsK? zkW5V&k#Vs*O^;p?f=q6N`?sw4Y7lRFlS8lhpI=MD{-rUb%t1FksAx64(2sZ?+OJ7R z30rs2s)Q4Z>0!P8c7BW2?j|v3Ml`|X;doK_!}OcM61 z>}5ez&o&>hbJ5gOH<#KLhMLiox7BIFIM<-o=xk7WO+A3dG-%%Wqptu`m-zRAZmT); zC=viNzXngfiGUK-cQ^ih;JpHyfLs?LgL?lkUr_gqxZR3Vuw`tm6>d8{3=$*qHzCV2 zTlwVhn_nYSOIMAp6V#g2Kv^aimH_kh2la$v>35WVf1X*z&5|qy&X+GX<~3E<=@GKS z_n)={!@&(kYWL;t^sS8 zB1ALJ7J8?7?qA{3b(DGN#<{Sd|9Kzn<`~8j@cng7HybXvtfqH4_kTDsFAQ``eE(@_ zJ?j{>&pKZ`X#{RHxJ~+xu#0-}F$sbu?69>M&@a>x_HULKTjZW)UAM}GB?~5Lmi5lw zf7k8DjP+_K8rOp~3MrI@ZPiSrUe|~JyHr6J$H*Q*(V|?i35L1IWBNOKn0Nk-CVXch zg6z@%?J!SC1NSlgdQx(g(1l92WvtgJv48hH<>J5Tr{{E!lN|xHlk!^S47lDZY)avJ z_BN+l@mNQ}*8AT~>y+vL+?IfYiN;LO%o^h?_$3X4+nvLsAE5gLd?k7p;E|Ul4fM( zuh}vaty{KaFVIYYVGygCf}+>g z9sIN7*4MqeNf(BP0MXI=_uIsvs8}lI72^NfEfdjm4K}|cCeScyqD3d1` zlmA!qF}LqR&}ABd&33f3<@oz9FJpYn-<9gq2%mkt4AL&&&0iBMw*gnRtc!=I{H99|C_rc(F@6)LmV2fDTh`zS3sE7p?9#KRwF|oT9 z#a7h2&ij6MjQijJzhm4n4vfv-YpH4YrbXE(4FI)=?fV8Pg)ko(^b zAeHSu-BRr?zsjW*nQZ@d1eaR18VmA8^*F%th(U|d z;58c2L_V6KMencu><1w=l+oTntS?IM?5ghvOkS|$a;(P(gefNAmRLnM~O4Y@3Q zFkb}K%nr~cM2!c|1Rd)10zA$x63}D}p@iYGAUQ!FNexF=ZOQpXOk}RC-v*u8{@ZHa4H9wSu0M5*rRq za+8JNTIMzpF>W&7CX%o*E}M>QCX!K%klp80siok#f@V?j$s8{Zh1H>O3Q7>=U^Afr zR!_&6146MGLAM6bNIZ@S>`6td;XDG%tW{fVSP2c|(^9=S43S5rsi}~INC_F3Xf+s= z7-G5IMw^Yv0oP&)(J2pz9RCc)^l>1&0;@5~1QIdF3S47SG3k1`5Gm(T zTw*Y!kSC{Tg$RY5We&1}4kTU5hFIV}MMM>YWf1Ks1Op?$2#kgxn!>Vq;S8e#frFz| zo}izK#-kKMu18CRJUlErU{V0Tqe|FL@S7gAYn?>3N8;x(Ej)rm$F~I=BiK2+7a}I-5pz zFABa3NxvKxZgx!%fPi1ZcrnGFd~kFq(VZn49(KA z^a76GEyp8xDh%4ac%hX9|Iyg&*cFPD7xQtFSnnAyI3EYn$$>3@Y zLMV|^Lk=>>-oR$#DZxUH_3XDbq&;rG9(U=sZNn)Uq zf?~TZk_D(N7X?kScrY%rRZ3E+ts*DdsR*$_mz-2#H&1{2d%C^uE@va01+tPF1?VzqP}3S$6hMx^lRF1|z{ zBI$TwB_@|4K(LU=5S5G*v0+}OSuHd%m|P`K1>^X1GP#Px7Q!uVf!h_bvO-p#(#-SG zOf-$sgb;IucDccgb*LeX9xl~rVSciL<5QSyHoO|kWU9$9qZ6f|0j5D!XsiYmRYem- zvI56Rr^}2`fT0M1ti&ft4R|j}1GnnTI;F>IR5Lk9i_)WHfcGd$G#STb;eA3V;$wVv z5dIG+_*E#rS0;lKH5A~*SZhd^ovu1!Uv*5oqL-?H-L5U>(8AQivgd zKt#M0Yw?p5ew)JWWYM@}q8x6J$pcENM8mN=HTW2U z4!Oq@@WMH0ir9NxoIJ`ti3j3_5rD3eEG z1QLmYR*nv-Fyc8L6wYgdn?q)7fDVI&f&_?4#Xww0gpm?iG@CVm;`t+nbp^awND3}E ze7eHLl&LHMyemMnxk5B!W-mx6Gm#WTiq%Nh2TOE;1G+)#hVQr00u;jPOo=D zNTnaIglIG&Sx*)#aAdke;^xEQIIKvoM{;SAUBwdx5mq1h{C}XS#X*ALkP^k9Q36Ox zreCi>0Ym{G*#AuLVN|=_<8YaPQuzl;|DpP5H|ig%Zx)%!L6IeZ6Y{)xq(|#vGQ9{0 zYjIGC2(m!I6A`dt2#;q6i6#ZiXa<~}s^r=H8amlTR7R*i)+o`zxYS6-6)WW?JOCXk z)5CTcSVTKtMK%*e6jH=_4Q_)%qr$Oic7qH63Ry-YNQRt{htRj(kom@nB!=#bsSO`a<2=qpbK&Nxc zMGl2mF2i{6DgsMkLr7#8IYYo@ks&;mPr*37RFd24!ea;uKTU~%+zh4B%~vS7N(X_- z5ArFH*dnoV$rhrG17r#b?N!V02!cg{m2zA%zK+8Mm_U@1EGDZ3&XFr1E6$;UWE!c! zN@8M-79|x&lZKpd9hfE&A_;X)4;SKkaDE*sAVpXW0uRNGbddNWrVB$ynHhMy#g7Q6 z1s*?BXoei(2+^h^@dPpvg)!<~kzPnhoL`SL$$erkmI*v4vSf3_;9iT}$F^(16`2b3 z0FflcsqjLf7u=AcT{gdqWT%lWE(J;b8#65(D}8pR;? zN##J6!7Uz--^vQ|I9{KRg2PC~7K8$@P?ds-WXicX3Cp8Kpk%6`na0!lf&l@6stc$* zBrQbHxnyV&ogxX!sS1b*x$!uwNG;}=x#S>4jFS3)k}V(+Vk^q7coBGl!H~kCnx9D9lWO&JRb3c?vb%%+=}b zK8@aoL@D%Uxew{&h1h->ng?mA4i4Tdvop{f3XTzEd(Bu7JVZ43F`%qSIIyoM1iE!e z)h0lxtQa2;EA){N9J3Y94TwYL5Qij@FhowOgDc=8%yxm#q*Ibf0-KFu_o}ocp;1N9 zQ3J+6$S&eLG!b`_23Zmv+8-c#Icx~#Au`Dxp-kr#hiG1vi3E2VttNv6twgh2euJOs zXR$;Ulftf{VdQRw-(;Ym{AR3_%k&ap1Uw%__8RD1mQF-KV-Xg*kpfBq6tlrVCFpHL z24puxN&sRQ-;6=hoN$EJssRoNmqHe@TWfUM?Ix03uT)X#5Zr=wG0{lK>*CpTa*oW! zXVWa;T`SLMr}2d_C!5C$D7ey)(XJ(l!7^xM841hhM6?A0C#K6K2s07pQB$dEESC+G zQOGZ6X$=;K94mv<=oqV)Xw&-5c!oGc;0qZXCsV*82K`o-%I>Gb?MxcQ;wD&7Rzc7g zf)jmGI}#14<-QP)%f<-lN&x}Epa+54B?L@%t&O84%T0C?Q)N`R5ezh)z|e}-VxQPR zBTE4Mdt4p`n3q>;lIs;7F2^GdsikbQMTL+n{XV=K%c81;bhKV;F$EdIpdKOtIH!`7 zN*u+3#yZ?o9N+Bq(tQqamufU&5SS1e;lNwa4!6x?v|4Ebr%y}`$Z0056v!c_7*Jlh zjOgR)sCE#< z5ZF4A1Nc1^6s8!65MHD;tGN;-j^F{aB|7j*qJ*KN!&nNQ#{-fNk?wSJG!7FL1zrJv zx+p$1%cXa-+!!Z?WC|J35{UqA#Rom0_CYl}$WDY2FDLM&Xga9wM3^#METcF{2(UE) zx5wqP$Z!UqjsP$gA$4O3ynkpm$F1<a}*8(ulX|fI|{Pcs@aHq?5oyqFCk<`^;Q1iVZ`VH2=RzpJMT1V9JS>kc8#q zn|MMR#hUqlCw-FLYBl{+Py8p(W2IJ)8t+C>_$WBhV<%Gd29rc5rXwge3|Wel`*jGK zo?us))M@|$3@1!Y6U#v{getO|0z!+Ii!v!SL`X>?z#J4D%#Fk2aqJK&LhxlaKaLs- z0BuGMN=+7qUu+5qjCwPXMB-Z*W{@#VMwnfsFnJYJnAOb;$w(?a!|oyy6&R6|Db!gx zQYy>Fa$B_uktiglh*=gr&19i@kO)iAhIOLQ5X!4l00!rYWvOsS^@k#In634l{O%^O_3zTqJmscwotrAlR<-5YP49Chvt{rWK1*4LRI0B0uqI* z3JR4Tl1C!;s^nfh3+x_|qX4l;bc1d2TOD>k)nOsQF&ZS#NB3$SB#GU|VF?3lix=mT zMf_M}mAcK4(5;ZD(GH@`?UDOIMkk8ZUNe%x4$4GIniM0IL)d@_E5kqm4a$gjF&K24 zQpgZW(^bjLb-Vvjz96psnAqZ8GeUUKjC3O&W* zSNi>AA%$&4_&FpqjmTvY{49eR4k@%SY(&qhCd|DoxVZy-8T2atMkutnkzQ=^N5K(BECCH6*!}O356ssM_MKd5?1YflX z1rwsUwN?j=D*%;Rl+eI-M<5a@AoBDA10Seb6`F2Vvox%T!jPgNAIs~NC}4a)N+R<^ z0R-Gm=Mfcd9~^D6XxV`Pplak`kmeJT{Ae>z5Ja1Npd%O^yd4O#xF8imc%?Xi)9EAR z%8LRCJ_3hidQjzN^ZgtqQ)R-Sl=uJ*i53NMA(n@vaVXtnx?5{@vv_cnUqCezctS2< z5@sYx#4w;~GBF&65}?#9NX3C*EWe+^^22oo9Et43Q}kq{0%KC>T=Jku4r9>+YCo2s zhm*+>JrCpYF*vJKLWB8Ce5F!>1hh%7a_Qt2lSv9lBh_ptIPF}77$cBlc|45Kg;3Je zBtL@g5de83wkix{DO}+NE5L{XI-M<~ybdK&tuR^T zLXp_*gcNiP)krlNwIRFQ$@0*VC;`HwQk(wi(vRbKky;x)fQGX@cqGpXVeDcDQGgR^ z$x(4p6zYsF^mH*-4>dR5A?Kqt^OmUMz-YQyE-%K2yt=s%?C(mn$}*=@D*V z)J34z7!g!)pl{bhMupx(!&ZF8`YPyF9 z^Ans}GRvU`1r3SH0Xdj>uYt|A!KH2r3QqNNgk+$egD!QzZ$;>XY@HesP(=crBQl)K zK?A~CgFuo!zMzi_AvnNtBrKQ@5$v%i0D_ZmrXuBdQ072$rBW5vjr8u&Qx{#d+$Ss@(=lY}!lF&|s;|*3G1Sbn&Kr_lw zXq?(<50UvsyTUCZlGq|7kA$b##37Xw+#*vY9>0r?r^u{6Gz5wKQmig$;OLk(teS#_ zu^e~=E8>f2t5b^dDY0CUlYoZEE+WMj0##0`QK}Z(;9wpSoKCL)A0#?x^Pdw&BqjFB z&_yj;B(-3W3F5$nw`r$POzV~J$!&^|nzk(oBL_v(FRVX4wF-Tk)d>5`aA<4wyYw-y5hJ=y_%*?QZTG!N z5{cR{A%XK3?ZLV~&0EJ!{q?!&XJb{}%pMa4qb=8qBs-*Mmhwg`v}1OUKP>Gx=?%Om z+FaE9xc?v680=lo(karBpMryDHIfbyLU$^hP0b%KZmsKn^K0(UFUvk&7%{BuOTMvU zMnXYS&c$8ltN#3IZ2EHBUiToJzu9*CK+~xn6KnTg9G%{}U8jsC>vmT@>(x-E%8%_k zT>Rr}*%i1@b>DwKcjmfBMMXJe z^6fuA@BevHUYE6W0BKe&BW-Qh`0aP+e!s#S!^m&jaT9ad(04=Y>*q~Nj$=e6e(jzi zPwLnkQ$$_ROLukl%nm)yPH)%Gc((a{b?5Snqkn3}6Oe~#V`FAbip|{~c6BaDgcG5v zU%rXAVh=5zpLK|yb?lqkboY?=(gx$tNS`fQeSj{r$%!cAqSA4d^!6@(+Y-?uDQR9tWYsVs@k(1w5KG)vb zXEL91v|msAUX&Av+SLDA*Ml*#%)}`pCAVfJ^FPH+laGZZ^@@ufmk}4cU;S-|B_;9q zqqSa3ul&VmkCGqHk4m0576VUQ{`t|t`%{pIFC-;aC^MG)wI}iSs8}Yfx%!rb)hqvD z`(ykbB?#TTqM;IIA^zD)ew||6jJ*elJy!glRk^O=WRCoL2j7i`m|0I#1?jV<_LAj2 z?0fU2(R}K? zY%3V>)NP4nyBgLn_miOVQ$6cE^JdAr!;PbRVyyC-w-<`>SNbU3H^%#Q;^och($Ynf z-c_dQ@V@F#udeOL>8;ps?o>)!QVf;4P<{cK9`zu#*WZ9@{$t6;JC{ z)V_+!LiX`TL(5C`?>4t&TLs(f#+pCg}Shxt#H`Zew|L3?o{j<&{Gb(F#L6f zdsdqrvof;})b=zK59mCmxeR~0HS2WAq<2I5G_v*`RKl)OmR#vEIREUj9Bm6};a+`0 ze!TrCofe|Jf&IkNm~+a$*x<$M^j@*@ahIs@S_pw3VATbA##1mii5p4n0Rt zuE0ahAlMfz9!NWTr_%kjZdg@DSx&};Bl1@_b`O({bIu&lR*t?f@*{f{)?RW1>|M>p zxld;#odPIP6&L;ncJT=a;iY{?_!7$rz4z_zUNC-Pz7jU>Xs?nU(!7WGO$ylyRNL0C zU&63%>nHCkBd(k=bG?+SP6 zySs-w^jJc`rLtG%{4Ug0L*e5R_1cl4FAat@UqYW=Je-qbjGgs-;Ueja7GTs)W9QMA zYoi7xKcY=wrzFM;R0rURy|EA~>hbQX--yc}29?Im@1BY9uO1tn$}1T+?Zlb1b=E9V zbw_ntV!Qs6%bcs^FPrLyt+=({()+}Rj~B;u=w3vQYK?G$AYR=V`trt<(VrVbb|1dYIe?*&#jwY(W=B2`p`YLwA-?w5`^Ng_jT&{!#?!Ewd$4w4kX8ir}1<4*Url6 zn$}@fLhR|?#0$uwkvJ*rT{{Rw&Mb{Qw{BZhHFn!w^pn)K8z)vx5pH4k>~b7$xX=gx z<)%jW>gx9TBSH5^cXdbX%GVbsPWi5@{Bq0E@!`tRxrZFr;2*k3W81E4Jo1tI{oCV( zocC!T`s_aT!+bp||3?!3%n~K+&M2gE_=u>Y9PRAvEGB+In120$66X4=+jH;Gu%l4& z0Dt9F&Y;^{4wVjTq=8sP=H9xA+HIv(YNP6*c&*^?kNu5WDZw6f_ z<7q38pPSt;R2xNp_$X;xmxIxV$v3;5$Xa5}NGwWA`m{_op&=7i*Gie0IDL%kl(0og zB8B#r3Cp{-IiVouga1zGP-5)qZo0+s9cmWS`kAsiH6gdRspjq5pVAFnI*5ev!*2!ydT-SSAKlI@!ElN6AG;S4(r*6 z5(~!7+ZX((5B~Y}_55}5n59ZsGdcUVumkm5Ny`P{=yhDgVtNcU)!3%^Uv1hHc45oU zmBUjKN4GAqxd}Bn+Vs7*UFNW3{`%z?+kzD|tY5wc0U*EkxLH9xGf_d>+Gc#x`v2b9yDgQE5D%4!z!vAPY|!ncjE$ilsGYgHxF#oRN0*eu@T0~G z?}D%+R`LX4j-^(Jw?qvb`QuVz!TT4DS}Ab7|wHzeu^nE4fG}?2hJzq$5aB-Ij!}Zj2h3cxSI!qJfp?Wqz8Omy{^l z!iImn;OG&%U30qd*@ad1s$bW~8D+;N2P?llny9E9!#%L6KcB6!Zvsf$ z^kU<})srk0&4u${;}-4oH{NsI_1=i-UT|h<8teImwdc4Yc(tZe8TkdL1|8<+aE>epL)6F|?i z5pVB+qGU9jbC5|+=)wjaGFPe@99e0m}FWVn3zK5IjLbH@&+H!+s7dp^ocN|VOUTAS1S ze9WK42k(XI_Qg2&m8SWAB|P8rykyeq>`D1d5Te8>Uw!ER5`8@n9;S`lE+PhMOz96^z9zTJzJ?MxK<#pnv$!zT=5{U zL&3GYPpA7&U3qhUb>}gYu^nm-*lx$p4gYe!54Y>R!OFXR(2j)??<7~q^8WV(?iNmZ zl^i2o0p@dRVzw-5(BM2mQtFJqh%?*QF!!iWwT~d<`Kq}~kAH@HUx_ugX*pxZmWc~` zW4T*+>!6JIiMgMbp=`IVHs8}M9L4TQKRaY=u@Eq`?s7a`KE{#%VD)zS9`%5?V*sfv zhwctvHD>BD=SDfXrjbUDnRS1TWfG+uh~xOnrP&j;eZoMI@x^vOm{qdWf3%_mn{p)9hoRsWy zBMlddp0Ara;oYIvS%wz4TFFLg$Cg{o*Z#)W9-Y+v9?cgV_3c4^{oj)Gy(@8_uZk8- zLS30*>4F^+zBec(aaD36e+t;XX(zAi`zFN=3OkZYwknc(N7;Ri_wHT^9W7lAw1?qpw)ZYgy5EoN_K?$vDq zCaksG0I`Wo;-G1@VY<_vstqQfxMF8YBv8fRImGc8K<`PH?&&mVwWc5M+$S}UZtk8qW@(Sf z!>XEH`*UVImk1g2(p#OHnM0;jF<~1vY(MED!eF2GR?n69yJL$&Oa=L|53bJ@Lu8<$y<=Xx5*2ey2yOA-a0U?qjPH5=Vq{R`j3Ic0>M%IDhqg@#FH>A24KK~C2V^@gqx;pccHHg>qfSS)PTu?V-lV$anb`$x9;~+Pg5M6m zje6W*SciU6ed`gYrgC*WoJ=JdZ!6Lk`i`|ICro%uM$7HL3f@!IvsYEu?nDlwmfqWr~BT_DX(b#f+rc5 z_XUe`HqER(7z?~Lt^j#)BtG_@J0mf!{XJ7n@ubG1vUe4a3pcrz%3@~OchB~B z)IFa6X_t5BkQLFm0f{TQAC|d%ZJvwbK6MlKX}u+z`?MJ6R1@WI%Jq5KsZsfRv2$)d z4}56d2{(47xIU%r=;j+Qmn0N;qQ8I5#XK$%<%Ofp&p0#VLU+r7i67b7QPsCv&h+M2 z?QJtcQXbv*Ljq?t3T`;J&v^Mnh2dNF$nf#;VU>?J>T}Wd<_Ln9 zeO#2dXCT%Mg^l|6x534NhHsy5OvHMf!_ItvzRo`7eee6Oy3A)+d8w0HRl{CB8mOPY zI_33<#v__mHSgibBw^v$17s73LhNrRyK?2c#knVD#OODB-|oNB9#NjT{0bq4Zn;_Z z*M}4&Bj=h*vV2=rpRP6S3*QF%*h;d7gz2;S6;*yFEYN-Kl?Ro_XV&kx$R1~fH&3S& zR6ht@e|CONnx>`z4s}b5%0IKNi#>>K7#_MNk*#{ZWNyzMF<-xw^<5KF{Ct&v`O5b9 z(vGFw8(5N)LkZWP=#=rKnwY;MO8D+1r4@X7#`|}BS{y%>*KM|ZnKr6AdE%mx7tTKH z)zZ>Kj;I`vXqBBlOQ^T~{cwt`{ieE_9z7TD>NW|!HZJzC9mMkW zh}pDyeDcYWWA9WZE_&V%+wppA>43O4_prOB9zPV`^y?h*V&bDdJXq4;C;4TLgsjFi zX;el(;49L^(!xdUU*)uG-b)*E zc4{Bb_uSnkebAv56y5gVvV_irzU}&Y7WIshQ&8rswcm=m+|A6}jvW1Ps}9|Id(Om> z*{|raljAy_J8FHX$=Ms-Hv3lMSl;lLgO;x7)^qVQ>qT(u`yT59lAf=F@t)lZvpG>C z(lhBD!@AflVAbhi@-JPS{?zhUEe4I~$AnD|zUhnJ@VI2)O5z$u4Am$R$;+&h7stO; z?&)!*V>riUNblz@Yo$)u52m*!^-%U8MW0sMX_>CL7^$^1kOV7zQ^w3m8Bl&NIZ@7k z--9%oUcqo^-XEpFzKv*d4o}i4&ZMw>c`diAuFXPMCnrX&RwZ-w;o~bxwpEeeYkt?P zeKcrD*patnOz;HiUHt569bkeJp*gGp+qy~{igKbplZ}VxjUC+z%)O;_rQ~ha5^+@i z+)CrLjx;3_UqIF!^xB*qdvoH?EpnGU{u^xRpi+{l{N>q(mpOEMlN4|vw9asc)!iQlXI;j`oFyvb*xspW^o?~eV1QWAe07}BaG zP(47M>@^{JPt;=G8hieU2Vh;66?cDH|1UtK4~uSMQ($GdLBJw{&d$X-y5lLtX(>CV z)V4U??(zJmc0g}4D)@lRKT3~o8bt4OzX{Y3ZFxsB6fs$k8&1^|f|g;q$vtm&%=^=G zD!kqIu@EraoWG76xOc6WSzYr9_sG9a**0{K{j<(8OnKFLu|wDV9iV>YxM{dr{v#Qg zmo8kjHm-o~9n6)F8)E%l_hm}!s-L^E5~|{P_eu&UJ-kf8EEycGZM(n4*%=^Xj{Q0x zlv)#FrGKmg7Vx6H2?Z~`Cw7MSR9gk7?h=v)hu3_VD{3R6yqJ_J`};>x&bPU~R|oDC zPU@pSl@&KzjOG902Ke-utjh`m36*RcM-wXuzlht1YT5^CqX$iovm2LZ{K{9@OUgjoN#@6ua4 zht*>V`~NBc(_ReM;xoZ^Zhfy8SKfB=KoQU}sYDSo>-nN*TZn*k-l1>aADRxv8pi*$ z3oPfJ>g9pdgO5uRVh{Fm$R@rBg8c&w9+%h_{A0w1 zy=WOvqj%p!EF4^gDKl_iItklW~+Gz4`yC z54{GoW0C$--BVIHIw140Ek0-M-(tZ;Ockq5%O5(Wtr9<$oN)hR@ zU8QkDTJ4ITmUHCK@1IRiS4>>}`qJ8{GG z;TdGuy-AIudX5V1oBa8jObDcaqFl=;tDjl%S$}?Y&&d@tn<6zY-`iVH#Y(UP#qGB) zJ4|em2RGFZ`go531C``N*=N(kbMf-BoaqA+*I@0-b#=>!glqepC@yl0cn7ZLD*MQk zV`1+=J!uujwtAfFOp5EtRr^4ZhiP`iT9nl~->bv`++e`~BmP&$8Y1!Y3;>9RiJiAVk(`Tfb#r@IcnLy$>JlOJ@Q zuDvZ@X2el01&@hO5@Tk$`)phy0ybSebn)|q2FA#iyW%4E7$A!pT;Hayo8L}!d{iUnYoPF`;IAo0Z=&|KJ;2$JaDYF zO=Dcum6X)Yf{oP8bzAQLoHi{Ig0ng^nU~3P!NJK}?XlEsI@LOn#fXBV+AIMa*K^i`ToWmUkF-gbze?iLD!>uJT_C zY>OV;YTnqw_uz^{vd-=4j>N7LqO z!QH$)j)z42pXQI}wRg90dRIO!9MQfs9jK=Axuu}cd9>JB)3fP8e&@455oFqreqO%M zeC=$}cR=aR5BdXuebu3eg2MxWn=+vUXv;HTepy{o*|4}=P&;q|_O^O>Ij3?^V#C=@ zPrJ6HjEx`MDcbQ@-{onCmko^vw3RygK#JvTW-Acc#T}<7%UKw|*L_1N9)CUpwMPT&Ke$E!Ruo-B*nH{f$(ywyB;qY#9(^hYoE< ztca@KISg@d;o|GNBDS3NHCl^!my+CRZZtJH`O(oQ%7M{OigMQ1c1xGi&n!uaoQ0g& zRM=a4?Qcm<|FchigRZ*8#b$JDb{r$mY~QV5Rv9rOerw9hK=IW5A4!9{hm$Vi zpBE4O`0RZ8rL5&k;F=oxsd?Qx681b9&{`K$I}#{4-MZW_@tu0*uRob~;n0hFM_ z6ZdIJTcbA0c27XKpRunDOuhel*J1AOUyW0Hvs#|EcJZsy68pS6CFQjuznz;uyG-9F z@L+nLW#DK|)%WxG?U)wAN3+i3HNwReB=S0{>ug z41FHDQoS>xC>E?a359d8?X%+33Cx=fg6+Z-QgTM{+i!591cUcEIqku&%FoxcmnIZM z$`DDt2&uXFJo5eE?c4r0MPHA}leYG`RvR2}ew8R{%6K?y^v|ccFR%H&wqbI6Uq8Dv zx$MQ7Nt?}g2X20K@9ikcnbF)^kvWf?*&xszi!X?XQ0x6c54yOg-qVzM3HR`c_>d!TMHjUkYn%QGIy7z&zZy#@|D;&82 z{-mJwX2wzT!)^yw(lNm=0Y%xX9*W8-b3cuIB-}3B=!Cxv1`dD!I6HrndRa(VZsj!m zTwm9v{c1++EXnk>^qVoWvMx1G?LK*6qUFXe^!2tG(NV!pJrO6PjINR2=MOt3cXX;L z#=FmD#g%pd6&K`+-QG7hFDHF`v-e`J^wxW#7jfu&sfzWKK@f>BEi3(nr>wNmxD~CWp=8O`oR1Tjx z2STpy@vZyXmq!@&56WI|QlDQwGwKYgZS_#=;+WdD*G;eYzU%WrcCjQr7OOry%2>y_ zZJqSg`~#e_jrkNct9pOu7A^Yqk9-#(zxoou)lKk3sOvkDVt!66?&wa_+uIHEb=R9W zBlg{%F5D5y2q?h8GkfjFS*3UOF_mv}XPCY{-#oEy$+ar_?vWPi=Xs$$8>?>3*^luz zeXp3@S$2i?DtWJ8m1G{2)X4|fFmYRU&FO{puW+WwEH>9=O*=9OLU_(0c z_UP)mhd(XUJpzY{pE?@)4bKWx9}4@VzM?@a)t;Pfb1xn|T`eJfh{``J-FbIVEF~s`RGX{%}YLb zS7ux|d-L`I?T|%V*55nvw(F7`*O#YcPcQy7w5qU3m+<*EWnD(p)L)wA;=!`<%Imks zr;W)fPR;&(XOZE2Vyce041E)tus5B5C+X;#H_hkkt)q_}xajNqEPe0TtuudCv2J{s z>}&4uyI*Vc#IMqy(v366^nj|ymNgsBe!=guQ|})*RwcHf|3%?)i&zWaQ;WYmuS z(qB8KQmht!`G*__1X@b_b)*F@!#8hv&#Bs*dEIcU1`CxrqSId7vxqC z5^W%#xmmRnf9h)Gj=mpvH|Om^>=7^8%Ab;*-A1jjy&Y;8@8{m8w#{C!XTjX>Q_om# zfd8EBUAuVLV6XhU+I#Z-p5I4b+q;kSp-O-FRkRU1U-e&mvL9KOV1(@6G&vHltw8OXfqvvN6kckCXj< z{ZG{aC>fMJg7q2Zc-2$b_*nY)3H=UUZ&OLF*M`lEA6x}GP z|LZ60Wkbp{+V!rssC%ovEJfYR*8Z+>y40_yTz$uno>ct%;)_i1>le(4~Kh8^1M zeG)g_ysYl|swWz3$22B#s$%HG=CZLre5Rzv3%xRDWAQ_lpO+u%u%^tfxM$lHpN^zF zV&D2wznqeDaCiqp`Tj)Y{vH?B{oOIW>RMf(%al^l%+*N!x8@7adZZT(Y}q!||JRgT z;~5`pPwL`sP(L1BdOz`~`j`5<=UsOF{_{bh|BAibcDK*=38zO-k!Q^r?>b#Ku2!;W z{lf3ZYd+t62MsP@oEtAlym9w#w*!rjJ%>iEb?y+x?Yc78b*Hj=^JGfn z!sM?}gLj@xD$9M|X^5n}$n*MY>VV`fThA9MC3TF(F00Rc={xnzh%v{K>TYCW^iL*} zzGgl4P2Z7pzcPn?9uEFZ;T`S|*59D$^Nz7QtF zQYS1jjEk*Vmwn^;+rd4DUHkGb={2kG5^pRuO#1szbnJ9~Tx$-}t2>`? zcg|V(gKza^Ic59zdtYBq4a`mwbh;M;3Xs!RK0GPCxI5S|NF+Y>e9Jp!RvRO4xr@wA}gShf;e^m{h z-arJ$wCu7qk*|wA+qFcC-Qc==cx}$)uXp`5hsrdkJCQIqp|kjS%C)?|p65h;{~Ko? z5g%Up}KRuB5p{}XFcFH|oX%^*zDE1y7zxN3~u|>Ahe8%zie`bn}(9q1;rN@xwH^nK=&;zI2$r;lndU)Z^n@CY{MkN!QU2UKj2Z z9vd^J*Z9FMA$QFP!LzJ&Ls&O{4}IQm%Jw(ehbfC+YpxED3UA}<>tLbAx{^a zeU`Ok+e3cQ(~f<^50+mACVtduPnX!}Hqs74XHp{I*$ZUO>Stq~{$5eFn6&!sknszl zJ{|v>Us2RSdVbmVy=nBSjk||D8nuA?b@}8!i^SXRiOr~yDcdiBH~Z$MEJ7y3?pX8T`toOwLd z?bpXGgV9WGgBbf521T-jNU{vKeaVs~8f(@fds)Z6mFz0Tpb{05NXoeFRJJ6yk|7Bp z+1q|+yq@QIJ-xm_;j&h5OwiXm9>loVmCG^h z)7LC49rEBAkqMsROK+9ybCXCj{`V&AK}p(% z@TcNJNK1v1W5~;!$0vm>U%semGMG>AXSwTen?49XeUR$nwYt1mlDyi+!W;9>41RI& zgJX71TrJ~dS`=u4@9;v;KV)IP!*C~@9_PY)(o9Jt24;!r5i9e&o%IEjl)eg(c)$SM zLK3t~AfrF4Ra2z&ZArDeS?9)4>qflh$^BNRB~-)CNB{j#(%v9K_NZVo!)TSH=~ZdD z4kY92h|u29G{;-h&Y?qIm8ZlkwB)N<;h_XQ=il@?7;W`(cM5w*s8A7^@%>2a!(K7ehEtcPOCB-k7^x@2n2>s4X!B3Y1 z#?#yoW0sEtrrL71i*SQVw|`K=OpC+@>b&QP(QiQAik7~wyhGQ|bzU*Pc{CZ=C7if) z($W><%yJ74if3EZVJYU7*6K(~i|wP1~3>}3Zd$e`bvllr3wqY{Zl z0n06$Dhj;wdwPy7IX94`1R!KBe`4?y@#dc=IBdI(9sSu?&QokQZ&URF|NB^)ETX7j zI;U+_&k1TNCdys`i+Gu`unm|Mn(qT_e zHV{%ya8|4mK~3 zOrs;^F2 z?pc*ea{}UO{E02B@2CB*iw%CqzArvxIUzHMe89wwK^Z%M9RJ#p;)F6feE#+W>$rNj zA(jF*%RC9#=#oWk!Ot(L7L+N*ZV{U+X?7qQBWHwcb9@}jx%Mu{hSll*{ffH6v5fQC zMTP7eispnew(c$xr(dW}9`=kd{e`OmXyP+qj$$Vb>hfQcr!Je-d3W?Qp+YI8w`)Bw ztwww*K4YdGQm5OMs;O9@WxPd~BJeT({`sjh&o1Y)OaaLNT=Uj_;lpLqWV$2cWQ>AW5 zw_I4yj$>w%yf+PdUgc8`r<=Abmr>{tz-AfNQ@*n%lJ(Z=8#Z$(%CkJG@KFn0r`v7l zh9jg?S9Bu}v3J4=0fQe{oyjrp1*y&Bl4e-8tgB02*on^GoP*TZ>)Ht*b$i%4OyplP zG}23=Ysux$BdjnPs_(=9;p{r;9HD2Xx$WLGlWkmXTp(x2GILZ;XH8xEWz+TZ!?#Qo zJ$iG?Uq1(0HmQ4a>D^HoOrvA+^rBKr`!dMd1#}=|!gn3;_d;I|k%$?L+Q(cg#cW+S zc}22Ja1m@PGeRnh5pO&-wky3EK%|FfegJ>PDQW%Ef%GwIt~B{-&sR7}3)1$<^nx~? zfEpHwT+yZjU)z@y{ks0~E0P8g47CF5JkuLED5waEvA%lPlI+FB+Te*hmq!mx7^Bm=lNQ_nc%Cf`m20D^+T8@5 zPtmm<8GnqeL`-HPjZZ|e z*R?Y_g2h&ZrGqplldx^)I`ujN7M#Ys<>$Yh0b6xza2NLlBMVbwp8_g&_&9Kb$hrYf z*vbyC7v=0ISTwTp-sAT^KZ-chzP6^BxHu3jRc zMKr=6N7w!}ne98>Y4c>Bq=VTBreadJL3WUSwTG`LZ3kwIQZ?#9+Hu$|w zh^=|8VHu$qB*Ioe(?0Lm;I|*($vnUmyEGir`3|WNg%$gQ3^d(89f$*>Vc0J*sXPbQ zmUU(Ho}(bmT(V_BVzdG-64+ht|cRd#Mkq(nlHTy^5!2>O){K1d*=ns$xJDciwFTH5^) zBY8aKUv~v8DA3GsJUotsaA3B>qh{)yNhlg!J;3jWoxzr1r40&g`Q^8dR{eJG!DHzO z_aY5`ES+|8$%6OH6%$OWA-xqMz( z6`17e`HocYn*%i|UvI43aM?GrX5;Dy(3qh>0mHIr{y|z=_-GNS*iIfdklGvMdJe2+ z>SVIU$y^^xQ*$nY*_FkT zJQEE1rK|xI1XPzJnAQR;o-{H;c8*k9ap!l@yqd(tR5d1JetbgEX+1Z=hDVVxFqxz^Yu9aO=q69u=GYGx!}}8*;QwGv77K*~38e83J#tmt2YDo$ zr?IV$$xu&dsvUyaYCkh`pb#3_D39YxMY87aC5!XV*jPG3;wE-VjT?UGt>YE{-o{M%FrtVH@BiOWuP!~3X($-Y7oC@d|(^9RN*I{0!iK^NX2PX;Bi=4 z*ML`9HCdV*cg z)2)P|lColX21F!1Y=0uc9CB3t_42-~9w0>uCCDY5-+EdbX(5t52wtvItb;ZMkwC-4 zDtdN|wt*2*p?%l;;EX3z=TR{YAK8*qV($@Tau~RoxEb^J-oKh8(E(|0v4~2p6vzR! z%aW+A$~sSW=BrHQ9~6_al6!c$7!;P?KD=EkX?HduSo;QwPtL^d#wH_$Y#hItsn^z? z${i+-ZDfxa9n&{$%Zp27A`6~AJM<%{wxZ9CVO z7uR1YizT(&wPp@T@E+q-AbW^mvqW}$%pvZ4A4Vv4HsjMRcqg&E4RYp zw&CvD=|I`PTT7tHzQMj1C0x6=dO~b8E0QVl3xe5LSc0L>MCg(S0oR({Q-H>Ic&#G=uQPyaa~e|-<|L(jCduWq@1F4Up|~&BIY~Sf zP3@gNOIq!8O(w)0xeJ-jUXvBOfohz2Uq_q!<)>K}RtH2BUEY=m9hV5bfU#yTH7xMr zJBgEEHAatiKqmB>YX)D|9@>$bG_K2Q|DJnZQBTquYR&m1x~k_iVrWE^2N$_GsqJSA zS4P0XZLKG8{$VhuJ2G^EYX}!jrJT*~@(FmM{095jXeX zlB5+~>7yM;hrfqtJ%nLwhP01=K|j5-KaLjqO}@y6q|kXZ;gWR2B&=@;WbhzO5!4F~}Sh~s;P!zeZ*Ptq+p>{zR>1?Ts@XyE3DZbH`v_5$&i!MPR3EJ1Ooh6wP=KEGdlP^0); z;gv_a54D@{#39U5d-o++C}xoU(tphdN?W|D0Qe!U-G&`wnX3ymT1OB@5BK>Z=t0*! z%unnm%r7CgLIFyQ?8mu5H|gF3QLjC9HbyLyx+41RRKp#p8lqH%@a@}u=_>344E?1) zEouu7h{nNqw!iXAb(8gpKKqpTnZUElX$tye1e(%52^hnG3}1oxeGsBr|DTm>|08sy zX-NYZg)w@xD3`l~7pF+?vjSsl=xTKxdsTui#}lw|tv4ddKvNM?I#>$I$e_MfSBuvI zuv4(YXWc2%Mjh6mQN7|E2z;d^&0Gn^O1K)|vj6KI+S8j>{i*4h_jdqd>oPH>^BVP- zfxtuKJFrJFKPwL@;?qtj>|@kxRYC+$d_(tY9RN2Rxj24jSE7LtEb8fV$Naqer=~@! zcb|3lD-3f%V@`kQCB5Mny|_1KK0PCaVr>FuSwVwFq!F)}l?IFTr8 z`utB#FV1GdxIz8(2{0X^AoN1HcYJqC{K<~@GY|Z)zN9Az9~?9I;7?7#)i2f5jnj|HuzH(Z9}=Y8!mD&{)jL n*8FQQS_FTDSk=9a{Biv8^vF(np|v3Vd^Llio*D7J4vG99ULF}Q literal 0 HcmV?d00001