diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 06d57bd947af3..c8bd0e96c1730 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -20,6 +20,7 @@ qtbase5-dev rclcpp rviz_common + std_srvs tier4_control_msgs tier4_external_api_msgs tier4_planning_msgs diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index aa1dcfcd1651d..ab81cdb6eadcb 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -61,6 +61,7 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa auto * velocity_limit_layout = new QHBoxLayout(); v_layout->addWidget(makeOperationModeGroup()); v_layout->addWidget(makeControlModeGroup()); + v_layout->addWidget(makeAutoParkModeGroup()); { auto * h_layout = new QHBoxLayout(); h_layout->addWidget(makeRoutingGroup()); @@ -137,6 +138,25 @@ QGroupBox * AutowareStatePanel::makeControlModeGroup() return group; } +QGroupBox * AutowareStatePanel::makeAutoParkModeGroup() +{ + auto * group = new QGroupBox("AutoParking"); + auto * grid = new QGridLayout; + + park_mode_label_ptr_ = new QLabel("INIT"); + park_mode_label_ptr_->setAlignment(Qt::AlignCenter); + park_mode_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(park_mode_label_ptr_, 0, 0); + + p_enable_button_ptr_ = new QPushButton("Enable"); + p_enable_button_ptr_->setCheckable(true); + connect(p_enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickEngageAutoPark())); + grid->addWidget(p_enable_button_ptr_, 0, 1); + + group->setLayout(grid); + return group; +} + QGroupBox * AutowareStatePanel::makeRoutingGroup() { auto * group = new QGroupBox("Routing"); @@ -222,6 +242,9 @@ void AutowareStatePanel::onInitialize() sub_operation_mode_ = raw_node_->create_subscription( "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onOperationMode, this, _1)); + sub_auto_park_status_ = raw_node_->create_subscription( + "/planning/auto_parking/status", rclcpp::QoS{1}.transient_local(), + std::bind(&AutowareStatePanel::onAutoParkStatus, this, _1)); client_change_to_autonomous_ = raw_node_->create_client("/api/operation_mode/change_to_autonomous"); @@ -241,6 +264,9 @@ void AutowareStatePanel::onInitialize() client_enable_direct_control_ = raw_node_->create_client("/api/operation_mode/disable_autoware_control"); + client_auto_park_ = + raw_node_->create_client("/planning/auto_parking/set_status"); + // Routing sub_route_ = raw_node_->create_subscription( "/api/routing/state", rclcpp::QoS{1}.transient_local(), @@ -352,6 +378,27 @@ void AutowareStatePanel::onOperationMode(const OperationModeState::ConstSharedPt changeButtonState(disable_button_ptr_, msg->is_autoware_control_enabled); } +void AutowareStatePanel::onAutoParkStatus(const std_msgs::msg::Bool::ConstSharedPtr msg) +{ + auto_park_running_ = msg->data; + auto changeButtonState = [this]( + QPushButton * button, const bool is_desired_mode_available, + const uint8_t current_mode = OperationModeState::UNKNOWN, + const uint8_t desired_mode = OperationModeState::STOP) { + if (is_desired_mode_available && current_mode != desired_mode) { + activateButton(button); + } else { + deactivateButton(button); + } + }; + if (auto_park_running_) { + updateLabel(park_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green + } else { + updateLabel(park_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow + } + changeButtonState(p_enable_button_ptr_, !auto_park_running_); +} + void AutowareStatePanel::onRoute(const RouteState::ConstSharedPtr msg) { QString text = ""; @@ -571,6 +618,9 @@ void AutowareStatePanel::onClickAutonomous() void AutowareStatePanel::onClickStop() { callServiceWithoutResponse(client_change_to_stop_); + if (auto_park_running_) { + onClickDisengageAutoPark(); + } } void AutowareStatePanel::onClickLocal() { @@ -584,6 +634,30 @@ void AutowareStatePanel::onClickAutowareControl() { callServiceWithoutResponse(client_enable_autoware_control_); } +void AutowareStatePanel::onClickEngageAutoPark() +{ + auto req = std::make_shared(); + req->data = true; + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); + if (!client_auto_park_->service_is_ready()) { + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); + return; + } + client_auto_park_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); +} +void AutowareStatePanel::onClickDisengageAutoPark() +{ + auto req = std::make_shared(); + req->data = false; + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); + if (!client_auto_park_->service_is_ready()) { + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); + return; + } + client_auto_park_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); +} void AutowareStatePanel::onClickDirectControl() { callServiceWithoutResponse(client_enable_direct_control_); diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index 8f67a90215bd1..5f28a39c1cd64 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -36,6 +36,8 @@ #include #include #include +#include +#include #include #include #include @@ -69,6 +71,8 @@ public Q_SLOTS: // NOLINT for Qt void onClickLocal(); void onClickRemote(); void onClickAutowareControl(); + void onClickEngageAutoPark(); + void onClickDisengageAutoPark(); void onClickDirectControl(); void onClickClearRoute(); void onClickInitByGnss(); @@ -80,6 +84,7 @@ public Q_SLOTS: // NOLINT for Qt // Layout QGroupBox * makeOperationModeGroup(); QGroupBox * makeControlModeGroup(); + QGroupBox * makeAutoParkModeGroup(); QGroupBox * makeRoutingGroup(); QGroupBox * makeLocalizationGroup(); QGroupBox * makeMotionGroup(); @@ -106,6 +111,7 @@ public Q_SLOTS: // NOLINT for Qt QPushButton * remote_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_operation_mode_; + rclcpp::Subscription::SharedPtr sub_auto_park_status_; rclcpp::Client::SharedPtr client_change_to_autonomous_; rclcpp::Client::SharedPtr client_change_to_stop_; rclcpp::Client::SharedPtr client_change_to_local_; @@ -118,6 +124,13 @@ public Q_SLOTS: // NOLINT for Qt rclcpp::Client::SharedPtr client_enable_autoware_control_; rclcpp::Client::SharedPtr client_enable_direct_control_; + //// Auto Parking Mode + QLabel * park_mode_label_ptr_{nullptr}; + QPushButton * p_enable_button_ptr_{nullptr}; + rclcpp::Client::SharedPtr client_auto_park_; + bool auto_park_running_; + void onAutoParkStatus(const std_msgs::msg::Bool::ConstSharedPtr msg); + //// Functions void onOperationMode(const OperationModeState::ConstSharedPtr msg); void changeOperationMode(const rclcpp::Client::SharedPtr client); diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml index 98315919b540a..df656f0ac5d2a 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -31,7 +31,27 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/planning/auto_parking/CMakeLists.txt b/planning/auto_parking/CMakeLists.txt new file mode 100644 index 0000000000000..7937a424b2a8c --- /dev/null +++ b/planning/auto_parking/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(auto_parking) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(auto_parking_node SHARED + src/auto_parking/auto_parking_node.cpp +) + +rclcpp_components_register_node(auto_parking_node + PLUGIN "auto_parking::AutoParkingNode" + EXECUTABLE auto_parking) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/planning/auto_parking/README.md b/planning/auto_parking/README.md new file mode 100644 index 0000000000000..50a73fad4fd25 --- /dev/null +++ b/planning/auto_parking/README.md @@ -0,0 +1,108 @@ +# auto_parking + +## auto_parking_node + +`auto_parking_node` is a node that enables automated valet parking by publishing appropriate goal poses towards an empty parking space in the nearest parking lot. + +The auto parking is done in the following two phases. + +Phase 1: +Depending on initial pose when the auto parking feature is engaged, a goal pose is set at the exit of the nearest parking lot. This pose is found by first getting nearest parking lot, the parking spaces inside and lanelets inside the parking lot from the lanelet map. By ordering the lanelets based on routing distance from current position, the exit lanelet is found. Ideally the auto_parking feature should be engaged from outside parking lot so the ordering is proper. The ego vehicle then drives to parking lot exit lanelets, passing through the parking lot using either LANEDRIVING or PARKING scenario based on start pose. + +Phase 2: +Once inside the parking lot, while driving to exit lanelet goal, the node searches for a free parking space using the astar algorithm. Once a free space is found a new goal pose is published and trajectory is generated by the freespace_planner as scenario is PARKING. + +### Input topics + +| Name | Type | Description | +| ------------------------- | --------------------------------------- | ------------------------------------- | +| `~/input/engage` | autoware_auto_vehicle_msgs::msg::Engage | status of autoware AUTONOMOUS | +| `~/input/lanelet_map_bin` | autoware_auto_mapping_msgs::HADMapBin | vector map, for drivable areas | +| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid from costmap generator | +| `~input/odometry` | nav_msgs::Odometry | for getting current pose | + +### Output topics + +| Name | Type | Description | +| ------------------------ | ------------------------------- | -------------------------------------- | +| `~/output/fixed_goal` | geometry_msgs::msg::PoseStamped | current goal published by auto_parking | +| `~/output/active_status` | std_msgs::msg::Bool | active status of auto_park node | + +### Service + +| Name | Type | Description | +| ---------------------- | ---------------------- | ------------------------- | +| `~/service/set_active` | std_srvs::srv::SetBool | set active status of node | + +### Client + +| Name | Type | Description | +| ------------------ | ------------------------------------ | -------------------------------- | +| `~/service/engage` | tier4_external_api_msgs::srv::Engage | client to engage AUTONOMOUS mode | + +### How to launch + +1. Write your remapping info in `auto-parking.launch.xml` or add args when executing `roslaunch` +2. Launch node using `ros2 launch auto-parking auto-parking.launch.xml` +3. Call service to start auto parking after placing ego on map `ros2 service call /planning/auto_parking/set_status std_srvs/srv/SetBool "{data: True}"` +4. To stop/reset call with False arg `ros2 service call /planning/auto_parking/set_status std_srvs/srv/SetBool "{data: False}"` + +### Parameters + +{{ json_to_markdown("schema/auto_parking.schema.json") }} + +### Flowchart + +```plantuml +@startuml +title onTimer +start + +if (active?) then (yes) +else (no) + stop +endif + +if (all input data are ready?) then (yes) +else (no) + :set active false; + stop +endif + +:get current pose; + +if (parking lot goal not set and set AutoParking init?) then (yes): + :publish parking lot goal; +else (no) + stop +endif + +if (parking lot goal set and Arrived?) then (yes): + :set active false; + stop +endif + +if (parking space goal not set and inside ParkingLot?) then (yes): + :findParkingSapce; + :publish parking space goal; +endif + +if (parking space goal set and inside ParkingLot?) then (yes): + :check if parking space goal still valid; + if (not valid) then (yes): + :publish parking lot goal; + :continue search; + endif +endif + +if (parking space goal set and Arrived?) then (yes): + stop +endif + +if (autonomous not engaged?) then (yes): + :engage autonomous; +endif + +stop +@enduml +``` diff --git a/planning/auto_parking/config/auto_parking.param.yaml b/planning/auto_parking/config/auto_parking.param.yaml new file mode 100644 index 0000000000000..c2487b84ea2ac --- /dev/null +++ b/planning/auto_parking/config/auto_parking.param.yaml @@ -0,0 +1,31 @@ +/**: + ros__parameters: + # -- Auto Park Node Configurations -- + th_arrived_distance_m: 1.0 + th_parking_space_distance_m: 10.0 + update_rate: 5.0 + vehicle_shape_margin_m: 0.2 + + # -- Configurations common to the all planners -- + # base configs + time_limit: 30000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 9.0 + turning_radius_size: 3 + # search configs + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 2.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + obstacle_threshold: 100 + + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: true + use_curve_weight: false + use_complete_astar: true + distance_heuristic_weight: 1.0 diff --git a/planning/auto_parking/include/auto_parking/auto_parking_node.hpp b/planning/auto_parking/include/auto_parking/auto_parking_node.hpp new file mode 100644 index 0000000000000..3ef929d3b6c99 --- /dev/null +++ b/planning/auto_parking/include/auto_parking/auto_parking_node.hpp @@ -0,0 +1,157 @@ +// 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. + +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef AUTO_PARKING__AUTO_PARKING_NODE_HPP_ +#define AUTO_PARKING__AUTO_PARKING_NODE_HPP_ + +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using Pose = geometry_msgs::msg::Pose; +using PoseStamped = geometry_msgs::msg::PoseStamped; +using geometry_msgs::msg::TransformStamped; + +using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; +using EngageSrv = tier4_external_api_msgs::srv::Engage; + +using freespace_planning_algorithms::AbstractPlanningAlgorithm; +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::AstarSearch; +using freespace_planning_algorithms::PlannerCommonParam; +using freespace_planning_algorithms::VehicleShape; +using nav_msgs::msg::OccupancyGrid; + +namespace auto_parking +{ + +struct AutoParkParam +{ + double th_arrived_distance_m; // threshold to check arrival at goal 1.0 + double th_parking_space_distance_m; // search for parking spaces within this radius 10 + double update_rate; // Timer() update rate 2 + double vehicle_shape_margin_m; // margin to add to vehicle dimensions for astar collision check, + // collision margin 0.2 +}; + +class AutoParkingNode : public rclcpp::Node +{ +public: + explicit AutoParkingNode(const rclcpp::NodeOptions & node_options); + + void onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); + void goalPublisher(const PoseStamped msg); + void reset(); + void onTimer(); + void filterGoalPoseinParkingLot(const lanelet::ConstLineString3d center_line, Pose & goal); + bool findParkingSpace(); + + bool isInParkingLot(); + bool initAutoParking(); + bool isArrived(const Pose & goal); + + void onEngage(EngageMsg::ConstSharedPtr msg); + void engageAutonomous(); + void onSetActiveStatus( + const std_srvs::srv::SetBool::Request::SharedPtr req, + std_srvs::srv::SetBool::Response::SharedPtr res); + + // functions used in the fpa constructor + PlannerCommonParam getPlannerCommonParam(); + TransformStamped getTransform(const std::string & from, const std::string & to); + +private: + // ros + rclcpp::TimerBase::SharedPtr timer_; + nav_msgs::msg::Odometry::ConstSharedPtr odom_; + + rclcpp::Publisher::SharedPtr goal_pose_pub_; + rclcpp::Publisher::SharedPtr active_status_pub_; + + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr lanelet_map_sub_; + rclcpp::Subscription::SharedPtr engage_sub_; + rclcpp::Subscription::SharedPtr occupancy_grid_sub_; + + rclcpp::Client::SharedPtr client_engage_; + + rclcpp::Service::SharedPtr srv_set_active_; + + std::shared_ptr lanelet_map_ptr_; + std::shared_ptr nearest_parking_lot_; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; + lanelet::routing::RoutingGraphPtr routing_graph_ptr_; + lanelet::ConstLineStrings3d nearest_parking_spaces_; + lanelet::ConstLanelets parking_lot_lanelets_; + OccupancyGrid::ConstSharedPtr occupancy_grid_; + + // fpa algo vars + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::unique_ptr algo_; + + // params + AutoParkParam node_param_; + VehicleShape vehicle_shape_; + + std::unique_ptr logger_configure_; + + PoseStamped current_goal_; + PoseStamped current_pose_; + lanelet::ConstLanelet current_lanelet_; + std::unique_ptr stop_checker_; + Pose parking_goal_; + bool set_parking_lot_goal_; + bool set_parking_space_goal_; + bool active_; + bool is_engaged_; +}; +} // namespace auto_parking +#endif // AUTO_PARKING__AUTO_PARKING_NODE_HPP_ diff --git a/planning/auto_parking/launch/auto_parking.launch.xml b/planning/auto_parking/launch/auto_parking.launch.xml new file mode 100644 index 0000000000000..6a1f266e58234 --- /dev/null +++ b/planning/auto_parking/launch/auto_parking.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/auto_parking/package.xml b/planning/auto_parking/package.xml new file mode 100644 index 0000000000000..01d5bdb2b746d --- /dev/null +++ b/planning/auto_parking/package.xml @@ -0,0 +1,33 @@ + + + + auto_parking + 0.0.0 + The auto_parking package + Aswin Vijay + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_auto_vehicle_msgs + freespace_planning_algorithms + geometry_msgs + lanelet2_extension + motion_utils + nav_msgs + rclcpp + rclcpp_components + std_srvs + tf2 + tf2_ros + tier4_autoware_utils + tier4_external_api_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/auto_parking/schema/auto_parking.schema.json b/planning/auto_parking/schema/auto_parking.schema.json new file mode 100644 index 0000000000000..020ebc58adfcd --- /dev/null +++ b/planning/auto_parking/schema/auto_parking.schema.json @@ -0,0 +1,54 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Auto Parking Node", + "type": "object", + "definitions": { + "auto_parking": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "description": "timer's update rate", + "default": "5.0", + "minimum": 0.0 + }, + "th_arrived_distance_m": { + "type": "number", + "description": "threshold distance to check if vehicle has arrived at the trajectory's endpoint", + "default": "1.0", + "minimum": 0.0 + }, + "th_parking_space_distance_m": { + "type": "number", + "description": "threshold radius within which to search parking spaces", + "default": "10.0", + "minimum": 0.0 + }, + "vehicle_shape_margin_m": { + "type": "number", + "description": "vehcile margin for astar search", + "default": "0.2", + "minimum": 0.0 + } + }, + "required": [ + "update_rate", + "th_arrived_distance_m", + "th_parking_space_distance_m", + "vehicle_shape_margin_m" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/auto_parking" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/planning/auto_parking/src/auto_parking/auto_parking_node.cpp b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp new file mode 100644 index 0000000000000..d1dcb23bcdc66 --- /dev/null +++ b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp @@ -0,0 +1,548 @@ +// 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. + +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "auto_parking/auto_parking_node.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +using lanelet::utils::getId; +using lanelet::utils::to2D; +using Pose = geometry_msgs::msg::Pose; +using PoseStamped = geometry_msgs::msg::PoseStamped; + +namespace +{ +// get nearest parking lot by distance +std::shared_ptr filterNearestParkinglot( + const std::shared_ptr & lanelet_map_ptr, + const lanelet::BasicPoint2d & current_position) +{ + const auto all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr); + const auto linked_parking_lot = std::make_shared(); + *linked_parking_lot = *std::min_element( + all_parking_lots.begin(), all_parking_lots.end(), + [&](const lanelet::ConstPolygon3d & p1, const lanelet::ConstPolygon3d & p2) { + const double dist_p1 = boost::geometry::distance(current_position, to2D(p1).basicPolygon()); + const double dist_p2 = boost::geometry::distance(current_position, to2D(p2).basicPolygon()); + return dist_p1 < dist_p2; + }); + if (linked_parking_lot) { + return linked_parking_lot; + } else { + return {}; + } +} + +Pose transformPose(const Pose & pose, const TransformStamped & transform) +{ + PoseStamped transformed_pose; + PoseStamped orig_pose; + orig_pose.pose = pose; + tf2::doTransform(orig_pose, transformed_pose, transform); + + return transformed_pose.pose; +} +} // namespace + +namespace auto_parking +{ +void AutoParkingNode::goalPublisher(const PoseStamped msg) +{ + goal_pose_pub_->publish(msg); +} + +TransformStamped AutoParkingNode::getTransform(const std::string & from, const std::string & to) +{ + TransformStamped tf; + try { + tf = + tf_buffer_->lookupTransform(from, to, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + } catch (const tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + } + return tf; +} + +void AutoParkingNode::onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +{ + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); +} + +void AutoParkingNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + odom_ = msg; + + geometry_msgs::msg::TwistStamped twist; + twist.header = msg->header; + twist.twist = msg->twist.twist; + stop_checker_->addTwist(twist); +} + +void AutoParkingNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg) +{ + occupancy_grid_ = msg; +} + +void AutoParkingNode::onSetActiveStatus( + const std_srvs::srv::SetBool::Request::SharedPtr req, + std_srvs::srv::SetBool::Response::SharedPtr res) +{ + if (req->data) { + reset(); + active_ = true; + } else { + active_ = false; + } + res->success = true; + return; +} + +PlannerCommonParam AutoParkingNode::getPlannerCommonParam() +{ + PlannerCommonParam p; + + // search configs + p.time_limit = declare_parameter("time_limit"); + p.minimum_turning_radius = declare_parameter("minimum_turning_radius"); + p.maximum_turning_radius = declare_parameter("maximum_turning_radius"); + p.turning_radius_size = declare_parameter("turning_radius_size"); + p.maximum_turning_radius = std::max(p.maximum_turning_radius, p.minimum_turning_radius); + p.turning_radius_size = std::max(p.turning_radius_size, 1); + + p.theta_size = declare_parameter("theta_size"); + p.angle_goal_range = declare_parameter("angle_goal_range"); + p.curve_weight = declare_parameter("curve_weight"); + p.reverse_weight = declare_parameter("reverse_weight"); + p.lateral_goal_range = declare_parameter("lateral_goal_range"); + p.longitudinal_goal_range = declare_parameter("longitudinal_goal_range"); + + // costmap configs + p.obstacle_threshold = declare_parameter("obstacle_threshold"); + + return p; +} + +bool AutoParkingNode::isArrived(const Pose & goal) +{ + // Check distance to current goal + if ( + node_param_.th_arrived_distance_m < + tier4_autoware_utils::calcDistance2d(current_pose_.pose, goal)) { + return false; + } + return true; +} + +void AutoParkingNode::filterGoalPoseinParkingLot( + const lanelet::ConstLineString3d center_line, Pose & goal) +{ + for (size_t i = 0; i < center_line.size(); i++) { + const lanelet::Point3d search_point( + lanelet::InvalId, center_line[i].x(), center_line[i].y(), 0.0); + if (lanelet::geometry::within(search_point, nearest_parking_lot_->basicPolygon())) { + goal.position.x = center_line[i].x(); + goal.position.y = center_line[i].y(); + goal.position.z = 0.0; + const auto yaw = std::atan2( + center_line[i + 1].y() - center_line[i].y(), center_line[i + 1].x() - center_line[i].x()); + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + goal.orientation = tf2::toMsg(q); + return; + } + } +} + +bool AutoParkingNode::isInParkingLot() +{ + const auto & p = current_pose_.pose.position; + const lanelet::Point3d search_point(lanelet::InvalId, p.x, p.y, p.z); + if (lanelet::geometry::within(search_point, nearest_parking_lot_->basicPolygon())) { + return true; + } + return false; +} + +bool AutoParkingNode::initAutoParking() +{ + const auto & p = current_pose_.pose.position; + const lanelet::Point3d search_point(lanelet::InvalId, p.x, p.y, p.z); + + nearest_parking_lot_ = filterNearestParkinglot(lanelet_map_ptr_, search_point.basicPoint2d()); + if (!nearest_parking_lot_) { + RCLCPP_INFO(get_logger(), "No parking lot found!!"); + return false; + } + + const auto & all_parking_spaces_ = lanelet::utils::query::getAllParkingSpaces(lanelet_map_ptr_); + + nearest_parking_spaces_ = + lanelet::utils::query::getLinkedParkingSpaces(*nearest_parking_lot_, all_parking_spaces_); + + const auto & all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); + const auto & all_road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + + parking_lot_lanelets_ = + lanelet::utils::query::getLinkedLanelets(*nearest_parking_lot_, all_road_lanelets); + + // Get closest lanelet to ego-vehicle + if (!lanelet::utils::query::getClosestLanelet( + all_road_lanelets, current_pose_.pose, ¤t_lanelet_)) { + RCLCPP_INFO(get_logger(), "No close lanelet to vehicle found!!"); + return false; + } + + // Order parking lot lanelets by route length + // - Sort lanelets + std::sort( + parking_lot_lanelets_.begin(), parking_lot_lanelets_.end(), + [&](const lanelet::ConstLanelet & l1, const lanelet::ConstLanelet & l2) { + const auto l1_length = + routing_graph_ptr_->shortestPath(current_lanelet_, l1, {}, false)->size(); + const auto l2_length = + routing_graph_ptr_->shortestPath(current_lanelet_, l2, {}, false)->size(); + return (l1_length < l2_length); + }); + + // Set parking lot goal, goal at exit of parking lot + // Assuming parking lot has one entry, longest route is from + // entry to exit. + lanelet::ConstLineString3d exit_llt_cline = + parking_lot_lanelets_.back().centerline(); // final lanelet from sorted lanelets + filterGoalPoseinParkingLot(exit_llt_cline, parking_goal_); + + // print out sortet lanelet ID's for DEBUG. + // for (const auto & lanelet : parking_lot_lanelets_) { + // // check if parking space is close to lanelet + // RCLCPP_INFO(get_logger(),"LaneId: %li \n", getId(lanelet)); + // } + + return true; +} + +bool AutoParkingNode::findParkingSpace() +{ + const auto & p = current_pose_.pose.position; + const lanelet::Point2d search_point(lanelet::InvalId, p.x, p.y); + + // Set occupancy map and current pose + algo_->setMap(*occupancy_grid_); + const auto current_pose_in_costmap_frame = transformPose( + current_pose_.pose, + getTransform(occupancy_grid_->header.frame_id, current_pose_.header.frame_id)); + + // Cycle through parking spaces + for (const auto & parking_space : nearest_parking_spaces_) { + const double dist = + boost::geometry::distance(to2D(parking_space).basicLineString(), search_point); + // Check if parking space is nearby + if (node_param_.th_parking_space_distance_m < dist) { + continue; + } + + // Compute two goal poses for each parking space + double distance_thresh = + boost::geometry::distance( + parking_space.front().basicPoint(), parking_space.back().basicPoint()) / + 4; + lanelet::ConstPoints3d p_space_fw_bk; + p_space_fw_bk.push_back(parking_space.back()); + p_space_fw_bk.push_back(parking_space.front()); + + // Compute parking space poses, orientations i = 0 , 1 + // Check if freespace trajectory available then set parking space goal + // Check back to front pose first then opposite + + for (auto i = 0; i < 2; i++) { + auto in1 = i % 2; + auto in2 = (i + 1) % 2; + + Eigen::Vector3d direction = p_space_fw_bk[in2].basicPoint() - p_space_fw_bk[in1].basicPoint(); + direction.normalize(); + + // Set pose + const Eigen::Vector3d goal_pose = + p_space_fw_bk[in1].basicPoint() + direction * distance_thresh; + const auto yaw = std::atan2(direction.y(), direction.x()); + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + + Pose goal; + goal.position.x = goal_pose.x(); + goal.position.y = goal_pose.y(); + goal.position.z = goal_pose.z(); + goal.orientation = tf2::toMsg(q); + + current_goal_.pose = goal; + current_goal_.header.frame_id = odom_->header.frame_id; + current_goal_.header.stamp = rclcpp::Time(); + + const auto goal_pose_in_costmap_frame = transformPose( + current_goal_.pose, + getTransform(occupancy_grid_->header.frame_id, current_goal_.header.frame_id)); + bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); + + if (result) { + RCLCPP_INFO(get_logger(), "Auto-parking: Found free parking space!"); + return true; + } + + continue; + } + } + + // No free parking spaces found + return false; +} + +void AutoParkingNode::onEngage(EngageMsg::ConstSharedPtr msg) +{ + is_engaged_ = msg->engage; +} + +void AutoParkingNode::engageAutonomous() +{ + // engage + auto req = std::make_shared(); + req->engage = true; + RCLCPP_INFO(get_logger(), "Auto-parking: auto-engage client request"); + if (!client_engage_->service_is_ready()) { + RCLCPP_INFO(get_logger(), "Auto-parking: auto-engage client is unavailable"); + return; + } + client_engage_->async_send_request( + req, []([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); +} + +void AutoParkingNode::reset() +{ + set_parking_lot_goal_ = false; + set_parking_space_goal_ = false; + active_ = false; +} + +void AutoParkingNode::onTimer() +{ + // Publish active status message + std_msgs::msg::Bool is_active_msg; + is_active_msg.data = active_; + active_status_pub_->publish(is_active_msg); + + if (!active_) { + return; + } + + current_pose_.pose = odom_->pose.pose; + current_pose_.header = odom_->header; + + // Check all inputs are ready + if ( + !odom_ || !lanelet_map_ptr_ || !routing_graph_ptr_ || !engage_sub_ || !client_engage_ || + current_pose_.header.frame_id == "") { + active_ = false; + return; + } + + // Publish parking lot entrance goal + if (!set_parking_lot_goal_) { + // Initialize variables + if (!initAutoParking()) { + RCLCPP_INFO(get_logger(), "Auto-parking: Initialization failed!"); + active_ = false; + return; + } + current_goal_.header.frame_id = odom_->header.frame_id; + current_goal_.header.stamp = rclcpp::Time(); + current_goal_.pose = parking_goal_; + goalPublisher(current_goal_); + RCLCPP_INFO(get_logger(), "Auto-parking: Publishing parking lot goal"); + set_parking_lot_goal_ = true; + } + + // Arrived at parking lot exit without finding parking space + if (set_parking_lot_goal_ && isArrived(parking_goal_)) { + RCLCPP_INFO(get_logger(), "Auto-parking: Failed to find parking space"); + active_ = false; + return; + } + + if (isInParkingLot() && occupancy_grid_) { + // Search parking spaces if inside parking lot + if (!set_parking_space_goal_) { + RCLCPP_INFO_THROTTLE(get_logger(), *this->get_clock(), 2000, "Auto-parking: Searching..."); + if (findParkingSpace()) { + goalPublisher(current_goal_); + RCLCPP_INFO(get_logger(), "Auto-parking: Publishing parking space goal"); + set_parking_space_goal_ = true; + } + } + // if parking space goal set + // check if astar goal is still valid, else replan + else { + // Set occupancy map and current pose + algo_->setMap(*occupancy_grid_); + const auto current_pose_in_costmap_frame = transformPose( + current_pose_.pose, + getTransform(occupancy_grid_->header.frame_id, current_pose_.header.frame_id)); + const auto goal_pose_in_costmap_frame = transformPose( + current_goal_.pose, + getTransform(occupancy_grid_->header.frame_id, current_goal_.header.frame_id)); + bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); + if (!result) { + set_parking_space_goal_ = false; + current_goal_.pose = parking_goal_; + goalPublisher(current_goal_); + return; + } + } + } + + // Arrived at parking space goal + if (set_parking_space_goal_ && isArrived(current_goal_.pose)) { + RCLCPP_INFO(get_logger(), "Auto-parking: Complete!"); + active_ = false; + return; + } + + // If ego stopped/stuck replan: TODO + // const auto is_vehicle_stopped = stop_checker_->isVehicleStopped(1.0); + + // Engage autonomous once a goal is set + // For smooth transition remove vehicle stopping after + // publishing goal pose in freespace planner node + if (!is_engaged_ && !isArrived(current_goal_.pose)) { + engageAutonomous(); + } +} + +AutoParkingNode::AutoParkingNode(const rclcpp::NodeOptions & node_options) +: Node("auto_parking", node_options) +{ + // Auto-park params + { + auto & p = node_param_; + p.th_arrived_distance_m = declare_parameter("th_arrived_distance_m"); + p.th_parking_space_distance_m = declare_parameter("th_parking_space_distance_m"); + p.update_rate = declare_parameter("update_rate"); + p.vehicle_shape_margin_m = declare_parameter("vehicle_shape_margin_m"); + } + + // set vehicle_info for astar + { + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_shape_.length = vehicle_info.vehicle_length_m + node_param_.vehicle_shape_margin_m; + vehicle_shape_.width = vehicle_info.vehicle_width_m + node_param_.vehicle_shape_margin_m; + vehicle_shape_.base2back = + vehicle_info.rear_overhang_m + node_param_.vehicle_shape_margin_m / 2; + } + + // Subscribers + { + lanelet_map_sub_ = this->create_subscription( + "~/input/lanelet_map_bin", rclcpp::QoS{10}.transient_local(), + std::bind(&AutoParkingNode::onMap, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + "~/input/odometry", rclcpp::QoS{100}, + std::bind(&AutoParkingNode::onOdometry, this, std::placeholders::_1)); + + engage_sub_ = create_subscription( + "~/input/engage", rclcpp::QoS{5}, + std::bind(&AutoParkingNode::onEngage, this, std::placeholders::_1)); + + occupancy_grid_sub_ = create_subscription( + "~/input/occupancy_grid", 10, + std::bind(&AutoParkingNode::onOccupancyGrid, this, std::placeholders::_1)); + } + + // Publishers + { + rclcpp::QoS qos{1}; + qos.transient_local(); // latch + goal_pose_pub_ = this->create_publisher("~/output/fixed_goal", qos); + active_status_pub_ = this->create_publisher("~/output/active_status", qos); + } + + // Service + { + srv_set_active_ = create_service( + "~/service/set_active", + std::bind( + &AutoParkingNode::onSetActiveStatus, this, std::placeholders::_1, std::placeholders::_2)); + } + + // Client + { + client_engage_ = this->create_client("~/service/engage"); + } + + // TF + { + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } + + // Timer + { + const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&AutoParkingNode::onTimer, this)); + } + + // Initialize Freespace planning algorithm - astar + { + const auto planner_common_param = getPlannerCommonParam(); + algo_ = std::make_unique(planner_common_param, vehicle_shape_, *this); + } + + reset(); + stop_checker_ = std::make_unique(this, 1.0 + 1.0); + logger_configure_ = std::make_unique(this); +} +} // namespace auto_parking + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(auto_parking::AutoParkingNode)