diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 162c0394cc4d9..58002c148870c 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -6,7 +6,6 @@ missingIncludeSystem noConstructor unknownMacro unmatchedSuppression -unreadVariable unusedFunction useInitializationList useStlAlgorithm diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 403fe7e0f728f..6de0c3251696b 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -134,7 +134,7 @@ perception/autoware_raindrop_cluster_filter/** dai.nguyen@tier4.jp yoshi.ri@tier perception/autoware_shape_estimation/** kcolak@leodrive.ai yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/autoware_tensorrt_classifier/** kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp -perception/autoware_tensorrt_yolox/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp +perception/autoware_tensorrt_yolox/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp kotaro.uetake@tier4.jp perception/autoware_tracking_object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/autoware_traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp perception/autoware_traffic_light_classifier/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp yukihiro.saito@tier4.jp @@ -200,6 +200,7 @@ planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4. planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_sampler_common/** maxime.clement@tier4.jp sensing/autoware_image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp +sensing/autoware_imu_corrector/** taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/autoware_image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp sensing/autoware_pcl_extensions/** david.wong@tier4.jp kenzo.lobos@tier4.jp ryu.yamamoto@tier4.jp sensing/autoware_pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -221,7 +222,7 @@ system/autoware_component_monitor/** memin@leodrive.ai system/autoware_processing_time_checker/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp system/bluetooth_monitor/** fumihito.ito@tier4.jp system/component_state_monitor/** isamu.takagi@tier4.jp -system/default_ad_api/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/autoware_default_adapi/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index 126e66772920a..0ce42fbe1080f 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -2504,6 +2504,15 @@ extern template bool isTargetPointFront & trajectory, + const geometry_msgs::msg::Point & current_ego_point, const float min_velocity = 1.0f); + } // namespace autoware::motion_utils #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index ce26952c3d634..080d8ca8c7437 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -599,4 +599,27 @@ template bool isTargetPointFront & trajectory, + const geometry_msgs::msg::Point & current_ego_point, const float min_velocity) +{ + const auto nearest_segment_idx = findNearestSegmentIndex(trajectory, current_ego_point); + if (nearest_segment_idx + 1 == trajectory.size()) { + return; + } + for (auto & p : trajectory) { + p.time_from_start = rclcpp::Duration::from_seconds(0); + } + // TODO(Maxime): some points can have very low velocities which introduce huge time errors + // Temporary solution: use a minimum velocity + for (auto idx = nearest_segment_idx + 1; idx < trajectory.size(); ++idx) { + const auto & from = trajectory[idx - 1]; + const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps); + if (velocity != 0.0) { + auto & to = trajectory[idx]; + const auto t = universe_utils::calcDistance2d(from, to) / velocity; + to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start; + } + } +} } // namespace autoware::motion_utils diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index 6ae9ec8a4d4cb..e983d9be77e36 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include namespace autoware::universe_utils @@ -169,6 +170,7 @@ class TimeKeeper std::shared_ptr current_time_node_; //!< Shared pointer to the current time node std::shared_ptr root_node_; //!< Shared pointer to the root time node + std::thread::id root_node_thread_id_; //!< ID of the thread that started the tracking autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; //!< StopWatch object for tracking the processing time diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp index 64f4e95f2a46e..8c715c5ccd7e3 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -14,6 +14,9 @@ #include "autoware/universe_utils/system/time_keeper.hpp" +#include +#include + #include #include @@ -129,7 +132,14 @@ void TimeKeeper::start_track(const std::string & func_name) if (current_time_node_ == nullptr) { current_time_node_ = std::make_shared(func_name); root_node_ = current_time_node_; + root_node_thread_id_ = std::this_thread::get_id(); } else { + if (root_node_thread_id_ != std::this_thread::get_id()) { + RCLCPP_WARN( + rclcpp::get_logger("TimeKeeper"), + "TimeKeeper::start_track() is called from a different thread. Ignoring the call."); + return; + } current_time_node_ = current_time_node_->add_child(func_name); } stop_watch_.tic(func_name); @@ -145,6 +155,9 @@ void TimeKeeper::comment(const std::string & comment) void TimeKeeper::end_track(const std::string & func_name) { + if (root_node_thread_id_ != std::this_thread::get_id()) { + return; + } if (current_time_node_->get_name() != func_name) { throw std::runtime_error(fmt::format( "You must call end_track({}) first, but end_track({}) is called", @@ -178,7 +191,7 @@ ScopedTimeTrack::ScopedTimeTrack(const std::string & func_name, TimeKeeper & tim time_keeper_.start_track(func_name_); } -ScopedTimeTrack::~ScopedTimeTrack() +ScopedTimeTrack::~ScopedTimeTrack() // NOLINT { time_keeper_.end_track(func_name_); } diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp index 71ca7cc74bec5..0540fa8a283ad 100644 --- a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -11,7 +11,6 @@ // 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 "autoware/universe_utils/system/time_keeper.hpp" #include @@ -20,34 +19,74 @@ #include #include -#include +#include #include -TEST(system, TimeKeeper) +class TimeKeeperTest : public ::testing::Test { - using autoware::universe_utils::ScopedTimeTrack; - using autoware::universe_utils::TimeKeeper; +protected: + std::ostringstream oss; + rclcpp::Node::SharedPtr node; + rclcpp::Publisher::SharedPtr publisher; + std::unique_ptr time_keeper; - rclcpp::Node node{"sample_node"}; - - auto publisher = node.create_publisher( - "~/debug/processing_time_tree", 1); + void SetUp() override + { + node = std::make_shared("test_node"); + publisher = node->create_publisher( + "~/debug/processing_time_tree", 1); + time_keeper = std::make_unique(&oss, publisher); + } +}; - TimeKeeper time_keeper(&std::cerr, publisher); +TEST_F(TimeKeeperTest, BasicFunctionality) +{ + using autoware::universe_utils::ScopedTimeTrack; - ScopedTimeTrack st{"main_func", time_keeper}; + { + ScopedTimeTrack st{"main_func", *time_keeper}; - { // funcA - ScopedTimeTrack st{"funcA", time_keeper}; - std::this_thread::sleep_for(std::chrono::seconds(1)); - } + { // funcA + ScopedTimeTrack st{"funcA", *time_keeper}; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } - { // funcB - ScopedTimeTrack st{"funcB", time_keeper}; - std::this_thread::sleep_for(std::chrono::seconds(1)); - { // funcC - ScopedTimeTrack st{"funcC", time_keeper}; - std::this_thread::sleep_for(std::chrono::seconds(1)); + { // funcB + ScopedTimeTrack st{"funcB", *time_keeper}; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + { // funcC + ScopedTimeTrack st{"funcC", *time_keeper}; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } } } + + // Check if the output contains all function names + std::string output = oss.str(); + EXPECT_TRUE(output.find("main_func") != std::string::npos); + EXPECT_TRUE(output.find("funcA") != std::string::npos); + EXPECT_TRUE(output.find("funcB") != std::string::npos); + EXPECT_TRUE(output.find("funcC") != std::string::npos); +} + +TEST_F(TimeKeeperTest, MultiThreadWarning) +{ + testing::internal::CaptureStderr(); + + std::thread t([this]() { + time_keeper->start_track("ThreadFunction"); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + time_keeper->end_track("ThreadFunction"); + }); + + time_keeper->start_track("MainFunction"); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + time_keeper->end_track("MainFunction"); + + t.join(); + + std::string err = testing::internal::GetCapturedStderr(); + EXPECT_TRUE( + err.find("TimeKeeper::start_track() is called from a different thread. Ignoring the call.") != + std::string::npos); } diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp index 16c3f4b5def96..2368e07432031 100644 --- a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp @@ -99,6 +99,7 @@ void BirdEyeViewController::reset() y_property_->setFloat(0); } +// cppcheck-suppress unusedFunction void BirdEyeViewController::handleMouseEvent(rviz_common::ViewportMouseEvent & event) { if (event.shift()) { @@ -178,11 +179,13 @@ void BirdEyeViewController::update(float dt, float ros_dt) updateCamera(); } +// cppcheck-suppress unusedFunction void BirdEyeViewController::lookAt(const Ogre::Vector3 & point) { setPosition(point - target_scene_node_->getPosition()); } +// cppcheck-suppress unusedFunction void BirdEyeViewController::onTargetFrameChanged( const Ogre::Vector3 & old_reference_position, const Ogre::Quaternion & /*old_reference_orientation*/) diff --git a/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp index ed47a15270c3a..97216f238d593 100644 --- a/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp +++ b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp @@ -226,6 +226,7 @@ void ThirdPersonViewController::updateCamera() distance_property_->getFloat() * CAMERA_OFFSET); } +// cppcheck-suppress unusedFunction void ThirdPersonViewController::updateTargetSceneNode() { if (FramePositionTrackingViewController::getNewTransform()) { diff --git a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp index cf4ecdd9774b8..3a40c0c3b1d51 100644 --- a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp +++ b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp @@ -30,13 +30,6 @@ namespace traffic_light_utils { -bool isRoiValid( - const tier4_perception_msgs::msg::TrafficLightRoi & roi, uint32_t width, uint32_t height); - -void setRoiInvalid(tier4_perception_msgs::msg::TrafficLightRoi & roi); - -bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficLight & signal); - void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence = -1); /** diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index 8aea884510ec2..a3da21c1bb578 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -17,29 +17,6 @@ namespace traffic_light_utils { -bool isRoiValid( - const tier4_perception_msgs::msg::TrafficLightRoi & roi, uint32_t width, uint32_t height) -{ - uint32_t x1 = roi.roi.x_offset; - uint32_t x2 = roi.roi.x_offset + roi.roi.width; - uint32_t y1 = roi.roi.y_offset; - uint32_t y2 = roi.roi.y_offset + roi.roi.height; - return roi.roi.width > 0 && roi.roi.height > 0 && x1 < width && y1 < height && x2 < width && - y2 < height; -} - -void setRoiInvalid(tier4_perception_msgs::msg::TrafficLightRoi & roi) -{ - roi.roi.height = roi.roi.width = 0; -} - -bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficLight & signal) -{ - return signal.elements.size() == 1 && - signal.elements[0].shape == tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN && - signal.elements[0].color == tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; -} - void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence) { signal.elements.resize(1); diff --git a/common/traffic_light_utils/test/test_traffic_light_utils.cpp b/common/traffic_light_utils/test/test_traffic_light_utils.cpp index a80510693c487..a9e601370945d 100644 --- a/common/traffic_light_utils/test/test_traffic_light_utils.cpp +++ b/common/traffic_light_utils/test/test_traffic_light_utils.cpp @@ -18,46 +18,6 @@ namespace traffic_light_utils { -TEST(isRoiValid, roi_validity) -{ - tier4_perception_msgs::msg::TrafficLightRoi test_roi; - test_roi.roi.x_offset = 300; - test_roi.roi.y_offset = 200; - test_roi.roi.width = 340; - test_roi.roi.height = 200; - uint32_t img_width = 640; - uint32_t img_heigh = 480; - EXPECT_FALSE(isRoiValid(test_roi, img_width, img_heigh)); - test_roi.roi.width = 339; - EXPECT_TRUE(isRoiValid(test_roi, img_width, img_heigh)); -} - -TEST(setRoiInvalid, set_roi_size) -{ - tier4_perception_msgs::msg::TrafficLightRoi test_roi; - test_roi.roi.x_offset = 300; - test_roi.roi.y_offset = 200; - test_roi.roi.width = 300; - test_roi.roi.height = 200; - EXPECT_EQ(test_roi.roi.width, (uint32_t)300); - EXPECT_EQ(test_roi.roi.height, (uint32_t)200); - setRoiInvalid(test_roi); - EXPECT_EQ(test_roi.roi.width, (uint32_t)0); - EXPECT_EQ(test_roi.roi.height, (uint32_t)0); -} - -TEST(isSignalUnknown, signal_element) -{ - tier4_perception_msgs::msg::TrafficLight test_signal; - tier4_perception_msgs::msg::TrafficLightElement element; - element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; - element.shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; - test_signal.elements.push_back(element); - EXPECT_TRUE(isSignalUnknown(test_signal)); - test_signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::RED; - EXPECT_FALSE(isSignalUnknown(test_signal)); -} - TEST(setSignalUnknown, set_signal_element) { tier4_perception_msgs::msg::TrafficLight test_signal; diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index 1382a24d98b0a..66d4483fbdf84 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -8,8 +8,6 @@ This module has following assumptions. -- It is used when driving at low speeds (about 15 km/h). - - The predicted path of the ego vehicle can be made from either the path created from sensors or the path created from a control module, or both. - The current speed and angular velocity can be obtained from the sensors of the ego vehicle, and it uses points as obstacles. @@ -25,7 +23,7 @@ The pros and cons of both approaches are: IMU angular velocity: - (+) Usually, it has high accuracy -- (-)Vehicle vibration might introduce noise. +- (-) Vehicle vibration might introduce noise. Steering angle: @@ -186,7 +184,7 @@ The AEB module can also prevent collisions when the ego vehicle is moving backwa When vehicle odometry information is faulty, it is possible that the MPC fails to predict a correct path for the ego vehicle. If the MPC predicted path is wrong, collision avoidance will not work as intended on the planning modules. However, the AEB’s IMU path does not depend on the MPC and could be able to predict a collision when the other modules cannot. As an example you can see a figure of a hypothetical case in which the MPC path is wrong and only the AEB’s IMU path detects a collision. -![backward driving](./image/wrong-mpc.drawio.svg) +![wrong mpc](./image/wrong-mpc.drawio.svg) ## Parameters @@ -217,6 +215,8 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t ## Limitations +- The distance required to stop after collision detection depends on the ego vehicle's speed and deceleration performance. To avoid collisions, it's necessary to increase the detection distance and set a higher deceleration rate. However, this creates a trade-off as it may also increase the number of unnecessary activations. Therefore, it's essential to consider what role this module should play and adjust the parameters accordingly. + - AEB might not be able to react with obstacles that are close to the ground. It depends on the performance of the pre-processing methods applied to the point cloud. - Longitudinal acceleration information obtained from sensors is not used due to the high amount of noise. diff --git a/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg index 2d1716519631d..1d3dd824df5f0 100644 --- a/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg +++ b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg @@ -1,134 +1,73 @@ - - - - - - - + + + + + + + + + + + + + + + + +
+
+ + Closest point distance +
+ +
+
+ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- - Closest Point Distance -
-
-
- - - - - - - - - - - - - - - - - -
-
-
RSS distance
-
-
- - - - + + + + + + +
+
+ RSS distance +
+
+ diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 24eaa8516a732..c29247a978c73 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -156,10 +156,7 @@ class CollisionDataKeeper * @brief Get the closest object data * @return Object data of the closest object */ - [[nodiscard]] ObjectData get() const - { - return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); - } + [[nodiscard]] std::optional get() const { return closest_object_; } /** * @brief Get the previous closest object data diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index dc8c876bd56f6..e8744d85dc11a 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -393,12 +393,15 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) const auto diag_level = DiagnosticStatus::ERROR; stat.summary(diag_level, error_msg); const auto & data = collision_data_keeper_.get(); - stat.addf("RSS", "%.2f", data.rss); - stat.addf("Distance", "%.2f", data.distance_to_object); - stat.addf("Object Speed", "%.2f", data.velocity); - if (publish_debug_markers_) { - addCollisionMarker(data, debug_markers); + if (data.has_value()) { + stat.addf("RSS", "%.2f", data.value().rss); + stat.addf("Distance", "%.2f", data.value().distance_to_object); + stat.addf("Object Speed", "%.2f", data.value().velocity); + if (publish_debug_markers_) { + addCollisionMarker(data.value(), debug_markers); + } } + addVirtualStopWallMarker(info_markers); } else { const std::string error_msg = "[AEB]: No Collision"; @@ -476,17 +479,19 @@ bool AEB::checkCollision(MarkerArray & debug_markers) return std::make_optional(*closest_object_point_itr); }); + const bool has_collision = (closest_object_point.has_value()) + ? hasCollision(current_v, closest_object_point.value()) + : false; + // Add debug markers if (publish_debug_markers_) { const auto [color_r, color_g, color_b, color_a] = debug_colors; addMarker( - this->get_clock()->now(), path, ego_polys, objects, closest_object_point, color_r, color_g, - color_b, color_a, debug_ns, debug_markers); + this->get_clock()->now(), path, ego_polys, objects, collision_data_keeper_.get(), color_r, + color_g, color_b, color_a, debug_ns, debug_markers); } // check collision using rss distance - return (closest_object_point.has_value()) - ? hasCollision(current_v, closest_object_point.value()) - : false; + return has_collision; }; // step3. make function to check collision with ego path created with sensor data diff --git a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml index f683d8b4f0736..6d9b93ab98870 100644 --- a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml +++ b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml @@ -61,6 +61,10 @@ emergency_acc: -5.0 # denotes acceleration emergency_jerk: -3.0 + # acceleration feedback + lpf_acc_error_gain: 0.98 + acc_feedback_gain: 0.0 + # acceleration limit max_acc: 3.0 min_acc: -5.0 diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp index e8834665de3ec..a0bceda625b21 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp @@ -58,6 +58,10 @@ class DebugValues STOP_DIST = 28, FF_SCALE = 29, ACC_CMD_FF = 30, + ERROR_ACC = 31, + ERROR_ACC_FILTERED = 32, + ACC_CMD_ACC_FB_APPLIED = 33, + SIZE // this is the number of enum elements }; diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp index 081e78e2de214..1ec43282ca4d3 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp @@ -61,6 +61,8 @@ class LowpassFilter1d m_x = ret; return ret; } + + void setGain(const double g) { m_gain = g; } }; } // namespace autoware::motion::control::pid_longitudinal_controller #endif // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 57999372dceed..1d4192d51d98d 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -196,6 +196,10 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro }; EmergencyStateParams m_emergency_state_params; + // acc feedback + double m_acc_feedback_gain; + std::shared_ptr m_lpf_acc_error{nullptr}; + // acceleration limit double m_max_acc; double m_min_acc; diff --git a/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json b/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json index fbbd2dfc7a59f..ef1272582e52b 100644 --- a/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json +++ b/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json @@ -251,6 +251,16 @@ "description": "target jerk in an EMERGENCY state [m/s^3]", "default": "-3.0" }, + "lpf_acc_error_gain": { + "type": "number", + "description": "gain of low-pass filter for acceleration", + "default": "0.98" + }, + "acc_feedback_gain": { + "type": "number", + "description": "acceleration feedback gain", + "default": "0.0" + }, "max_acc": { "type": "number", "description": "max value of output acceleration [m/s^2]", diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 9190c065eca3c..96e2796a91d22 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -164,6 +164,13 @@ PidLongitudinalController::PidLongitudinalController( p.jerk = node.declare_parameter("emergency_jerk"); // [m/s^3] } + // parameters for acc feedback + { + const double lpf_acc_error_gain{node.declare_parameter("lpf_acc_error_gain")}; + m_lpf_acc_error = std::make_shared(0.0, lpf_acc_error_gain); + m_acc_feedback_gain = node.declare_parameter("acc_feedback_gain"); + } + // parameters for acceleration limit m_max_acc = node.declare_parameter("max_acc"); // [m/s^2] m_min_acc = node.declare_parameter("min_acc"); // [m/s^2] @@ -291,6 +298,10 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac update_param("kd", kd); m_pid_vel.setGains(kp, ki, kd); + double lpf_vel_error_gain{node_parameters_->get_parameter("lpf_vel_error_gain").as_double()}; + update_param("lpf_vel_error_gain", lpf_vel_error_gain); + m_lpf_vel_error->setGain(lpf_vel_error_gain); + double max_pid{node_parameters_->get_parameter("max_out").as_double()}; double min_pid{node_parameters_->get_parameter("min_out").as_double()}; double max_p{node_parameters_->get_parameter("max_p_effort").as_double()}; @@ -365,6 +376,12 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac update_param("emergency_jerk", p.jerk); } + // acceleration feedback + update_param("acc_feedback_gain", m_acc_feedback_gain); + double lpf_acc_error_gain{node_parameters_->get_parameter("lpf_acc_error_gain").as_double()}; + update_param("lpf_acc_error_gain", lpf_acc_error_gain); + m_lpf_acc_error->setGain(lpf_acc_error_gain); + // acceleration limit update_param("min_acc", m_min_acc); @@ -753,6 +770,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); + m_lpf_acc_error->reset(0.0); return changeState(ControlState::DRIVE); } @@ -844,8 +862,22 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( // store acceleration without slope compensation m_prev_raw_ctrl_cmd = raw_ctrl_cmd; + // calc acc feedback + const double acc_err = control_data.current_motion.acc - raw_ctrl_cmd.acc; + m_debug_values.setValues(DebugValues::TYPE::ERROR_ACC, acc_err); + m_lpf_acc_error->filter(acc_err); + m_debug_values.setValues(DebugValues::TYPE::ERROR_ACC_FILTERED, m_lpf_acc_error->getValue()); + + const double acc_cmd = raw_ctrl_cmd.acc - m_lpf_acc_error->getValue() * m_acc_feedback_gain; + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_FB_APPLIED, acc_cmd); + RCLCPP_DEBUG( + logger_, + "[acc feedback]: raw_ctrl_cmd.acc: %1.3f, control_data.current_motion.acc: %1.3f, acc_cmd: " + "%1.3f", + raw_ctrl_cmd.acc, control_data.current_motion.acc, acc_cmd); + ctrl_cmd_as_pedal_pos.acc = - applySlopeCompensation(raw_ctrl_cmd.acc, control_data.slope_angle, control_data.shift); + applySlopeCompensation(acc_cmd, control_data.slope_angle, control_data.shift); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, ctrl_cmd_as_pedal_pos.acc); ctrl_cmd_as_pedal_pos.vel = raw_ctrl_cmd.vel; } diff --git a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml index ab127c2d47b97..e458edaf6c471 100644 --- a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml +++ b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml @@ -1,130 +1,130 @@ - - + + - - + + - - + + - + - + - + - + - + - + - + - - + + - + - + - - + + - + - + - + - - + + - + - + - + - + - - + + - + - + - - + + @@ -133,100 +133,114 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + + - + - + - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -234,17 +248,17 @@ - + - + - + @@ -254,13 +268,13 @@ - + - + @@ -272,11 +286,10 @@ - + - diff --git a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml index 5027c94afe7c1..68fb172b19d2a 100644 --- a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -60,6 +60,10 @@ emergency_acc: -5.0 # denotes acceleration emergency_jerk: -3.0 + # acceleration feedback + lpf_acc_error_gain: 0.98 + acc_feedback_gain: 0.0 + # acceleration limit max_acc: 3.0 min_acc: -5.0 diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml index 2efd1a595b2f4..143fbc9be5bfb 100644 --- a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml +++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -1,7 +1,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml index d45d21b3ed949..dbf5d16834681 100644 --- a/launch/tier4_autoware_api_launch/package.xml +++ b/launch/tier4_autoware_api_launch/package.xml @@ -12,11 +12,11 @@ autoware_cmake ad_api_adaptors + autoware_default_adapi autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor autoware_path_distance_calculator awapi_awiv_adapter - default_ad_api rosbridge_server topic_tools diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 4a7696ec9e65e..13319521e35ab 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -30,8 +30,8 @@ simple_1d_filter_parameters: #Simple1DFilter parameters z_filter_proc_dev: 1.0 - roll_filter_proc_dev: 0.01 - pitch_filter_proc_dev: 0.01 + roll_filter_proc_dev: 0.1 + pitch_filter_proc_dev: 0.1 diagnostics: # for diagnostics diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index f3830c303ab48..650eca26d4e96 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -141,6 +142,10 @@ class EKFLocalizer : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_tf_; //!< @brief tf broadcaster std::shared_ptr tf_br_; + //!< @brief tf buffer + tf2_ros::Buffer tf2_buffer_; + //!< @brief tf listener + tf2_ros::TransformListener tf2_listener_; //!< @brief logger configure module std::unique_ptr logger_configure_; diff --git a/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json index ad2f638a18d5f..00679ee92ad24 100644 --- a/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json +++ b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json @@ -13,12 +13,12 @@ "roll_filter_proc_dev": { "type": "number", "description": "Simple1DFilter - Roll filter process deviation", - "default": 0.01 + "default": 0.1 }, "pitch_filter_proc_dev": { "type": "number", "description": "Simple1DFilter - Pitch filter process deviation", - "default": 0.01 + "default": 0.1 } }, "required": ["z_filter_proc_dev", "roll_filter_proc_dev", "pitch_filter_proc_dev"], diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 707d5e585cb0c..f1e4d0c958c77 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -44,6 +44,8 @@ using std::placeholders::_1; EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) : rclcpp::Node("ekf_localizer", node_options), warning_(std::make_shared(this)), + tf2_buffer_(this->get_clock()), + tf2_listener_(tf2_buffer_), params_(this), ekf_dt_(params_.ekf_dt), pose_queue_(params_.pose_smoothing_steps), @@ -285,21 +287,14 @@ bool EKFLocalizer::get_transform_from_tf( std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform) { - tf2::BufferCore tf_buffer; - tf2_ros::TransformListener tf_listener(tf_buffer); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - parent_frame = erase_leading_slash(parent_frame); child_frame = erase_leading_slash(child_frame); - for (int i = 0; i < 50; ++i) { - try { - transform = tf_buffer.lookupTransform(parent_frame, child_frame, tf2::TimePointZero); - return true; - } catch (tf2::TransformException & ex) { - warning_->warn(ex.what()); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } + try { + transform = tf2_buffer_.lookupTransform(parent_frame, child_frame, tf2::TimePointZero); + return true; + } catch (tf2::TransformException & ex) { + warning_->warn(ex.what()); } return false; } diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 3e8b9b99737c0..b59686c211b53 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -961,7 +961,8 @@ void NDTScanMatcher::service_ndt_align_main( diagnostics_ndt_align_->add_key_value("is_succeed_transform_initial_pose", true); // transform pose_frame to map_frame - const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); + auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); + initial_pose_msg_in_map_frame.header.stamp = req->pose_with_covariance.header.stamp; map_update_module_->update_map( initial_pose_msg_in_map_frame.pose.pose.position, diagnostics_ndt_align_); diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 94bf98f272487..2928f51f7256d 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -51,9 +51,9 @@ If the score of initial pose estimation result is lower than score threshold, ER ## Connection with Default AD API -This `pose_initializer` is used via default AD API. For detailed description of the API description, please refer to [the description of `default_ad_api`](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/localization.md). +This `pose_initializer` is used via default AD API. For detailed description of the API description, please refer to [the description of `autoware_default_adapi`](https://github.com/autowarefoundation/autoware.universe/blob/main/system/autoware_default_adapi/document/localization.md). -drawing +drawing ## Initialize pose via CLI @@ -136,4 +136,4 @@ pose: ``` It behaves the same as "initialpose (from rviz)". -The position.z and the covariance will be overwritten by [ad_api_adaptors](https://github.com/autowarefoundation/autoware.universe/tree/main/system/default_ad_api_helpers/ad_api_adaptors), so there is no need to input them. +The position.z and the covariance will be overwritten by [ad_api_adaptors](https://github.com/autowarefoundation/autoware.universe/tree/main/system/autoware_default_adapi_helpers/ad_api_adaptors), so there is no need to input them. diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 3d9b51ae7add7..f999dcd8b0f3f 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -351,7 +351,6 @@ dc | dc dc dc dc ||zc| // paint current point if it is inside bbox int label2d = feature_object.object.classification.front().label; if (!isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, p_z)) { - data = &painted_pointcloud_msg.data[0]; // cppcheck-suppress invalidPointerCast auto p_class = reinterpret_cast(&output[stride + class_offset]); for (const auto & cls : isClassTable_) { diff --git a/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml b/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml index b65f0de892dcc..2c668639c2a56 100644 --- a/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml +++ b/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/perception/autoware_multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/autoware_multi_object_tracker/launch/multi_object_tracker.launch.xml index db76e181a6afa..88f63e00ed511 100644 --- a/perception/autoware_multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/autoware_multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp index a8288d2720f48..41c7294ded849 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -120,8 +120,6 @@ bool extractCommonPointCloud( const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, sensor_msgs::msg::PointCloud2 & output_obstacle_pc); -unsigned char getApproximateOccupancyState(const unsigned char & value); - } // namespace utils } // namespace autoware::occupancy_grid_map diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp index 902de2148cffe..9d190664b8858 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp @@ -191,21 +191,5 @@ bool extractCommonPointCloud( return true; } -/** - * @brief Convert unsigned char value to closest cost value - * @param cost Cost value - * @return Probability - */ -unsigned char getApproximateOccupancyState(const unsigned char & value) -{ - if (value >= cost_value::OCCUPIED_THRESHOLD) { - return cost_value::LETHAL_OBSTACLE; - } else if (value <= cost_value::FREE_THRESHOLD) { - return cost_value::FREE_SPACE; - } else { - return cost_value::NO_INFORMATION; - } -} - } // namespace utils } // namespace autoware::occupancy_grid_map diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index 0a78b10055dd0..28a5759eeb7b0 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -369,42 +369,6 @@ OccupancyGridMapFixedBlindSpot GridMapFusionNode::SingleFrameOccupancyFusion( return fused_map; } -/** - * @brief Update occupancy grid map with asynchronous input (currently unused) - * - * @param occupancy_grid_msg - */ -void GridMapFusionNode::updateGridMap( - const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg) -{ - // get updater map origin - - // origin is set to current updater map - auto map_for_update = OccupancyGridMsgToCostmap2D(*occupancy_grid_msg); - - // update - occupancy_grid_map_updater_ptr_->update(map_for_update); -} - -nav2_costmap_2d::Costmap2D GridMapFusionNode::OccupancyGridMsgToCostmap2D( - const nav_msgs::msg::OccupancyGrid & occupancy_grid_map) -{ - nav2_costmap_2d::Costmap2D costmap2d( - occupancy_grid_map.info.width, occupancy_grid_map.info.height, - occupancy_grid_map.info.resolution, occupancy_grid_map.info.origin.position.x, - occupancy_grid_map.info.origin.position.y, 0); - - for (unsigned int i = 0; i < occupancy_grid_map.info.width; i++) { - for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { - const unsigned int index = i + j * occupancy_grid_map.info.width; - costmap2d.setCost( - i, j, cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); - } - } - - return costmap2d; -} - OccupancyGridMapFixedBlindSpot GridMapFusionNode::OccupancyGridMsgToGridMap( const nav_msgs::msg::OccupancyGrid & occupancy_grid_map) { diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp index 691575299a8c7..23aeb76826e5f 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp @@ -66,16 +66,12 @@ class GridMapFusionNode : public rclcpp::Node const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, const float & robot_pose_z, const nav2_costmap_2d::Costmap2D & occupancy_grid_map); - nav2_costmap_2d::Costmap2D OccupancyGridMsgToCostmap2D( - const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); OccupancyGridMapFixedBlindSpot OccupancyGridMsgToGridMap( const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); OccupancyGridMapFixedBlindSpot SingleFrameOccupancyFusion( std::vector & occupancy_grid_maps, const builtin_interfaces::msg::Time latest_stamp, const std::vector & weights); - void updateGridMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg); - void setPeriod(const int64_t new_period); void timer_callback(); void publish(); diff --git a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/preprocess.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/preprocess.hpp index 947d6cc6ecc95..7971fbd815deb 100644 --- a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/preprocess.hpp +++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/preprocess.hpp @@ -21,7 +21,9 @@ #include #include -namespace autoware::tensorrt_yolox +namespace autoware +{ +namespace tensorrt_yolox { struct Roi { @@ -195,5 +197,6 @@ extern void multi_scale_resize_bilinear_letterbox_nhwc_to_nchw32_batch_gpu( extern void argmax_gpu( unsigned char * dst, float * src, int d_w, int d_h, int s_w, int s_h, int s_c, int batch, cudaStream_t stream); -} // namespace autoware::tensorrt_yolox +} // namespace tensorrt_yolox +} // namespace autoware #endif // AUTOWARE__TENSORRT_YOLOX__PREPROCESS_HPP_ diff --git a/perception/autoware_tensorrt_yolox/package.xml b/perception/autoware_tensorrt_yolox/package.xml index 3373a07d8b434..cd9dc6bac015d 100644 --- a/perception/autoware_tensorrt_yolox/package.xml +++ b/perception/autoware_tensorrt_yolox/package.xml @@ -7,6 +7,7 @@ Daisuke Nishimatsu Dan Umeda Manato Hirabayashi + Kotaro Uetake Apache License 2.0 diff --git a/perception/autoware_tensorrt_yolox/src/preprocess.cu b/perception/autoware_tensorrt_yolox/src/preprocess.cu index ba9d456f32749..6e8ded20df709 100644 --- a/perception/autoware_tensorrt_yolox/src/preprocess.cu +++ b/perception/autoware_tensorrt_yolox/src/preprocess.cu @@ -21,7 +21,9 @@ #define MIN(x, y) x < y ? x : y -namespace autoware::tensorrt_yolox +namespace autoware +{ +namespace tensorrt_yolox { constexpr size_t block = 512; @@ -629,4 +631,5 @@ void argmax_gpu( N, dst, src, d_h, d_w, s_c, s_h, s_w, batch); } -} // namespace autoware::tensorrt_yolox +} // namespace tensorrt_yolox +} // namespace autoware diff --git a/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp index 17a385da7cb41..f40767274ebfd 100644 --- a/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp @@ -80,13 +80,6 @@ autoware_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( TrackedObject objectClassificationMerger( const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); -// probability merger -float probabilityMerger(const float main_prob, const float sub_prob, const MergePolicy policy); - -// shape merger -autoware_perception_msgs::msg::Shape shapeMerger( - const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); - // update tracked object void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj); diff --git a/perception/autoware_tracking_object_merger/src/utils/utils.cpp b/perception/autoware_tracking_object_merger/src/utils/utils.cpp index 3a24cc0f8321d..04d7f94f2886c 100644 --- a/perception/autoware_tracking_object_merger/src/utils/utils.cpp +++ b/perception/autoware_tracking_object_merger/src/utils/utils.cpp @@ -290,31 +290,6 @@ bool objectsHaveSameMotionDirections(const TrackedObject & main_obj, const Track } } -/** - * @brief compare two tracked objects yaw is reverted or not - * - * @param main_obj - * @param sub_obj - * @return true - * @return false - */ -bool objectsYawIsReverted(const TrackedObject & main_obj, const TrackedObject & sub_obj) -{ - // get yaw - const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); - const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); - // calc yaw diff - const auto yaw_diff = std::fabs(main_yaw - sub_yaw); - const auto normalized_yaw_diff = autoware::universe_utils::normalizeRadian(yaw_diff); // -pi ~ pi - // evaluate if yaw is reverted - constexpr double yaw_threshold = M_PI / 2.0; // 90 deg - if (std::abs(normalized_yaw_diff) >= yaw_threshold) { - return true; - } else { - return false; - } -} - // object kinematics merger // currently only support velocity fusion autoware_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( @@ -409,66 +384,6 @@ TrackedObject objectClassificationMerger( } } -// probability merger -float probabilityMerger(const float main_prob, const float sub_prob, const MergePolicy policy) -{ - if (policy == MergePolicy::SKIP) { - return main_prob; - } else if (policy == MergePolicy::OVERWRITE) { - return sub_prob; - } else if (policy == MergePolicy::FUSION) { - return static_cast(mean(main_prob, sub_prob)); - } else { - std::cerr << "unknown merge policy in probabilityMerger function." << std::endl; - return main_prob; - } -} - -// shape merger -autoware_perception_msgs::msg::Shape shapeMerger( - const autoware_perception_msgs::msg::Shape & main_obj_shape, - const autoware_perception_msgs::msg::Shape & sub_obj_shape, const MergePolicy policy) -{ - autoware_perception_msgs::msg::Shape output_shape; - // copy main object at first - output_shape = main_obj_shape; - - if (main_obj_shape.type != sub_obj_shape.type) { - // if shape type is different, return main object - return output_shape; - } - - if (policy == MergePolicy::SKIP) { - return output_shape; - } else if (policy == MergePolicy::OVERWRITE) { - return sub_obj_shape; - } else if (policy == MergePolicy::FUSION) { - // write down fusion method for each shape type - if (main_obj_shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - // if shape type is bounding box, merge bounding box - output_shape.dimensions.x = mean(main_obj_shape.dimensions.x, sub_obj_shape.dimensions.x); - output_shape.dimensions.y = mean(main_obj_shape.dimensions.y, sub_obj_shape.dimensions.y); - output_shape.dimensions.z = mean(main_obj_shape.dimensions.z, sub_obj_shape.dimensions.z); - return output_shape; - } else if (main_obj_shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { - // if shape type is cylinder, merge cylinder - // (TODO) implement - return output_shape; - } else if (main_obj_shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { - // if shape type is polygon, merge polygon - // (TODO) - return output_shape; - } else { - // when type is unknown, print warning and do nothing - std::cerr << "unknown shape type in shapeMerger function." << std::endl; - return output_shape; - } - } else { - std::cerr << "unknown merge policy in shapeMerger function." << std::endl; - return output_shape; - } -} - void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) { // do not update if motion direction is different diff --git a/perception/autoware_traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml b/perception/autoware_traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml index 9f035e7f1ae6c..39026c85c1cef 100644 --- a/perception/autoware_traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml +++ b/perception/autoware_traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml @@ -12,7 +12,7 @@ - + diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index 8e8a962ed1071..267fd9932e781 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -416,7 +416,6 @@ void FreespacePlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) void FreespacePlannerNode::updateData() { occupancy_grid_ = occupancy_grid_sub_.takeData(); - scenario_ = scenario_sub_.takeData(); { auto msgs = odom_sub_.takeData(); @@ -440,11 +439,6 @@ bool FreespacePlannerNode::isDataReady() is_ready = false; } - if (!scenario_) { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "Waiting for scenario."); - is_ready = false; - } - if (!odom_) { RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "Waiting for odometry."); is_ready = false; @@ -455,14 +449,15 @@ bool FreespacePlannerNode::isDataReady() void FreespacePlannerNode::onTimer() { - updateData(); - - if (!isDataReady()) { + scenario_ = scenario_sub_.takeData(); + if (!isActive(scenario_)) { + reset(); return; } - if (!isActive(scenario_)) { - reset(); + updateData(); + + if (!isDataReady()) { return; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 9e4d6d2f85f50..b527fa3e1dac0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -84,12 +84,31 @@ bool isReferencePath( std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose); PathWithLaneId cropForwardPoints( const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length); + +/** + * @brief extend target_path by extend_length + * @param target_path original target path to extend + * @param reference_path reference path to extend + * @param extend_length length to extend + * @param remove_connected_zero_velocity flag to remove zero velocity if the last point of + * target_path has zero velocity + * @return extended path + */ PathWithLaneId extendPath( - const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, - const double extend_length); + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const double extend_length, const bool remove_connected_zero_velocity); +/** + * @brief extend target_path to extend_pose + * @param target_path original target path to extend + * @param reference_path reference path to extend + * @param extend_pose pose to extend + * @param remove_connected_zero_velocity flag to remove zero velocity if the last point of + * target_path has zero velocity + * @return extended path + */ PathWithLaneId extendPath( - const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, - const Pose & extend_pose); + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const Pose & extend_pose, const bool remove_connected_zero_velocity); std::vector createPathFootPrints( const PathWithLaneId & path, const double base_to_front, const double base_to_rear, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index ff248fbe02a1c..4d4d31446aa3f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -283,6 +283,10 @@ void GoalPlannerModule::onTimer() RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; } + // TODO(someone): The generated path inherits the velocity of the path of the previous module. + // Therefore, if the velocity of the path of the previous module changes (e.g. stop points are + // inserted, deleted), the path should be regenerated. + return false; }); if (!need_update) { @@ -1206,8 +1210,6 @@ bool GoalPlannerModule::hasDecidedPath( const std::shared_ptr & safety_check_params, const std::shared_ptr goal_searcher) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - return checkDecidingPathStatus( planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, objects_filtering_params, safety_check_params, goal_searcher) @@ -1240,8 +1242,6 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( const std::shared_ptr & safety_check_params, const std::shared_ptr goal_searcher) const { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto & prev_status = thread_safe_data_.get_prev_data().deciding_path_status; const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; @@ -1632,8 +1632,12 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const const double s_end = s_current + common_parameters.forward_path_length; return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); }); + + // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it by + // setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is the + // role of the goal planner, and the intermediate zero velocity after extension is unnecessary. const auto extended_prev_path = goal_planner_utils::extendPath( - getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length); + getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length, true); // calculate search start offset pose from the closest goal candidate pose with // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp index 23ca76432859d..fd6b0cb639387 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -148,8 +148,12 @@ std::optional ShiftPullOver::generatePullOverPath( lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length > lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length; if (extend_previous_module_path) { // case1 + // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it + // by setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is + // the role of the goal planner, and the intermediate zero velocity after extension is + // unnecessary. return goal_planner_utils::extendPath( - prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose); + prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose, true); } else { // case2 return goal_planner_utils::cropPath(prev_module_path, shift_end_pose); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index f4b11f4f8e46d..6bceec531b653 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -389,7 +389,7 @@ PathWithLaneId cropForwardPoints( PathWithLaneId extendPath( const PathWithLaneId & target_path, const PathWithLaneId & reference_path, - const double extend_length) + const double extend_length, const bool remove_connected_zero_velocity) { const auto & target_terminal_pose = target_path.points.back().point.pose; @@ -409,6 +409,11 @@ PathWithLaneId extendPath( } auto extended_path = target_path; + auto & target_terminal_vel = extended_path.points.back().point.longitudinal_velocity_mps; + if (remove_connected_zero_velocity && target_terminal_vel < 0.01) { + target_terminal_vel = clipped_path.points.front().point.longitudinal_velocity_mps; + } + const auto start_point = std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) { const bool is_forward = @@ -427,7 +432,7 @@ PathWithLaneId extendPath( PathWithLaneId extendPath( const PathWithLaneId & target_path, const PathWithLaneId & reference_path, - const Pose & extend_pose) + const Pose & extend_pose, const bool remove_connected_zero_velocity) { const auto & target_terminal_pose = target_path.points.back().point.pose; const size_t target_path_terminal_idx = autoware::motion_utils::findNearestSegmentIndex( @@ -435,7 +440,7 @@ PathWithLaneId extendPath( const double extend_distance = autoware::motion_utils::calcSignedArcLength( reference_path.points, target_path_terminal_idx, extend_pose.position); - return extendPath(target_path, reference_path, extend_distance); + return extendPath(target_path, reference_path, extend_distance, remove_connected_zero_velocity); } std::vector createPathFootPrints( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index e3ef44f1081ed..24257e8855e33 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -109,7 +109,7 @@ class NormalLaneChange : public LaneChangeBase bool isStoppedAtRedTrafficLight() const override; - TurnSignalInfo get_current_turn_signal_info() override; + TurnSignalInfo get_current_turn_signal_info() const final; protected: lanelet::ConstLanelets getLaneChangeLanes( @@ -117,6 +117,8 @@ class NormalLaneChange : public LaneChangeBase int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; + TurnSignalInfo get_terminal_turn_signal_info() const final; + std::vector sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 6c7dc31e857e5..51d5dbe0f195c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -213,7 +213,7 @@ class LaneChangeBase LaneChangeModuleType getModuleType() const { return type_; } - TurnSignalDecider getTurnSignalDecider() { return planner_data_->turn_signal_decider; } + TurnSignalDecider getTurnSignalDecider() const { return planner_data_->turn_signal_decider; } Direction getDirection() const { @@ -229,7 +229,7 @@ class LaneChangeBase void resetStopPose() { lane_change_stop_pose_ = std::nullopt; } - virtual TurnSignalInfo get_current_turn_signal_info() = 0; + virtual TurnSignalInfo get_current_turn_signal_info() const = 0; protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; @@ -245,6 +245,31 @@ class LaneChangeBase virtual lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0; + virtual TurnSignalInfo get_terminal_turn_signal_info() const = 0; + + TurnSignalInfo get_turn_signal(const Pose & start, const Pose & end) const + { + TurnSignalInfo turn_signal; + switch (direction_) { + case Direction::LEFT: + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + break; + case Direction::RIGHT: + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + break; + default: + turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + break; + } + + turn_signal.desired_start_point = start; + turn_signal.desired_end_point = end; + turn_signal.required_start_point = turn_signal.desired_start_point; + turn_signal.required_end_point = turn_signal.desired_end_point; + + return turn_signal; + } + LaneChangeStatus status_{}; PathShifter path_shifter_{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 66c454f795ebb..819cccd9365b6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -73,7 +73,7 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr); * @return distance to last fit width position along the lane */ double calc_dist_to_last_fit_width( - const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const lanelet::ConstLanelets & lanelets, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1); /** diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index a7cf89193b37f..979004d439bdc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -258,11 +258,17 @@ bool LaneChangeInterface::canTransitFailureState() if (!module_type_->isValidPath()) { log_debug_throttled("Transit to failure state due not to find valid path"); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) { log_debug_throttled("Abort process has completed."); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index c6d05f90fe138..01bdbe77f1c99 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -181,81 +181,64 @@ bool NormalLaneChange::isStoppedAtRedTrafficLight() const status_.lane_change_path.info.length.sum()); } -TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() +TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() const { const auto original_turn_signal_info = prev_module_output_.turn_signal_info; - const auto & current_lanes = get_current_lanes(); - const auto is_valid = getLaneChangeStatus().is_valid_path; - const auto & lane_change_path = getLaneChangeStatus().lane_change_path; - const auto & lane_change_param = getLaneChangeParam(); - - if (getModuleType() != LaneChangeModuleType::NORMAL || current_lanes.empty() || !is_valid) { + if (getModuleType() != LaneChangeModuleType::NORMAL || get_current_lanes().empty()) { return original_turn_signal_info; } - // check direction - TurnSignalInfo current_turn_signal_info; - const auto & current_pose = getEgoPose(); - const auto direction = getDirection(); - if (direction == Direction::LEFT) { - current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else if (direction == Direction::RIGHT) { - current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + if (direction_ != Direction::LEFT && direction_ != Direction::RIGHT) { + return original_turn_signal_info; } const auto & path = prev_module_output_.path; - if (path.points.empty()) { - current_turn_signal_info.desired_start_point = current_pose; - current_turn_signal_info.required_start_point = current_pose; - current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; - current_turn_signal_info.required_end_point = lane_change_path.info.lane_changing_end; - return current_turn_signal_info; + const auto & original_command = original_turn_signal_info.turn_signal.command; + if ( + !path.points.empty() && original_command != TurnIndicatorsCommand::DISABLE && + original_command != TurnIndicatorsCommand::NO_COMMAND) { + return get_terminal_turn_signal_info(); } - const auto min_length_for_turn_signal_activation = - lane_change_param.min_length_for_turn_signal_activation; - const auto & route_handler = getRouteHandler(); - const auto & common_parameter = getCommonParam(); + return get_turn_signal(getEgoPose(), getLaneChangePath().info.lane_changing_end); +} + +TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const +{ + const auto & lane_change_param = getLaneChangeParam(); + const auto & common_param = getCommonParam(); + const auto & current_pose = getEgoPose(); + const auto & path = prev_module_output_.path; + + const auto original_turn_signal_info = prev_module_output_.turn_signal_info; + const auto shift_intervals = - route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); + getRouteHandler()->getLateralIntervalsToPreferredLane(get_current_lanes().back()); const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals); - const double nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold; - const double base_to_front = common_parameter.base_link2front; - const double buffer = - next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front; + const double buffer = next_lane_change_buffer + + lane_change_param.min_length_for_turn_signal_activation + + common_param.base_link2front; const double path_length = autoware::motion_utils::calcArcLength(path.points); - const size_t current_nearest_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes); const auto start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path.points, 0, std::max(path_length - buffer, 0.0)); - if (dist_to_terminal - base_to_front < buffer && start_pose) { - // modify turn signal - current_turn_signal_info.desired_start_point = *start_pose; - current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; - current_turn_signal_info.required_start_point = current_turn_signal_info.desired_start_point; - current_turn_signal_info.required_end_point = current_turn_signal_info.desired_end_point; - - const auto & original_command = original_turn_signal_info.turn_signal.command; - if ( - original_command == TurnIndicatorsCommand::DISABLE || - original_command == TurnIndicatorsCommand::NO_COMMAND) { - return current_turn_signal_info; - } - // check the priority of turn signals - return getTurnSignalDecider().overwrite_turn_signal( - path, current_pose, current_nearest_seg_idx, original_turn_signal_info, - current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); - } + if (!start_pose) return original_turn_signal_info; + + const auto terminal_turn_signal_info = + get_turn_signal(*start_pose, getLaneChangePath().info.lane_changing_end); + + const double nearest_dist_threshold = common_param.ego_nearest_dist_threshold; + const double nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; + const size_t current_nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - // not in the vicinity of the end of the path. return original - return original_turn_signal_info; + return getTurnSignalDecider().overwrite_turn_signal( + path, current_pose, current_nearest_seg_idx, original_turn_signal_info, + terminal_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); } LaneChangePath NormalLaneChange::getLaneChangePath() const @@ -319,14 +302,15 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.path = abort_path_->path; insertStopPoint(get_current_lanes(), output.path); } else { - output.path = getLaneChangePath().path; + output.path = status_.lane_change_path.path; const auto found_extended_path = extendPath(); if (found_extended_path) { output.path = utils::combinePath(output.path, *found_extended_path); } output.reference_path = getReferencePath(); - output.turn_signal_info = updateOutputTurnSignal(); + output.turn_signal_info = + get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end); if (isStopState()) { const auto current_velocity = getEgoVelocity(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 9585449b8e0e2..ef1648103ddde 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -58,7 +58,7 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr) } double calc_dist_to_last_fit_width( - const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const lanelet::ConstLanelets & lanelets, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin) { if (lanelets.empty()) return 0.0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md index 658edce2ccb3c..b6282e39e3910 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md @@ -89,18 +89,18 @@ The Planner Manager's responsibilities include: ### Input -| Name | Required? | Type | Description | -| :---------------------------- | :-------: | :------------------------------------------------------ | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | for ego velocity. | -| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | for ego acceleration. | -| ~/input/objects | ○ | `autoware_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | -| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | -| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | traffic signals information from the perception module | -| ~/input/vector_map | ○ | `autoware_map_msgs::msg::LaneletMapBin` | vector map information. | -| ~/input/route | ○ | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal. | -| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | -| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | -| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/operation-mode.md) | +| Name | Required? | Type | Description | +| :---------------------------- | :-------: | :------------------------------------------------------ | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | for ego velocity. | +| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | for ego acceleration. | +| ~/input/objects | ○ | `autoware_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | +| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | +| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | traffic signals information from the perception module | +| ~/input/vector_map | ○ | `autoware_map_msgs::msg::LaneletMapBin` | vector map information. | +| ~/input/route | ○ | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal. | +| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | +| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | +| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/autoware_default_adapi/document/operation-mode.md) | - ○ Mandatory: Planning Module would not work if anyone of this is not present. - △ Optional: Some module would not work, but Planning Module can still be operated. diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 86bcf9df318bd..acf1dc1878087 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md index 99396eb0edf22..54b7cebb7c8ff 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md @@ -129,33 +129,24 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | -------------------- | ------ | ---------------------------------------------------------------- | | `time_threshold` | double | [s] consider objects that will reach an overlap within this time | -| Parameter /intervals | Type | Description | -| --------------------- | ------ | ------------------------------------------------------- | -| `ego_time_buffer` | double | [s] extend the ego time interval by this buffer | -| `objects_time_buffer` | double | [s] extend the time intervals of objects by this buffer | - | Parameter /ttc | Type | Description | | -------------- | ------ | ------------------------------------------------------------------------------------------------------ | | `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap | -| Parameter /objects | Type | Description | -| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | -| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | -| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | - -| Parameter /overlap | Type | Description | -| ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | -| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | -| `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | - -| Parameter /action | Type | Description | -| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | -| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | -| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | -| `slowdown.velocity` | double | [m] slow down velocity | -| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | +| Parameter /objects | Type | Description | +| ------------------------------- | ------ | --------------------------------------------------------------------- | +| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | +| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | + +| Parameter /overlap | Type | Description | +| ------------------ | ------ | --------------------------------------------------------------------- | +| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | + +| Parameter /action | Type | Description | +| ----------------------------- | ------ | --------------------------------------------------------------------- | +| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | +| `slowdown.velocity` | double | [m] slow down velocity | +| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | | Parameter /ego | Type | Description | | -------------------- | ------ | ---------------------------------------------------- | diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml index a0cf69ee71027..67db597d80752 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -1,32 +1,23 @@ /**: ros__parameters: out_of_lane: # module to stop or slowdown before overlapping another lane with other objects - mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc" + mode: ttc # mode used to consider a conflict with an object. "threshold", or "ttc" skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane + ignore_overlaps_over_lane_changeable_lanelets: true # if true, overlaps on lane changeable lanelets are ignored + max_arc_length: 100.0 # [m] maximum trajectory arc length that is checked for out_of_lane collisions threshold: time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time - intervals: # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego - ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer - objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer ttc: threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap objects: minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored - use_predicted_paths: true # if true, use the predicted paths to estimate future positions. - # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. - distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights ignore_behind_ego: true # if true, objects behind the ego vehicle are ignored - overlap: - minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered - extra_length: 0.0 # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) - action: # action to insert in the trajectory if an object causes a conflict at an overlap - skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed precision: 0.1 # [m] precision when inserting a stop pose in the trajectory longitudinal_distance_buffer: 1.5 # [m] safety distance buffer to keep in front of the ego vehicle lateral_distance_buffer: 1.0 # [m] safety distance buffer to keep on the side of the ego vehicle @@ -38,8 +29,8 @@ distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap ego: - min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego - extra_front_offset: 0.0 # [m] extra front distance - extra_rear_offset: 0.0 # [m] extra rear distance - extra_right_offset: 0.0 # [m] extra right distance - extra_left_offset: 0.0 # [m] extra left distance + # extra footprint offsets to calculate out of lane collisions + extra_front_offset: 0.0 # [m] extra footprint front distance + extra_rear_offset: 0.0 # [m] extra footprint rear distance + extra_right_offset: 0.0 # [m] extra footprint right distance + extra_left_offset: 0.0 # [m] extra footprint left distance diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml index 4908dbb4467be..3336278c761c6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -15,7 +15,6 @@ autoware_cmake - autoware_lanelet2_extension autoware_motion_utils autoware_motion_velocity_planner_common autoware_perception_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp index 8b03fa66eab55..b7cd55ce2e642 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -15,74 +15,117 @@ #include "calculate_slowdown_points.hpp" #include "footprint.hpp" +#include "types.hpp" #include #include +#include -#include +#include +#include +#include +#include + +#include #include +#include +#include +#include + namespace autoware::motion_velocity_planner::out_of_lane { -bool can_decelerate( - const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel) +std::optional calculate_last_avoiding_pose( + const std::vector & trajectory, + const universe_utils::Polygon2d & footprint, const lanelet::BasicPolygons2d & polygons_to_avoid, + const double min_arc_length, const double max_arc_length, const double precision) { - // TODO(Maxime): use the library function - const auto dist_ahead_of_ego = autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, ego_data.pose.position, point.pose.position); - const auto acc_to_target_vel = - (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); - return acc_to_target_vel < std::abs(ego_data.max_decel); + geometry_msgs::msg::Pose interpolated_pose{}; + bool is_avoiding_pose = false; + + auto from = min_arc_length; + auto to = max_arc_length; + while (to - from > precision) { + auto l = from + 0.5 * (to - from); + interpolated_pose = motion_utils::calcInterpolatedPose(trajectory, l); + const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose); + is_avoiding_pose = + std::all_of(polygons_to_avoid.begin(), polygons_to_avoid.end(), [&](const auto & polygon) { + return boost::geometry::disjoint(interpolated_footprint, polygon); + }); + if (is_avoiding_pose) { + from = l; + } else { + to = l; + } + } + if (is_avoiding_pose) { + return interpolated_pose; + } + return std::nullopt; } -std::optional calculate_last_in_lane_pose( - const EgoData & ego_data, const Slowdown & decision, - const autoware::universe_utils::Polygon2d & footprint, - const std::optional & prev_slowdown_point, const PlannerParam & params) +std::optional calculate_pose_ahead_of_collision( + const EgoData & ego_data, const OutOfLanePoint & point_to_avoid, + const universe_utils::Polygon2d & footprint, const double precision) { - const auto from_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, 0, ego_data.first_trajectory_idx); - const auto to_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, 0, decision.target_trajectory_idx); - TrajectoryPoint interpolated_point; - for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { - // TODO(Maxime): binary search - interpolated_point.pose = - autoware::motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); - const auto respect_decel_limit = - !params.skip_if_over_max_decel || prev_slowdown_point || - can_decelerate(ego_data, interpolated_point, decision.velocity); - const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.pose); - const auto is_overlap_lane = boost::geometry::overlaps( - interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon()); - const auto is_overlap_extra_lane = - prev_slowdown_point && - boost::geometry::overlaps( - interpolated_footprint, - prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon()); - if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane) - return interpolated_point; + const auto first_avoid_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0UL, point_to_avoid.trajectory_index); + for (auto l = first_avoid_arc_length - precision; l >= ego_data.min_stop_arc_length; + l -= precision) { + const auto interpolated_pose = + motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); + const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose); + if (boost::geometry::intersects(interpolated_footprint, point_to_avoid.outside_ring)) { + return interpolated_pose; + } } return std::nullopt; } -std::optional calculate_slowdown_point( - const EgoData & ego_data, const std::vector & decisions, - const std::optional & prev_slowdown_point, PlannerParam params) +std::optional calculate_slowdown_point( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params) { + const auto point_to_avoid_it = std::find_if( + out_of_lane_data.outside_points.cbegin(), out_of_lane_data.outside_points.cend(), + [&](const auto & p) { return p.to_avoid; }); + if (point_to_avoid_it == out_of_lane_data.outside_points.cend()) { + return std::nullopt; + } + const auto raw_footprint = make_base_footprint(params, true); // ignore extra footprint offsets + const auto base_footprint = make_base_footprint(params); params.extra_front_offset += params.lon_dist_buffer; params.extra_right_offset += params.lat_dist_buffer; params.extra_left_offset += params.lat_dist_buffer; - const auto base_footprint = make_base_footprint(params); + const auto expanded_footprint = make_base_footprint(params); // with added distance buffers + lanelet::BasicPolygons2d polygons_to_avoid; + for (const auto & ll : point_to_avoid_it->overlapped_lanelets) { + polygons_to_avoid.push_back(ll.polygon2d().basicPolygon()); + } + // points are ordered by trajectory index so the first one has the smallest index and arc length + const auto first_outside_idx = out_of_lane_data.outside_points.front().trajectory_index; + const auto first_outside_arc_length = + motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0UL, first_outside_idx); + std::optional slowdown_point; // search for the first slowdown decision for which a stop point can be inserted - for (const auto & decision : decisions) { - const auto last_in_lane_pose = - calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params); - if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; + // we first try to use the expanded footprint (distance buffers + extra footprint offsets) + for (const auto & footprint : {expanded_footprint, base_footprint, raw_footprint}) { + slowdown_point = calculate_last_avoiding_pose( + ego_data.trajectory_points, footprint, polygons_to_avoid, ego_data.min_stop_arc_length, + first_outside_arc_length, params.precision); + if (slowdown_point) { + break; + } } - return std::nullopt; + // fallback to simply stopping ahead of the collision to avoid (regardless of being out of lane or + // not) + if (!slowdown_point) { + slowdown_point = calculate_pose_ahead_of_collision( + ego_data, *point_to_avoid_it, expanded_footprint, params.precision); + } + return slowdown_point; } } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index 145f21c94c0d0..394334d434558 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -19,40 +19,39 @@ #include +#include + #include -#include namespace autoware::motion_velocity_planner::out_of_lane { - -/// @brief estimate whether ego can decelerate without breaking the deceleration limit -/// @details assume ego wants to reach the target point at the target velocity -/// @param [in] ego_data ego data -/// @param [in] point target point -/// @param [in] target_vel target_velocity -bool can_decelerate( - const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel); - -/// @brief calculate the last pose along the trajectory where ego does not overlap the lane to avoid +/// @brief calculate the last pose along the trajectory where ego does not go out of lane /// @param [in] ego_data ego data -/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed) /// @param [in] footprint the ego footprint -/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits -/// @param [in] params parameters +/// @param [in] min_arc_length minimum arc length for the search +/// @param [in] max_arc_length maximum arc length for the search +/// @param [in] precision [m] search precision /// @return the last pose that is not out of lane (if found) -std::optional calculate_last_in_lane_pose( - const EgoData & ego_data, const Slowdown & decision, - const autoware::universe_utils::Polygon2d & footprint, - const std::optional & prev_slowdown_point, const PlannerParam & params); +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const autoware::universe_utils::Polygon2d & footprint, + const double min_arc_length, const double max_arc_length, const double precision); + +/// @brief calculate the slowdown pose just ahead of a point to avoid +/// @param ego_data ego data (trajectory, velocity, etc) +/// @param point_to_avoid the point to avoid +/// @param footprint the ego footprint +/// @param params parameters +/// @return optional slowdown point to insert in the trajectory +std::optional calculate_pose_ahead_of_collision( + const EgoData & ego_data, const OutOfLanePoint & point_to_avoid, + const universe_utils::Polygon2d & footprint, const double precision); /// @brief calculate the slowdown point to insert in the trajectory /// @param ego_data ego data (trajectory, velocity, etc) -/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) -/// @param prev_slowdown_point previously calculated slowdown point +/// @param out_of_lane_data data about out of lane areas /// @param params parameters /// @return optional slowdown point to insert in the trajectory -std::optional calculate_slowdown_point( - const EgoData & ego_data, const std::vector & decisions, - const std::optional & prev_slowdown_point, PlannerParam params); +std::optional calculate_slowdown_point( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp index d9a85e266f790..cb29f9b3b42ea 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -16,12 +16,22 @@ #include "types.hpp" +#include +#include +#include #include +#include +#include #include +#include + +#include +#include +#include + #include -#include namespace autoware::motion_velocity_planner::out_of_lane::debug { @@ -36,209 +46,167 @@ visualization_msgs::msg::Marker get_base_marker() base_marker.id = 0; base_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; base_marker.action = visualization_msgs::msg::Marker::ADD; - base_marker.pose.position = autoware::universe_utils::createMarkerPosition(0.0, 0.0, 0); - base_marker.pose.orientation = autoware::universe_utils::createMarkerOrientation(0, 0, 0, 1.0); - base_marker.scale = autoware::universe_utils::createMarkerScale(0.1, 0.1, 0.1); - base_marker.color = autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + base_marker.pose.position = universe_utils::createMarkerPosition(0.0, 0.0, 0); + base_marker.pose.orientation = universe_utils::createMarkerOrientation(0, 0, 0, 1.0); + base_marker.scale = universe_utils::createMarkerScale(0.1, 0.1, 0.1); + base_marker.color = universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); return base_marker; } -void add_footprint_markers( +void add_polygons_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb) + const lanelet::BasicPolygons2d & polygons, const double z, const std::string & ns) { auto debug_marker = get_base_marker(); - debug_marker.ns = "footprints"; - for (const auto & f : footprints) { - debug_marker.points.clear(); - for (const auto & p : f) - debug_marker.points.push_back( - autoware::universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - debug_marker.points.clear(); + debug_marker.ns = ns; + debug_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + for (const auto & f : polygons) { + boost::geometry::for_each_segment(f, [&](const auto & s) { + const auto & [p1, p2] = s; + debug_marker.points.push_back(universe_utils::createMarkerPosition(p1.x(), p1.y(), z + 0.5)); + debug_marker.points.push_back(universe_utils::createMarkerPosition(p2.x(), p2.y(), z + 0.5)); + }); } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); + debug_marker_array.markers.push_back(debug_marker); } void add_current_overlap_marker( visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygon2d & current_footprint, - const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb) + const lanelet::BasicPolygon2d & current_footprint, const double z) { auto debug_marker = get_base_marker(); debug_marker.ns = "current_overlap"; debug_marker.points.clear(); for (const auto & p : current_footprint) - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition(p.x(), p.y(), z)); + debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z)); if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front()); - if (current_overlapped_lanelets.empty()) - debug_marker.color = autoware::universe_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); - else - debug_marker.color = autoware::universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + debug_marker.color = universe_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; - for (const auto & ll : current_overlapped_lanelets) { - debug_marker.points.clear(); - for (const auto & p : ll.polygon3d()) - debug_marker.points.push_back( - autoware::universe_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.5)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); -} - -void add_lanelet_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::ConstLanelets & lanelets, const std::string & ns, - const std_msgs::msg::ColorRGBA & color, const size_t prev_nb) -{ - auto debug_marker = get_base_marker(); - debug_marker.ns = ns; - debug_marker.color = color; - for (const auto & ll : lanelets) { - debug_marker.points.clear(); - - // add a small z offset to draw above the lanelet map - for (const auto & p : ll.polygon3d()) - debug_marker.points.push_back( - autoware::universe_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.1)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - debug_marker.action = debug_marker.DELETE; - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); } -void add_range_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const std::vector & trajectory_points, - const size_t first_ego_idx, const double z, const size_t prev_nb) +void add_ttc_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const EgoData & ego_data, + const OutOfLaneData & out_of_lane_data, const size_t prev_nb) { auto debug_marker = get_base_marker(); - debug_marker.ns = "ranges"; - debug_marker.color = autoware::universe_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); - for (const auto & range : ranges) { - debug_marker.points.clear(); - debug_marker.points.push_back( - trajectory_points[first_ego_idx + range.entering_trajectory_idx].pose.position); - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - range.entering_point.x(), range.entering_point.y(), z)); - for (const auto & overlap : range.debug.overlaps) { - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); + debug_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + debug_marker.scale.z = 0.5; + debug_marker.ns = "ttcs"; + for (const auto & p : out_of_lane_data.outside_points) { + if (p.to_avoid) { + debug_marker.color.r = 1.0; + debug_marker.color.g = 0.0; + } else { + debug_marker.color.r = 0.0; + debug_marker.color.g = 1.0; + } + if (p.ttc) { + debug_marker.pose = ego_data.trajectory_points[p.trajectory_index].pose; + debug_marker.text = std::to_string(*p.ttc); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; } - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - range.exiting_point.x(), range.exiting_point.y(), z)); - debug_marker.points.push_back( - trajectory_points[first_ego_idx + range.exiting_trajectory_idx].pose.position); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; } - debug_marker.action = debug_marker.DELETE; - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker.action = visualization_msgs::msg::Marker::DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) { debug_marker_array.markers.push_back(debug_marker); + } } - -void add_decision_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, +size_t add_stop_line_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const StopLinesRtree & rtree, const double z, const size_t prev_nb) { auto debug_marker = get_base_marker(); - debug_marker.action = debug_marker.ADD; - debug_marker.id = 0; - debug_marker.ns = "decisions"; - debug_marker.color = autoware::universe_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); - debug_marker.points.clear(); - for (const auto & range : ranges) { - debug_marker.type = debug_marker.LINE_STRIP; - if (range.debug.decision) { - debug_marker.points.push_back(autoware::universe_utils::createMarkerPosition( - range.entering_point.x(), range.entering_point.y(), z)); - debug_marker.points.push_back( - range.debug.object->kinematics.initial_pose_with_covariance.pose.position); + debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + debug_marker.ns = "stop_lines"; + const auto & add_lanelets_markers = [&](const auto & lanelets) { + for (const auto & ll : lanelets) { + debug_marker.points.clear(); + for (const auto & p : ll.polygon2d().basicPolygon()) { + debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); + } + debug_marker.points.push_back(debug_marker.points.front()); + debug_marker_array.markers.push_back(debug_marker); + ++debug_marker.id; } - debug_marker_array.markers.push_back(debug_marker); + }; + for (const auto & [_, stop_line] : rtree) { debug_marker.points.clear(); - debug_marker.id++; - - debug_marker.type = debug_marker.TEXT_VIEW_FACING; - debug_marker.pose.position.x = range.entering_point.x(); - debug_marker.pose.position.y = range.entering_point.y(); - debug_marker.pose.position.z = z; - std::stringstream ss; - ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time - << "\n"; - if (range.debug.object) { - debug_marker.pose.position.x += - range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x; - debug_marker.pose.position.y += - range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y; - debug_marker.pose.position.x /= 2; - debug_marker.pose.position.y /= 2; - ss << "Obj: " << range.debug.times.object.enter_time << " - " - << range.debug.times.object.exit_time << "\n"; + debug_marker.color.r = 1.0; + for (const auto & p : stop_line.stop_line) { + debug_marker.points.push_back(universe_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); } - debug_marker.scale.z = 1.0; - debug_marker.text = ss.str(); debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; + ++debug_marker.id; + debug_marker.color.r = 0.0; + add_lanelets_markers(stop_line.lanelets); } - debug_marker.action = debug_marker.DELETE; + const auto max_id = debug_marker.id; + debug_marker.action = visualization_msgs::msg::Marker::DELETE; for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) { debug_marker_array.markers.push_back(debug_marker); } + return max_id; } } // namespace -visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data) + +visualization_msgs::msg::MarkerArray create_debug_marker_array( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data) { - constexpr auto z = 0.0; + const auto z = ego_data.pose.position.z; visualization_msgs::msg::MarkerArray debug_marker_array; - debug::add_footprint_markers( - debug_marker_array, debug_data.footprints, z, debug_data.prev_footprints); - debug::add_current_overlap_marker( - debug_marker_array, debug_data.current_footprint, debug_data.current_overlapped_lanelets, z, - debug_data.prev_current_overlapped_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data.trajectory_lanelets, "trajectory_lanelets", - autoware::universe_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), - debug_data.prev_trajectory_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data.ignored_lanelets, "ignored_lanelets", - autoware::universe_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), - debug_data.prev_ignored_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data.other_lanelets, "other_lanelets", - autoware::universe_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), - debug_data.prev_other_lanelets); - debug::add_range_markers( - debug_marker_array, debug_data.ranges, debug_data.trajectory_points, - debug_data.first_trajectory_idx, z, debug_data.prev_ranges); - debug::add_decision_markers(debug_marker_array, debug_data.ranges, z, debug_data.prev_ranges); + // add_polygons_markers(debug_marker_array, ego_data.trajectory_footprints, z, "footprints"); + + lanelet::BasicPolygons2d drivable_lane_polygons; + for (const auto & poly : ego_data.drivable_lane_polygons) { + drivable_lane_polygons.push_back(poly.outer); + } + add_polygons_markers(debug_marker_array, drivable_lane_polygons, z, "ego_lane"); + + lanelet::BasicPolygons2d out_of_lane_areas; + for (const auto & p : out_of_lane_data.outside_points) { + out_of_lane_areas.push_back(p.outside_ring); + } + add_polygons_markers(debug_marker_array, out_of_lane_areas, z, "out_of_lane_areas"); + + lanelet::BasicPolygons2d object_polygons; + for (const auto & o : objects.objects) { + for (const auto & path : o.kinematics.predicted_paths) { + for (const auto & pose : path.path) { + // limit the draw distance to improve performance + if (universe_utils::calcDistance2d(pose, ego_data.pose) < 50.0) { + const auto poly = universe_utils::toPolygon2d(pose, o.shape).outer(); + lanelet::BasicPolygon2d ll_poly(poly.begin(), poly.end()); + object_polygons.push_back(ll_poly); + } + } + } + } + add_polygons_markers(debug_marker_array, object_polygons, z, "objects"); + + add_current_overlap_marker(debug_marker_array, ego_data.current_footprint, z); + + add_ttc_markers(debug_marker_array, ego_data, out_of_lane_data, debug_data.prev_ttcs); + debug_data.prev_ttcs = out_of_lane_data.outside_points.size(); + + debug_data.prev_stop_line = add_stop_line_markers( + debug_marker_array, ego_data.stop_lines_rtree, z, debug_data.prev_stop_line); + return debug_marker_array; } -autoware::motion_utils::VirtualWalls create_virtual_walls( - const DebugData & debug_data, const PlannerParam & params) +motion_utils::VirtualWalls create_virtual_walls( + const geometry_msgs::msg::Pose & pose, const bool stop, const PlannerParam & params) { - autoware::motion_utils::VirtualWalls virtual_walls; - autoware::motion_utils::VirtualWall wall; + motion_utils::VirtualWalls virtual_walls; + motion_utils::VirtualWall wall; wall.text = "out_of_lane"; wall.longitudinal_offset = params.front_offset; - wall.style = autoware::motion_utils::VirtualWallType::slowdown; - for (const auto & slowdown : debug_data.slowdowns) { - wall.pose = slowdown.point.pose; - virtual_walls.push_back(wall); - } + wall.style = stop ? motion_utils::VirtualWallType::stop : motion_utils::VirtualWallType::slowdown; + wall.pose = pose; + virtual_walls.push_back(wall); return virtual_walls; } } // namespace autoware::motion_velocity_planner::out_of_lane::debug diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp index ea225442443c8..dc1f942655612 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp @@ -23,9 +23,11 @@ namespace autoware::motion_velocity_planner::out_of_lane::debug { -visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data); -autoware::motion_utils::VirtualWalls create_virtual_walls( - const DebugData & debug_data, const PlannerParam & params); +visualization_msgs::msg::MarkerArray create_debug_marker_array( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data); +motion_utils::VirtualWalls create_virtual_walls( + const geometry_msgs::msg::Pose & pose, const bool stop, const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane::debug #endif // DEBUG_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp deleted file mode 100644 index 2be4c89b02779..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp +++ /dev/null @@ -1,389 +0,0 @@ -// Copyright 2024 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 "decisions.hpp" - -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -namespace autoware::motion_velocity_planner::out_of_lane -{ -double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx) -{ - return autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory_points, ego_data.pose.position, ego_data.first_trajectory_idx + target_idx); -} - -double time_along_trajectory( - const EgoData & ego_data, const size_t target_idx, const double min_velocity) -{ - const auto dist = distance_along_trajectory(ego_data, target_idx); - const auto v = std::max( - std::max(ego_data.velocity, min_velocity), - ego_data.trajectory_points[ego_data.first_trajectory_idx + target_idx] - .longitudinal_velocity_mps * - 0.5); - return dist / v; -} - -bool object_is_incoming( - const lanelet::BasicPoint2d & object_position, - const std::shared_ptr route_handler, - const lanelet::ConstLanelet & lane) -{ - const auto lanelets = route_handler->getPrecedingLaneletSequence(lane, 50.0); - if (boost::geometry::within(object_position, lane.polygon2d().basicPolygon())) return true; - for (const auto & lls : lanelets) - for (const auto & ll : lls) - if (boost::geometry::within(object_position, ll.polygon2d().basicPolygon())) return true; - return false; -} - -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double dist_buffer, - const rclcpp::Logger & logger) -{ - // skip the dynamic object if it is not in a lane preceding the overlapped lane - // lane changes are intentionally not considered - const auto object_point = lanelet::BasicPoint2d( - object.kinematics.initial_pose_with_covariance.pose.position.x, - object.kinematics.initial_pose_with_covariance.pose.position.y); - if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; - - const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; - const auto max_lon_deviation = object.shape.dimensions.x / 2.0; - auto worst_enter_time = std::optional(); - auto worst_exit_time = std::optional(); - - for (const auto & predicted_path : object.kinematics.predicted_paths) { - const auto unique_path_points = - autoware::motion_utils::removeOverlapPoints(predicted_path.path); - if (unique_path_points.size() < 2) continue; - const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); - const auto enter_point = - geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto enter_segment_idx = - autoware::motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); - const auto enter_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( - unique_path_points, enter_segment_idx, enter_point); - const auto enter_lat_dist = std::abs(autoware::motion_utils::calcLateralOffset( - unique_path_points, enter_point, enter_segment_idx)); - const auto enter_segment_length = autoware::universe_utils::calcDistance2d( - unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); - const auto enter_offset_ratio = enter_offset / enter_segment_length; - const auto enter_time = - static_cast(enter_segment_idx) * time_step + enter_offset_ratio * time_step; - - const auto exit_point = - geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto exit_segment_idx = - autoware::motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); - const auto exit_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( - unique_path_points, exit_segment_idx, exit_point); - const auto exit_lat_dist = std::abs( - autoware::motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); - const auto exit_segment_length = autoware::universe_utils::calcDistance2d( - unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); - const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); - const auto exit_time = - static_cast(exit_segment_idx) * time_step + exit_offset_ratio * time_step; - - RCLCPP_DEBUG( - logger, "\t\t\tPredicted path (time step = %2.2fs): enter @ %2.2fs, exit @ %2.2fs", time_step, - enter_time, exit_time); - // predicted path is too far from the overlapping range to be relevant - const auto is_far_from_entering_point = enter_lat_dist > max_deviation; - const auto is_far_from_exiting_point = exit_lat_dist > max_deviation; - if (is_far_from_entering_point && is_far_from_exiting_point) { - RCLCPP_DEBUG( - logger, - " * far_from_enter (%d) = %2.2fm | far_from_exit (%d) = %2.2fm | max_dev = %2.2fm\n", - is_far_from_entering_point, enter_lat_dist, is_far_from_exiting_point, exit_lat_dist, - max_deviation); - continue; - } - const auto is_last_predicted_path_point = - (exit_segment_idx + 2 == unique_path_points.size() || - enter_segment_idx + 2 == unique_path_points.size()); - const auto does_not_reach_overlap = - is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation); - if (does_not_reach_overlap) { - RCLCPP_DEBUG( - logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n", - std::min(exit_offset, enter_offset), max_lon_deviation); - continue; - } - - const auto same_driving_direction_as_ego = enter_time < exit_time; - if (same_driving_direction_as_ego) { - worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; - worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; - } else { - worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; - worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; - } - } - if (worst_enter_time && worst_exit_time) { - RCLCPP_DEBUG( - logger, "\t\t\t * found enter/exit time [%2.2f, %2.2f]\n", *worst_enter_time, - *worst_exit_time); - return std::make_pair(*worst_enter_time, *worst_exit_time); - } - RCLCPP_DEBUG(logger, "\t\t\t * enter/exit time not found\n"); - return {}; -} - -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const DecisionInputs & inputs, const rclcpp::Logger & logger) -{ - const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; - const auto object_point = lanelet::BasicPoint2d(p.x, p.y); - const auto half_size = object.shape.dimensions.x / 2.0; - lanelet::ConstLanelets object_lanelets; - for (const auto & ll : inputs.lanelets) - if (boost::geometry::within(object_point, ll.polygon2d().basicPolygon())) - object_lanelets.push_back(ll); - - geometry_msgs::msg::Pose pose; - pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto range_enter_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; - pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto range_exit_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; - const auto range_size = std::abs(range_enter_length - range_exit_length); - auto worst_enter_dist = std::optional(); - auto worst_exit_dist = std::optional(); - for (const auto & lane : object_lanelets) { - const auto path = inputs.route_handler->getRoutingGraphPtr()->shortestPath(lane, range.lane); - RCLCPP_DEBUG( - logger, "\t\t\tPath ? %d [from %ld to %ld]\n", path.has_value(), lane.id(), range.lane.id()); - if (path) { - lanelet::ConstLanelets lls; - for (const auto & ll : *path) lls.push_back(ll); - pose.position.set__x(object_point.x()).set__y(object_point.y()); - const auto object_curr_length = lanelet::utils::getArcCoordinates(lls, pose).length; - pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto enter_dist = - lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; - pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto exit_dist = - lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; - RCLCPP_DEBUG( - logger, "\t\t\t%2.2f -> [%2.2f(%2.2f, %2.2f) - %2.2f(%2.2f, %2.2f)]\n", object_curr_length, - enter_dist, range.entering_point.x(), range.entering_point.y(), exit_dist, - range.exiting_point.x(), range.exiting_point.y()); - const auto already_entered_range = std::abs(enter_dist - exit_dist) > range_size * 2.0; - if (already_entered_range) continue; - // multiple paths to the overlap -> be conservative and use the "worst" case - // "worst" = min/max arc length depending on if the lane is running opposite to the ego - // trajectory - const auto is_opposite = enter_dist > exit_dist; - if (!worst_enter_dist) - worst_enter_dist = enter_dist; - else if (is_opposite) - worst_enter_dist = std::max(*worst_enter_dist, enter_dist); - else - worst_enter_dist = std::min(*worst_enter_dist, enter_dist); - if (!worst_exit_dist) - worst_exit_dist = exit_dist; - else if (is_opposite) - worst_exit_dist = std::max(*worst_exit_dist, exit_dist); - else - worst_exit_dist = std::min(*worst_exit_dist, exit_dist); - } - } - if (worst_enter_dist && worst_exit_dist) { - const auto v = object.kinematics.initial_twist_with_covariance.twist.linear.x; - return std::make_pair((*worst_enter_dist - half_size) / v, (*worst_exit_dist + half_size) / v); - } - return {}; -} - -bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) -{ - const auto enter_within_threshold = - range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; - const auto exit_within_threshold = - range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; - return enter_within_threshold || exit_within_threshold; -} - -bool intervals_condition( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - const auto opposite_way_condition = [&]() -> bool { - const auto ego_exits_before_object_enters = - range_times.ego.exit_time + params.intervals_ego_buffer < - range_times.object.enter_time + params.intervals_obj_buffer; - RCLCPP_DEBUG( - logger, - "\t\t\t[Intervals] (opposite way) ego exit %2.2fs < obj enter %2.2fs ? -> should not " - "enter = %d\n", - range_times.ego.exit_time + params.intervals_ego_buffer, - range_times.object.enter_time + params.intervals_obj_buffer, ego_exits_before_object_enters); - return ego_exits_before_object_enters; - }; - const auto same_way_condition = [&]() -> bool { - const auto object_enters_during_overlap = - range_times.ego.enter_time - params.intervals_ego_buffer < - range_times.object.enter_time + params.intervals_obj_buffer && - range_times.object.enter_time - params.intervals_obj_buffer - range_times.ego.exit_time < - range_times.ego.exit_time + params.intervals_ego_buffer; - const auto object_exits_during_overlap = - range_times.ego.enter_time - params.intervals_ego_buffer < - range_times.object.exit_time + params.intervals_obj_buffer && - range_times.object.exit_time - params.intervals_obj_buffer - range_times.ego.exit_time < - range_times.ego.exit_time + params.intervals_ego_buffer; - RCLCPP_DEBUG( - logger, - "\t\t\t[Intervals] obj enters during overlap ? %d OR obj exits during overlap %d ? -> should " - "not " - "enter = %d\n", - object_enters_during_overlap, object_exits_during_overlap, - object_enters_during_overlap || object_exits_during_overlap); - return object_enters_during_overlap || object_exits_during_overlap; - }; - - const auto object_is_going_same_way = - range_times.object.enter_time < range_times.object.exit_time; - return (object_is_going_same_way && same_way_condition()) || - (!object_is_going_same_way && opposite_way_condition()); -} - -bool ttc_condition( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - const auto ttc_at_enter = range_times.ego.enter_time - range_times.object.enter_time; - const auto ttc_at_exit = range_times.ego.exit_time - range_times.object.exit_time; - const auto collision_during_overlap = (ttc_at_enter < 0.0) != (ttc_at_exit < 0.0); - const auto ttc_is_bellow_threshold = - std::min(std::abs(ttc_at_enter), std::abs(ttc_at_exit)) <= params.ttc_threshold; - RCLCPP_DEBUG( - logger, "\t\t\t[TTC] (%2.2fs - %2.2fs) -> %d\n", ttc_at_enter, ttc_at_exit, - (collision_during_overlap || ttc_is_bellow_threshold)); - return collision_during_overlap || ttc_is_bellow_threshold; -} - -bool will_collide_on_range( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - RCLCPP_DEBUG( - logger, " enter at %2.2fs, exits at %2.2fs\n", range_times.object.enter_time, - range_times.object.exit_time); - return (params.mode == "threshold" && threshold_condition(range_times, params)) || - (params.mode == "intervals" && intervals_condition(range_times, params, logger)) || - (params.mode == "ttc" && ttc_condition(range_times, params, logger)); -} - -bool should_not_enter( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger) -{ - RangeTimes range_times{}; - range_times.ego.enter_time = - time_along_trajectory(inputs.ego_data, range.entering_trajectory_idx, params.ego_min_velocity); - range_times.ego.exit_time = - time_along_trajectory(inputs.ego_data, range.exiting_trajectory_idx, params.ego_min_velocity); - RCLCPP_DEBUG( - logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", - range.entering_trajectory_idx, range.exiting_trajectory_idx, range.lane.id(), - range_times.ego.enter_time, range_times.ego.exit_time); - for (const auto & object : inputs.objects.objects) { - RCLCPP_DEBUG( - logger, "\t\t[%s] going at %2.2fm/s", - autoware::universe_utils::toHexString(object.object_id).c_str(), - object.kinematics.initial_twist_with_covariance.twist.linear.x); - if (object.kinematics.initial_twist_with_covariance.twist.linear.x < params.objects_min_vel) { - RCLCPP_DEBUG(logger, " SKIP (velocity bellow threshold %2.2fm/s)\n", params.objects_min_vel); - continue; // skip objects with velocity bellow a threshold - } - // skip objects that are already on the interval - const auto enter_exit_time = - params.objects_use_predicted_paths - ? object_time_to_range( - object, range, inputs.route_handler, params.objects_dist_buffer, logger) - : object_time_to_range(object, range, inputs, logger); - if (!enter_exit_time) { - RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); - continue; // skip objects that are not driving towards the overlapping range - } - - range_times.object.enter_time = enter_exit_time->first; - range_times.object.exit_time = enter_exit_time->second; - if (will_collide_on_range(range_times, params, logger)) { - range.debug.times = range_times; - range.debug.object = object; - return true; - } - } - range.debug.times = range_times; - return false; -} - -void set_decision_velocity( - std::optional & decision, const double distance, const PlannerParam & params) -{ - if (distance < params.stop_dist_threshold) { - decision->velocity = 0.0; - } else if (distance < params.slow_dist_threshold) { - decision->velocity = params.slow_velocity; - } else { - decision.reset(); - } -} - -std::optional calculate_decision( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger) -{ - std::optional decision; - if (should_not_enter(range, inputs, params, logger)) { - decision.emplace(); - decision->target_trajectory_idx = inputs.ego_data.first_trajectory_idx + - range.entering_trajectory_idx; // add offset from curr pose - decision->lane_to_avoid = range.lane; - const auto ego_dist_to_range = - distance_along_trajectory(inputs.ego_data, range.entering_trajectory_idx); - set_decision_velocity(decision, ego_dist_to_range, params); - } - return decision; -} - -std::vector calculate_decisions( - const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger) -{ - std::vector decisions; - for (const auto & range : inputs.ranges) { - if (range.entering_trajectory_idx == 0UL) continue; // skip if we already entered the range - const auto optional_decision = calculate_decision(range, inputs, params, logger); - range.debug.decision = optional_decision; - if (optional_decision) decisions.push_back(*optional_decision); - } - return decisions; -} - -} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp deleted file mode 100644 index 455cee272f7be..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright 2024 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 DECISIONS_HPP_ -#define DECISIONS_HPP_ - -#include "types.hpp" - -#include -#include - -#include - -#include - -#include -#include -#include -#include - -namespace autoware::motion_velocity_planner::out_of_lane -{ -/// @brief calculate the distance along the ego trajectory between ego and some target trajectory -/// index -/// @param [in] ego_data data related to the ego vehicle -/// @param [in] target_idx target ego trajectory index -/// @return distance between ego and the target [m] -double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx); -/// @brief estimate the time when ego will reach some target trajectory index -/// @param [in] ego_data data related to the ego vehicle -/// @param [in] target_idx target ego trajectory index -/// @param [in] min_velocity minimum ego velocity used to estimate the time -/// @return time taken by ego to reach the target [s] -double time_along_trajectory(const EgoData & ego_data, const size_t target_idx); -/// @brief use an object's predicted paths to estimate the times it will reach the enter and exit -/// points of an overlapping range -/// @details times when the predicted paths of the object enters/exits the range are calculated -/// but may not exist (e.g,, predicted path ends before reaching the end of the range) -/// @param [in] object dynamic object -/// @param [in] range overlapping range -/// @param [in] route_handler route handler used to estimate the path of the dynamic object -/// @param [in] logger ros logger -/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in -/// the opposite direction, time at enter > time at exit -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double dist_buffer, - const rclcpp::Logger & logger); -/// @brief use the lanelet map to estimate the times when an object will reach the enter and exit -/// points of an overlapping range -/// @param [in] object dynamic object -/// @param [in] range overlapping range -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range -/// @param [in] logger ros logger -/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in -/// the opposite direction, time at enter > time at exit. -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const DecisionInputs & inputs, const rclcpp::Logger & logger); -/// @brief decide whether an object is coming in the range at the same time as ego -/// @details the condition depends on the mode (threshold, intervals, ttc) -/// @param [in] range_times times when ego and the object enter/exit the range -/// @param [in] params parameters -/// @param [in] logger ros logger -bool will_collide_on_range( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger); -/// @brief check whether we should avoid entering the given range -/// @param [in] range the range to check -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return true if we should avoid entering the range -bool should_not_enter( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger); -/// @brief set the velocity of a decision (or unset it) based on the distance away from the range -/// @param [out] decision decision to update (either set its velocity or unset the decision) -/// @param [in] distance distance between ego and the range corresponding to the decision -/// @param [in] params parameters -void set_decision_velocity( - std::optional & decision, const double distance, const PlannerParam & params); -/// @brief calculate the decision to slowdown or stop before an overlapping range -/// @param [in] range the range to check -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return an optional decision to slowdown or stop -std::optional calculate_decision( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger); -/// @brief calculate decisions to slowdown or stop before some overlapping ranges -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return the calculated decisions to slowdown or stop -std::vector calculate_decisions( - const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); -} // namespace autoware::motion_velocity_planner::out_of_lane - -#endif // DECISIONS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index a53129f372e3d..79bdefee4c4c7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -53,7 +54,7 @@ void cut_predicted_path_beyond_line( auto cut_idx = stop_line_idx; double arc_length = 0; while (cut_idx > 0 && arc_length < object_front_overhang) { - arc_length += autoware::universe_utils::calcDistance2d( + arc_length += universe_utils::calcDistance2d( predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); --cut_idx; } @@ -102,12 +103,11 @@ void cut_predicted_path_beyond_red_lights( } autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const std::shared_ptr planner_data, const EgoData & ego_data, - const PlannerParam & params) + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) { autoware_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = planner_data->predicted_objects.header; - for (const auto & object : planner_data->predicted_objects.objects) { + filtered_objects.header = planner_data.predicted_objects.header; + for (const auto & object : planner_data.predicted_objects.objects) { const auto is_pedestrian = std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; @@ -125,10 +125,10 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( auto filtered_object = object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; - const auto no_overlap_path = autoware::motion_utils::removeOverlapPoints(predicted_path.path); + const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); if (no_overlap_path.size() <= 1) return true; - const auto lat_offset_to_current_ego = std::abs( - autoware::motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); + const auto lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); const auto is_crossing_ego = lat_offset_to_current_ego <= object.shape.dimensions.y / 2.0 + std::max( @@ -136,23 +136,21 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( params.right_offset + params.extra_right_offset); return is_low_confidence || is_crossing_ego; }; - if (params.objects_use_predicted_paths) { - auto & predicted_paths = filtered_object.kinematics.predicted_paths; - const auto new_end = - std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); - predicted_paths.erase(new_end, predicted_paths.end()); - if (params.objects_cut_predicted_paths_beyond_red_lights) - for (auto & predicted_path : predicted_paths) - cut_predicted_path_beyond_red_lights( - predicted_path, ego_data, filtered_object.shape.dimensions.x); - predicted_paths.erase( - std::remove_if( - predicted_paths.begin(), predicted_paths.end(), - [](const auto & p) { return p.path.empty(); }), - predicted_paths.end()); - } + auto & predicted_paths = filtered_object.kinematics.predicted_paths; + const auto new_end = + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); + predicted_paths.erase(new_end, predicted_paths.end()); + if (params.objects_cut_predicted_paths_beyond_red_lights) + for (auto & predicted_path : predicted_paths) + cut_predicted_path_beyond_red_lights( + predicted_path, ego_data, filtered_object.shape.dimensions.x); + predicted_paths.erase( + std::remove_if( + predicted_paths.begin(), predicted_paths.end(), + [](const auto & p) { return p.path.empty(); }), + predicted_paths.end()); - if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) + if (!filtered_object.kinematics.predicted_paths.empty()) filtered_objects.objects.push_back(filtered_object); } return filtered_objects; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index baf07e4b06ea5..3b2ae11718810 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -19,7 +19,6 @@ #include -#include #include namespace autoware::motion_velocity_planner::out_of_lane @@ -52,8 +51,7 @@ void cut_predicted_path_beyond_red_lights( /// @param [in] params parameters /// @return filtered predicted objects autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const std::shared_ptr planner_data, const EgoData & ego_data, - const PlannerParam & params); + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp index f5d68c4fa9bba..564bf5de7ef2e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -21,15 +21,13 @@ #include #include -#include #include namespace autoware::motion_velocity_planner::out_of_lane { -autoware::universe_utils::Polygon2d make_base_footprint( - const PlannerParam & p, const bool ignore_offset) +universe_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool ignore_offset) { - autoware::universe_utils::Polygon2d base_footprint; + universe_utils::Polygon2d base_footprint; const auto front_offset = ignore_offset ? 0.0 : p.extra_front_offset; const auto rear_offset = ignore_offset ? 0.0 : p.extra_rear_offset; const auto right_offset = ignore_offset ? 0.0 : p.extra_right_offset; @@ -43,10 +41,10 @@ autoware::universe_utils::Polygon2d make_base_footprint( } lanelet::BasicPolygon2d project_to_pose( - const autoware::universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) + const universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) { const auto angle = tf2::getYaw(pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); @@ -59,22 +57,15 @@ std::vector calculate_trajectory_footprints( const auto base_footprint = make_base_footprint(params); std::vector trajectory_footprints; trajectory_footprints.reserve(ego_data.trajectory_points.size()); - double length = 0.0; - const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) + - params.front_offset + params.extra_front_offset; - for (auto i = ego_data.first_trajectory_idx; - i < ego_data.trajectory_points.size() && length < range; ++i) { + for (auto i = ego_data.first_trajectory_idx; i < ego_data.trajectory_points.size(); ++i) { const auto & trajectory_pose = ego_data.trajectory_points[i].pose; const auto angle = tf2::getYaw(trajectory_pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back( p.x() + trajectory_pose.position.x, p.y() + trajectory_pose.position.y); trajectory_footprints.push_back(footprint); - if (i + 1 < ego_data.trajectory_points.size()) - length += autoware::universe_utils::calcDistance2d( - trajectory_pose, ego_data.trajectory_points[i + 1].pose); } return trajectory_footprints; } @@ -84,7 +75,7 @@ lanelet::BasicPolygon2d calculate_current_ego_footprint( { const auto base_footprint = make_base_footprint(params, ignore_offset); const auto angle = tf2::getYaw(ego_data.pose.orientation); - const auto rotated_footprint = autoware::universe_utils::rotatePolygon(base_footprint, angle); + const auto rotated_footprint = universe_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp index ebe7ab0fa9d7f..88baeefba6f40 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp @@ -21,24 +21,21 @@ #include -namespace autoware::motion_velocity_planner -{ -namespace out_of_lane +namespace autoware::motion_velocity_planner::out_of_lane { /// @brief create the base footprint of ego /// @param [in] p parameters used to create the footprint /// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the /// footprint /// @return base ego footprint -autoware::universe_utils::Polygon2d make_base_footprint( +universe_utils::Polygon2d make_base_footprint( const PlannerParam & p, const bool ignore_offset = false); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project /// @param [in] pose projection pose /// @return footprint projected to the given pose lanelet::BasicPolygon2d project_to_pose( - const autoware::universe_utils::Polygon2d & base_footprint, - const geometry_msgs::msg::Pose & pose); + const universe_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); /// @brief calculate the trajectory footprints /// @details the resulting polygon follows the format used by the lanelet library: clockwise order /// and implicit closing edge @@ -54,7 +51,6 @@ std::vector calculate_trajectory_footprints( /// footprint lanelet::BasicPolygon2d calculate_current_ego_footprint( const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset = false); -} // namespace out_of_lane -} // namespace autoware::motion_velocity_planner +} // namespace autoware::motion_velocity_planner::out_of_lane #endif // FOOTPRINT_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp index 853f2bb3ec9d8..f520a564519f0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -14,36 +14,46 @@ #include "lanelets_selection.hpp" +#include "types.hpp" + #include #include +#include +#include #include -#include #include #include -#include namespace autoware::motion_velocity_planner::out_of_lane { +namespace +{ +bool is_road_lanelet(const lanelet::ConstLanelet & lanelet) +{ + return lanelet.hasAttribute(lanelet::AttributeName::Subtype) && + lanelet.attribute(lanelet::AttributeName::Subtype) == lanelet::AttributeValueString::Road; +} +} // namespace + lanelet::ConstLanelets consecutive_lanelets( - const std::shared_ptr route_handler, - const lanelet::ConstLanelet & lanelet) + const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet) { - lanelet::ConstLanelets consecutives = route_handler->getRoutingGraphPtr()->following(lanelet); - const auto previous = route_handler->getRoutingGraphPtr()->previous(lanelet); + lanelet::ConstLanelets consecutives = route_handler.getRoutingGraphPtr()->following(lanelet); + const auto previous = route_handler.getRoutingGraphPtr()->previous(lanelet); consecutives.insert(consecutives.end(), previous.begin(), previous.end()); return consecutives; } lanelet::ConstLanelets get_missing_lane_change_lanelets( const lanelet::ConstLanelets & trajectory_lanelets, - const std::shared_ptr route_handler) + const route_handler::RouteHandler & route_handler) { lanelet::ConstLanelets missing_lane_change_lanelets; - const auto & routing_graph = *route_handler->getRoutingGraphPtr(); + const auto & routing_graph = *route_handler.getRoutingGraphPtr(); lanelet::ConstLanelets adjacents; lanelet::ConstLanelets consecutives; for (const auto & ll : trajectory_lanelets) { @@ -67,9 +77,9 @@ lanelet::ConstLanelets get_missing_lane_change_lanelets( } lanelet::ConstLanelets calculate_trajectory_lanelets( - const EgoData & ego_data, const std::shared_ptr route_handler) + const EgoData & ego_data, const route_handler::RouteHandler & route_handler) { - const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); lanelet::ConstLanelets trajectory_lanelets; lanelet::BasicLineString2d trajectory_ls; for (const auto & p : ego_data.trajectory_points) @@ -77,7 +87,9 @@ lanelet::ConstLanelets calculate_trajectory_lanelets( const auto candidates = lanelet_map_ptr->laneletLayer.search(lanelet::geometry::boundingBox2d(trajectory_ls)); for (const auto & ll : candidates) { - if (!boost::geometry::disjoint(trajectory_ls, ll.polygon2d().basicPolygon())) { + if ( + is_road_lanelet(ll) && + !boost::geometry::disjoint(trajectory_ls, ll.polygon2d().basicPolygon())) { trajectory_lanelets.push_back(ll); } } @@ -89,43 +101,65 @@ lanelet::ConstLanelets calculate_trajectory_lanelets( } lanelet::ConstLanelets calculate_ignored_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, - const std::shared_ptr route_handler, - const PlannerParam & params) + const lanelet::ConstLanelets & trajectory_lanelets, + const route_handler::RouteHandler & route_handler) { lanelet::ConstLanelets ignored_lanelets; - // ignore lanelets directly behind ego - const auto behind = - autoware::universe_utils::calcOffsetPose(ego_data.pose, params.rear_offset, 0.0, 0.0); - const lanelet::BasicPoint2d behind_point(behind.position.x, behind.position.y); - const auto behind_lanelets = lanelet::geometry::findWithin2d( - route_handler->getLaneletMapPtr()->laneletLayer, behind_point, 0.0); - for (const auto & l : behind_lanelets) { - const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, l.second.id()); - if (!is_trajectory_lanelet) ignored_lanelets.push_back(l.second); + // ignore lanelets directly preceding a trajectory lanelet + for (const auto & trajectory_lanelet : trajectory_lanelets) { + for (const auto & ll : route_handler.getPreviousLanelets(trajectory_lanelet)) { + const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, ll.id()); + if (!is_trajectory_lanelet) ignored_lanelets.push_back(ll); + } } return ignored_lanelets; } -lanelet::ConstLanelets calculate_other_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, - const lanelet::ConstLanelets & ignored_lanelets, - const std::shared_ptr route_handler, - const PlannerParam & params) +void calculate_drivable_lane_polygons( + EgoData & ego_data, const route_handler::RouteHandler & route_handler) { - lanelet::ConstLanelets other_lanelets; - const lanelet::BasicPoint2d ego_point(ego_data.pose.position.x, ego_data.pose.position.y); - const auto lanelets_within_range = lanelet::geometry::findWithin2d( - route_handler->getLaneletMapPtr()->laneletLayer, ego_point, - std::max(params.slow_dist_threshold, params.stop_dist_threshold) + params.front_offset + - params.extra_front_offset); - for (const auto & ll : lanelets_within_range) { - if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") - continue; - const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, ll.second.id()); - const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); - if (!is_trajectory_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second); + const auto route_lanelets = calculate_trajectory_lanelets(ego_data, route_handler); + const auto ignored_lanelets = + out_of_lane::calculate_ignored_lanelets(route_lanelets, route_handler); + for (const auto & ll : route_lanelets) { + out_of_lane::Polygons tmp; + boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); + ego_data.drivable_lane_polygons = tmp; + } + for (const auto & ll : ignored_lanelets) { + out_of_lane::Polygons tmp; + boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); + ego_data.drivable_lane_polygons = tmp; + } +} + +void calculate_overlapped_lanelets( + OutOfLanePoint & out_of_lane_point, const route_handler::RouteHandler & route_handler) +{ + out_of_lane_point.overlapped_lanelets = lanelet::ConstLanelets(); + const auto candidates = route_handler.getLaneletMapPtr()->laneletLayer.search( + lanelet::geometry::boundingBox2d(out_of_lane_point.outside_ring)); + for (const auto & ll : candidates) { + if ( + is_road_lanelet(ll) && !contains_lanelet(out_of_lane_point.overlapped_lanelets, ll.id()) && + boost::geometry::within(out_of_lane_point.outside_ring, ll.polygon2d().basicPolygon())) { + out_of_lane_point.overlapped_lanelets.push_back(ll); + } + } +} + +void calculate_overlapped_lanelets( + OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler) +{ + for (auto it = out_of_lane_data.outside_points.begin(); + it != out_of_lane_data.outside_points.end();) { + calculate_overlapped_lanelets(*it, route_handler); + if (it->overlapped_lanelets.empty()) { + // do not keep out of lane points that do not overlap any lanelet + out_of_lane_data.outside_points.erase(it); + } else { + ++it; + } } - return other_lanelets; } } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp index 94023ae08ad99..a7729deb539b6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -44,6 +44,7 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane /// @return lanelets crossed by the ego vehicle lanelet::ConstLanelets calculate_trajectory_lanelets( const EgoData & ego_data, const std::shared_ptr route_handler); + /// @brief calculate lanelets that may not be crossed by the trajectory but may be overlapped during /// a lane change /// @param [in] trajectory_lanelets lanelets driven by the ego vehicle @@ -52,29 +53,27 @@ lanelet::ConstLanelets calculate_trajectory_lanelets( /// trajectory_lanelets) lanelet::ConstLanelets get_missing_lane_change_lanelets( const lanelet::ConstLanelets & trajectory_lanelets, - const std::shared_ptr route_handler); + const std::shared_ptr & route_handler); + /// @brief calculate lanelets that should be ignored -/// @param [in] ego_data data about the ego vehicle -/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle +/// @param [in] trajectory_lanelets lanelets followed by the ego vehicle /// @param [in] route_handler route handler -/// @param [in] params parameters /// @return lanelets to ignore lanelet::ConstLanelets calculate_ignored_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, - const std::shared_ptr route_handler, - const PlannerParam & params); -/// @brief calculate lanelets that should be checked by the module -/// @param [in] ego_data data about the ego vehicle -/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle -/// @param [in] ignored_lanelets lanelets to ignore -/// @param [in] route_handler route handler -/// @param [in] params parameters -/// @return lanelets to check for overlaps -lanelet::ConstLanelets calculate_other_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, - const lanelet::ConstLanelets & ignored_lanelets, - const std::shared_ptr route_handler, - const PlannerParam & params); + const lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr & route_handler); + +/// @brief calculate the polygons representing the ego lane and add it to the ego data +/// @param [inout] ego_data ego data +/// @param [in] route_handler route handler with map information +void calculate_drivable_lane_polygons( + EgoData & ego_data, const route_handler::RouteHandler & route_handler); + +/// @brief calculate the lanelets overlapped at each out of lane point +/// @param [out] out_of_lane_data data with the out of lane points +/// @param [in] route_handler route handler with the lanelet map +void calculate_overlapped_lanelets( + OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // LANELETS_SELECTION_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp new file mode 100644 index 0000000000000..c375bcc35c1ee --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp @@ -0,0 +1,166 @@ +// Copyright 2024 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 "out_of_lane_collisions.hpp" + +#include "types.hpp" + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +void update_collision_times( + OutOfLaneData & out_of_lane_data, const std::unordered_set & potential_collision_indexes, + const universe_utils::Polygon2d & object_footprint, const double time) +{ + for (const auto index : potential_collision_indexes) { + auto & out_of_lane_point = out_of_lane_data.outside_points[index]; + if (out_of_lane_point.collision_times.count(time) == 0UL) { + const auto has_collision = + !boost::geometry::disjoint(out_of_lane_point.outside_ring, object_footprint.outer()); + if (has_collision) { + out_of_lane_point.collision_times.insert(time); + } + } + } +} + +void calculate_object_path_time_collisions( + OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedPath & object_path, + const autoware_perception_msgs::msg::Shape & object_shape) +{ + const auto time_step = rclcpp::Duration(object_path.time_step).seconds(); + auto time = time_step; + for (const auto & object_pose : object_path.path) { + time += time_step; + const auto object_footprint = universe_utils::toPolygon2d(object_pose, object_shape); + std::vector query_results; + out_of_lane_data.outside_areas_rtree.query( + boost::geometry::index::intersects(object_footprint.outer()), + std::back_inserter(query_results)); + std::unordered_set potential_collision_indexes; + for (const auto & [_, index] : query_results) { + potential_collision_indexes.insert(index); + } + update_collision_times(out_of_lane_data, potential_collision_indexes, object_footprint, time); + } +} + +void calculate_objects_time_collisions( + OutOfLaneData & out_of_lane_data, + const std::vector & objects) +{ + for (const auto & object : objects) { + for (const auto & path : object.kinematics.predicted_paths) { + calculate_object_path_time_collisions(out_of_lane_data, path, object.shape); + } + } +} + +void calculate_min_max_arrival_times( + OutOfLanePoint & out_of_lane_point, + const std::vector & trajectory) +{ + auto min_time = std::numeric_limits::infinity(); + auto max_time = -std::numeric_limits::infinity(); + for (const auto & t : out_of_lane_point.collision_times) { + min_time = std::min(t, min_time); + max_time = std::max(t, max_time); + } + if (min_time <= max_time) { + out_of_lane_point.min_object_arrival_time = min_time; + out_of_lane_point.max_object_arrival_time = max_time; + const auto & ego_time = + rclcpp::Duration(trajectory[out_of_lane_point.trajectory_index].time_from_start).seconds(); + if (ego_time >= min_time && ego_time <= max_time) { + out_of_lane_point.ttc = 0.0; + } else { + out_of_lane_point.ttc = + std::min(std::abs(ego_time - min_time), std::abs(ego_time - max_time)); + } + } +}; + +void calculate_collisions_to_avoid( + OutOfLaneData & out_of_lane_data, + const std::vector & trajectory, + const PlannerParam & params) +{ + for (auto & out_of_lane_point : out_of_lane_data.outside_points) { + calculate_min_max_arrival_times(out_of_lane_point, trajectory); + } + for (auto & p : out_of_lane_data.outside_points) { + if (params.mode == "ttc") { + p.to_avoid = p.ttc && p.ttc <= params.ttc_threshold; + } else { + p.to_avoid = p.min_object_arrival_time && p.min_object_arrival_time <= params.time_threshold; + } + } +} + +OutOfLaneData calculate_outside_points(const EgoData & ego_data) +{ + OutOfLaneData out_of_lane_data; + out_of_lane::OutOfLanePoint p; + for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { + p.trajectory_index = i + ego_data.first_trajectory_idx; + const auto & footprint = ego_data.trajectory_footprints[i]; + out_of_lane::Polygons out_of_lane_polygons; + boost::geometry::difference(footprint, ego_data.drivable_lane_polygons, out_of_lane_polygons); + for (const auto & area : out_of_lane_polygons) { + if (!area.outer.empty()) { + p.outside_ring = area.outer; + out_of_lane_data.outside_points.push_back(p); + } + } + } + return out_of_lane_data; +} + +OutOfLaneData calculate_out_of_lane_areas(const EgoData & ego_data) +{ + auto out_of_lane_data = calculate_outside_points(ego_data); + std::vector rtree_nodes; + for (auto i = 0UL; i < out_of_lane_data.outside_points.size(); ++i) { + OutAreaNode n; + n.first = boost::geometry::return_envelope( + out_of_lane_data.outside_points[i].outside_ring); + n.second = i; + rtree_nodes.push_back(n); + } + out_of_lane_data.outside_areas_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; + return out_of_lane_data; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp new file mode 100644 index 0000000000000..b9048cc358a07 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp @@ -0,0 +1,55 @@ +// Copyright 2024 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 OUT_OF_LANE_COLLISIONS_HPP_ +#define OUT_OF_LANE_COLLISIONS_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +/// @brief calculate the times and points where ego collides with an object's path outside of its +/// lane +void calculate_object_path_time_collisions( + OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedPath & object_path, + const autoware_perception_msgs::msg::Shape & object_shape); + +/// @brief calculate the times and points where ego collides with an object outside of its lane +void calculate_objects_time_collisions( + OutOfLaneData & out_of_lane_data, + const std::vector & objects); + +/// @brief calculate the collisions to avoid +/// @details either uses the time to collision or just the time when the object will arrive at the +/// point +void calculate_collisions_to_avoid( + OutOfLaneData & out_of_lane_data, + const std::vector & trajectory, + const PlannerParam & params); + +/// @brief calculate the areas where ego will drive outside of its lane +/// @details the OutOfLaneData points and rtree are filled +OutOfLaneData calculate_out_of_lane_areas(const EgoData & ego_data); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // OUT_OF_LANE_COLLISIONS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index a6b34a1566e19..ca7ed3b9dc7bd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -16,11 +16,10 @@ #include "calculate_slowdown_points.hpp" #include "debug.hpp" -#include "decisions.hpp" #include "filter_predicted_objects.hpp" #include "footprint.hpp" #include "lanelets_selection.hpp" -#include "overlapping_range.hpp" +#include "out_of_lane_collisions.hpp" #include "types.hpp" #include @@ -34,11 +33,13 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -56,65 +57,54 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) logger_ = node.get_logger(); clock_ = node.get_clock(); init_parameters(node); - velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); - processing_diag_publisher_ = std::make_shared( + processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); processing_time_publisher_ = node.create_publisher( "~/debug/" + ns_ + "/processing_time_ms", 1); } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { - using autoware::universe_utils::getOrDeclareParameter; + using universe_utils::getOrDeclareParameter; auto & pp = params_; pp.mode = getOrDeclareParameter(node, ns_ + ".mode"); pp.skip_if_already_overlapping = getOrDeclareParameter(node, ns_ + ".skip_if_already_overlapping"); + pp.ignore_lane_changeable_lanelets = + getOrDeclareParameter(node, ns_ + ".ignore_overlaps_over_lane_changeable_lanelets"); + pp.max_arc_length = getOrDeclareParameter(node, ns_ + ".max_arc_length"); pp.time_threshold = getOrDeclareParameter(node, ns_ + ".threshold.time_threshold"); - pp.intervals_ego_buffer = getOrDeclareParameter(node, ns_ + ".intervals.ego_time_buffer"); - pp.intervals_obj_buffer = - getOrDeclareParameter(node, ns_ + ".intervals.objects_time_buffer"); pp.ttc_threshold = getOrDeclareParameter(node, ns_ + ".ttc.threshold"); pp.objects_min_vel = getOrDeclareParameter(node, ns_ + ".objects.minimum_velocity"); - pp.objects_use_predicted_paths = - getOrDeclareParameter(node, ns_ + ".objects.use_predicted_paths"); pp.objects_min_confidence = getOrDeclareParameter(node, ns_ + ".objects.predicted_path_min_confidence"); - pp.objects_dist_buffer = getOrDeclareParameter(node, ns_ + ".objects.distance_buffer"); pp.objects_cut_predicted_paths_beyond_red_lights = getOrDeclareParameter(node, ns_ + ".objects.cut_predicted_paths_beyond_red_lights"); pp.objects_ignore_behind_ego = getOrDeclareParameter(node, ns_ + ".objects.ignore_behind_ego"); - pp.overlap_min_dist = getOrDeclareParameter(node, ns_ + ".overlap.minimum_distance"); - pp.overlap_extra_length = getOrDeclareParameter(node, ns_ + ".overlap.extra_length"); - - pp.skip_if_over_max_decel = - getOrDeclareParameter(node, ns_ + ".action.skip_if_over_max_decel"); pp.precision = getOrDeclareParameter(node, ns_ + ".action.precision"); pp.min_decision_duration = getOrDeclareParameter(node, ns_ + ".action.min_duration"); pp.lon_dist_buffer = getOrDeclareParameter(node, ns_ + ".action.longitudinal_distance_buffer"); pp.lat_dist_buffer = getOrDeclareParameter(node, ns_ + ".action.lateral_distance_buffer"); pp.slow_velocity = getOrDeclareParameter(node, ns_ + ".action.slowdown.velocity"); - pp.slow_dist_threshold = - getOrDeclareParameter(node, ns_ + ".action.slowdown.distance_threshold"); pp.stop_dist_threshold = getOrDeclareParameter(node, ns_ + ".action.stop.distance_threshold"); - pp.ego_min_velocity = getOrDeclareParameter(node, ns_ + ".ego.min_assumed_velocity"); pp.extra_front_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_front_offset"); pp.extra_rear_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_rear_offset"); pp.extra_left_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_left_offset"); pp.extra_right_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_right_offset"); - const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); + const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.front_offset = vehicle_info.max_longitudinal_offset_m; pp.rear_offset = vehicle_info.min_longitudinal_offset_m; pp.left_offset = vehicle_info.max_lateral_offset_m; @@ -123,44 +113,82 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) void OutOfLaneModule::update_parameters(const std::vector & parameters) { - using autoware::universe_utils::updateParam; + using universe_utils::updateParam; auto & pp = params_; updateParam(parameters, ns_ + ".mode", pp.mode); updateParam(parameters, ns_ + ".skip_if_already_overlapping", pp.skip_if_already_overlapping); + updateParam(parameters, ns_ + ".max_arc_length", pp.max_arc_length); + updateParam( + parameters, ns_ + ".ignore_overlaps_over_lane_changeable_lanelets", + pp.ignore_lane_changeable_lanelets); updateParam(parameters, ns_ + ".threshold.time_threshold", pp.time_threshold); - updateParam(parameters, ns_ + ".intervals.ego_time_buffer", pp.intervals_ego_buffer); - updateParam(parameters, ns_ + ".intervals.objects_time_buffer", pp.intervals_obj_buffer); updateParam(parameters, ns_ + ".ttc.threshold", pp.ttc_threshold); updateParam(parameters, ns_ + ".objects.minimum_velocity", pp.objects_min_vel); - updateParam(parameters, ns_ + ".objects.use_predicted_paths", pp.objects_use_predicted_paths); updateParam( parameters, ns_ + ".objects.predicted_path_min_confidence", pp.objects_min_confidence); - updateParam(parameters, ns_ + ".objects.distance_buffer", pp.objects_dist_buffer); updateParam( parameters, ns_ + ".objects.cut_predicted_paths_beyond_red_lights", pp.objects_cut_predicted_paths_beyond_red_lights); updateParam(parameters, ns_ + ".objects.ignore_behind_ego", pp.objects_ignore_behind_ego); - updateParam(parameters, ns_ + ".overlap.minimum_distance", pp.overlap_min_dist); - updateParam(parameters, ns_ + ".overlap.extra_length", pp.overlap_extra_length); - updateParam(parameters, ns_ + ".action.skip_if_over_max_decel", pp.skip_if_over_max_decel); updateParam(parameters, ns_ + ".action.precision", pp.precision); updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration); updateParam(parameters, ns_ + ".action.longitudinal_distance_buffer", pp.lon_dist_buffer); updateParam(parameters, ns_ + ".action.lateral_distance_buffer", pp.lat_dist_buffer); updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity); - updateParam(parameters, ns_ + ".action.slowdown.distance_threshold", pp.slow_dist_threshold); updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold); - updateParam(parameters, ns_ + ".ego.min_assumed_velocity", pp.ego_min_velocity); updateParam(parameters, ns_ + ".ego.extra_front_offset", pp.extra_front_offset); updateParam(parameters, ns_ + ".ego.extra_rear_offset", pp.extra_rear_offset); updateParam(parameters, ns_ + ".ego.extra_left_offset", pp.extra_left_offset); updateParam(parameters, ns_ + ".ego.extra_right_offset", pp.extra_right_offset); } +void OutOfLaneModule::limit_trajectory_size( + out_of_lane::EgoData & ego_data, + const std::vector & ego_trajectory_points, + const double max_arc_length) +{ + ego_data.first_trajectory_idx = + motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + ego_data.longitudinal_offset_to_first_trajectory_index = + motion_utils::calcLongitudinalOffsetToSegment( + ego_trajectory_points, ego_data.first_trajectory_idx, ego_data.pose.position); + auto l = -ego_data.longitudinal_offset_to_first_trajectory_index; + ego_data.trajectory_points.push_back(ego_trajectory_points[ego_data.first_trajectory_idx]); + for (auto i = ego_data.first_trajectory_idx + 1; i < ego_trajectory_points.size(); ++i) { + l += universe_utils::calcDistance2d(ego_trajectory_points[i - 1], ego_trajectory_points[i]); + if (l >= max_arc_length) { + break; + } + ego_data.trajectory_points.push_back(ego_trajectory_points[i]); + } +} + +void OutOfLaneModule::calculate_min_stop_and_slowdown_distances( + out_of_lane::EgoData & ego_data, const PlannerData & planner_data, + std::optional & previous_slowdown_pose_, const double slow_velocity) +{ + ego_data.min_stop_distance = planner_data.calculate_min_deceleration_distance(0.0).value_or(0.0); + ego_data.min_slowdown_distance = + planner_data.calculate_min_deceleration_distance(slow_velocity).value_or(0.0); + if (previous_slowdown_pose_) { + // Ensure we do not remove the previous slowdown point due to the min distance limit + const auto previous_slowdown_pose_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.first_trajectory_idx, previous_slowdown_pose_->position); + ego_data.min_stop_distance = + std::min(previous_slowdown_pose_arc_length, ego_data.min_stop_distance); + ego_data.min_slowdown_distance = + std::min(previous_slowdown_pose_arc_length, ego_data.min_slowdown_distance); + } + ego_data.min_stop_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0UL, ego_data.first_trajectory_idx) + + ego_data.longitudinal_offset_to_first_trajectory_index + + ego_data.min_stop_distance; +} + void prepare_stop_lines_rtree( out_of_lane::EgoData & ego_data, const PlannerData & planner_data, const double search_distance) { @@ -202,163 +230,140 @@ VelocityPlanningResult OutOfLaneModule::plan( const std::shared_ptr planner_data) { VelocityPlanningResult result; - autoware::universe_utils::StopWatch stopwatch; + universe_utils::StopWatch stopwatch; stopwatch.tic(); + + stopwatch.tic("preprocessing"); out_of_lane::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; - ego_data.trajectory_points = ego_trajectory_points; - ego_data.first_trajectory_idx = - autoware::motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); - ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x; - ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); - stopwatch.tic("preprocessing"); - prepare_stop_lines_rtree(ego_data, *planner_data, 100.0); + limit_trajectory_size(ego_data, ego_trajectory_points, params_.max_arc_length); + calculate_min_stop_and_slowdown_distances( + ego_data, *planner_data, previous_slowdown_pose_, params_.slow_velocity); + prepare_stop_lines_rtree(ego_data, *planner_data, params_.max_arc_length); const auto preprocessing_us = stopwatch.toc("preprocessing"); stopwatch.tic("calculate_trajectory_footprints"); - const auto current_ego_footprint = + ego_data.current_footprint = out_of_lane::calculate_current_ego_footprint(ego_data, params_, true); - const auto trajectory_footprints = - out_of_lane::calculate_trajectory_footprints(ego_data, params_); + ego_data.trajectory_footprints = out_of_lane::calculate_trajectory_footprints(ego_data, params_); const auto calculate_trajectory_footprints_us = stopwatch.toc("calculate_trajectory_footprints"); - // Calculate lanelets to ignore and consider + stopwatch.tic("calculate_lanelets"); - const auto trajectory_lanelets = - out_of_lane::calculate_trajectory_lanelets(ego_data, planner_data->route_handler); - const auto ignored_lanelets = out_of_lane::calculate_ignored_lanelets( - ego_data, trajectory_lanelets, planner_data->route_handler, params_); - const auto other_lanelets = out_of_lane::calculate_other_lanelets( - ego_data, trajectory_lanelets, ignored_lanelets, planner_data->route_handler, params_); + out_of_lane::calculate_drivable_lane_polygons(ego_data, *planner_data->route_handler); const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); - debug_data_.reset_data(); - debug_data_.trajectory_points = ego_trajectory_points; - debug_data_.footprints = trajectory_footprints; - debug_data_.trajectory_lanelets = trajectory_lanelets; - debug_data_.ignored_lanelets = ignored_lanelets; - debug_data_.other_lanelets = other_lanelets; - debug_data_.first_trajectory_idx = ego_data.first_trajectory_idx; - - if (params_.skip_if_already_overlapping) { - debug_data_.current_footprint = current_ego_footprint; - const auto overlapped_lanelet_it = - std::find_if(other_lanelets.begin(), other_lanelets.end(), [&](const auto & ll) { - return boost::geometry::intersects(ll.polygon2d().basicPolygon(), current_ego_footprint); - }); - if (overlapped_lanelet_it != other_lanelets.end()) { - debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it); - RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module\n"); - return result; - } - } - // Calculate overlapping ranges - stopwatch.tic("calculate_overlapping_ranges"); - const auto ranges = out_of_lane::calculate_overlapping_ranges( - trajectory_footprints, trajectory_lanelets, other_lanelets, params_); - const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); - // Calculate stop and slowdown points - out_of_lane::DecisionInputs inputs; - inputs.ranges = ranges; - inputs.ego_data = ego_data; + stopwatch.tic("calculate_out_of_lane_areas"); + auto out_of_lane_data = calculate_out_of_lane_areas(ego_data); + out_of_lane::calculate_overlapped_lanelets(out_of_lane_data, *planner_data->route_handler); + const auto calculate_out_of_lane_areas_us = stopwatch.toc("calculate_out_of_lane_areas"); + stopwatch.tic("filter_predicted_objects"); - inputs.objects = out_of_lane::filter_predicted_objects(planner_data, ego_data, params_); + const auto objects = out_of_lane::filter_predicted_objects(*planner_data, ego_data, params_); const auto filter_predicted_objects_us = stopwatch.toc("filter_predicted_objects"); - inputs.route_handler = planner_data->route_handler; - inputs.lanelets = other_lanelets; - stopwatch.tic("calculate_decisions"); - const auto decisions = out_of_lane::calculate_decisions(inputs, params_, logger_); - const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); - stopwatch.tic("calc_slowdown_points"); + + stopwatch.tic("calculate_time_collisions"); + out_of_lane::calculate_objects_time_collisions(out_of_lane_data, objects.objects); + const auto calculate_time_collisions_us = stopwatch.toc("calculate_time_collisions"); + + stopwatch.tic("calculate_times"); + // calculate times + out_of_lane::calculate_collisions_to_avoid(out_of_lane_data, ego_data.trajectory_points, params_); + const auto calculate_times_us = stopwatch.toc("calculate_times"); + + if ( + params_.skip_if_already_overlapping && !ego_data.drivable_lane_polygons.empty() && + !lanelet::geometry::within(ego_data.current_footprint, ego_data.drivable_lane_polygons)) { + RCLCPP_WARN(logger_, "Ego is already out of lane, skipping the module\n"); + debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array( + ego_data, out_of_lane_data, objects, debug_data_)); + return result; + } + if ( // reset the previous inserted point if the timer expired - prev_inserted_point_ && - (clock_->now() - prev_inserted_point_time_).seconds() > params_.min_decision_duration) - prev_inserted_point_.reset(); - auto point_to_insert = - out_of_lane::calculate_slowdown_point(ego_data, decisions, prev_inserted_point_, params_); - const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); - stopwatch.tic("insert_slowdown_points"); - debug_data_.slowdowns.clear(); - if ( // reset the timer if there is no previous inserted point or if we avoid the same lane - point_to_insert && - (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() == - point_to_insert->slowdown.lane_to_avoid.id())) - prev_inserted_point_time_ = clock_->now(); - // reuse previous stop point if there is no new one or if its velocity is not higher than the new + previous_slowdown_pose_ && + (clock_->now() - previous_slowdown_time_).seconds() > params_.min_decision_duration) { + previous_slowdown_pose_.reset(); + } + + stopwatch.tic("calculate_slowdown_point"); + auto slowdown_pose = out_of_lane::calculate_slowdown_point(ego_data, out_of_lane_data, params_); + const auto calculate_slowdown_point_us = stopwatch.toc("calculate_slowdown_point"); + + if ( // reset the timer if there is no previous inserted point + slowdown_pose && (!previous_slowdown_pose_)) { + previous_slowdown_time_ = clock_->now(); + } + // reuse previous stop pose if there is no new one or if its velocity is not higher than the new // one and its arc length is lower - const auto should_use_prev_inserted_point = [&]() { - if ( - point_to_insert && prev_inserted_point_ && - prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) { - const auto arc_length = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, 0LU, point_to_insert->point.pose.position); - const auto prev_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); + const auto should_use_previous_pose = [&]() { + if (slowdown_pose && previous_slowdown_pose_) { + const auto arc_length = + motion_utils::calcSignedArcLength(ego_trajectory_points, 0LU, slowdown_pose->position); + const auto prev_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, previous_slowdown_pose_->position); return prev_arc_length < arc_length; } - return !point_to_insert && prev_inserted_point_; + return !slowdown_pose && previous_slowdown_pose_; }(); - if (should_use_prev_inserted_point) { + if (should_use_previous_pose) { // if the trajectory changed the prev point is no longer on the trajectory so we project it - const auto insert_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); - prev_inserted_point_->point.pose = - autoware::motion_utils::calcInterpolatedPose(ego_trajectory_points, insert_arc_length); - point_to_insert = prev_inserted_point_; + const auto new_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.first_trajectory_idx, previous_slowdown_pose_->position); + slowdown_pose = motion_utils::calcInterpolatedPose(ego_trajectory_points, new_arc_length); } - if (point_to_insert) { - prev_inserted_point_ = point_to_insert; - RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); - debug_data_.slowdowns = {*point_to_insert}; - if (point_to_insert->slowdown.velocity == 0.0) - result.stop_points.push_back(point_to_insert->point.pose.position); - else + if (slowdown_pose) { + const auto arc_length = + motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.first_trajectory_idx, slowdown_pose->position) - + ego_data.longitudinal_offset_to_first_trajectory_index; + const auto slowdown_velocity = + arc_length <= params_.stop_dist_threshold ? 0.0 : params_.slow_velocity; + previous_slowdown_pose_ = slowdown_pose; + if (slowdown_velocity == 0.0) { + result.stop_points.push_back(slowdown_pose->position); + } else { result.slowdown_intervals.emplace_back( - point_to_insert->point.pose.position, point_to_insert->point.pose.position, - point_to_insert->slowdown.velocity); - - const auto is_approaching = autoware::motion_utils::calcSignedArcLength( - ego_trajectory_points, ego_data.pose.position, - point_to_insert->point.pose.position) > 0.1 && - ego_data.velocity > 0.1; - const auto status = is_approaching ? autoware::motion_utils::VelocityFactor::APPROACHING - : autoware::motion_utils::VelocityFactor::STOPPED; + slowdown_pose->position, slowdown_pose->position, slowdown_velocity); + } + + const auto is_approaching = + motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.pose.position, slowdown_pose->position) > 0.1 && + planner_data->current_odometry.twist.twist.linear.x > 0.1; + const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING + : motion_utils::VelocityFactor::STOPPED; velocity_factor_interface_.set( - ego_trajectory_points, ego_data.pose, point_to_insert->point.pose, status, "out_of_lane"); + ego_trajectory_points, ego_data.pose, *slowdown_pose, status, "out_of_lane"); result.velocity_factor = velocity_factor_interface_.get(); - } else if (!decisions.empty()) { - RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)"); + virtual_wall_marker_creator.add_virtual_walls( + out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_)); + virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); + } else if (std::any_of( + out_of_lane_data.outside_points.begin(), out_of_lane_data.outside_points.end(), + [](const auto & p) { return p.to_avoid; })) { + RCLCPP_WARN( + logger_, "[out_of_lane] Could not insert slowdown point because of deceleration limits"); } - const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); - debug_data_.ranges = inputs.ranges; + stopwatch.tic("gen_debug"); + const auto markers = + out_of_lane::debug::create_debug_marker_array(ego_data, out_of_lane_data, objects, debug_data_); + const auto markers_us = stopwatch.toc("gen_debug"); + stopwatch.tic("pub"); + debug_publisher_->publish(markers); + const auto pub_markers_us = stopwatch.toc("pub"); const auto total_time_us = stopwatch.toc(); - RCLCPP_DEBUG( - logger_, - "Total time = %2.2fus\n" - "\tpreprocessing = %2.0fus\n" - "\tcalculate_lanelets = %2.0fus\n" - "\tcalculate_trajectory_footprints = %2.0fus\n" - "\tcalculate_overlapping_ranges = %2.0fus\n" - "\tfilter_pred_objects = %2.0fus\n" - "\tcalculate_decisions = %2.0fus\n" - "\tcalc_slowdown_points = %2.0fus\n" - "\tinsert_slowdown_points = %2.0fus\n", - preprocessing_us, total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us, - calculate_overlapping_ranges_us, filter_predicted_objects_us, calculate_decisions_us, - calc_slowdown_points_us, insert_slowdown_points_us); - debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_)); - virtual_wall_marker_creator.add_virtual_walls( - out_of_lane::debug::create_virtual_walls(debug_data_, params_)); - virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); std::map processing_times; processing_times["preprocessing"] = preprocessing_us / 1000; processing_times["calculate_lanelets"] = calculate_lanelets_us / 1000; processing_times["calculate_trajectory_footprints"] = calculate_trajectory_footprints_us / 1000; - processing_times["calculate_overlapping_ranges"] = calculate_overlapping_ranges_us / 1000; + processing_times["calculate_out_of_lane_areas"] = calculate_out_of_lane_areas_us / 1000; processing_times["filter_pred_objects"] = filter_predicted_objects_us / 1000; - processing_times["calculate_decision"] = calculate_decisions_us / 1000; - processing_times["calc_slowdown_points"] = calc_slowdown_points_us / 1000; - processing_times["insert_slowdown_points"] = insert_slowdown_points_us / 1000; + processing_times["calculate_time_collisions"] = calculate_time_collisions_us / 1000; + processing_times["calculate_times"] = calculate_times_us / 1000; + processing_times["calculate_slowdown_point"] = calculate_slowdown_point_us / 1000; + processing_times["generate_markers"] = markers_us / 1000; + processing_times["publish_markers"] = pub_markers_us / 1000; processing_times["Total"] = total_time_us / 1000; processing_diag_publisher_->publish(processing_times); tier4_debug_msgs::msg::Float64Stamped processing_time_msg; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index 310a73c04d643..5225a6dd4abd9 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -46,13 +47,24 @@ class OutOfLaneModule : public PluginModuleInterface private: void init_parameters(rclcpp::Node & node); + /// @brief resize the trajectory to start from the segment closest to ego and to have at most the + /// given length + static void limit_trajectory_size( + out_of_lane::EgoData & ego_data, + const std::vector & ego_trajectory_points, + const double max_arc_length); + /// @brief calculate the minimum stop and slowdown distances of ego + static void calculate_min_stop_and_slowdown_distances( + out_of_lane::EgoData & ego_data, const PlannerData & planner_data, + std::optional & previous_slowdown_pose_, const double slow_velocity); + out_of_lane::PlannerParam params_; inline static const std::string ns_ = "out_of_lane"; std::string module_name_; - std::optional prev_inserted_point_{}; - rclcpp::Clock::SharedPtr clock_{}; - rclcpp::Time prev_inserted_point_time_{}; + rclcpp::Clock::SharedPtr clock_; + std::optional previous_slowdown_pose_; + rclcpp::Time previous_slowdown_time_; protected: // Debug diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp deleted file mode 100644 index 097e71a51bfaf..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// Copyright 2024 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 "overlapping_range.hpp" - -#include -#include - -#include - -#include -#include - -#include - -namespace autoware::motion_velocity_planner::out_of_lane -{ - -Overlap calculate_overlap( - const lanelet::BasicPolygon2d & trajectory_footprint, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet) -{ - Overlap overlap; - const auto & left_bound = lanelet.leftBound2d().basicLineString(); - const auto & right_bound = lanelet.rightBound2d().basicLineString(); - // TODO(Maxime): these intersects (for each trajectory footprint, for each lanelet) are too - // expensive - const auto overlap_left = boost::geometry::intersects(trajectory_footprint, left_bound); - const auto overlap_right = boost::geometry::intersects(trajectory_footprint, right_bound); - - lanelet::BasicPolygons2d overlapping_polygons; - if (overlap_left || overlap_right) - boost::geometry::intersection( - trajectory_footprint, lanelet.polygon2d().basicPolygon(), overlapping_polygons); - for (const auto & overlapping_polygon : overlapping_polygons) { - for (const auto & point : overlapping_polygon) { - if (overlap_left && overlap_right) - overlap.inside_distance = boost::geometry::distance(left_bound, right_bound); - else if (overlap_left) - overlap.inside_distance = - std::max(overlap.inside_distance, boost::geometry::distance(point, left_bound)); - else if (overlap_right) - overlap.inside_distance = - std::max(overlap.inside_distance, boost::geometry::distance(point, right_bound)); - geometry_msgs::msg::Pose p; - p.position.x = point.x(); - p.position.y = point.y(); - const auto length = lanelet::utils::getArcCoordinates(trajectory_lanelets, p).length; - if (length > overlap.max_arc_length) { - overlap.max_arc_length = length; - overlap.max_overlap_point = point; - } - if (length < overlap.min_arc_length) { - overlap.min_arc_length = length; - overlap.min_overlap_point = point; - } - } - } - return overlap; -} - -OverlapRanges calculate_overlapping_ranges( - const std::vector & trajectory_footprints, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, - const PlannerParam & params) -{ - OverlapRanges ranges; - OtherLane other_lane(lanelet); - std::vector overlaps; - for (auto i = 0UL; i < trajectory_footprints.size(); ++i) { - const auto overlap = calculate_overlap(trajectory_footprints[i], trajectory_lanelets, lanelet); - const auto has_overlap = overlap.inside_distance > params.overlap_min_dist; - if (has_overlap) { // open/update the range - overlaps.push_back(overlap); - if (!other_lane.range_is_open) { - other_lane.first_range_bound.index = i; - other_lane.first_range_bound.point = overlap.min_overlap_point; - other_lane.first_range_bound.arc_length = - overlap.min_arc_length - params.overlap_extra_length; - other_lane.first_range_bound.inside_distance = overlap.inside_distance; - other_lane.range_is_open = true; - } - other_lane.last_range_bound.index = i; - other_lane.last_range_bound.point = overlap.max_overlap_point; - other_lane.last_range_bound.arc_length = overlap.max_arc_length + params.overlap_extra_length; - other_lane.last_range_bound.inside_distance = overlap.inside_distance; - } else if (other_lane.range_is_open) { // !has_overlap: close the range if it is open - ranges.push_back(other_lane.close_range()); - ranges.back().debug.overlaps = overlaps; - overlaps.clear(); - } - } - // close the range if it is still open - if (other_lane.range_is_open) { - ranges.push_back(other_lane.close_range()); - ranges.back().debug.overlaps = overlaps; - overlaps.clear(); - } - return ranges; -} - -OverlapRanges calculate_overlapping_ranges( - const std::vector & trajectory_footprints, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, - const PlannerParam & params) -{ - OverlapRanges ranges; - for (auto & lanelet : lanelets) { - const auto lanelet_ranges = - calculate_overlapping_ranges(trajectory_footprints, trajectory_lanelets, lanelet, params); - ranges.insert(ranges.end(), lanelet_ranges.begin(), lanelet_ranges.end()); - } - return ranges; -} - -} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp deleted file mode 100644 index bc5872db16e03..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2024 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 OVERLAPPING_RANGE_HPP_ -#define OVERLAPPING_RANGE_HPP_ - -#include "types.hpp" - -#include - -#include - -#include - -namespace autoware::motion_velocity_planner::out_of_lane -{ - -/// @brief calculate the overlap between the given footprint and lanelet -/// @param [in] path_footprint footprint used to calculate the overlap -/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego -/// trajectory -/// @param [in] lanelet lanelet used to calculate the overlap -/// @return the found overlap between the footprint and the lanelet -Overlap calculate_overlap( - const lanelet::BasicPolygon2d & trajectory_footprint, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet); -/// @brief calculate the overlapping ranges between the trajectory footprints and a lanelet -/// @param [in] trajectory_footprints footprints used to calculate the overlaps -/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego -/// trajectory -/// @param [in] lanelet lanelet used to calculate the overlaps -/// @param [in] params parameters -/// @return the overlapping ranges found between the footprints and the lanelet -OverlapRanges calculate_overlapping_ranges( - const std::vector & trajectory_footprints, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, - const PlannerParam & params); -/// @brief calculate the overlapping ranges between the trajectory footprints and some lanelets -/// @param [in] trajectory_footprints footprints used to calculate the overlaps -/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego -/// trajectory -/// @param [in] lanelets lanelets used to calculate the overlaps -/// @param [in] params parameters -/// @return the overlapping ranges found between the footprints and the lanelets, sorted by -/// increasing arc length along the trajectory -OverlapRanges calculate_overlapping_ranges( - const std::vector & trajectory_footprints, - const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, - const PlannerParam & params); -} // namespace autoware::motion_velocity_planner::out_of_lane - -#endif // OVERLAPPING_RANGE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp index 96c54064e296c..8067d9e8b3410 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -28,52 +28,46 @@ #include #include +#include -#include -#include -#include #include +#include #include #include #include namespace autoware::motion_velocity_planner::out_of_lane { -using autoware_planning_msgs::msg::TrajectoryPoint; +using Polygons = boost::geometry::model::multi_polygon; -/// @brief parameters for the "out of lane" module +/// @brief parameters for the out_of_lane module struct PlannerParam { std::string mode; // mode used to consider a conflict with an object bool skip_if_already_overlapping; // if true, do not run the module when ego already overlaps // another lane + double max_arc_length; // [m] maximum arc length along the trajectory to check for collision + bool ignore_lane_changeable_lanelets; // if true, ignore overlaps on lane changeable lanelets - double time_threshold; // [s](mode="threshold") objects time threshold - double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range - double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range + double time_threshold; // [s](mode="threshold") objects time threshold double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object - double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range - bool objects_use_predicted_paths; // whether to use the objects' predicted paths bool objects_cut_predicted_paths_beyond_red_lights; // whether to cut predicted paths beyond red // lights' stop lines - double objects_min_vel; // [m/s] objects lower than this velocity will be ignored - double objects_min_confidence; // minimum confidence to consider a predicted path - double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in - // the other lane + double objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path bool objects_ignore_behind_ego; // if true, objects behind the ego vehicle are ignored - double overlap_extra_length; // [m] extra length to add around an overlap range - double overlap_min_dist; // [m] min distance inside another lane to consider an overlap - // action to insert in the trajectory if an object causes a conflict at an overlap - bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - double lon_dist_buffer; // [m] safety distance buffer to keep in front of the ego vehicle - double lat_dist_buffer; // [m] safety distance buffer to keep on the side of the ego vehicle - double slow_velocity; - double slow_dist_threshold; - double stop_dist_threshold; - double precision; // [m] precision when inserting a stop pose in the trajectory - double min_decision_duration; // [s] minimum duration needed a decision can be canceled + // action to insert in the trajectory if an object causes a collision at an overlap + double lon_dist_buffer; // [m] safety distance buffer to keep in front of the ego vehicle + double lat_dist_buffer; // [m] safety distance buffer to keep on the side of the ego vehicle + double slow_velocity; // [m/s] slowdown velocity + double stop_dist_threshold; // [m] if a collision is detected bellow this distance ahead of ego, + // try to insert a stop point + double precision; // [m] precision when inserting a stop pose in the trajectory + double + min_decision_duration; // [s] duration needed before a stop or slowdown point can be removed + // ego dimensions used to create its polygon footprint double front_offset; // [m] front offset (from vehicle info) double rear_offset; // [m] rear offset (from vehicle info) @@ -85,98 +79,6 @@ struct PlannerParam double extra_left_offset; // [m] extra left distance }; -struct EnterExitTimes -{ - double enter_time{}; - double exit_time{}; -}; -struct RangeTimes -{ - EnterExitTimes ego{}; - EnterExitTimes object{}; -}; - -/// @brief action taken by the "out of lane" module -struct Slowdown -{ - size_t target_trajectory_idx{}; // we want to slowdown before this trajectory index - double velocity{}; // desired slow down velocity - lanelet::ConstLanelet lane_to_avoid{}; // we want to slowdown before entering this lane -}; -/// @brief slowdown to insert in a trajectory -struct SlowdownToInsert -{ - Slowdown slowdown{}; - autoware_planning_msgs::msg::TrajectoryPoint point{}; -}; - -/// @brief bound of an overlap range (either the first, or last bound) -struct RangeBound -{ - size_t index{}; - lanelet::BasicPoint2d point{}; - double arc_length{}; - double inside_distance{}; -}; - -/// @brief representation of an overlap between the ego footprint and some other lane -struct Overlap -{ - double inside_distance = 0.0; ///!< distance inside the overlap - double min_arc_length = std::numeric_limits::infinity(); - double max_arc_length = 0.0; - lanelet::BasicPoint2d min_overlap_point{}; ///!< point with min arc length - lanelet::BasicPoint2d max_overlap_point{}; ///!< point with max arc length -}; - -/// @brief range along the trajectory where ego overlaps another lane -struct OverlapRange -{ - lanelet::ConstLanelet lane{}; - size_t entering_trajectory_idx{}; - size_t exiting_trajectory_idx{}; - lanelet::BasicPoint2d entering_point{}; // pose of the overlapping point closest along the lane - lanelet::BasicPoint2d exiting_point{}; // pose of the overlapping point furthest along the lane - double inside_distance{}; // [m] how much ego footprint enters the lane - mutable struct - { - std::vector overlaps{}; - std::optional decision{}; - RangeTimes times{}; - std::optional object{}; - } debug; -}; -using OverlapRanges = std::vector; -/// @brief representation of a lane and its current overlap range -struct OtherLane -{ - bool range_is_open = false; - RangeBound first_range_bound{}; - RangeBound last_range_bound{}; - lanelet::ConstLanelet lanelet{}; - lanelet::BasicPolygon2d polygon{}; - - explicit OtherLane(lanelet::ConstLanelet ll) : lanelet(std::move(ll)) - { - polygon = lanelet.polygon2d().basicPolygon(); - } - - [[nodiscard]] OverlapRange close_range() - { - OverlapRange range; - range.lane = lanelet; - range.entering_trajectory_idx = first_range_bound.index; - range.entering_point = first_range_bound.point; - range.exiting_trajectory_idx = last_range_bound.index; - range.exiting_point = last_range_bound.point; - range.inside_distance = - std::max(first_range_bound.inside_distance, last_range_bound.inside_distance); - range_is_open = false; - last_range_bound = {}; - return range; - } -}; - namespace bgi = boost::geometry::index; struct StopLine { @@ -185,68 +87,52 @@ struct StopLine }; using StopLineNode = std::pair; using StopLinesRtree = bgi::rtree>; -using OutAreaRtree = bgi::rtree, bgi::rstar<16>>; +using OutAreaNode = std::pair; +using OutAreaRtree = bgi::rtree>; /// @brief data related to the ego vehicle struct EgoData { - std::vector trajectory_points{}; + std::vector trajectory_points; + geometry_msgs::msg::Pose pose; size_t first_trajectory_idx{}; - double velocity{}; // [m/s] - double max_decel{}; // [m/s²] - geometry_msgs::msg::Pose pose{}; + double longitudinal_offset_to_first_trajectory_index{}; + double min_stop_distance{}; + double min_slowdown_distance{}; + double min_stop_arc_length{}; + + Polygons drivable_lane_polygons; + + lanelet::BasicPolygon2d current_footprint; + std::vector trajectory_footprints; + StopLinesRtree stop_lines_rtree; }; -/// @brief data needed to make decisions -struct DecisionInputs +struct OutOfLanePoint { - OverlapRanges ranges; - EgoData ego_data; - autoware_perception_msgs::msg::PredictedObjects objects; - std::shared_ptr route_handler; - lanelet::ConstLanelets lanelets; + size_t trajectory_index; + lanelet::BasicPolygon2d outside_ring; + std::set collision_times; + std::optional min_object_arrival_time; + std::optional max_object_arrival_time; + std::optional ttc; + lanelet::ConstLanelets overlapped_lanelets; + bool to_avoid = false; +}; +struct OutOfLaneData +{ + std::vector outside_points; + OutAreaRtree outside_areas_rtree; }; /// @brief debug data struct DebugData { - std::vector footprints; - std::vector slowdowns; - geometry_msgs::msg::Pose ego_pose; - OverlapRanges ranges; - lanelet::BasicPolygon2d current_footprint; - lanelet::ConstLanelets current_overlapped_lanelets; - lanelet::ConstLanelets trajectory_lanelets; - lanelet::ConstLanelets ignored_lanelets; - lanelet::ConstLanelets other_lanelets; - std::vector trajectory_points; - size_t first_trajectory_idx; - - size_t prev_footprints = 0; - size_t prev_slowdowns = 0; - size_t prev_ranges = 0; - size_t prev_current_overlapped_lanelets = 0; - size_t prev_ignored_lanelets = 0; - size_t prev_trajectory_lanelets = 0; - size_t prev_other_lanelets = 0; - void reset_data() - { - prev_footprints = footprints.size(); - footprints.clear(); - prev_slowdowns = slowdowns.size(); - slowdowns.clear(); - prev_ranges = ranges.size(); - ranges.clear(); - prev_current_overlapped_lanelets = current_overlapped_lanelets.size(); - current_overlapped_lanelets.clear(); - prev_ignored_lanelets = ignored_lanelets.size(); - ignored_lanelets.clear(); - prev_trajectory_lanelets = trajectory_lanelets.size(); - trajectory_lanelets.clear(); - prev_other_lanelets = other_lanelets.size(); - other_lanelets.clear(); - } + size_t prev_out_of_lane_areas = 0; + size_t prev_ttcs = 0; + size_t prev_objects = 0; + size_t prev_stop_line = 0; }; } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt index f1eb7ff047fdc..ffc06aa8cc2f8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt @@ -5,9 +5,20 @@ find_package(autoware_cmake REQUIRED) autoware_package() -# ament_auto_add_library(${PROJECT_NAME}_lib SHARED -# DIRECTORY src -# ) +ament_auto_add_library(${PROJECT_NAME}_lib SHARED + DIRECTORY src +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_collision_checker.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME}_lib + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() ament_auto_package(INSTALL_TO_SHARE include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp new file mode 100644 index 0000000000000..bf471c62af969 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp @@ -0,0 +1,69 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +namespace bgi = boost::geometry::index; +using RtreeNode = std::pair; +using Rtree = bgi::rtree>; + +/// @brief collision as a trajectory index and corresponding collision points +struct Collision +{ + size_t trajectory_index{}; + autoware::universe_utils::MultiPoint2d collision_points; +}; + +/// @brief collision checker for a trajectory as a sequence of 2D footprints +class CollisionChecker +{ + const autoware::universe_utils::MultiPolygon2d trajectory_footprints_; + std::shared_ptr rtree_; + +public: + /// @brief construct the collisions checker + /// @param trajectory_footprints footprints of the trajectory to be used for collision checking + /// @details internally, the rtree is built with the packing algorithm + explicit CollisionChecker(autoware::universe_utils::MultiPolygon2d trajectory_footprints); + + /// @brief efficiently calculate collisions with a geometry + /// @tparam Geometry boost geometry type + /// @param geometry geometry to check for collisions + /// @return collisions between the trajectory footprints and the input geometry + template + [[nodiscard]] std::vector get_collisions(const Geometry & geometry) const; + + /// @brief direct access to the rtree storing the polygon footprints + /// @return rtree of the polygon footprints + [[nodiscard]] std::shared_ptr get_rtree() const { return rtree_; } + + /// @brief get the size of the trajectory used by this collision checker + [[nodiscard]] size_t trajectory_size() const { return trajectory_footprints_.size(); } +}; +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp index 5dbb872d99ca9..a96587c4465d6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp @@ -15,7 +15,10 @@ #ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#include +#include #include +#include #include #include @@ -37,12 +40,9 @@ #include #include -#include -#include #include #include #include -#include namespace autoware::motion_velocity_planner { @@ -59,11 +59,11 @@ struct PlannerData } // msgs from callbacks that are used for data-ready - nav_msgs::msg::Odometry current_odometry{}; - geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration{}; - autoware_perception_msgs::msg::PredictedObjects predicted_objects{}; - pcl::PointCloud no_ground_pointcloud{}; - nav_msgs::msg::OccupancyGrid occupancy_grid{}; + nav_msgs::msg::Odometry current_odometry; + geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration; + autoware_perception_msgs::msg::PredictedObjects predicted_objects; + pcl::PointCloud no_ground_pointcloud; + nav_msgs::msg::OccupancyGrid occupancy_grid; std::shared_ptr route_handler; // nearest search @@ -79,7 +79,7 @@ struct PlannerData tier4_v2x_msgs::msg::VirtualTrafficLightStateArray virtual_traffic_light_states; // velocity smoother - std::shared_ptr velocity_smoother_{}; + std::shared_ptr velocity_smoother_; // parameters autoware::vehicle_info_utils::VehicleInfo vehicle_info_; @@ -88,7 +88,7 @@ struct PlannerData *@brief queries the traffic signal information of given Id. if keep_last_observation = true, *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation */ - std::optional get_traffic_signal( + [[nodiscard]] std::optional get_traffic_signal( const lanelet::Id id, const bool keep_last_observation = false) const { const auto & traffic_light_id_map = @@ -98,6 +98,15 @@ struct PlannerData } return std::make_optional(traffic_light_id_map.at(id)); } + + [[nodiscard]] std::optional calculate_min_deceleration_distance( + const double target_velocity) const + { + return motion_utils::calcDecelDistWithJerkAndAccConstraints( + current_odometry.twist.twist.linear.x, target_velocity, + current_acceleration.accel.accel.linear.x, velocity_smoother_->getMinDecel(), + std::abs(velocity_smoother_->getMinJerk()), velocity_smoother_->getMinJerk()); + } }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index 062e6a4cb2fc0..515a30348cbee 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -16,7 +16,6 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common - autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs autoware_planning_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp new file mode 100644 index 0000000000000..6e84b63a3c7fc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp @@ -0,0 +1,70 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/motion_velocity_planner_common/collision_checker.hpp" + +namespace autoware::motion_velocity_planner +{ +CollisionChecker::CollisionChecker(autoware::universe_utils::MultiPolygon2d trajectory_footprints) +: trajectory_footprints_(std::move(trajectory_footprints)) +{ + std::vector nodes; + nodes.reserve(trajectory_footprints_.size()); + for (auto i = 0UL; i < trajectory_footprints_.size(); ++i) { + nodes.emplace_back( + boost::geometry::return_envelope(trajectory_footprints_[i]), + i); + } + rtree_ = std::make_shared(nodes.begin(), nodes.end()); +} + +template +std::vector CollisionChecker::get_collisions(const Geometry & geometry) const +{ + std::vector collisions; + std::vector approximate_results; + autoware::universe_utils::MultiPoint2d intersections; + ; + rtree_->query(bgi::intersects(geometry), std::back_inserter(approximate_results)); + for (const auto & result : approximate_results) { + intersections.clear(); + boost::geometry::intersection(trajectory_footprints_[result.second], geometry, intersections); + if (!intersections.empty()) { + Collision c; + c.trajectory_index = result.second; + c.collision_points = intersections; + collisions.push_back(c); + } + } + return collisions; +} + +template std::vector CollisionChecker::get_collisions( + const autoware::universe_utils::Point2d & geometry) const; +template std::vector CollisionChecker::get_collisions( + const autoware::universe_utils::Line2d & geometry) const; +template std::vector +CollisionChecker::get_collisions( + const autoware::universe_utils::MultiPolygon2d & geometry) const; + +// @warn Multi geometries usually lead to very inefficient queries +template std::vector +CollisionChecker::get_collisions( + const autoware::universe_utils::MultiPoint2d & geometry) const; +template std::vector +CollisionChecker::get_collisions( + const autoware::universe_utils::MultiLineString2d & geometry) const; +template std::vector CollisionChecker::get_collisions< + autoware::universe_utils::Polygon2d>(const autoware::universe_utils::Polygon2d & geometry) const; +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp new file mode 100644 index 0000000000000..df813db336a9d --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp @@ -0,0 +1,176 @@ +// Copyright 2024 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 "autoware/motion_velocity_planner_common/collision_checker.hpp" + +#include + +#include + +#include +#include + +using autoware::motion_velocity_planner::CollisionChecker; +using autoware::universe_utils::Line2d; +using autoware::universe_utils::MultiLineString2d; +using autoware::universe_utils::MultiPoint2d; +using autoware::universe_utils::MultiPolygon2d; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; + +Point2d random_point() +{ + static std::random_device r; + static std::default_random_engine e1(r()); + static std::uniform_real_distribution uniform_dist(-100, 100); + return {uniform_dist(e1), uniform_dist(e1)}; +} + +Line2d random_line() +{ + const auto point = random_point(); + const auto point2 = Point2d{point.x() + 1, point.y() + 1}; + const auto point3 = Point2d{point2.x() - 1, point2.y() + 1}; + const auto point4 = Point2d{point3.x() + 1, point3.y() + 1}; + return {point, point2, point3, point4}; +} + +Polygon2d random_polygon() +{ + Polygon2d polygon; + const auto point = random_point(); + const auto point2 = Point2d{point.x() + 1, point.y() + 4}; + const auto point3 = Point2d{point.x() + 4, point.y() + 4}; + const auto point4 = Point2d{point.x() + 3, point.y() + 1}; + polygon.outer() = {point, point2, point3, point4, point}; + return polygon; +} + +bool all_within(const MultiPoint2d & pts1, const MultiPoint2d & pts2) +{ + // results from the collision checker and the direct checks can have some small precision errors + constexpr auto eps = 1e-3; + for (const auto & p1 : pts1) { + bool found = false; + for (const auto & p2 : pts2) { + if (boost::geometry::comparable_distance(p1, p2) < eps) { + found = true; + break; + } + } + if (!found) return false; + } + return true; +} + +TEST(TestCollisionChecker, Benchmark) +{ + constexpr auto nb_ego_footprints = 1000; + constexpr auto nb_obstacles = 1000; + MultiPolygon2d ego_footprints; + ego_footprints.reserve(nb_ego_footprints); + for (auto i = 0; i < nb_ego_footprints; ++i) { + ego_footprints.push_back(random_polygon()); + } + const auto cc_constr_start = std::chrono::system_clock::now(); + CollisionChecker collision_checker(ego_footprints); + const auto cc_constr_end = std::chrono::system_clock::now(); + const auto cc_constr_ns = + std::chrono::duration_cast(cc_constr_end - cc_constr_start).count(); + std::printf( + "Collision checker construction (with %d footprints): %ld ns\n", nb_ego_footprints, + cc_constr_ns); + MultiPolygon2d poly_obstacles; + MultiPoint2d point_obstacles; + MultiLineString2d line_obstacles; + for (auto i = 0; i < nb_obstacles; ++i) { + poly_obstacles.push_back(random_polygon()); + point_obstacles.push_back(random_point()); + line_obstacles.push_back(random_line()); + } + const auto check_obstacles_one_by_one = [&](const auto & obstacles) { + std::chrono::nanoseconds collision_checker_ns{}; + std::chrono::nanoseconds naive_ns{}; + for (const auto & obs : obstacles) { + const auto cc_start = std::chrono::system_clock::now(); + const auto collisions = collision_checker.get_collisions(obs); + MultiPoint2d cc_collision_points; + for (const auto & c : collisions) + cc_collision_points.insert( + cc_collision_points.end(), c.collision_points.begin(), c.collision_points.end()); + const auto cc_end = std::chrono::system_clock::now(); + const auto naive_start = std::chrono::system_clock::now(); + MultiPoint2d naive_collision_points; + for (const auto & ego_footprint : ego_footprints) { + MultiPoint2d points; + boost::geometry::intersection(ego_footprint, obs, points); + naive_collision_points.insert(naive_collision_points.end(), points.begin(), points.end()); + } + const auto naive_end = std::chrono::system_clock::now(); + const auto equal = all_within(cc_collision_points, naive_collision_points) && + all_within(naive_collision_points, cc_collision_points); + EXPECT_TRUE(equal); + if (!equal) { + std::cout << "cc: " << boost::geometry::wkt(cc_collision_points) << std::endl; + std::cout << "naive: " << boost::geometry::wkt(naive_collision_points) << std::endl; + } + collision_checker_ns += + std::chrono::duration_cast(cc_end - cc_start); + naive_ns += std::chrono::duration_cast(naive_end - naive_start); + } + std::printf("%20s%10ld ns\n", "collision checker : ", collision_checker_ns.count()); + std::printf("%20s%10ld ns\n", "naive : ", naive_ns.count()); + }; + const auto check_obstacles = [&](const auto & obstacles) { + std::chrono::nanoseconds collision_checker_ns{}; + std::chrono::nanoseconds naive_ns{}; + const auto cc_start = std::chrono::system_clock::now(); + const auto collisions = collision_checker.get_collisions(obstacles); + MultiPoint2d cc_collision_points; + for (const auto & c : collisions) + cc_collision_points.insert( + cc_collision_points.end(), c.collision_points.begin(), c.collision_points.end()); + const auto cc_end = std::chrono::system_clock::now(); + const auto naive_start = std::chrono::system_clock::now(); + MultiPoint2d naive_collision_points; + boost::geometry::intersection(ego_footprints, obstacles, naive_collision_points); + const auto naive_end = std::chrono::system_clock::now(); + const auto equal = all_within(cc_collision_points, naive_collision_points) && + all_within(naive_collision_points, cc_collision_points); + EXPECT_TRUE(equal); + if (!equal) { + std::cout << "cc: " << boost::geometry::wkt(cc_collision_points) << std::endl; + std::cout << "naive: " << boost::geometry::wkt(naive_collision_points) << std::endl; + } + collision_checker_ns += std::chrono::duration_cast(cc_end - cc_start); + naive_ns += std::chrono::duration_cast(naive_end - naive_start); + std::printf("%20s%10ld ns\n", "collision checker : ", collision_checker_ns.count()); + std::printf("%20s%10ld ns\n", "naive : ", naive_ns.count()); + }; + + std::cout << "* check one by one\n"; + std::printf("%d Polygons:\n", nb_obstacles); + check_obstacles_one_by_one(poly_obstacles); + std::printf("%d Lines:\n", nb_obstacles); + check_obstacles_one_by_one(line_obstacles); + std::printf("%d Points:\n", nb_obstacles); + check_obstacles_one_by_one(point_obstacles); + std::cout << "* check all at once\n"; + std::printf("%d Polygons:\n", nb_obstacles); + check_obstacles(poly_obstacles); + std::printf("%d Lines:\n", nb_obstacles); + check_obstacles(line_obstacles); + std::printf("%d Points:\n", nb_obstacles); + check_obstacles(point_obstacles); +} diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index 373eebbbd93b5..c1e68a232c55d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -17,7 +17,6 @@ rosidl_default_generators - autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils autoware_motion_velocity_planner_common diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index be9eab56d2320..9a08616b8fe7b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -85,6 +84,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & "~/output/velocity_factors", 1); processing_time_publisher_ = this->create_publisher("~/debug/processing_time_ms", 1); + debug_viz_pub_ = + this->create_publisher("~/debug/markers", 1); diagnostics_pub_ = this->create_publisher("/diagnostics", 10); // Parameters @@ -99,7 +100,7 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & // Initialize PlannerManager for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. - if (name == "") { + if (name.empty()) { break; } planner_manager_.load_module_plugin(*this, name); @@ -135,7 +136,7 @@ bool MotionVelocityPlannerNode::update_planner_data() const auto check_with_log = [&](const auto ptr, const auto & log) { constexpr auto throttle_duration = 3000; // [ms] if (!ptr) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, log); + RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, "%s", log); is_ready = false; return false; } @@ -278,7 +279,6 @@ void MotionVelocityPlannerNode::on_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{ input_trajectory_msg->points.begin(), input_trajectory_msg->points.end()}; - auto output_trajectory_msg = generate_trajectory(input_trajectory_points); output_trajectory_msg.header = input_trajectory_msg->header; processing_times["generate_trajectory"] = stop_watch.toc(true); @@ -333,7 +333,8 @@ void MotionVelocityPlannerNode::insert_slowdown( autoware::motion_utils::insertTargetPoint(to_seg_idx, slowdown_interval.to, trajectory.points); if (from_insert_idx && to_insert_idx) { for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx) - trajectory.points[idx].longitudinal_velocity_mps = slowdown_interval.velocity; + trajectory.points[idx].longitudinal_velocity_mps = + static_cast(slowdown_interval.velocity); } else { RCLCPP_WARN(get_logger(), "Failed to insert slowdown point"); } @@ -368,16 +369,14 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s autoware::motion_velocity_planner::TrajectoryPoints clipped; autoware::motion_velocity_planner::TrajectoryPoints traj_smoothed; clipped.insert( - clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + clipped.end(), std::next(traj_resampled.begin(), static_cast(traj_resampled_closest)), + traj_resampled.end()); if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { RCLCPP_ERROR(get_logger(), "failed to smooth"); } - traj_smoothed.insert( - traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); - if (external_v_limit) { autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( - traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); + 0LU, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } return traj_smoothed; } @@ -385,13 +384,21 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points) { + universe_utils::StopWatch stop_watch; autoware_planning_msgs::msg::Trajectory output_trajectory_msg; output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; - if (smooth_velocity_before_planning_) + if (smooth_velocity_before_planning_) { + stop_watch.tic("smooth"); input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); - const auto resampled_trajectory = - autoware::motion_utils::resampleTrajectory(output_trajectory_msg, 0.5); - + RCLCPP_DEBUG(get_logger(), "smooth: %2.2f us", stop_watch.toc("smooth")); + } + autoware_planning_msgs::msg::Trajectory smooth_velocity_trajectory; + smooth_velocity_trajectory.points = { + input_trajectory_points.begin(), input_trajectory_points.end()}; + auto resampled_trajectory = + autoware::motion_utils::resampleTrajectory(smooth_velocity_trajectory, 0.5); + motion_utils::calculate_time_from_start( + resampled_trajectory.points, planner_data_.current_odometry.pose.pose.position); const auto planning_results = planner_manager_.plan_velocities( resampled_trajectory.points, std::make_shared(planner_data_)); diff --git a/sensing/imu_corrector/CMakeLists.txt b/sensing/autoware_imu_corrector/CMakeLists.txt similarity index 79% rename from sensing/imu_corrector/CMakeLists.txt rename to sensing/autoware_imu_corrector/CMakeLists.txt index b3be5be12b75d..5e417ce94597a 100644 --- a/sensing/imu_corrector/CMakeLists.txt +++ b/sensing/autoware_imu_corrector/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 3.14) -project(imu_corrector) +project(autoware_imu_corrector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(${PROJECT_NAME} SHARED +ament_auto_add_library(imu_corrector SHARED src/imu_corrector_core.cpp ) @@ -13,14 +13,14 @@ ament_auto_add_library(gyro_bias_estimator SHARED src/gyro_bias_estimation_module.cpp ) -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "imu_corrector::ImuCorrector" - EXECUTABLE ${PROJECT_NAME}_node +rclcpp_components_register_node(imu_corrector + PLUGIN "autoware::imu_corrector::ImuCorrector" + EXECUTABLE imu_corrector_node EXECUTOR SingleThreadedExecutor ) rclcpp_components_register_node(gyro_bias_estimator - PLUGIN "imu_corrector::GyroBiasEstimator" + PLUGIN "autoware::imu_corrector::GyroBiasEstimator" EXECUTABLE gyro_bias_estimator_node EXECUTOR SingleThreadedExecutor ) diff --git a/sensing/imu_corrector/README.md b/sensing/autoware_imu_corrector/README.md similarity index 99% rename from sensing/imu_corrector/README.md rename to sensing/autoware_imu_corrector/README.md index 2df12c94b7d3b..805c20c0cb35c 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/autoware_imu_corrector/README.md @@ -1,4 +1,4 @@ -# imu_corrector +# autoware_imu_corrector ## imu_corrector diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/autoware_imu_corrector/config/gyro_bias_estimator.param.yaml similarity index 100% rename from sensing/imu_corrector/config/gyro_bias_estimator.param.yaml rename to sensing/autoware_imu_corrector/config/gyro_bias_estimator.param.yaml diff --git a/sensing/imu_corrector/config/imu_corrector.param.yaml b/sensing/autoware_imu_corrector/config/imu_corrector.param.yaml similarity index 100% rename from sensing/imu_corrector/config/imu_corrector.param.yaml rename to sensing/autoware_imu_corrector/config/imu_corrector.param.yaml diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/autoware_imu_corrector/launch/gyro_bias_estimator.launch.xml similarity index 74% rename from sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml rename to sensing/autoware_imu_corrector/launch/gyro_bias_estimator.launch.xml index 100168b17171a..774b8f14f9501 100644 --- a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml +++ b/sensing/autoware_imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -3,10 +3,10 @@ - - + + - + diff --git a/sensing/imu_corrector/launch/imu_corrector.launch.xml b/sensing/autoware_imu_corrector/launch/imu_corrector.launch.xml similarity index 61% rename from sensing/imu_corrector/launch/imu_corrector.launch.xml rename to sensing/autoware_imu_corrector/launch/imu_corrector.launch.xml index 8430f2a50a85c..456ce58750658 100755 --- a/sensing/imu_corrector/launch/imu_corrector.launch.xml +++ b/sensing/autoware_imu_corrector/launch/imu_corrector.launch.xml @@ -2,9 +2,9 @@ - + - + diff --git a/sensing/imu_corrector/package.xml b/sensing/autoware_imu_corrector/package.xml similarity index 90% rename from sensing/imu_corrector/package.xml rename to sensing/autoware_imu_corrector/package.xml index f5c719d2b4f7e..4869b5566ea05 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/autoware_imu_corrector/package.xml @@ -1,9 +1,9 @@ - imu_corrector + autoware_imu_corrector 1.0.0 - The ROS 2 imu_corrector package + The ROS 2 autoware_imu_corrector package Yamato Ando Taiki Yamada Apache License 2.0 diff --git a/sensing/imu_corrector/schema/imu_corrector.json b/sensing/autoware_imu_corrector/schema/imu_corrector.json similarity index 100% rename from sensing/imu_corrector/schema/imu_corrector.json rename to sensing/autoware_imu_corrector/schema/imu_corrector.json diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.cpp similarity index 97% rename from sensing/imu_corrector/src/gyro_bias_estimation_module.cpp rename to sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.cpp index 7bd6b38490495..18d7965470f05 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp +++ b/sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.cpp @@ -14,11 +14,10 @@ #include "gyro_bias_estimation_module.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" - +#include #include -namespace imu_corrector +namespace autoware::imu_corrector { /** @@ -116,4 +115,4 @@ geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias_base_link() const return gyro_bias_base; } -} // namespace imu_corrector +} // namespace autoware::imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp b/sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.hpp similarity index 94% rename from sensing/imu_corrector/src/gyro_bias_estimation_module.hpp rename to sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.hpp index 9eff50d296a95..ed166e4daf63c 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp +++ b/sensing/autoware_imu_corrector/src/gyro_bias_estimation_module.hpp @@ -22,7 +22,7 @@ #include #include -namespace imu_corrector +namespace autoware::imu_corrector { class GyroBiasEstimationModule { @@ -36,6 +36,6 @@ class GyroBiasEstimationModule private: std::pair gyro_bias_pair_; }; -} // namespace imu_corrector +} // namespace autoware::imu_corrector #endif // GYRO_BIAS_ESTIMATION_MODULE_HPP_ diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp similarity index 98% rename from sensing/imu_corrector/src/gyro_bias_estimator.cpp rename to sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp index 6882fbf068fd1..ec91311455911 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp @@ -14,14 +14,14 @@ #include "gyro_bias_estimator.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include #include #include #include -namespace imu_corrector +namespace autoware::imu_corrector { GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options) : rclcpp::Node("gyro_bias_validator", options), @@ -243,7 +243,7 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW stat.add("gyro_bias_threshold", f(gyro_bias_threshold_)); } -} // namespace imu_corrector +} // namespace autoware::imu_corrector #include -RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::GyroBiasEstimator) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::imu_corrector::GyroBiasEstimator) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.hpp similarity index 95% rename from sensing/imu_corrector/src/gyro_bias_estimator.hpp rename to sensing/autoware_imu_corrector/src/gyro_bias_estimator.hpp index 1313f2cb06507..68539a2181fd3 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.hpp @@ -14,9 +14,9 @@ #ifndef GYRO_BIAS_ESTIMATOR_HPP_ #define GYRO_BIAS_ESTIMATOR_HPP_ -#include "autoware/universe_utils/ros/transform_listener.hpp" #include "gyro_bias_estimation_module.hpp" +#include #include #include @@ -30,7 +30,7 @@ #include #include -namespace imu_corrector +namespace autoware::imu_corrector { class GyroBiasEstimator : public rclcpp::Node { @@ -97,6 +97,6 @@ class GyroBiasEstimator : public rclcpp::Node DiagnosticsInfo diagnostics_info_; }; -} // namespace imu_corrector +} // namespace autoware::imu_corrector #endif // GYRO_BIAS_ESTIMATOR_HPP_ diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/autoware_imu_corrector/src/imu_corrector_core.cpp similarity index 97% rename from sensing/imu_corrector/src/imu_corrector_core.cpp rename to sensing/autoware_imu_corrector/src/imu_corrector_core.cpp index c6e12d2481f89..a9acb45888945 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/autoware_imu_corrector/src/imu_corrector_core.cpp @@ -51,7 +51,7 @@ geometry_msgs::msg::Vector3 transform_vector3( return vec_stamped_transformed.vector; } -namespace imu_corrector +namespace autoware::imu_corrector { ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & options) : rclcpp::Node("imu_corrector", options), @@ -124,7 +124,7 @@ void ImuCorrector::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_ imu_pub_->publish(imu_msg_base_link); } -} // namespace imu_corrector +} // namespace autoware::imu_corrector #include -RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::ImuCorrector) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::imu_corrector::ImuCorrector) diff --git a/sensing/imu_corrector/src/imu_corrector_core.hpp b/sensing/autoware_imu_corrector/src/imu_corrector_core.hpp similarity index 89% rename from sensing/imu_corrector/src/imu_corrector_core.hpp rename to sensing/autoware_imu_corrector/src/imu_corrector_core.hpp index c02aa88a313a7..0c4839f6b5290 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.hpp +++ b/sensing/autoware_imu_corrector/src/imu_corrector_core.hpp @@ -14,9 +14,8 @@ #ifndef IMU_CORRECTOR_CORE_HPP_ #define IMU_CORRECTOR_CORE_HPP_ -#include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "autoware/universe_utils/ros/transform_listener.hpp" - +#include +#include #include #include @@ -27,7 +26,7 @@ #include #include -namespace imu_corrector +namespace autoware::imu_corrector { class ImuCorrector : public rclcpp::Node { @@ -57,6 +56,6 @@ class ImuCorrector : public rclcpp::Node std::string output_frame_; }; -} // namespace imu_corrector +} // namespace autoware::imu_corrector #endif // IMU_CORRECTOR_CORE_HPP_ diff --git a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp b/sensing/autoware_imu_corrector/test/test_gyro_bias_estimation_module.cpp similarity index 97% rename from sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp rename to sensing/autoware_imu_corrector/test/test_gyro_bias_estimation_module.cpp index 257552e15cbab..573dbe84d6027 100644 --- a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp +++ b/sensing/autoware_imu_corrector/test/test_gyro_bias_estimation_module.cpp @@ -18,7 +18,7 @@ #include -namespace imu_corrector +namespace autoware::imu_corrector { class GyroBiasEstimationModuleTest : public ::testing::Test { @@ -88,4 +88,4 @@ TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataExceptionWhenVehicleMovi ASSERT_THROW(module.update_bias(pose_list, gyro_list), std::runtime_error); } } -} // namespace imu_corrector +} // namespace autoware::imu_corrector diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index 2410a9dccebea..0a051d780f5f7 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -273,7 +273,7 @@ PointCloudConcatenateDataSynchronizerComponent::computeTransformToAdjustForOldTi // return identity if old_stamp is newer than new_stamp if (old_stamp > new_stamp) { - RCLCPP_WARN_STREAM_THROTTLE( + RCLCPP_DEBUG_STREAM_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), "old_stamp is newer than new_stamp,"); return Eigen::Matrix4f::Identity(); diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp index d7a6a8284d704..5d9c3ec74115a 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp @@ -64,6 +64,7 @@ class SimModelDelaySteerAccGearedWoFallGuard : public SimModelInterface VX, STEER, PEDAL_ACCX, + ACCX, }; enum IDX_U { PEDAL_ACCX_DES = 0, GEAR, SLOPE_ACCX, STEER_DES }; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp index c699d704724f5..ce962eba9275a 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp @@ -23,7 +23,7 @@ SimModelDelaySteerAccGearedWoFallGuard::SimModelDelaySteerAccGearedWoFallGuard( double dt, double acc_delay, double acc_time_constant, double steer_delay, double steer_time_constant, double steer_dead_band, double steer_bias, double debug_acc_scaling_factor, double debug_steer_scaling_factor) -: SimModelInterface(6 /* dim x */, 4 /* dim u */), +: SimModelInterface(7 /* dim x */, 4 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), vx_rate_lim_(vx_rate_lim), @@ -64,7 +64,7 @@ double SimModelDelaySteerAccGearedWoFallGuard::getVy() } double SimModelDelaySteerAccGearedWoFallGuard::getAx() { - return state_(IDX::PEDAL_ACCX); + return state_(IDX::ACCX); } double SimModelDelaySteerAccGearedWoFallGuard::getWz() { @@ -103,6 +103,8 @@ void SimModelDelaySteerAccGearedWoFallGuard::update(const double & dt) // stop condition is satisfied state_(IDX::VX) = 0.0; } + + state_(IDX::ACCX) = (state_(IDX::VX) - prev_state(IDX::VX)) / dt; } void SimModelDelaySteerAccGearedWoFallGuard::initializeInputQueue(const double & dt) diff --git a/simulator/simple_planning_simulator/test/actuation_cmd_map/steer_map.csv b/simulator/simple_planning_simulator/test/actuation_cmd_map/steer_map.csv index 7adb0f6a9e583..1562a7b346677 100644 --- a/simulator/simple_planning_simulator/test/actuation_cmd_map/steer_map.csv +++ b/simulator/simple_planning_simulator/test/actuation_cmd_map/steer_map.csv @@ -1,4 +1,4 @@ default,-0.6,0,0.6 --10,-0.03,-0.03,-0.03 +-10,-0.5,-0.5,-0.5 0,0.0,0.0,0.0 -10,0.03,0.03,0.03 +10,0.5,0.5,0.5 diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index cb58628121de2..d774617dc77f1 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -221,6 +221,7 @@ void isOnForward(const Odometry & state, const Odometry & init) { double forward_thr = 1.0; double dx = state.pose.pose.position.x - init.pose.pose.position.x; + std::cout << "isOnForward: dx: " << dx << ", forward_thr: " << forward_thr << std::endl; EXPECT_GT(dx, forward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); } @@ -228,6 +229,7 @@ void isOnBackward(const Odometry & state, const Odometry & init) { double backward_thr = -1.0; double dx = state.pose.pose.position.x - init.pose.pose.position.x; + std::cout << "isOnBackward: dx: " << dx << ", backward_thr: " << backward_thr << std::endl; EXPECT_LT(dx, backward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); } @@ -237,6 +239,8 @@ void isOnForwardLeft(const Odometry & state, const Odometry & init) double left_thr = 0.1f; double dx = state.pose.pose.position.x - init.pose.pose.position.x; double dy = state.pose.pose.position.y - init.pose.pose.position.y; + std::cout << "isOnForwardLeft: dx: " << dx << ", forward_thr: " << forward_thr << ", dy: " << dy + << ", left_thr: " << left_thr << std::endl; EXPECT_GT(dx, forward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); EXPECT_GT(dy, left_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); } @@ -247,6 +251,8 @@ void isOnBackwardRight(const Odometry & state, const Odometry & init) double right_thr = -0.1; double dx = state.pose.pose.position.x - init.pose.pose.position.x; double dy = state.pose.pose.position.y - init.pose.pose.position.y; + std::cout << "isOnBackwardRight: dx: " << dx << ", backward_thr: " << backward_thr + << ", dy: " << dy << ", right_thr: " << right_thr << std::endl; EXPECT_LT(dx, backward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); EXPECT_LT(dy, right_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); } @@ -302,7 +308,6 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) const auto common_params = get_common_params(params); const auto command_type = common_params.first; const auto vehicle_model_type = common_params.second; - std::cout << "\n\n vehicle model = " << vehicle_model_type << std::endl << std::endl; // optional parameters std::optional conversion_type{}; // for ActuationCmdParamType @@ -338,7 +343,12 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) node_options.append_parameter_override("vgr_coef_b", 0.053); node_options.append_parameter_override("vgr_coef_c", 0.042); if (conversion_type.has_value()) { + std::cout << "\n\n vehicle model = " << vehicle_model_type + << ", conversion_type = " << conversion_type.value() << std::endl + << std::endl; node_options.append_parameter_override("convert_steer_cmd_method", conversion_type.value()); + } else { + std::cout << "\n\n vehicle model = " << vehicle_model_type << std::endl << std::endl; } declareVehicleInfoParams(node_options); diff --git a/system/default_ad_api/CMakeLists.txt b/system/autoware_default_adapi/CMakeLists.txt similarity index 55% rename from system/default_ad_api/CMakeLists.txt rename to system/autoware_default_adapi/CMakeLists.txt index 799a73508a061..2dd39b455d2d2 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/autoware_default_adapi/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(default_ad_api) +project(autoware_default_adapi) find_package(autoware_cmake REQUIRED) autoware_package() @@ -24,20 +24,20 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_nodes(${PROJECT_NAME} - "default_ad_api::AutowareStateNode" - "default_ad_api::DiagnosticsNode" - "default_ad_api::FailSafeNode" - "default_ad_api::HeartbeatNode" - "default_ad_api::InterfaceNode" - "default_ad_api::LocalizationNode" - "default_ad_api::MotionNode" - "default_ad_api::OperationModeNode" - "default_ad_api::PerceptionNode" - "default_ad_api::PlanningNode" - "default_ad_api::RoutingNode" - "default_ad_api::VehicleNode" - "default_ad_api::VehicleInfoNode" - "default_ad_api::VehicleDoorNode" + "autoware::default_adapi::AutowareStateNode" + "autoware::default_adapi::DiagnosticsNode" + "autoware::default_adapi::FailSafeNode" + "autoware::default_adapi::HeartbeatNode" + "autoware::default_adapi::InterfaceNode" + "autoware::default_adapi::LocalizationNode" + "autoware::default_adapi::MotionNode" + "autoware::default_adapi::OperationModeNode" + "autoware::default_adapi::PerceptionNode" + "autoware::default_adapi::PlanningNode" + "autoware::default_adapi::RoutingNode" + "autoware::default_adapi::VehicleNode" + "autoware::default_adapi::VehicleInfoNode" + "autoware::default_adapi::VehicleDoorNode" ) if(BUILD_TESTING) diff --git a/system/default_ad_api/README.md b/system/autoware_default_adapi/README.md similarity index 93% rename from system/default_ad_api/README.md rename to system/autoware_default_adapi/README.md index 7bc496010eee9..e638c335ab8f2 100644 --- a/system/default_ad_api/README.md +++ b/system/autoware_default_adapi/README.md @@ -1,4 +1,4 @@ -# default_ad_api +# autoware_default_adapi ## Notes @@ -25,7 +25,7 @@ This is a sample to call API using HTTP. This is a debug script to check the conditions for transition to autonomous mode. ```bash -$ ros2 run default_ad_api guide.py +$ ros2 run autoware_default_adapi guide.py The vehicle pose is not estimated. Please set an initial pose or check GNSS. The route is not set. Please set a goal pose. diff --git a/system/default_ad_api/config/default_ad_api.param.yaml b/system/autoware_default_adapi/config/default_adapi.param.yaml similarity index 64% rename from system/default_ad_api/config/default_ad_api.param.yaml rename to system/autoware_default_adapi/config/default_adapi.param.yaml index a15abe091764c..ddbf103878953 100644 --- a/system/default_ad_api/config/default_ad_api.param.yaml +++ b/system/autoware_default_adapi/config/default_adapi.param.yaml @@ -1,8 +1,8 @@ -/default_ad_api/node/autoware_state: +/adapi/node/autoware_state: ros__parameters: update_rate: 10.0 -/default_ad_api/node/motion: +/adapi/node/motion: ros__parameters: require_accept_start: false stop_check_duration: 1.0 diff --git a/system/default_ad_api/document/autoware-state.md b/system/autoware_default_adapi/document/autoware-state.md similarity index 71% rename from system/default_ad_api/document/autoware-state.md rename to system/autoware_default_adapi/document/autoware-state.md index e26756de1f4ba..eb31a4d41bc1d 100644 --- a/system/default_ad_api/document/autoware-state.md +++ b/system/autoware_default_adapi/document/autoware-state.md @@ -2,7 +2,7 @@ ## Overview -Since `/autoware/state` was so widely used, default_ad_api creates it from the states of AD API for backwards compatibility. +Since `/autoware/state` was so widely used, this packages creates it from the states of AD API for backwards compatibility. The diagnostic checks that ad_service_state_monitor used to perform have been replaced by component_state_monitor. The service `/autoware/shutdown` to change autoware state to finalizing is also supported for compatibility. @@ -11,6 +11,6 @@ The service `/autoware/shutdown` to change autoware state to finalizing is also ## Conversion This is the correspondence between AD API states and autoware states. -The launch state is the data that default_ad_api node holds internally. +The launch state is the data that this node holds internally. ![autoware-state-table](images/autoware-state-table.drawio.svg) diff --git a/system/default_ad_api/document/fail-safe.md b/system/autoware_default_adapi/document/fail-safe.md similarity index 100% rename from system/default_ad_api/document/fail-safe.md rename to system/autoware_default_adapi/document/fail-safe.md diff --git a/system/default_ad_api/document/images/autoware-state-architecture.drawio.svg b/system/autoware_default_adapi/document/images/autoware-state-architecture.drawio.svg similarity index 92% rename from system/default_ad_api/document/images/autoware-state-architecture.drawio.svg rename to system/autoware_default_adapi/document/images/autoware-state-architecture.drawio.svg index 40aebd20ae43f..f3e0129a2f3f2 100644 --- a/system/default_ad_api/document/images/autoware-state-architecture.drawio.svg +++ b/system/autoware_default_adapi/document/images/autoware-state-architecture.drawio.svg @@ -6,7 +6,7 @@ width="761px" height="381px" viewBox="-0.5 -0.5 761 381" - content="<mxfile><diagram id="X9Q9Zxk0nsySa-CThXZE" name="Page-1">5Vvfb5swEP5rIm0vUzA/kjxuWbe9TJrUh21PlQsOWAOMjGmS/fWzgx0Mpk1oEqAslVp8PhNz333nO+PO7HWy+0phFn0nAYpnYB7sZvbnGQCLhct/C8G+FDgLrxSEFAelyKoE9/gvksK5lBY4QHlNkRESM5zVhT5JU+SzmgxSSrZ1tQ2J69+awRAZgnsfxqb0Jw5YJKWWt6o6viEcRvKrl2BRdiRQKcsnySMYkK0msu9m9poSwsqrZLdGsbCdsks57sszvceJUZSyswbIieVsrx4OBfxZZZNQFpGQpDC+q6SfIpbEvNPil5QUaYDEzea8ZX67nFBOCuo3QGSQhkipOaVMfLc2UM75KyIJYnTPFSiKIcNPdRyghDM86smhHymFe00hIzhluXbnH0LAFaRjuks5N+mXYNWwXjd9flHOQLW0R6lEB0SeQWf5CnTqkGhYvR4ddxToLJxu6JzQvxwdefsnGBfykX2SZCRFh1nkDDIkOE9SzAg1kKzjtI0wQ/cZPOCw5VGzjt0Gx/GaxPw2Yqy9ccWPkJOUafLyw+U5o+QP0nq8w+clL3hClKHdi/gqtwd1w9pz2d5WwRAoakRaHFSAtLmEBsuLVrcHiFhqjdE4AbxRcMKa16Gwli9z4oT+5ZxQzlRxYga8mAmXzGDKr0NxHRO+mOK/3DAkVf381rqKEj/SpkTXwpxbWLuVYt0zt2w6ThzzlAG9Mfo1wxow2WeBW7FvgBUJjJZ9wOvGvhP6l7MPGOzjtmc4DStmTIICYD4gBxzLtOKtOWD3wYFzDaBmo7kZyRBVITjh5d5E/Mz2hoy1q/79zBltrDVqLatjbWZdOdY6BgkYybDPRX6E/D8y4ha5mchsRDUgeikrsokwpVkT9BuR56YVb80Ud7RMada9Vsc62bp2nexeyBRYMJKShBT5RMjiNWNTn2SxBlhWPJMsKlftP30BV40W/Lnp/peQf3BV87fe93knB5Wt/azLzkOb4exRRBmnGfBPRJkT+hdHGTBAVt6GjjUKdJr7PKfQOaF/+RrgGWtAgDawiNkDDB5ghs3Y/66+SbQWNlPFrGg0S473Bv5vd4e1sSK3bbA6zo3WB2D3TiTQssWjdtdHlkypivDcZKqhf3mYM7d4AgzDlOQM+1PNj+xlj/kRcPv3/7aye7j86PnK9j96p9Wry111Vx3tMCsT0oWl2r9VDsqvq4RUNPZa4weimE8e0S45KmjJgtR5hwGc9xWJhqgvt/whlYNPOJOwHNOtF96t3HphYFGZuonClF5PNIOJ12MssfvfCwOrlggw0r0wp+NemHPl9G3VhRJRwQKyTQ1A8whm4jJCO8ix5KqZFrml9BjMwTns2SF1zm50bGpmg72yCfTOJrvttMnVz8edbQDzQMchGczFDCPIqePBRHhT+piLP3yRzRF9wj56OET0h2OqON9CMeiw5Xso4usDH9GG0KksAM0isCWZvFn9bptvZy/MZ7gFVWwAN7FPS1Jy3PLQzeNewzxmdZO2vbB+uymfWzeuaxq37WyefQ3bmm96ZDD4z5ewITExCyJZzU8i0lq9WZY3q3P5ZepX/XODffcP</diagram></mxfile>" + content="<mxfile><diagram id="X9Q9Zxk0nsySa-CThXZE" name="Page-1">5VvNkps4EH4aV20uKSN+bB8TZ5K9bFWq5rCbk0sDMqgCiBJibOfpVzKSEQiPTbCBcTwHS62WLLr7a3U3mpm9TvbfKMyif0iA4hmYB/uZ/WUGgOU6Lv8SlENJ8Sy7JIQUB5KpIjzjX0gS55Ja4ADlNUZGSMxwVif6JE2Rz2o0SCnZ1dm2JK7/agZDZBCefRib1H9xwCJJtbxVNfA3wmEkf3oJFuVAAhWzfJI8ggHZaST7aWavKSGsbCX7NYqF8JRcynlfz4yeNkZRyq6aIDeWs4N6OBTwZ5VdQllEQpLC+Kmifo5YEvNBizcpKdIAicXmvGf+utxQTgrqN5TIIA2RYnNKmvhtbaLc8zdEEsTogTNQFEOGX+t6gFKd4YlPTv1EKTxoDBnBKcu1lb8LAmeQluku5d6kXYJVQ3rd+Hmj3IHqaY9SkY4aOaOd5W9op64STVe/rx13EtpZON20c4G/v3bk8q8wLuQj+yTJSIqOu8gZZEhgnqSYEWposq6nXYQZes7gUQ877jbrutviOF6TmC8j5tpbV/wJOkmZRi8/nJ4zSn4ibcQ7ft6ygldEGdq/qV9l9qAuWHsu+7vKGQIFjUjzg0ohbSahqeVNqdsjeCx1xmiYAN4kMGHN66qwlm9j4gJ/f0woY6owMQNezIRJZjDl7VC0Y8IPU/yLC4akapwvrbMo8gttUnQuzLGFtaUU6s4s2TScOOYhA3pn8Gu6NWCizwL3Qt8IJxKYLPqA1w19F/j7ow8Y6OOyZzgNK2Q8BATAfEQMOJYpxXtjwB4CA9cKQO1GMzOSIapccMLzvQexM9sb09euhrczZ7K+1si1rI65mXVjX+sYIGAkwz4n+RHyf0qPW+RmILMV2YAYpazIHgQpzZxgWI88N6V4b6S4k0VKM++1OubJ1q3zZLcnUmDBSEoSUuQPAhav6ZuGBIs1wrHimWBRserw4Qu4qbfgz00P/wn6R1d1f+hjX/ZyUtk7zLpUHtoEZ0/CyzhNh3/By1zg7+1lwAhReZt2rElop1nnuaSdC/z9zwDPOAOEV99xAW8CtIVFzDYwgBk2z4C/6sWitZCdSmpFp5l6fDDs4P1WWhsnc1uh1XHudE4Ae3BAgZZSj6qyTyyoUpnhtUFVg7+/uzNLPQGGYUpyhv1HjZPs5YBxEnCHt/+29Hu8OOl8hvsHvdsa1ORuWl1He8zKwHRhqf4PFYvydhWYis5B63xHFPPNI9olVgUt0ZC69zCC8fYIOBSjMvQHjigsxzTvhXcv816c1YmphUd6XdF0Kt6APsUevjYGVi2eYKK1Madjbcy5cRi36gKJqGAB2aWGQvMIZqIZoT3kuuSsmebBJfXk1ME16Nkjde9ucmhqRoWDogkMjia77fbJze/LXS0A84LHMSjMxQ4jyKHjwURYU/qSiy8YbHJEX7GPNkePvjmFjPMdFJOOJeBjMl+f+IK2hD7KAdBMBluCyrvl8bb5trZnPMMlqHwDuIt8WoKSU+lDF497C/GYWU7a9gL7/YZ8bl24rinctrt69i1ka775kc7gDz/CxtSJmRjJrP4hPK01mGR5t7qnX4Z+1X872E//Aw==</diagram></mxfile>" > @@ -148,14 +148,14 @@
- default_ad_api + autoware_default_adapi
(localization, routing, operation mode)
- default_ad_api... + autoware_default_adapi... @@ -207,14 +207,14 @@
- default_ad_api + autoware_default_adapi
(autoware state)
- default_ad_api... + autoware_default_adapi... @@ -326,7 +326,7 @@ - Viewer does not support full SVG 1.1 + Text is not SVG - cannot display diff --git a/system/default_ad_api/document/images/autoware-state-table.drawio.svg b/system/autoware_default_adapi/document/images/autoware-state-table.drawio.svg similarity index 100% rename from system/default_ad_api/document/images/autoware-state-table.drawio.svg rename to system/autoware_default_adapi/document/images/autoware-state-table.drawio.svg diff --git a/system/default_ad_api/document/images/localization.drawio.svg b/system/autoware_default_adapi/document/images/localization.drawio.svg similarity index 100% rename from system/default_ad_api/document/images/localization.drawio.svg rename to system/autoware_default_adapi/document/images/localization.drawio.svg diff --git a/system/default_ad_api/document/images/motion-architecture.drawio.svg b/system/autoware_default_adapi/document/images/motion-architecture.drawio.svg similarity index 100% rename from system/default_ad_api/document/images/motion-architecture.drawio.svg rename to system/autoware_default_adapi/document/images/motion-architecture.drawio.svg diff --git a/system/default_ad_api/document/images/motion-state.drawio.svg b/system/autoware_default_adapi/document/images/motion-state.drawio.svg similarity index 100% rename from system/default_ad_api/document/images/motion-state.drawio.svg rename to system/autoware_default_adapi/document/images/motion-state.drawio.svg diff --git a/system/default_ad_api/document/images/operation-mode-architecture.drawio.svg b/system/autoware_default_adapi/document/images/operation-mode-architecture.drawio.svg similarity index 100% rename from system/default_ad_api/document/images/operation-mode-architecture.drawio.svg rename to system/autoware_default_adapi/document/images/operation-mode-architecture.drawio.svg diff --git a/system/default_ad_api/document/images/operation-mode-state.drawio.svg b/system/autoware_default_adapi/document/images/operation-mode-state.drawio.svg similarity index 100% rename from system/default_ad_api/document/images/operation-mode-state.drawio.svg rename to system/autoware_default_adapi/document/images/operation-mode-state.drawio.svg diff --git a/system/default_ad_api/document/images/operation-mode-table.drawio.svg b/system/autoware_default_adapi/document/images/operation-mode-table.drawio.svg similarity index 100% rename from system/default_ad_api/document/images/operation-mode-table.drawio.svg rename to system/autoware_default_adapi/document/images/operation-mode-table.drawio.svg diff --git a/system/default_ad_api/document/images/routing.drawio.svg b/system/autoware_default_adapi/document/images/routing.drawio.svg similarity index 100% rename from system/default_ad_api/document/images/routing.drawio.svg rename to system/autoware_default_adapi/document/images/routing.drawio.svg diff --git a/system/default_ad_api/document/interface.md b/system/autoware_default_adapi/document/interface.md similarity index 100% rename from system/default_ad_api/document/interface.md rename to system/autoware_default_adapi/document/interface.md diff --git a/system/default_ad_api/document/localization.md b/system/autoware_default_adapi/document/localization.md similarity index 100% rename from system/default_ad_api/document/localization.md rename to system/autoware_default_adapi/document/localization.md diff --git a/system/default_ad_api/document/motion.md b/system/autoware_default_adapi/document/motion.md similarity index 100% rename from system/default_ad_api/document/motion.md rename to system/autoware_default_adapi/document/motion.md diff --git a/system/default_ad_api/document/operation-mode.md b/system/autoware_default_adapi/document/operation-mode.md similarity index 100% rename from system/default_ad_api/document/operation-mode.md rename to system/autoware_default_adapi/document/operation-mode.md diff --git a/system/default_ad_api/document/routing.md b/system/autoware_default_adapi/document/routing.md similarity index 100% rename from system/default_ad_api/document/routing.md rename to system/autoware_default_adapi/document/routing.md diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/autoware_default_adapi/launch/default_adapi.launch.py similarity index 85% rename from system/default_ad_api/launch/default_ad_api.launch.py rename to system/autoware_default_adapi/launch/default_adapi.launch.py index ca1575a528db2..9f95b16ab640b 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/autoware_default_adapi/launch/default_adapi.launch.py @@ -25,17 +25,17 @@ def create_api_node(node_name, class_name, **kwargs): return ComposableNode( - namespace="default_ad_api/node", + namespace="adapi/node", name=node_name, - package="default_ad_api", - plugin="default_ad_api::" + class_name, + package="autoware_default_adapi", + plugin="autoware::default_adapi::" + class_name, parameters=[ParameterFile(LaunchConfiguration("config"))], ) def get_default_config(): - path = FindPackageShare("default_ad_api") - path = PathJoinSubstitution([path, "config/default_ad_api.param.yaml"]) + path = FindPackageShare("autoware_default_adapi") + path = PathJoinSubstitution([path, "config/default_adapi.param.yaml"]) return path @@ -57,16 +57,16 @@ def generate_launch_description(): create_api_node("vehicle_door", "VehicleDoorNode"), ] container = ComposableNodeContainer( - namespace="default_ad_api", + namespace="adapi", name="container", package="rclcpp_components", executable="component_container_mt", - ros_arguments=["--log-level", "default_ad_api.container:=WARN"], + ros_arguments=["--log-level", "adapi.container:=WARN"], composable_node_descriptions=components, ) web_server = Node( - namespace="default_ad_api", - package="default_ad_api", + namespace="adapi", + package="autoware_default_adapi", name="web_server", executable="web_server.py", ) diff --git a/system/default_ad_api/package.xml b/system/autoware_default_adapi/package.xml similarity index 94% rename from system/default_ad_api/package.xml rename to system/autoware_default_adapi/package.xml index c2751c96e995c..5c6e10a73fa9c 100644 --- a/system/default_ad_api/package.xml +++ b/system/autoware_default_adapi/package.xml @@ -1,9 +1,9 @@ - default_ad_api + autoware_default_adapi 0.1.0 - The default_ad_api package + The autoware_default_adapi package Takagi, Isamu Ryohsuke Mitsudome Yukihiro Saito diff --git a/system/default_ad_api/script/guide.py b/system/autoware_default_adapi/script/guide.py similarity index 100% rename from system/default_ad_api/script/guide.py rename to system/autoware_default_adapi/script/guide.py diff --git a/system/default_ad_api/script/web_server.py b/system/autoware_default_adapi/script/web_server.py similarity index 100% rename from system/default_ad_api/script/web_server.py rename to system/autoware_default_adapi/script/web_server.py diff --git a/system/default_ad_api/src/compatibility/autoware_state.cpp b/system/autoware_default_adapi/src/compatibility/autoware_state.cpp similarity index 97% rename from system/default_ad_api/src/compatibility/autoware_state.cpp rename to system/autoware_default_adapi/src/compatibility/autoware_state.cpp index 1aa49383aa48f..da110a711e35a 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.cpp +++ b/system/autoware_default_adapi/src/compatibility/autoware_state.cpp @@ -17,7 +17,7 @@ #include #include -namespace default_ad_api +namespace autoware::default_adapi { AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options) @@ -140,7 +140,7 @@ void AutowareStateNode::on_timer() pub_autoware_state_->publish(msg); } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::AutowareStateNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::AutowareStateNode) diff --git a/system/default_ad_api/src/compatibility/autoware_state.hpp b/system/autoware_default_adapi/src/compatibility/autoware_state.hpp similarity index 97% rename from system/default_ad_api/src/compatibility/autoware_state.hpp rename to system/autoware_default_adapi/src/compatibility/autoware_state.hpp index eff74728efcb0..d4707ea55fd87 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.hpp +++ b/system/autoware_default_adapi/src/compatibility/autoware_state.hpp @@ -28,7 +28,7 @@ // This file should be included after messages. #include "../utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class AutowareStateNode : public rclcpp::Node @@ -67,6 +67,6 @@ class AutowareStateNode : public rclcpp::Node void on_shutdown(const Trigger::Request::SharedPtr, const Trigger::Response::SharedPtr); }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // COMPATIBILITY__AUTOWARE_STATE_HPP_ diff --git a/system/default_ad_api/src/diagnostics.cpp b/system/autoware_default_adapi/src/diagnostics.cpp similarity index 94% rename from system/default_ad_api/src/diagnostics.cpp rename to system/autoware_default_adapi/src/diagnostics.cpp index 6eaaa5bf614a6..0c1550690f1b5 100644 --- a/system/default_ad_api/src/diagnostics.cpp +++ b/system/autoware_default_adapi/src/diagnostics.cpp @@ -17,7 +17,7 @@ #include #include -namespace default_ad_api +namespace autoware::default_adapi { DiagnosticsNode::DiagnosticsNode(const rclcpp::NodeOptions & options) : Node("diagnostics", options) @@ -75,7 +75,7 @@ void DiagnosticsNode::on_update(DiagGraph::ConstSharedPtr graph) pub_status_->publish(msg); } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::DiagnosticsNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::DiagnosticsNode) diff --git a/system/default_ad_api/src/diagnostics.hpp b/system/autoware_default_adapi/src/diagnostics.hpp similarity index 95% rename from system/default_ad_api/src/diagnostics.hpp rename to system/autoware_default_adapi/src/diagnostics.hpp index 302ffe39159df..b382887aaa694 100644 --- a/system/default_ad_api/src/diagnostics.hpp +++ b/system/autoware_default_adapi/src/diagnostics.hpp @@ -22,7 +22,7 @@ #include #include -namespace default_ad_api +namespace autoware::default_adapi { class DiagnosticsNode : public rclcpp::Node @@ -41,6 +41,6 @@ class DiagnosticsNode : public rclcpp::Node diagnostic_graph_utils::DiagGraphSubscription sub_graph_; }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // DIAGNOSTICS_HPP_ diff --git a/system/default_ad_api/src/fail_safe.cpp b/system/autoware_default_adapi/src/fail_safe.cpp similarity index 89% rename from system/default_ad_api/src/fail_safe.cpp rename to system/autoware_default_adapi/src/fail_safe.cpp index 92191a3d65bc9..130e2f523477c 100644 --- a/system/default_ad_api/src/fail_safe.cpp +++ b/system/autoware_default_adapi/src/fail_safe.cpp @@ -14,7 +14,7 @@ #include "fail_safe.hpp" -namespace default_ad_api +namespace autoware::default_adapi { FailSafeNode::FailSafeNode(const rclcpp::NodeOptions & options) : Node("fail_safe", options) @@ -35,7 +35,7 @@ void FailSafeNode::on_state(const MrmState::ConstSharedPtr msg) } } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::FailSafeNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::FailSafeNode) diff --git a/system/default_ad_api/src/fail_safe.hpp b/system/autoware_default_adapi/src/fail_safe.hpp similarity index 94% rename from system/default_ad_api/src/fail_safe.hpp rename to system/autoware_default_adapi/src/fail_safe.hpp index 6ae20b1b46e61..268fc6d4d95e4 100644 --- a/system/default_ad_api/src/fail_safe.hpp +++ b/system/autoware_default_adapi/src/fail_safe.hpp @@ -23,7 +23,7 @@ // This file should be included after messages. #include "utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class FailSafeNode : public rclcpp::Node @@ -39,6 +39,6 @@ class FailSafeNode : public rclcpp::Node void on_state(const MrmState::ConstSharedPtr msg); }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // FAIL_SAFE_HPP_ diff --git a/system/default_ad_api/src/heartbeat.cpp b/system/autoware_default_adapi/src/heartbeat.cpp similarity index 90% rename from system/default_ad_api/src/heartbeat.cpp rename to system/autoware_default_adapi/src/heartbeat.cpp index 4550302bb8bae..e048c347f3678 100644 --- a/system/default_ad_api/src/heartbeat.cpp +++ b/system/autoware_default_adapi/src/heartbeat.cpp @@ -16,7 +16,7 @@ #include -namespace default_ad_api +namespace autoware::default_adapi { HeartbeatNode::HeartbeatNode(const rclcpp::NodeOptions & options) : Node("heartbeat", options) @@ -36,7 +36,7 @@ HeartbeatNode::HeartbeatNode(const rclcpp::NodeOptions & options) : Node("heartb timer_ = rclcpp::create_timer(this, get_clock(), period, std::move(on_timer)); } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::HeartbeatNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::HeartbeatNode) diff --git a/system/default_ad_api/src/heartbeat.hpp b/system/autoware_default_adapi/src/heartbeat.hpp similarity index 93% rename from system/default_ad_api/src/heartbeat.hpp rename to system/autoware_default_adapi/src/heartbeat.hpp index d922b88489639..8b1108d7b1bd3 100644 --- a/system/default_ad_api/src/heartbeat.hpp +++ b/system/autoware_default_adapi/src/heartbeat.hpp @@ -21,7 +21,7 @@ // This file should be included after messages. #include "utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class HeartbeatNode : public rclcpp::Node @@ -35,6 +35,6 @@ class HeartbeatNode : public rclcpp::Node uint16_t sequence_ = 0; }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // HEARTBEAT_HPP_ diff --git a/system/default_ad_api/src/interface.cpp b/system/autoware_default_adapi/src/interface.cpp similarity index 87% rename from system/default_ad_api/src/interface.cpp rename to system/autoware_default_adapi/src/interface.cpp index 997f9e01fc6a9..0d9134102c42a 100644 --- a/system/default_ad_api/src/interface.cpp +++ b/system/autoware_default_adapi/src/interface.cpp @@ -14,7 +14,7 @@ #include "interface.hpp" -namespace default_ad_api +namespace autoware::default_adapi { InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interface", options) @@ -29,7 +29,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interf adaptor.init_srv(srv_, on_interface_version); } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::InterfaceNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::InterfaceNode) diff --git a/system/default_ad_api/src/interface.hpp b/system/autoware_default_adapi/src/interface.hpp similarity index 93% rename from system/default_ad_api/src/interface.hpp rename to system/autoware_default_adapi/src/interface.hpp index 46c70abf17f36..ce0bdb04e2d2e 100644 --- a/system/default_ad_api/src/interface.hpp +++ b/system/autoware_default_adapi/src/interface.hpp @@ -21,7 +21,7 @@ // This file should be included after messages. #include "utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class InterfaceNode : public rclcpp::Node @@ -33,6 +33,6 @@ class InterfaceNode : public rclcpp::Node Srv srv_; }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // INTERFACE_HPP_ diff --git a/system/default_ad_api/src/localization.cpp b/system/autoware_default_adapi/src/localization.cpp similarity index 90% rename from system/default_ad_api/src/localization.cpp rename to system/autoware_default_adapi/src/localization.cpp index 97544a7868d9c..7e6a094fb0fd2 100644 --- a/system/default_ad_api/src/localization.cpp +++ b/system/autoware_default_adapi/src/localization.cpp @@ -16,7 +16,7 @@ #include "utils/localization_conversion.hpp" -namespace default_ad_api +namespace autoware::default_adapi { LocalizationNode::LocalizationNode(const rclcpp::NodeOptions & options) @@ -36,7 +36,7 @@ void LocalizationNode::on_initialize( res->status = localization_conversion::convert_call(cli_initialize_, req); } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::LocalizationNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::LocalizationNode) diff --git a/system/default_ad_api/src/localization.hpp b/system/autoware_default_adapi/src/localization.hpp similarity index 95% rename from system/default_ad_api/src/localization.hpp rename to system/autoware_default_adapi/src/localization.hpp index a24e2110fabd1..d9104f393f712 100644 --- a/system/default_ad_api/src/localization.hpp +++ b/system/autoware_default_adapi/src/localization.hpp @@ -22,7 +22,7 @@ // This file should be included after messages. #include "utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class LocalizationNode : public rclcpp::Node @@ -42,6 +42,6 @@ class LocalizationNode : public rclcpp::Node const autoware_ad_api::localization::Initialize::Service::Response::SharedPtr res); }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // LOCALIZATION_HPP_ diff --git a/system/default_ad_api/src/motion.cpp b/system/autoware_default_adapi/src/motion.cpp similarity index 97% rename from system/default_ad_api/src/motion.cpp rename to system/autoware_default_adapi/src/motion.cpp index 9d4461fc76f1e..87d4df371abb7 100644 --- a/system/default_ad_api/src/motion.cpp +++ b/system/autoware_default_adapi/src/motion.cpp @@ -17,7 +17,7 @@ #include #include -namespace default_ad_api +namespace autoware::default_adapi { MotionNode::MotionNode(const rclcpp::NodeOptions & options) @@ -159,7 +159,7 @@ void MotionNode::on_accept( res->status.success = true; } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::MotionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::MotionNode) diff --git a/system/default_ad_api/src/motion.hpp b/system/autoware_default_adapi/src/motion.hpp similarity index 96% rename from system/default_ad_api/src/motion.hpp rename to system/autoware_default_adapi/src/motion.hpp index 053e9b2427a10..043eb6568e141 100644 --- a/system/default_ad_api/src/motion.hpp +++ b/system/autoware_default_adapi/src/motion.hpp @@ -25,7 +25,7 @@ // This file should be included after messages. #include "utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class MotionNode : public rclcpp::Node @@ -65,6 +65,6 @@ class MotionNode : public rclcpp::Node const autoware_ad_api::motion::AcceptStart::Service::Response::SharedPtr res); }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // MOTION_HPP_ diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/autoware_default_adapi/src/operation_mode.cpp similarity index 97% rename from system/default_ad_api/src/operation_mode.cpp rename to system/autoware_default_adapi/src/operation_mode.cpp index 510788dd09d08..cd452f2fa83a7 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/autoware_default_adapi/src/operation_mode.cpp @@ -18,7 +18,7 @@ #include #include -namespace default_ad_api +namespace autoware::default_adapi { using ServiceResponse = autoware_adapi_v1_msgs::srv::ChangeOperationMode::Response; @@ -157,7 +157,7 @@ void OperationModeNode::update_state() } } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::OperationModeNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::OperationModeNode) diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/autoware_default_adapi/src/operation_mode.hpp similarity index 98% rename from system/default_ad_api/src/operation_mode.hpp rename to system/autoware_default_adapi/src/operation_mode.hpp index 7cd609b5eb852..3b139ad875b8d 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/autoware_default_adapi/src/operation_mode.hpp @@ -30,7 +30,7 @@ // This file should be included after messages. #include "utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class OperationModeNode : public rclcpp::Node { @@ -94,6 +94,6 @@ class OperationModeNode : public rclcpp::Node void change_mode(const ResponseT res, const OperationModeRequest::_mode_type mode); }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // OPERATION_MODE_HPP_ diff --git a/system/default_ad_api/src/perception.cpp b/system/autoware_default_adapi/src/perception.cpp similarity index 96% rename from system/default_ad_api/src/perception.cpp rename to system/autoware_default_adapi/src/perception.cpp index 794bbf141ca8b..c92bac2f5f139 100644 --- a/system/default_ad_api/src/perception.cpp +++ b/system/autoware_default_adapi/src/perception.cpp @@ -16,7 +16,7 @@ #include -namespace default_ad_api +namespace autoware::default_adapi { using DynamicObjectArray = autoware_ad_api::perception::DynamicObjectArray; @@ -87,7 +87,7 @@ void PerceptionNode::object_recognize( pub_object_recognized_->publish(objects); } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::PerceptionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::PerceptionNode) diff --git a/system/default_ad_api/src/perception.hpp b/system/autoware_default_adapi/src/perception.hpp similarity index 95% rename from system/default_ad_api/src/perception.hpp rename to system/autoware_default_adapi/src/perception.hpp index ff3e4801b07ac..4eb83c57293ba 100644 --- a/system/default_ad_api/src/perception.hpp +++ b/system/autoware_default_adapi/src/perception.hpp @@ -31,7 +31,7 @@ // This file should be included after messages. #include "utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class PerceptionNode : public rclcpp::Node @@ -47,6 +47,6 @@ class PerceptionNode : public rclcpp::Node std::unordered_map hash_map, uint8_t input, uint8_t default_value); }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // PERCEPTION_HPP_ diff --git a/system/default_ad_api/src/planning.cpp b/system/autoware_default_adapi/src/planning.cpp similarity index 97% rename from system/default_ad_api/src/planning.cpp rename to system/autoware_default_adapi/src/planning.cpp index 374316e19e6b1..b81e48fada790 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/autoware_default_adapi/src/planning.cpp @@ -22,7 +22,7 @@ #include #include -namespace default_ad_api +namespace autoware::default_adapi { template @@ -155,7 +155,7 @@ void PlanningNode::on_timer() pub_steering_factors_->publish(steering); } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::PlanningNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::PlanningNode) diff --git a/system/default_ad_api/src/planning.hpp b/system/autoware_default_adapi/src/planning.hpp similarity index 97% rename from system/default_ad_api/src/planning.hpp rename to system/autoware_default_adapi/src/planning.hpp index 929eb517afa2e..f67de9f7b9221 100644 --- a/system/default_ad_api/src/planning.hpp +++ b/system/autoware_default_adapi/src/planning.hpp @@ -27,7 +27,7 @@ // This file should be included after messages. #include "utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class PlanningNode : public rclcpp::Node @@ -62,6 +62,6 @@ class PlanningNode : public rclcpp::Node KinematicState::ConstSharedPtr kinematic_state_; }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // PLANNING_HPP_ diff --git a/system/default_ad_api/src/routing.cpp b/system/autoware_default_adapi/src/routing.cpp similarity index 97% rename from system/default_ad_api/src/routing.cpp rename to system/autoware_default_adapi/src/routing.cpp index 0f2247c3aada8..767cfc37d3f25 100644 --- a/system/default_ad_api/src/routing.cpp +++ b/system/autoware_default_adapi/src/routing.cpp @@ -45,7 +45,7 @@ ResponseStatus route_is_not_set() } // namespace -namespace default_ad_api +namespace autoware::default_adapi { RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", options) @@ -166,7 +166,7 @@ void RoutingNode::on_change_route( res->status = conversion::convert_call(cli_set_lanelet_route_, req); } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::RoutingNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::RoutingNode) diff --git a/system/default_ad_api/src/routing.hpp b/system/autoware_default_adapi/src/routing.hpp similarity index 97% rename from system/default_ad_api/src/routing.hpp rename to system/autoware_default_adapi/src/routing.hpp index 406569aa0a36b..9033373c3a064 100644 --- a/system/default_ad_api/src/routing.hpp +++ b/system/autoware_default_adapi/src/routing.hpp @@ -24,7 +24,7 @@ // This file should be included after messages. #include "utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class RoutingNode : public rclcpp::Node @@ -76,6 +76,6 @@ class RoutingNode : public rclcpp::Node const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res); }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // ROUTING_HPP_ diff --git a/system/default_ad_api/src/utils/localization_conversion.cpp b/system/autoware_default_adapi/src/utils/localization_conversion.cpp similarity index 91% rename from system/default_ad_api/src/utils/localization_conversion.cpp rename to system/autoware_default_adapi/src/utils/localization_conversion.cpp index 3ddf259a4a590..1b327e2d2fc06 100644 --- a/system/default_ad_api/src/utils/localization_conversion.cpp +++ b/system/autoware_default_adapi/src/utils/localization_conversion.cpp @@ -16,7 +16,7 @@ #include -namespace default_ad_api::localization_conversion +namespace autoware::default_adapi::localization_conversion { InternalInitializeRequest convert_request(const ExternalInitializeRequest & external) @@ -37,4 +37,4 @@ ExternalResponse convert_response(const InternalResponse & internal) return external; } -} // namespace default_ad_api::localization_conversion +} // namespace autoware::default_adapi::localization_conversion diff --git a/system/default_ad_api/src/utils/localization_conversion.hpp b/system/autoware_default_adapi/src/utils/localization_conversion.hpp similarity index 92% rename from system/default_ad_api/src/utils/localization_conversion.hpp rename to system/autoware_default_adapi/src/utils/localization_conversion.hpp index 85b6e81e6cd91..d8e66a6ae44aa 100644 --- a/system/default_ad_api/src/utils/localization_conversion.hpp +++ b/system/autoware_default_adapi/src/utils/localization_conversion.hpp @@ -20,7 +20,7 @@ #include #include -namespace default_ad_api::localization_conversion +namespace autoware::default_adapi::localization_conversion { using ExternalInitializeRequest = @@ -39,6 +39,6 @@ ExternalResponse convert_call(ClientT & client, RequestT & req) return convert_response(client->call(convert_request(req))->status); } -} // namespace default_ad_api::localization_conversion +} // namespace autoware::default_adapi::localization_conversion #endif // UTILS__LOCALIZATION_CONVERSION_HPP_ diff --git a/system/default_ad_api/src/utils/route_conversion.cpp b/system/autoware_default_adapi/src/utils/route_conversion.cpp similarity index 98% rename from system/default_ad_api/src/utils/route_conversion.cpp rename to system/autoware_default_adapi/src/utils/route_conversion.cpp index 70951fc337b68..e8a748d52edee 100644 --- a/system/default_ad_api/src/utils/route_conversion.cpp +++ b/system/autoware_default_adapi/src/utils/route_conversion.cpp @@ -86,7 +86,7 @@ LaneletSegment convert(const RouteSegment & in) } // namespace -namespace default_ad_api::conversion +namespace autoware::default_adapi::conversion { ExternalRoute create_empty_route(const rclcpp::Time & stamp) @@ -170,4 +170,4 @@ ExternalResponse convert_response(const InternalResponse & internal) return external; } -} // namespace default_ad_api::conversion +} // namespace autoware::default_adapi::conversion diff --git a/system/default_ad_api/src/utils/route_conversion.hpp b/system/autoware_default_adapi/src/utils/route_conversion.hpp similarity index 96% rename from system/default_ad_api/src/utils/route_conversion.hpp rename to system/autoware_default_adapi/src/utils/route_conversion.hpp index 9a31b9e80c064..053dfd697c967 100644 --- a/system/default_ad_api/src/utils/route_conversion.hpp +++ b/system/autoware_default_adapi/src/utils/route_conversion.hpp @@ -28,7 +28,7 @@ #include #include -namespace default_ad_api::conversion +namespace autoware::default_adapi::conversion { using ExternalRoute = autoware_adapi_v1_msgs::msg::Route; @@ -62,6 +62,6 @@ ExternalResponse convert_call(ClientT & client, RequestT & req) return convert_response(client->call(convert_request(req))->status); } -} // namespace default_ad_api::conversion +} // namespace autoware::default_adapi::conversion #endif // UTILS__ROUTE_CONVERSION_HPP_ diff --git a/system/default_ad_api/src/utils/topics.hpp b/system/autoware_default_adapi/src/utils/topics.hpp similarity index 92% rename from system/default_ad_api/src/utils/topics.hpp rename to system/autoware_default_adapi/src/utils/topics.hpp index 97d91b33c9fa3..6018abb877285 100644 --- a/system/default_ad_api/src/utils/topics.hpp +++ b/system/autoware_default_adapi/src/utils/topics.hpp @@ -20,7 +20,7 @@ #include -namespace default_ad_api::utils +namespace autoware::default_adapi::utils { template @@ -39,6 +39,6 @@ void notify(PubT & pub, std::optional & prev, const MsgT & curr, FuncT && } } -} // namespace default_ad_api::utils +} // namespace autoware::default_adapi::utils #endif // UTILS__TOPICS_HPP_ diff --git a/system/default_ad_api/src/utils/types.hpp b/system/autoware_default_adapi/src/utils/types.hpp similarity index 93% rename from system/default_ad_api/src/utils/types.hpp rename to system/autoware_default_adapi/src/utils/types.hpp index 693e64a0dce55..ac4acb5fb19d6 100644 --- a/system/default_ad_api/src/utils/types.hpp +++ b/system/autoware_default_adapi/src/utils/types.hpp @@ -17,7 +17,7 @@ #include -namespace default_ad_api +namespace autoware::default_adapi { template @@ -29,6 +29,6 @@ using Cli = typename component_interface_utils::Client::SharedPtr; template using Srv = typename component_interface_utils::Service::SharedPtr; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // UTILS__TYPES_HPP_ diff --git a/system/default_ad_api/src/vehicle.cpp b/system/autoware_default_adapi/src/vehicle.cpp similarity index 98% rename from system/default_ad_api/src/vehicle.cpp rename to system/autoware_default_adapi/src/vehicle.cpp index 053e1258c7bef..5421f382795ef 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/autoware_default_adapi/src/vehicle.cpp @@ -21,7 +21,7 @@ #include -namespace default_ad_api +namespace autoware::default_adapi { using GearReport = vehicle_interface::GearStatus::Message; @@ -189,7 +189,7 @@ void VehicleNode::on_timer() publish_status(); } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::VehicleNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::VehicleNode) diff --git a/system/default_ad_api/src/vehicle.hpp b/system/autoware_default_adapi/src/vehicle.hpp similarity index 98% rename from system/default_ad_api/src/vehicle.hpp rename to system/autoware_default_adapi/src/vehicle.hpp index 56012152a132b..5e3818340b819 100644 --- a/system/default_ad_api/src/vehicle.hpp +++ b/system/autoware_default_adapi/src/vehicle.hpp @@ -30,7 +30,7 @@ // This file should be included after messages. #include "utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class VehicleNode : public rclcpp::Node @@ -80,6 +80,6 @@ class VehicleNode : public rclcpp::Node void on_timer(); }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // VEHICLE_HPP_ diff --git a/system/default_ad_api/src/vehicle_door.cpp b/system/autoware_default_adapi/src/vehicle_door.cpp similarity index 90% rename from system/default_ad_api/src/vehicle_door.cpp rename to system/autoware_default_adapi/src/vehicle_door.cpp index e37e91bdda8e2..e897bc37eff42 100644 --- a/system/default_ad_api/src/vehicle_door.cpp +++ b/system/autoware_default_adapi/src/vehicle_door.cpp @@ -16,7 +16,7 @@ #include "utils/topics.hpp" -namespace default_ad_api +namespace autoware::default_adapi { VehicleDoorNode::VehicleDoorNode(const rclcpp::NodeOptions & options) @@ -36,7 +36,7 @@ void VehicleDoorNode::on_status(vehicle_interface::DoorStatus::Message::ConstSha pub_status_, status_, *msg, utils::ignore_stamp); } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::VehicleDoorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::VehicleDoorNode) diff --git a/system/default_ad_api/src/vehicle_door.hpp b/system/autoware_default_adapi/src/vehicle_door.hpp similarity index 95% rename from system/default_ad_api/src/vehicle_door.hpp rename to system/autoware_default_adapi/src/vehicle_door.hpp index e83e0a164d3f6..8f6c2da83247d 100644 --- a/system/default_ad_api/src/vehicle_door.hpp +++ b/system/autoware_default_adapi/src/vehicle_door.hpp @@ -24,7 +24,7 @@ // This file should be included after messages. #include "utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class VehicleDoorNode : public rclcpp::Node @@ -44,6 +44,6 @@ class VehicleDoorNode : public rclcpp::Node std::optional status_; }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // VEHICLE_DOOR_HPP_ diff --git a/system/default_ad_api/src/vehicle_info.cpp b/system/autoware_default_adapi/src/vehicle_info.cpp similarity index 94% rename from system/default_ad_api/src/vehicle_info.cpp rename to system/autoware_default_adapi/src/vehicle_info.cpp index 8bf7da65f6e33..810290d1e8814 100644 --- a/system/default_ad_api/src/vehicle_info.cpp +++ b/system/autoware_default_adapi/src/vehicle_info.cpp @@ -30,7 +30,7 @@ auto create_point(double x, double y) } // namespace -namespace default_ad_api +namespace autoware::default_adapi { VehicleInfoNode::VehicleInfoNode(const rclcpp::NodeOptions & options) @@ -65,7 +65,7 @@ VehicleInfoNode::VehicleInfoNode(const rclcpp::NodeOptions & options) adaptor.init_srv(srv_dimensions_, on_vehicle_dimensions); } -} // namespace default_ad_api +} // namespace autoware::default_adapi #include -RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::VehicleInfoNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::default_adapi::VehicleInfoNode) diff --git a/system/default_ad_api/src/vehicle_info.hpp b/system/autoware_default_adapi/src/vehicle_info.hpp similarity index 93% rename from system/default_ad_api/src/vehicle_info.hpp rename to system/autoware_default_adapi/src/vehicle_info.hpp index c7171ddf52485..f13f222fc269e 100644 --- a/system/default_ad_api/src/vehicle_info.hpp +++ b/system/autoware_default_adapi/src/vehicle_info.hpp @@ -21,7 +21,7 @@ // This file should be included after messages. #include "utils/types.hpp" -namespace default_ad_api +namespace autoware::default_adapi { class VehicleInfoNode : public rclcpp::Node @@ -34,6 +34,6 @@ class VehicleInfoNode : public rclcpp::Node autoware_adapi_v1_msgs::msg::VehicleDimensions dimensions_; }; -} // namespace default_ad_api +} // namespace autoware::default_adapi #endif // VEHICLE_INFO_HPP_ diff --git a/system/default_ad_api/test/main.test.py b/system/autoware_default_adapi/test/main.test.py similarity index 91% rename from system/default_ad_api/test/main.test.py rename to system/autoware_default_adapi/test/main.test.py index 728f8a3604ada..3f7f7083ac46e 100644 --- a/system/default_ad_api/test/main.test.py +++ b/system/autoware_default_adapi/test/main.test.py @@ -26,7 +26,7 @@ @pytest.mark.launch_test @launch_testing.markers.keep_alive def generate_test_description(): - path = get_package_share_directory("default_ad_api") + "/launch/default_ad_api.launch.py" + path = get_package_share_directory("autoware_default_adapi") + "/launch/default_adapi.launch.py" specification = importlib.util.spec_from_file_location("launch_script", path) launch_script = importlib.util.module_from_spec(specification) specification.loader.exec_module(launch_script) @@ -40,7 +40,7 @@ def generate_test_description(): class TestMain(unittest.TestCase): def test_interface_version(self, launch_service, proc_info, proc_output): - prefix = get_package_share_directory("default_ad_api") + prefix = get_package_share_directory("autoware_default_adapi") target = prefix + "/test/node/interface_version.py" action = launch.actions.ExecuteProcess(cmd=["python3", target]) with launch_testing.tools.launch_process(launch_service, action, proc_info, proc_output): diff --git a/system/default_ad_api/test/node/interface_version.py b/system/autoware_default_adapi/test/node/interface_version.py similarity index 100% rename from system/default_ad_api/test/node/interface_version.py rename to system/autoware_default_adapi/test/node/interface_version.py diff --git a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index b361a0d2725c7..3750fdb77b258 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -266,15 +266,16 @@ bool AccelBrakeMapCalibrator::get_current_pitch_from_tf(double * pitch) bool AccelBrakeMapCalibrator::take_data() { // take data from subscribers - // take actuation data - ActuationStatusStamped::ConstSharedPtr actuation_status_ptr = actuation_status_sub_.takeData(); - ActuationCommandStamped::ConstSharedPtr actuation_cmd_ptr = actuation_cmd_sub_.takeData(); - if (actuation_status_ptr) { + if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::STATUS) { + ActuationStatusStamped::ConstSharedPtr actuation_status_ptr = actuation_status_sub_.takeData(); + if (!actuation_status_ptr) return false; take_actuation_status(actuation_status_ptr); - } else if (actuation_cmd_ptr) { + } + // take actuation data + if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::COMMAND) { + ActuationCommandStamped::ConstSharedPtr actuation_cmd_ptr = actuation_cmd_sub_.takeData(); + if (!actuation_cmd_ptr) return false; take_actuation_command(actuation_cmd_ptr); - } else { - return false; } // take velocity data