feat: add freespace_planner package

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
+)

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
+```

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

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 <freespace_planning_algorithms/astar_search.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
+#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_planning_msgs/msg/scenario.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <deque>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+
+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<Trajectory>::SharedPtr trajectory_pub_;
+  rclcpp::Publisher<PoseArray>::SharedPtr debug_pose_array_pub_;
+  rclcpp::Publisher<PoseArray>::SharedPtr debug_partial_pose_array_pub_;
+
+  rclcpp::Subscription<HADMapRoute>::SharedPtr route_sub_;
+  rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_sub_;
+  rclcpp::Subscription<Scenario>::SharedPtr scenario_sub_;
+  rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  // params
+  NodeParam node_param_;
+  PlannerCommonParam planner_common_param_;
+  AstarParam astar_param_;
+
+  // variables
+  std::unique_ptr<AbstractPlanningAlgorithm> algo_;
+  PoseStamped current_pose_;
+  PoseStamped goal_pose_;
+
+  Trajectory trajectory_;
+  Trajectory partial_trajectory_;
+  std::vector<size_t> 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<Odometry::ConstSharedPtr> 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_

planning/freespace_planner/launch/freespace_planner.launch.xml

@@ -0,0 +1,20 @@
+<launch>
+  <arg name="input_route" default="input_route"/>
+  <arg name="input_occupancy_grid" default="input_occupancy_grid"/>
+  <arg name="input_scenario" default="input_scenario"/>
+  <arg name="input_odometry" default="/localization/kinematic_state" />
+  <arg name="output_trajectory" default="output_trajectory"/>
+  <arg name="is_completed" default="is_completed"/>
+
+  <arg name="param_file" default="$(find-pkg-share freespace_planner)/config/freespace_planner.param.yaml"/>
+
+  <node pkg="freespace_planner" exec="freespace_planner" name="freespace_planner" output="screen">
+    <remap from="~/input/route" to="$(var input_route)" />
+    <remap from="~/input/occupancy_grid" to="$(var input_occupancy_grid)" />
+    <remap from="~/input/scenario" to="$(var input_scenario)" />
+    <remap from="~/input/odometry" to="$(var input_odometry)" />
+    <remap from="~/output/trajectory" to="$(var output_trajectory)" />
+    <remap from="is_completed" to="$(var is_completed)" />
+    <param from="$(var param_file)"/>
+  </node>
+</launch>

planning/freespace_planner/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>freespace_planner</name>
+  <version>0.1.0</version>
+  <description>The freespace_planner package</description>
+  <maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
+  <maintainer email="aohsato@gmail.com">Akihito OHSATO</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="aohsato@gmail.com">Akihito OHSATO</author>
+  <author email="antm678@ertl.jp">Tomohito ANDO</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>freespace_planning_algorithms</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>vehicle_info_util</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>