diff --git a/planning/freespace_planner/CMakeLists.txt b/planning/freespace_planner/CMakeLists.txt new file mode 100644 index 0000000000000..c3f56a7bef844 --- /dev/null +++ b/planning/freespace_planner/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.5) +project(freespace_planner) + +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 -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(freespace_planner_node SHARED + src/freespace_planner/freespace_planner_node.cpp +) + +rclcpp_components_register_node(freespace_planner_node + PLUGIN "freespace_planner::FreespacePlannerNode" + EXECUTABLE freespace_planner +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/planning/freespace_planner/README.md b/planning/freespace_planner/README.md new file mode 100644 index 0000000000000..8502327ada6b2 --- /dev/null +++ b/planning/freespace_planner/README.md @@ -0,0 +1,119 @@ +# The `freespace_planner` + +## freespace_planner_node + +`freespace_planner_node` is a global path planner node that plans trajectory +in the space having static/dynamic obstacles. This node is currently based on +Hybrid A\* search algorithm in `freespace_planning_algorithms` package. +Other algorithms such as rrt\* will be also added and selectable in the future. + +**Note** +Due to the constraint of trajectory following, the output trajectory will be split to include only the single direction path. +In other words, the output trajectory doesn't include both forward and backward trajectories at once. + +### Input topics + +| Name | Type | Description | +| ----------------------- | ---------------------------------- | --------------------------------------------------------- | +| `~input/route` | autoware_auto_planning_msgs::Route | route and goal pose | +| `~input/occupancy_grid` | nav_msgs::OccupancyGrid | costmap, for drivable areas | +| `~input/odometry` | nav_msgs::Odometry | vehicle velocity, for checking whether vehicle is stopped | +| `~input/scenario` | autoware_planning_msgs::Scenario | scenarios to be activated, for node activation | + +### Output topics + +| Name | Type | Description | +| -------------------- | --------------------------------------- | ------------------------------------------ | +| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | +| `is_completed` | bool (implemented as rosparam) | whether all split trajectory are published | + +### Output TFs + +None + +### How to launch + +1. Write your remapping info in `freespace_planner.launch` or add args when executing `roslaunch` +2. `roslaunch freespace_planner freespace_planner.launch` + +### Parameters + +#### Node parameters + +| Parameter | Type | Description | +| ---------------------------- | ------ | ------------------------------------------------------------------------------- | +| `update_rate` | double | timer's update rate | +| `waypoints_velocity` | double | velocity in output trajectory (currently, only constant velocity is supported) | +| `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 | +| `th_course_out_distance_m` | double | threshold distance to check if vehicle is out of course | +| `replan_when_obstacle_found` | bool | whether replanning when obstacle has found on the trajectory | +| `replan_when_course_out` | bool | whether replanning when vehicle is out of course | + +#### Planner common parameters + +| Parameter | Type | Description | +| ------------------------- | ------ | -------------------------------------------------- | +| `planning_algorithms` | string | algorithms used in the node | +| `time_limit` | double | time limit of planning | +| `robot_length` | double | robot length | +| `robot_width` | double | robot width | +| `robot_base2back` | double | distance from robot's back to robot's base_link | +| `minimum_turning_radius` | double | minimum turning radius of robot | +| `theta_size` | double | the number of angle's discretization | +| `lateral_goal_range` | double | goal range of lateral position | +| `longitudinal_goal_range` | double | goal range of longitudinal position | +| `angle_goal_range` | double | goal range of angle | +| `curve_weight` | double | additional cost factor for curve actions | +| `reverse_weight` | double | additional cost factor for reverse actions | +| `obstacle_threshold` | double | threshold for regarding a certain grid as obstacle | + +#### A\* search parameters + +| Parameter | Type | Description | +| --------------------------- | ------ | ------------------------------------------------------- | +| `only_behind_solutions` | bool | whether restricting the solutions to be behind the goal | +| `use_back` | bool | whether using backward trajectory | +| `distance_heuristic_weight` | double | heuristic weight for estimating node's cost | + +### Flowchart + +```plantuml +@startuml +title onTimer +start + +if (all input data are ready?) then (yes) +else (no) + stop +endif + +if (scenario is active?) then (yes) +else (no) + :reset internal data; + stop +endif + +:get current pose; + +if (replan is required?) then (yes) + :reset internal data; + :publish stop trajectory before planning new trajectory; + :plan new trajectory; +else (no) +endif + + +if (vehicle is stopped?) then (yes) + stop +else (no) +endif + +:split trajectory\n(internally managing the state); + +:publish trajectory; + +stop +@enduml +``` diff --git a/planning/freespace_planner/config/freespace_planner.param.yaml b/planning/freespace_planner/config/freespace_planner.param.yaml new file mode 100644 index 0000000000000..0514ae6ceaa14 --- /dev/null +++ b/planning/freespace_planner/config/freespace_planner.param.yaml @@ -0,0 +1,34 @@ +/**: + ros__parameters: + # -- Node Configurations -- + planning_algorithm: "astar" + waypoints_velocity: 5.0 + update_rate: 10.0 + th_arrived_distance_m: 1.0 + th_stopped_time_sec: 1.0 + th_stopped_velocity_mps: 0.01 + th_course_out_distance_m: 1.0 + replan_when_obstacle_found: true + replan_when_course_out: true + + # -- Configurations common to the all planners -- + # base configs + time_limit: 30000.0 + minimum_turning_radius: 9.0 + maximum_turning_radius: 9.0 + turning_radius_size: 1 + # 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 + distance_heuristic_weight: 1.0 diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp new file mode 100644 index 0000000000000..71ce090615a84 --- /dev/null +++ b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp @@ -0,0 +1,150 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Copyright 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 FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ +#define FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace freespace_planner +{ +using autoware_auto_planning_msgs::msg::HADMapRoute; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Scenario; +using freespace_planning_algorithms::AbstractPlanningAlgorithm; +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::PlannerCommonParam; +using geometry_msgs::msg::PoseArray; +using geometry_msgs::msg::PoseStamped; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::OccupancyGrid; +using nav_msgs::msg::Odometry; + +struct NodeParam +{ + std::string planning_algorithm; + double waypoints_velocity; // constant velocity on planned waypoints [km/h] + double update_rate; // replanning and publishing rate [Hz] + double th_arrived_distance_m; + double th_stopped_time_sec; + double th_stopped_velocity_mps; + double th_course_out_distance_m; + bool replan_when_obstacle_found; + bool replan_when_course_out; +}; + +class FreespacePlannerNode : public rclcpp::Node +{ +public: + explicit FreespacePlannerNode(const rclcpp::NodeOptions & node_options); + +private: + // ros + rclcpp::Publisher::SharedPtr trajectory_pub_; + rclcpp::Publisher::SharedPtr debug_pose_array_pub_; + rclcpp::Publisher::SharedPtr debug_partial_pose_array_pub_; + + rclcpp::Subscription::SharedPtr route_sub_; + rclcpp::Subscription::SharedPtr occupancy_grid_sub_; + rclcpp::Subscription::SharedPtr scenario_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + + rclcpp::TimerBase::SharedPtr timer_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // params + NodeParam node_param_; + PlannerCommonParam planner_common_param_; + AstarParam astar_param_; + + // variables + std::unique_ptr algo_; + PoseStamped current_pose_; + PoseStamped goal_pose_; + + Trajectory trajectory_; + Trajectory partial_trajectory_; + std::vector reversing_indices_; + size_t prev_target_index_; + size_t target_index_; + bool is_completed_ = false; + + HADMapRoute::ConstSharedPtr route_; + OccupancyGrid::ConstSharedPtr occupancy_grid_; + Scenario::ConstSharedPtr scenario_; + Odometry::ConstSharedPtr odom_; + + std::deque odom_buffer_; + + // functions used in the constructor + void getPlanningCommonParam(); + void getAstarParam(); + + // functions, callback + void onRoute(const HADMapRoute::ConstSharedPtr msg); + void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); + void onScenario(const Scenario::ConstSharedPtr msg); + void onOdometry(const Odometry::ConstSharedPtr msg); + + void onTimer(); + + void reset(); + bool isPlanRequired(); + void planTrajectory(); + void updateTargetIndex(); + void initializePlanningAlgorithm(); + + TransformStamped getTransform(const std::string & from, const std::string & to); +}; +} // namespace freespace_planner + +#endif // FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ diff --git a/planning/freespace_planner/launch/freespace_planner.launch.xml b/planning/freespace_planner/launch/freespace_planner.launch.xml new file mode 100644 index 0000000000000..b8ffb0b20de6d --- /dev/null +++ b/planning/freespace_planner/launch/freespace_planner.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml new file mode 100644 index 0000000000000..f6d46831657dc --- /dev/null +++ b/planning/freespace_planner/package.xml @@ -0,0 +1,35 @@ + + + + freespace_planner + 0.1.0 + The freespace_planner package + Kenji Miyake + Akihito OHSATO + Apache License 2.0 + Akihito OHSATO + Tomohito ANDO + + ament_cmake_auto + + autoware_auto_planning_msgs + autoware_planning_msgs + autoware_utils + freespace_planning_algorithms + geometry_msgs + nav_msgs + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tf2_ros + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp new file mode 100644 index 0000000000000..54f19c7884875 --- /dev/null +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -0,0 +1,537 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "freespace_planner/freespace_planner_node.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_planning_msgs::msg::Scenario; +using freespace_planning_algorithms::AstarSearch; +using freespace_planning_algorithms::PlannerWaypoint; +using freespace_planning_algorithms::PlannerWaypoints; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseArray; +using geometry_msgs::msg::PoseStamped; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; + +bool isActive(const Scenario::ConstSharedPtr & scenario) +{ + if (!scenario) { + return false; + } + + const auto & s = scenario->activating_scenarios; + if (std::find(std::begin(s), std::end(s), Scenario::PARKING) != std::end(s)) { + return true; + } + + return false; +} + +PoseArray trajectory2PoseArray(const Trajectory & trajectory) +{ + PoseArray pose_array; + pose_array.header = trajectory.header; + + for (const auto & point : trajectory.points) { + pose_array.poses.push_back(point.pose); + } + + return pose_array; +} + +std::vector getReversingIndices(const Trajectory & trajectory) +{ + std::vector indices; + + for (size_t i = 0; i < trajectory.points.size() - 1; ++i) { + if ( + trajectory.points.at(i).longitudinal_velocity_mps * + trajectory.points.at(i + 1).longitudinal_velocity_mps < + 0) { + indices.push_back(i); + } + } + + return indices; +} + +size_t getNextTargetIndex( + const size_t trajectory_size, const std::vector & reversing_indices, + const size_t current_target_index) +{ + if (!reversing_indices.empty()) { + for (const auto reversing_index : reversing_indices) { + if (reversing_index > current_target_index) { + return reversing_index; + } + } + } + + return trajectory_size - 1; +} + +Trajectory getPartialTrajectory( + const Trajectory & trajectory, const size_t start_index, const size_t end_index) +{ + Trajectory partial_trajectory; + partial_trajectory.header = trajectory.header; + partial_trajectory.header.stamp = rclcpp::Clock().now(); + + partial_trajectory.points.reserve(trajectory.points.size()); + for (size_t i = start_index; i <= end_index; ++i) { + partial_trajectory.points.push_back(trajectory.points.at(i)); + } + + // Modify velocity at start/end point + if (partial_trajectory.points.size() >= 2) { + partial_trajectory.points.front().longitudinal_velocity_mps = + partial_trajectory.points.at(1).longitudinal_velocity_mps; + } + if (!partial_trajectory.points.empty()) { + partial_trajectory.points.back().longitudinal_velocity_mps = 0; + } + + return partial_trajectory; +} + +double calcDistance2d(const Trajectory & trajectory, const Pose & pose) +{ + const auto idx = autoware_utils::findNearestIndex(trajectory.points, pose.position); + return autoware_utils::calcDistance2d(trajectory.points.at(idx), pose); +} + +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; +} + +Trajectory createTrajectory( + const PoseStamped & current_pose, const PlannerWaypoints & planner_waypoints, + const double & velocity) +{ + Trajectory trajectory; + trajectory.header = planner_waypoints.header; + + for (const auto & awp : planner_waypoints.waypoints) { + TrajectoryPoint point; + + point.pose = awp.pose.pose; + + point.pose.position.z = current_pose.pose.position.z; // height = const + point.longitudinal_velocity_mps = velocity / 3.6; // velocity = const + + // switch sign by forward/backward + point.longitudinal_velocity_mps = (awp.is_back ? -1 : 1) * point.longitudinal_velocity_mps; + + trajectory.points.push_back(point); + } + + return trajectory; +} + +Trajectory createStopTrajectory(const PoseStamped & current_pose) +{ + PlannerWaypoints waypoints; + PlannerWaypoint waypoint; + + waypoints.header.stamp = rclcpp::Clock().now(); + waypoints.header.frame_id = current_pose.header.frame_id; + waypoint.pose.header = waypoints.header; + waypoint.pose.pose = current_pose.pose; + waypoint.is_back = false; + waypoints.waypoints.push_back(waypoint); + + return createTrajectory(current_pose, waypoints, 0.0); +} + +bool isStopped( + const std::deque & odom_buffer, const double th_stopped_velocity_mps) +{ + for (const auto & odom : odom_buffer) { + if (std::abs(odom->twist.twist.linear.x) > th_stopped_velocity_mps) { + return false; + } + } + return true; +} + +} // namespace + +namespace freespace_planner +{ +FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_options) +: Node("freespace_planner", node_options) +{ + using std::placeholders::_1; + + // NodeParam + { + auto & p = node_param_; + p.planning_algorithm = declare_parameter("planning_algorithm", "astar"); + p.waypoints_velocity = declare_parameter("waypoints_velocity", 5.0); + p.update_rate = declare_parameter("update_rate", 1.0); + p.th_arrived_distance_m = declare_parameter("th_arrived_distance_m", 1.0); + p.th_stopped_time_sec = declare_parameter("th_stopped_time_sec", 1.0); + p.th_stopped_velocity_mps = declare_parameter("th_stopped_velocity_mps", 0.01); + p.th_course_out_distance_m = declare_parameter("th_course_out_distance_m", 3.0); + p.replan_when_obstacle_found = declare_parameter("replan_when_obstacle_found", true); + p.replan_when_course_out = declare_parameter("replan_when_course_out", true); + declare_parameter("is_completed", false); + } + + // Planning + getPlanningCommonParam(); + getAstarParam(); + + // Subscribers + { + route_sub_ = create_subscription( + "~/input/route", 1, std::bind(&FreespacePlannerNode::onRoute, this, _1)); + occupancy_grid_sub_ = create_subscription( + "~/input/occupancy_grid", 1, std::bind(&FreespacePlannerNode::onOccupancyGrid, this, _1)); + scenario_sub_ = create_subscription( + "~/input/scenario", 1, std::bind(&FreespacePlannerNode::onScenario, this, _1)); + odom_sub_ = create_subscription( + "~/input/odometry", 100, std::bind(&FreespacePlannerNode::onOdometry, this, _1)); + } + + // Publishers + { + rclcpp::QoS qos{1}; + qos.transient_local(); // latch + trajectory_pub_ = create_publisher("~/output/trajectory", qos); + debug_pose_array_pub_ = create_publisher("~/debug/pose_array", qos); + debug_partial_pose_array_pub_ = create_publisher("~/debug/partial_pose_array", qos); + } + + // TF + { + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } + + // Timer + { + auto timer_callback = std::bind(&FreespacePlannerNode::onTimer, this); + const auto period = rclcpp::Rate(node_param_.update_rate).period(); + timer_ = std::make_shared>( + get_clock(), period, std::move(timer_callback), get_node_base_interface()->get_context()); + get_node_timers_interface()->add_timer(timer_, nullptr); + } +} + +void FreespacePlannerNode::getPlanningCommonParam() +{ + auto & p = planner_common_param_; + + // base configs + p.time_limit = declare_parameter("time_limit", 5000.0); + + // robot configs + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + p.vehicle_shape.length = vehicle_info.vehicle_length_m; + p.vehicle_shape.width = vehicle_info.vehicle_width_m; + p.vehicle_shape.base2back = vehicle_info.rear_overhang_m; + // TODO(Kenji Miyake): obtain from vehicle_info + p.minimum_turning_radius = declare_parameter("minimum_turning_radius", 0.5); + p.maximum_turning_radius = declare_parameter("maximum_turning_radius", 6.0); + p.turning_radius_size = declare_parameter("turning_radius_size", 11); + 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); + + // search configs + p.theta_size = declare_parameter("theta_size", 48); + p.angle_goal_range = declare_parameter("angle_goal_range", 6.0); + p.curve_weight = declare_parameter("curve_weight", 1.2); + p.reverse_weight = declare_parameter("reverse_weight", 2.00); + p.lateral_goal_range = declare_parameter("lateral_goal_range", 0.5); + p.longitudinal_goal_range = declare_parameter("longitudinal_goal_range", 2.0); + + // costmap configs + p.obstacle_threshold = declare_parameter("obstacle_threshold", 100); +} + +void FreespacePlannerNode::getAstarParam() +{ + auto & p = astar_param_; + p.only_behind_solutions = declare_parameter("astar.only_behind_solutions", false); + p.use_back = declare_parameter("astar.use_back", true); + p.distance_heuristic_weight = declare_parameter("astar.distance_heuristic_weight", 1.0); +} + +void FreespacePlannerNode::onRoute(const HADMapRoute::ConstSharedPtr msg) +{ + route_ = msg; + + goal_pose_.header = msg->header; + goal_pose_.pose = msg->goal_pose; + + reset(); +} + +void FreespacePlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg) +{ + occupancy_grid_ = msg; +} + +void FreespacePlannerNode::onScenario(const Scenario::ConstSharedPtr msg) { scenario_ = msg; } + +void FreespacePlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) +{ + odom_ = msg; + + odom_buffer_.push_back(msg); + + // Delete old data in buffer + while (true) { + const auto time_diff = + rclcpp::Time(msg->header.stamp) - rclcpp::Time(odom_buffer_.front()->header.stamp); + + if (time_diff.seconds() < node_param_.th_stopped_time_sec) { + break; + } + + odom_buffer_.pop_front(); + } +} + +bool FreespacePlannerNode::isPlanRequired() +{ + if (trajectory_.points.empty()) { + return true; + } + + if (node_param_.replan_when_obstacle_found) { + algo_->setMap(*occupancy_grid_); + + const size_t nearest_index_partial = + autoware_utils::findNearestIndex(partial_trajectory_.points, current_pose_.pose.position); + const size_t end_index_partial = partial_trajectory_.points.size() - 1; + + const auto forward_trajectory = + getPartialTrajectory(partial_trajectory_, nearest_index_partial, end_index_partial); + + const bool is_obstacle_found = + algo_->hasObstacleOnTrajectory(trajectory2PoseArray(forward_trajectory)); + if (is_obstacle_found) { + RCLCPP_INFO(get_logger(), "Found obstacle"); + return true; + } + } + + if (node_param_.replan_when_course_out) { + const bool is_course_out = + calcDistance2d(trajectory_, current_pose_.pose) > node_param_.th_course_out_distance_m; + if (is_course_out) { + RCLCPP_INFO(get_logger(), "Course out"); + return true; + } + } + + return false; +} + +void FreespacePlannerNode::updateTargetIndex() +{ + const auto is_near_target = + autoware_utils::calcDistance2d(trajectory_.points.at(target_index_), current_pose_) < + node_param_.th_arrived_distance_m; + + const auto is_stopped = isStopped(odom_buffer_, node_param_.th_stopped_velocity_mps); + + if (is_near_target && is_stopped) { + const auto new_target_index = + getNextTargetIndex(trajectory_.points.size(), reversing_indices_, target_index_); + + if (new_target_index == target_index_) { + // Finished publishing all partial trajectories + is_completed_ = true; + this->set_parameter(rclcpp::Parameter("is_completed", true)); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "Freespace planning completed"); + } else { + // Switch to next partial trajectory + prev_target_index_ = target_index_; + target_index_ = + getNextTargetIndex(trajectory_.points.size(), reversing_indices_, target_index_); + } + } +} + +void FreespacePlannerNode::onTimer() +{ + // Check all inputs are ready + if (!occupancy_grid_ || !route_ || !scenario_ || !odom_) { + return; + } + + if (!isActive(scenario_)) { + reset(); + return; + } + + if (is_completed_) { + return; + } + + // Get current pose + constexpr const char * vehicle_frame = "base_link"; + current_pose_ = + autoware_utils::transform2pose(getTransform(occupancy_grid_->header.frame_id, vehicle_frame)); + if (current_pose_.header.frame_id == "") { + return; + } + + initializePlanningAlgorithm(); + if (isPlanRequired()) { + reset(); + + // Stop before planning new trajectory + const auto stop_trajectory = createStopTrajectory(current_pose_); + trajectory_pub_->publish(stop_trajectory); + debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory)); + debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory)); + + // Plan new trajectory + planTrajectory(); + } + + // StopTrajectory + if (trajectory_.points.size() <= 1) { + return; + } + + // Update partial trajectory + updateTargetIndex(); + partial_trajectory_ = getPartialTrajectory(trajectory_, prev_target_index_, target_index_); + + // Publish messages + trajectory_pub_->publish(partial_trajectory_); + debug_pose_array_pub_->publish(trajectory2PoseArray(trajectory_)); + debug_partial_pose_array_pub_->publish(trajectory2PoseArray(partial_trajectory_)); +} + +void FreespacePlannerNode::planTrajectory() +{ + // Extend robot shape + freespace_planning_algorithms::VehicleShape extended_vehicle_shape = + planner_common_param_.vehicle_shape; + constexpr double margin = 1.0; + extended_vehicle_shape.length += margin; + extended_vehicle_shape.width += margin; + extended_vehicle_shape.base2back += margin / 2; + + // Provide robot shape and map for the planner + algo_->setVehicleShape(extended_vehicle_shape); + algo_->setMap(*occupancy_grid_); + + // Calculate poses in costmap frame + 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( + goal_pose_.pose, getTransform(occupancy_grid_->header.frame_id, goal_pose_.header.frame_id)); + + // execute planning + const rclcpp::Time start = get_clock()->now(); + const bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); + const rclcpp::Time end = get_clock()->now(); + + RCLCPP_INFO(get_logger(), "Freespace planning: %f [s]", (end - start).seconds()); + + if (result) { + RCLCPP_INFO(get_logger(), "Found goal!"); + trajectory_ = + createTrajectory(current_pose_, algo_->getWaypoints(), node_param_.waypoints_velocity); + reversing_indices_ = getReversingIndices(trajectory_); + prev_target_index_ = 0; + target_index_ = + getNextTargetIndex(trajectory_.points.size(), reversing_indices_, prev_target_index_); + + } else { + RCLCPP_INFO(get_logger(), "Can't find goal..."); + reset(); + } +} + +void FreespacePlannerNode::reset() +{ + trajectory_ = Trajectory(); + partial_trajectory_ = Trajectory(); + is_completed_ = false; + this->set_parameter(rclcpp::Parameter("is_completed", false)); +} + +TransformStamped FreespacePlannerNode::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 FreespacePlannerNode::initializePlanningAlgorithm() +{ + if (node_param_.planning_algorithm == "astar") { + algo_.reset(new AstarSearch(planner_common_param_, astar_param_)); + } else { + throw std::runtime_error( + "No such algorithm named " + node_param_.planning_algorithm + " exists."); + } +} +} // namespace freespace_planner + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(freespace_planner::FreespacePlannerNode)