diff --git a/planning/scenario_selector/CMakeLists.txt b/planning/scenario_selector/CMakeLists.txt new file mode 100644 index 0000000000000..db178e34319c5 --- /dev/null +++ b/planning/scenario_selector/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.5) +project(scenario_selector) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +## Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# Target + +## Target executable +set(SCENARIO_SELECTOR_SRC + src/scenario_selector_node/scenario_selector_node.cpp +) + +## scenario_selector_node +ament_auto_add_library(scenario_selector_node SHARED + ${SCENARIO_SELECTOR_SRC} +) + +rclcpp_components_register_node(scenario_selector_node + PLUGIN "ScenarioSelectorNode" + EXECUTABLE scenario_selector +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/planning/scenario_selector/README.md b/planning/scenario_selector/README.md new file mode 100644 index 0000000000000..3ee4e3c0421db --- /dev/null +++ b/planning/scenario_selector/README.md @@ -0,0 +1,121 @@ +# scenario_selector + +## scenario_selector_node + +`scenario_selector_node` is a node that switches trajectories from each scenario. + +### Input topics + +| Name | Type | Description | +| -------------------------------- | ---------------------------------------- | ----------------------------------------------------- | +| `~input/lane_driving/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory of LaneDriving scenario | +| `~input/parking/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory of Parking scenario | +| `~input/lanelet_map` | autoware_auto_mapping_msgs::HADMapBin | | +| `~input/route` | autoware_auto_planning_msgs::HADMapRoute | route and goal pose | +| `~input/odometry` | nav_msgs::Odometry | for checking whether vehicle is stopped | +| `is_parking_completed` | bool (implemented as rosparam) | whether all split trajectory of Parking are published | + +### Output topics + +| Name | Type | Description | +| -------------------- | --------------------------------------- | ---------------------------------------------- | +| `~output/scenario` | autoware_planning_msgs::Scenario | current scenario and scenarios to be activated | +| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | + +### Output TFs + +None + +### How to launch + +1. Write your remapping info in `scenario_selector.launch` or add args when executing `roslaunch` +2. `roslaunch scenario_selector scenario_selector.launch` + - If you would like to use only a single scenario, `roslaunch scenario_selector dummy_scenario_selector_{scenario_name}.launch` + +### Parameters + +| Parameter | Type | Description | +| -------------------------- | ------ | ------------------------------------------------------------------------------- | +| `update_rate` | double | timer's update rate | +| `th_max_message_delay_sec` | double | threshold time of input messages' maximum delay | +| `th_arrived_distance_m` | double | threshold distance to check if vehicle has arrived at the trajectory's endpoint | +| `th_stopped_time_sec` | double | threshold time to check if vehicle is stopped | +| `th_stopped_velocity_mps` | double | threshold velocity to check if vehicle is stopped | + +### Flowchart + +```plantuml +@startuml +title onTimer +start + +:get current pose; + +if (all input data are ready?) then (yes) +else (no) + stop +endif + +if (scenario is initialized?) then (yes) +else (no) + :initialize scenario; +endif + +:select scenario; + +:publish scenario; + +:extract scenario trajectory; + +if (scenario trajectory is empty?) then (yes) +else (no) + :publish trajectory; +endif + +stop +@enduml +``` + +```plantuml +@startuml +title Scenario Transition +start + +if (current_scenario is completed?\n()) then (yes) +else (no) + stop +endif + +' Empty +if (scenario is initialized?) then (yes) +else (no) + if (is in lane?) then (yes) + :set LaneDriving; + else (no) + :set Parking; + endif + + stop +endif + +' LaneDriving +if (current scenario is LaneDriving?) then (yes) + if (is in parking lot & goal is not in lane?) then (yes) + :set Parking; + stop + endif +endif + +' Parking +if (current scenario is Parking?) then (yes) + if (parking is completed and is in lane?) then (yes) + :set LaneDriving; + stop + endif +endif + +:continue previous scenario; + +stop +@enduml +``` diff --git a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp new file mode 100644 index 0000000000000..95ba6bead9124 --- /dev/null +++ b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp @@ -0,0 +1,95 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_ +#define SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +class ScenarioSelectorNode : public rclcpp::Node +{ +public: + explicit ScenarioSelectorNode(const rclcpp::NodeOptions & node_options); + + void onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onRoute(const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg); + void onOdom(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + + void onTimer(); + void onLaneDrivingTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onParkingTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void publishTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + + void updateCurrentScenario(); + std::string selectScenarioByPosition(); + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr getScenarioTrajectory( + const std::string & scenario); + +private: + rclcpp::TimerBase::SharedPtr timer_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr + sub_lane_driving_trajectory_; + rclcpp::Subscription::SharedPtr + sub_parking_trajectory_; + rclcpp::Publisher::SharedPtr pub_trajectory_; + rclcpp::Publisher::SharedPtr pub_scenario_; + + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr lane_driving_trajectory_; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr parking_trajectory_; + autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr route_; + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_; + + std::string current_scenario_; + std::deque twist_buffer_; + + std::shared_ptr lanelet_map_ptr_; + std::shared_ptr routing_graph_ptr_; + std::shared_ptr traffic_rules_ptr_; + + // Parameters + double update_rate_; + double th_max_message_delay_sec_; + double th_arrived_distance_m_; + double th_stopped_time_sec_; + double th_stopped_velocity_mps_; +}; + +#endif // SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_ diff --git a/planning/scenario_selector/launch/dummy_scenario_selector_lane_driving.launch.xml b/planning/scenario_selector/launch/dummy_scenario_selector_lane_driving.launch.xml new file mode 100644 index 0000000000000..1cd2f23cefc3c --- /dev/null +++ b/planning/scenario_selector/launch/dummy_scenario_selector_lane_driving.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/planning/scenario_selector/launch/dummy_scenario_selector_parking.launch.xml b/planning/scenario_selector/launch/dummy_scenario_selector_parking.launch.xml new file mode 100644 index 0000000000000..b2671afbd55ef --- /dev/null +++ b/planning/scenario_selector/launch/dummy_scenario_selector_parking.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/planning/scenario_selector/launch/scenario_selector.launch.xml b/planning/scenario_selector/launch/scenario_selector.launch.xml new file mode 100644 index 0000000000000..efcf664f6aace --- /dev/null +++ b/planning/scenario_selector/launch/scenario_selector.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/scenario_selector/package.xml b/planning/scenario_selector/package.xml new file mode 100644 index 0000000000000..9f0fc12fda36f --- /dev/null +++ b/planning/scenario_selector/package.xml @@ -0,0 +1,32 @@ + + + + scenario_selector + 0.1.0 + The scenario_selector ROS2 package + Kenji Miyake + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_mapping_msgs + autoware_auto_planning_msgs + autoware_planning_msgs + lanelet2_extension + nav_msgs + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tf2_ros + + ros2cli + topic_tools + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp new file mode 100644 index 0000000000000..404ba966a2483 --- /dev/null +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -0,0 +1,384 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scenario_selector/scenario_selector_node.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace +{ +template +void onData(const T & data, T * buffer) +{ + *buffer = data; +} + +std::shared_ptr findNearestParkinglot( + const std::shared_ptr & lanelet_map_ptr, + const lanelet::BasicPoint2d & search_point) +{ + std::vector> nearest_lanelets = + lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 1); + + if (nearest_lanelets.empty()) { + return {}; + } + + const auto nearest_lanelet = nearest_lanelets.front().second; + const auto all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr); + + const auto linked_parking_lot = std::make_shared(); + const auto result = lanelet::utils::query::getLinkedParkingLot( + nearest_lanelet, all_parking_lots, linked_parking_lot.get()); + + if (result) { + return linked_parking_lot; + } else { + return {}; + } +} + +geometry_msgs::msg::PoseStamped::ConstSharedPtr getCurrentPose( + const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger) +{ + geometry_msgs::msg::TransformStamped tf_current_pose; + + try { + tf_current_pose = tf_buffer.lookupTransform("map", "base_link", tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger, "%s", ex.what()); + return nullptr; + } + + geometry_msgs::msg::PoseStamped::SharedPtr p(new geometry_msgs::msg::PoseStamped()); + p->header = tf_current_pose.header; + p->pose.orientation = tf_current_pose.transform.rotation; + p->pose.position.x = tf_current_pose.transform.translation.x; + p->pose.position.y = tf_current_pose.transform.translation.y; + p->pose.position.z = tf_current_pose.transform.translation.z; + + return geometry_msgs::msg::PoseStamped::ConstSharedPtr(p); +} + +bool isInLane( + const std::shared_ptr & lanelet_map_ptr, + const geometry_msgs::msg::Point & current_pos) +{ + const auto & p = current_pos; + const lanelet::Point3d search_point(lanelet::InvalId, p.x, p.y, p.z); + + std::vector> nearest_lanelets = + lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point.basicPoint2d(), 1); + + if (nearest_lanelets.empty()) { + return false; + } + + const auto nearest_lanelet = nearest_lanelets.front().second; + + return lanelet::geometry::within(search_point, nearest_lanelet.polygon3d()); +} + +bool isInParkingLot( + const std::shared_ptr & lanelet_map_ptr, + const geometry_msgs::msg::Pose & current_pose) +{ + const auto & p = current_pose.position; + const lanelet::Point3d search_point(lanelet::InvalId, p.x, p.y, p.z); + + const auto nearest_parking_lot = + findNearestParkinglot(lanelet_map_ptr, search_point.basicPoint2d()); + + if (!nearest_parking_lot) { + return false; + } + + return lanelet::geometry::within(search_point, nearest_parking_lot->basicPolygon()); +} + +bool isNearTrajectoryEnd( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory, + const geometry_msgs::msg::Pose & current_pose, const double th_dist) +{ + if (!trajectory || trajectory->points.empty()) { + return false; + } + + const auto & p1 = current_pose.position; + const auto & p2 = trajectory->points.back().pose.position; + + const auto dist = std::hypot(p1.x - p2.x, p1.y - p2.y); + + return dist < th_dist; +} + +bool isStopped( + const std::deque & twist_buffer, + const double th_stopped_velocity_mps) +{ + for (const auto & twist : twist_buffer) { + if (std::abs(twist->twist.linear.x) > th_stopped_velocity_mps) { + return false; + } + } + return true; +} + +} // namespace + +autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr +ScenarioSelectorNode::getScenarioTrajectory(const std::string & scenario) +{ + if (scenario == autoware_planning_msgs::msg::Scenario::LANEDRIVING) { + return lane_driving_trajectory_; + } + if (scenario == autoware_planning_msgs::msg::Scenario::PARKING) { + return parking_trajectory_; + } + RCLCPP_ERROR_STREAM(this->get_logger(), "invalid scenario argument: " << scenario); + return lane_driving_trajectory_; +} + +std::string ScenarioSelectorNode::selectScenarioByPosition() +{ + const auto is_in_lane = isInLane(lanelet_map_ptr_, current_pose_->pose.position); + const auto is_goal_in_lane = isInLane(lanelet_map_ptr_, route_->goal_pose.position); + const auto is_in_parking_lot = isInParkingLot(lanelet_map_ptr_, current_pose_->pose); + + if (current_scenario_ == autoware_planning_msgs::msg::Scenario::EMPTY) { + if (is_in_lane && is_goal_in_lane) { + return autoware_planning_msgs::msg::Scenario::LANEDRIVING; + } else if (is_in_parking_lot) { + return autoware_planning_msgs::msg::Scenario::PARKING; + } else { + return autoware_planning_msgs::msg::Scenario::LANEDRIVING; + } + } + + if (current_scenario_ == autoware_planning_msgs::msg::Scenario::LANEDRIVING) { + if (is_in_parking_lot && !is_goal_in_lane) { + return autoware_planning_msgs::msg::Scenario::PARKING; + } + } + + if (current_scenario_ == autoware_planning_msgs::msg::Scenario::PARKING) { + bool is_parking_completed; + this->get_parameter("is_parking_completed", is_parking_completed); + if (is_parking_completed && is_in_lane) { + this->set_parameter(rclcpp::Parameter("is_parking_completed", false)); + return autoware_planning_msgs::msg::Scenario::LANEDRIVING; + } + } + + return current_scenario_; +} + +void ScenarioSelectorNode::updateCurrentScenario() +{ + const auto prev_scenario = current_scenario_; + + const auto scenario_trajectory = getScenarioTrajectory(current_scenario_); + const auto is_near_trajectory_end = + isNearTrajectoryEnd(scenario_trajectory, current_pose_->pose, th_arrived_distance_m_); + + const auto is_stopped = isStopped(twist_buffer_, th_stopped_velocity_mps_); + + if (is_near_trajectory_end && is_stopped) { + current_scenario_ = selectScenarioByPosition(); + } + + if (current_scenario_ != prev_scenario) { + RCLCPP_INFO_STREAM( + this->get_logger(), "scenario changed: " << prev_scenario << " -> " << current_scenario_); + } +} + +void ScenarioSelectorNode::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 ScenarioSelectorNode::onRoute( + const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg) +{ + route_ = msg; + current_scenario_ = autoware_planning_msgs::msg::Scenario::EMPTY; +} + +void ScenarioSelectorNode::onOdom(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + auto twist = std::make_shared(); + twist->header = msg->header; + twist->twist = msg->twist.twist; + + twist_ = twist; + twist_buffer_.push_back(twist); + + // Delete old data in buffer + while (true) { + const auto time_diff = + rclcpp::Time(msg->header.stamp) - rclcpp::Time(twist_buffer_.front()->header.stamp); + + if (time_diff.seconds() < th_stopped_time_sec_) { + break; + } + + twist_buffer_.pop_front(); + } +} + +void ScenarioSelectorNode::onTimer() +{ + current_pose_ = getCurrentPose(tf_buffer_, this->get_logger()); + + // Check all inputs are ready + if (!current_pose_ || !lanelet_map_ptr_ || !route_ || !twist_) { + return; + } + + // Initialize Scenario + if (current_scenario_ == autoware_planning_msgs::msg::Scenario::EMPTY) { + current_scenario_ = selectScenarioByPosition(); + } + + updateCurrentScenario(); + autoware_planning_msgs::msg::Scenario scenario; + scenario.current_scenario = current_scenario_; + + if (current_scenario_ == autoware_planning_msgs::msg::Scenario::PARKING) { + scenario.activating_scenarios.push_back(current_scenario_); + } + + pub_scenario_->publish(scenario); +} + +void ScenarioSelectorNode::onLaneDrivingTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) +{ + lane_driving_trajectory_ = msg; + + if (current_scenario_ != autoware_planning_msgs::msg::Scenario::LANEDRIVING) { + return; + } + + publishTrajectory(msg); +} + +void ScenarioSelectorNode::onParkingTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) +{ + parking_trajectory_ = msg; + + if (current_scenario_ != autoware_planning_msgs::msg::Scenario::PARKING) { + return; + } + + publishTrajectory(msg); +} + +void ScenarioSelectorNode::publishTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) +{ + const auto now = this->now(); + const auto delay_sec = (now - msg->header.stamp).seconds(); + if (delay_sec <= th_max_message_delay_sec_) { + pub_trajectory_->publish(*msg); + } else { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "trajectory is delayed: scenario = %s, delay = %f, th_max_message_delay = %f", + current_scenario_.c_str(), delay_sec, th_max_message_delay_sec_); + } +} + +ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_options) +: Node("scenario_selector", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + current_scenario_(autoware_planning_msgs::msg::Scenario::EMPTY), + update_rate_(this->declare_parameter("update_rate", 10.0)), + th_max_message_delay_sec_(this->declare_parameter("th_max_message_delay_sec", 1.0)), + th_arrived_distance_m_(this->declare_parameter("th_arrived_distance_m", 1.0)), + th_stopped_time_sec_(this->declare_parameter("th_stopped_time_sec", 1.0)), + th_stopped_velocity_mps_(this->declare_parameter("th_stopped_velocity_mps", 0.01)) +{ + // Parameters + this->declare_parameter("is_parking_completed", false); + + // Input + sub_lane_driving_trajectory_ = + this->create_subscription( + "input/lane_driving/trajectory", rclcpp::QoS{1}, + std::bind(&ScenarioSelectorNode::onLaneDrivingTrajectory, this, std::placeholders::_1)); + + sub_parking_trajectory_ = this->create_subscription( + "input/parking/trajectory", rclcpp::QoS{1}, + std::bind(&ScenarioSelectorNode::onParkingTrajectory, this, std::placeholders::_1)); + + sub_lanelet_map_ = this->create_subscription( + "input/lanelet_map", rclcpp::QoS{1}.transient_local(), + std::bind(&ScenarioSelectorNode::onMap, this, std::placeholders::_1)); + sub_route_ = this->create_subscription( + "input/route", rclcpp::QoS{1}, + std::bind(&ScenarioSelectorNode::onRoute, this, std::placeholders::_1)); + sub_odom_ = this->create_subscription( + "input/odometry", rclcpp::QoS{100}, + std::bind(&ScenarioSelectorNode::onOdom, this, std::placeholders::_1)); + + // Output + pub_scenario_ = this->create_publisher( + "output/scenario", rclcpp::QoS{1}); + pub_trajectory_ = this->create_publisher( + "output/trajectory", rclcpp::QoS{1}); + + // Timer Callback + auto timer_callback = std::bind(&ScenarioSelectorNode::onTimer, this); + auto period = std::chrono::duration_cast( + std::chrono::duration(1.0 / static_cast(update_rate_))); + + timer_ = std::make_shared>( + this->get_clock(), period, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); + + // Wait for first tf + while (rclcpp::ok()) { + try { + tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero); + break; + } catch (tf2::TransformException & ex) { + RCLCPP_DEBUG(this->get_logger(), "waiting for initial pose..."); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + } +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ScenarioSelectorNode)