From 94904a532b6691244a852a83be92c98f761cf892 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 6 Dec 2021 16:18:46 +0900 Subject: [PATCH] feat: add freespace_planner package (#35) * release v0.4.0 * remove ROS1 packages temporarily Signed-off-by: mitsudome-r * Revert "remove ROS1 packages temporarily" This reverts commit a78b36113cc2ea65f3fc1c3488a363651b679881. Signed-off-by: mitsudome-r * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r * Rename launch files to launch.xml (#28) * port freespace plannet to ros2 (#41) * port freespace plannet to ros2 Signed-off-by: Takamasa Horibe * fix from review : add buildtool dependence & change function name to camelCase Signed-off-by: Takamasa Horibe * fix duration unit for RCLCPP_*_THROTTLE (#75) Signed-off-by: Takamasa Horibe * Convert calls of Duration to Duration::from_seconds where appropriate (#131) * Rename h files to hpp (#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Adjust copyright notice on 532 out of 699 source files (#143) * Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Enable lints in freespace_planner (#147) * add missing declaration of parameter (#233) Signed-off-by: mitsudome-r * Fix typos in planning modules (#866) (#275) * fix typos in planning * fix corresponding typos in planning * revert fixed typos temporarily due to its impact on launchers Co-authored-by: Kazuki Miyahara * Ros2 fix topic name part1 (#408) * Fix topic name of lane_departure_checker debug Signed-off-by: Takagi, Isamu * Fix topic name of mpc_follower debug Signed-off-by: Takagi, Isamu * Fix topic name of velocity_controller debug Signed-off-by: Takagi, Isamu * Fix topic name of motion_velocity_optimizer debug Signed-off-by: Takagi, Isamu * Fix topic name of lane_change_planner debug Signed-off-by: Takagi, Isamu * Fix topic name of behavior_velocity_planner debug Signed-off-by: Takagi, Isamu * Fix topic name of obstacle_avoidance_planner debug Signed-off-by: Takagi, Isamu * Fix topic name of behavior_velocity_planner Signed-off-by: Takagi, Isamu * Fix topic name of motion_velocity_optimizer Signed-off-by: Takagi, Isamu * Fix topic name of lane_departure_checker Signed-off-by: Takagi, Isamu * Fix topic name of mpc_follower Signed-off-by: Takagi, Isamu * Fix topic name of behavior_velocity_planner Signed-off-by: Takagi, Isamu * Fix topic name of velocity_controller Signed-off-by: Takagi, Isamu * Fix topic name of lane_change_planner Signed-off-by: Takagi, Isamu * Fix topic name of obstacle_avoidance_planner Signed-off-by: Takagi, Isamu * Fix topic name of obstacle_stop_planner Signed-off-by: Takagi, Isamu * Fix topic name of costmap_generator Signed-off-by: Takagi, Isamu * Fix topic name of freespace_planner Signed-off-by: Takagi, Isamu * Fix topic name of surround_obstacle_checker Signed-off-by: Takagi, Isamu * Fix topic name of costmap_generator Signed-off-by: Takagi, Isamu * Fix topic name of emergency_handler Signed-off-by: Takagi, Isamu * Fix lint errors Signed-off-by: Takagi, Isamu * Fix typo Signed-off-by: Takagi, Isamu * add use_sim-time option (#454) * Make planning modules components (#1263) Signed-off-by: wep21 * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * Rename AstarNavi to FreespacePlannerNode (#1322) * Rename AstarNavi Signed-off-by: Kenji Miyake * uncrustify Signed-off-by: Kenji Miyake * Porting freespace planner (#1290) * update HA* for multiple curvatures (#1196) * update HA* for multi-curvature planning Signed-off-by: Takamasa Horibe * restore param to old one Signed-off-by: Takamasa Horibe * cosmetic change Signed-off-by: Takamasa Horibe * Add todo for parameter name change Signed-off-by: Takamasa Horibe * add freespace planner config yaml (#1207) * add freespace planner config yaml * fixed typo * fixed comment * Fix variable names Signed-off-by: Kenji Miyake * Fix lint Signed-off-by: wep21 Co-authored-by: Takamasa Horibe Co-authored-by: RyuYamamoto Co-authored-by: Kenji Miyake Co-authored-by: wep21 * Fix wrong rate in freespace_planner (#1564) Signed-off-by: Kenji Miyake * suppress warnings for declare parameters (#1724) * fix for lanelet2_extension * fix for traffic light ssd fine detector * fix for topic_state_monitor * fix for dummy diag publisher * fix for remote cmd converter * fix for vehicle_info_util * fix for multi object tracker * fix for freespace planner * fix for autoware_error_monitor * add Werror for multi object tracker * fix for multi object tracker * add Werror for liraffic light ssd fine detector * add Werror for topic state monitor * add Werror * add Werror * add Werror * add Werror * fix style * set default arg (#1736) (#1737) * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA * add sort-package-xml hook in pre-commit (#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Master sync for parking (#1693) * Reedsshepp distance as a heuristic function in hybrid A star (#1297) * Use reeds sheep distance * Use average radius * Add standalone toy problem * Cleanup standalone node * Add plotter * Add rostest * Arrange directory * Better test and plot settings * Following PEP & small fix cpp * Avoid repeted heap allocation * Standalone reeds-shepp * Licence notice * Use struct instead of raw array * Update license * Removed comment * Add how to use python visualizer * Remove useless methods * Apply clang-format * Do not fully expose StateXYT * Remove StateXYT->q[3] conversion * Use StateXYZ & remove useless functions * Add license in test * Update planning/scenario_planning/parking/astar_search/include/astar_search/astar_search.h Co-authored-by: Takamasa Horibe * Update planning/scenario_planning/parking/astar_search/src/astar_search.cpp Co-authored-by: Takamasa Horibe * Install reeds-shepp * Apply markdownlint Co-authored-by: Takamasa Horibe Signed-off-by: wep21 * Boost collision checking by pre-caching the collision indexes for all yaw angles (#1298) * Precompute collision index for all theta to accelearte collision * Add test condition that checks solution's feasibility * Fix in response to the review * Update planning/scenario_planning/parking/astar_search/include/astar_search/astar_search.h Co-authored-by: tkimura4 * Use inline (#1572) * Modular planner (#1492) * Update * Check working (NOTE somehow 2x faster than the original... why???) * Split header and impl * AstarWaypoint => PlannerWaypoint * AstarParam => PlannerParam * Change package name : astar_search => planning_algorithms * Add override keyword for readability * Apply clang-format & Add License * Remove useless executable * Rearange some functions and members * Add include guard * Remove unused node status * Add virtual destructor * Rename test names * Removed duplicate transformPose * Do not expose transformPose * Compatible planning_algorithms * Pointer to AbstractAlgorithm * Apply clang-format * Removed needless method declaration * Renamed planning_algorithms => parking_planning_algorithms * Add explicit * Apply clang-format * Split parameter into PlannerCommonParam and AstarParam * Tweak * Remove unused line * Split rosparam into common_param and astar_param * Fix package stuff reflect to the chagne of planning_algorithm pkg name * Change class name * Small fix (complied! and check running on Autoware) * Add rosparam : planning_algorithm * Fix comment and ros_info message * Remove useless ; * Add note * Add namespace * Fix typo * Apply clang-format 10 * Remove array3d * Avoid using std * Rename: parking => freespace * Avoid using namespace hoge * Update readme of freesapce planner * Apply clang-format * Update readme for freesplace_planning_algorithms * [freespace_planning_algorithms] apply aedd8626762121ad7 Signed-off-by: Takamasa Horibe * fix inline definition Signed-off-by: Takamasa Horibe * rename directory Signed-off-by: Takamasa Horibe * rename function Signed-off-by: Takamasa Horibe * Fix bug yaw => index conversion * modify package.xml: fix license, add author Signed-off-by: Takamasa Horibe * fix license Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe * Fix bug nearest_index must not be greater than the current target_index_ (#1571) * Fixbug * Apply clang-format * Compute neareset index in the partial trajectory * extract partial_trajectory from current target trajectory Signed-off-by: Takamasa Horibe * add explicit guard Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe * Cleanup & modify readme freespace planner (#1607) * apply clang format Signed-off-by: Takamasa Horibe * align indent Signed-off-by: Takamasa Horibe * rename robot_shape -> vehicle_shape Signed-off-by: Takamasa Horibe * rename step -> distance for TODO Signed-off-by: Takamasa Horibe * modify include guard Signed-off-by: Takamasa Horibe * fix comparison warning Signed-off-by: Takamasa Horibe * update readme Signed-off-by: Takamasa Horibe * add namespace on ros-parameters Signed-off-by: Takamasa Horibe * use autoware_utils Signed-off-by: Takamasa Horibe * align indent Signed-off-by: Takamasa Horibe * fix comparison warning Signed-off-by: Takamasa Horibe Signed-off-by: wep21 * Rename files Signed-off-by: wep21 * Remove boost constants Signed-off-by: wep21 * Apply lint Signed-off-by: wep21 * Add missing geometry2 apis Signed-off-by: wep21 * Porting test code to ros2 Signed-off-by: wep21 * Apply lint Signed-off-by: wep21 * Install test script Signed-off-by: wep21 * Change file mode of test script Signed-off-by: wep21 * Fix map info data type in test script Signed-off-by: wep21 * Add namespace and message abbreviation Signed-off-by: wep21 * Fix typo Signed-off-by: wep21 * Fix lint Signed-off-by: wep21 * Fix for pre-commit Signed-off-by: Kenji Miyake * Fix tf initialization Signed-off-by: wep21 Co-authored-by: Hirokazu Ishida <38597814+HiroIshida@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 Co-authored-by: Kenji Miyake * set default arg (#1736) * Fix package.xml (#2056) Signed-off-by: Kenji Miyake * Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * rename topic name twist -> odometry (#568) Co-authored-by: Takayuki Murooka * Port parking planner packages from .Auto (#600) * Copy code of 'vehicle_constants_manager' * Fix vehicle_constants_manager for ROS galactic * Rm .iv costmap_generator freespace_planner freespace_planning_aglorihtms * Add astar_search (from .Auto) * Copy freespace_planner from .Auto * Update freespace_planner for .IV * Copy costmap_generator from .Auto * Copy and update had_map_utils from .Auto * Update costmap_generator * Copy costmap_generator_nodes * Update costmap_generator_nodes * Comment out all tests * Move vehicle_constant_managers to tmp_autoware_auto_dependencies * ignore pre-commit for back-ported packages Signed-off-by: Takamasa Horibe * ignore testing Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe * Port parking modules (#738) * Port costmap_generator * Port freespace_planner * fix readme * ci(pre-commit): autofix * misc Co-authored-by: mitsudome-r Co-authored-by: Nikolai Morin Co-authored-by: Takamasa Horibe Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: RyuYamamoto Co-authored-by: Kenji Miyake Co-authored-by: wep21 Co-authored-by: Hiroki OTA Co-authored-by: Hirokazu Ishida <38597814+HiroIshida@users.noreply.github.com> Co-authored-by: Takayuki Murooka Co-authored-by: Takayuki Murooka Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Fumiya Watanabe Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/freespace_planner/CMakeLists.txt | 34 ++ planning/freespace_planner/README.md | 119 ++++ .../config/freespace_planner.param.yaml | 34 ++ .../freespace_planner_node.hpp | 150 +++++ .../launch/freespace_planner.launch.xml | 20 + planning/freespace_planner/package.xml | 35 ++ .../freespace_planner_node.cpp | 537 ++++++++++++++++++ 7 files changed, 929 insertions(+) create mode 100644 planning/freespace_planner/CMakeLists.txt create mode 100644 planning/freespace_planner/README.md create mode 100644 planning/freespace_planner/config/freespace_planner.param.yaml create mode 100644 planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp create mode 100644 planning/freespace_planner/launch/freespace_planner.launch.xml create mode 100644 planning/freespace_planner/package.xml create mode 100644 planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp 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)