feat: add freespace_planning_algorithms package (autowarefoundation#33)

* Master sync for parking (autowarefoundation#1693) * Reedsshepp distance as a heuristic function in hybrid A star (autowarefoundation#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 <horibe.takamasa@gmail.com> * Update planning/scenario_planning/parking/astar_search/src/astar_search.cpp Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Install reeds-shepp * Apply markdownlint Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Boost collision checking by pre-caching the collision indexes for all yaw angles (autowarefoundation#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 <tomoya.kimura@tier4.jp> * Use inline (autowarefoundation#1572) * Modular planner (autowarefoundation#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 <horibe.takamasa@gmail.com> * fix inline definition Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * rename directory Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * rename function Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Fix bug yaw => index conversion * modify package.xml: fix license, add author Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix license Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Fix bug nearest_index must not be greater than the current target_index_ (autowarefoundation#1571) * Fixbug * Apply clang-format * Compute neareset index in the partial trajectory * extract partial_trajectory from current target trajectory Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add explicit guard Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Cleanup & modify readme freespace planner (autowarefoundation#1607) * apply clang format Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * align indent Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * rename robot_shape -> vehicle_shape Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * rename step -> distance for TODO Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * modify include guard Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix comparison warning Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * update readme Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add namespace on ros-parameters Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * use autoware_utils Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * align indent Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix comparison warning Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Rename files Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Remove boost constants Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Apply lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Add missing geometry2 apis Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Porting test code to ros2 Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Apply lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Install test script Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Change file mode of test script Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix map info data type in test script Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Add namespace and message abbreviation Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix typo Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix for pre-commit Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix tf initialization Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Hirokazu Ishida <38597814+HiroIshida@users.noreply.github.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix compile warnings (autowarefoundation#1852) Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Fix -Wunused-private-field Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Fix -Wunused-lambda-capture Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Fix -Wdelete-non-abstract-non-virtual-dtor Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Fix -Wrange-loop-construct Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Ignore lint error Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix for createQuaternionFromRPY/Yaw (autowarefoundation#2154) * Change formatter to clang-format and black (autowarefoundation#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 <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Port parking planner packages from .Auto (autowarefoundation#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 <horibe.takamasa@gmail.com> * ignore testing Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Port parking modules (autowarefoundation#738) * Port costmap_generator * Port freespace_planner * fix readme Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Hirokazu Ishida <38597814+HiroIshida@users.noreply.github.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
isamu-takagi · Dec 6, 2021 · 679d6bd · 679d6bd
1 parent b319f8a
commit 679d6bd
Show file tree

Hide file tree

Showing 13 changed files with 2,180 additions and 0 deletions.
diff --git a/planning/freespace_planning_algorithms/CMakeLists.txt b/planning/freespace_planning_algorithms/CMakeLists.txt
@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.5)
+project(freespace_planning_algorithms)
+
+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()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(reeds_shepp SHARED
+  src/reeds_shepp.cpp
+)
+
+ament_auto_add_library(freespace_planning_algorithms SHARED
+  src/abstract_algorithm.cpp
+  src/astar_search.cpp
+)
+
+target_link_libraries(freespace_planning_algorithms
+  reeds_shepp
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(freespace_planning_algorithms-test
+    test/src/test_freespace_planning_algorithms.cpp
+  )
+  target_link_libraries(freespace_planning_algorithms-test
+    freespace_planning_algorithms
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+)
+
+install(PROGRAMS
+  test/debug_plot.py
+  DESTINATION lib/${PROJECT_NAME}
+)
diff --git a/planning/freespace_planning_algorithms/README.md b/planning/freespace_planning_algorithms/README.md
@@ -0,0 +1,53 @@
+# `freespace planning algorithms`
+
+## Role
+
+This package is for development of path planning algorithms in free space.
+
+### Implemented algorithms
+
+- Hybrid A\*
+
+## Guide to implement a new algorithm
+
+- All planning algorithm class in this package must inherit `AbstractPlanningAlgorithm`
+  class. If necessary, please overwrite the virtual functions.
+- All algorithms must use `nav_msgs::OccupancyGrid`-typed costmap.
+  Thus, `AbstractPlanningAlgorithm` class mainly implements the collision checking
+  using the costmap, grid-based indexing, and coordinate transformation related to
+  costmap.
+- All algorithms must take both `PlannerCommonParam`-typed and algorithm-specific-
+  type structs as inputs of the constructor. For example, `AstarSearch` class's
+  constructor takes both `PlannerCommonParam` and `AstarParam`.
+
+## Running the standalone tests and visualization
+
+Building the package with ros-test and run tests:
+
+```sh
+colcon build --packages-select freespace_planning_algorithms
+colcon test --packages-select freespace_planning_algorithms
+```
+
+Inside the test, simulation results are stored in `/tmp/result_*.txt`.
+
+Note that the postfix corresponds to the testing scenario (multiple curvatures and single curvature cases).
+Loading these resulting files, by using [test/debug_plot.py](test/debug_plot.py),
+one can create plots visualizing the path and obstacles as shown
+in the figures below.
+
+The created figures are then again saved in `/tmp` with the name like `/tmp/result_multi0.png`.
+
+![sample output figure](figs/freespace_planning_algorithms_astar_test_results.png)
+
+The black cells, green box, and red box, respectively, indicate obstacles,
+start configuration, and goal configuration.
+The sequence of the blue boxes indicate the solution path.
+
+## License notice
+
+Files `src/reeds_shepp.cpp` and `include/astar_search/reeds_shepp.h`
+are fetched from [pyReedsShepp](https://github.com/ghliu/pyReedsShepp).
+Note that the implementation in `pyReedsShepp` is also heavily based on
+the code in [ompl](https://github.com/ompl/ompl).
+Both `pyReedsShepp` and `ompl` are distributed under 3-clause BSD license.
diff --git a/...e_planning_algorithms/figs/freespace_planning_algorithms_astar_test_results.png b/...e_planning_algorithms/figs/freespace_planning_algorithms_astar_test_results.png
diff --git a/planning/freespace_planning_algorithms/figs/result_multi2.png b/planning/freespace_planning_algorithms/figs/result_multi2.png
diff --git a/...reespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp b/...reespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp
@@ -0,0 +1,185 @@
+// Copyright 2021 Tier IV, Inc. 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_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_
+#define FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_
+
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#include <vector>
+
+// TODO(wep21): Remove these apis
+//              after they are implemented in ros2 geometry2.
+namespace tf2
+{
+inline void fromMsg(const geometry_msgs::msg::Point & in, tf2::Vector3 & out)
+{
+  out = tf2::Vector3(in.x, in.y, in.z);
+}
+
+template <>
+inline void doTransform(
+  const geometry_msgs::msg::Pose & t_in, geometry_msgs::msg::Pose & t_out,
+  const geometry_msgs::msg::TransformStamped & transform)
+{
+  tf2::Vector3 v;
+  fromMsg(t_in.position, v);
+  tf2::Quaternion r;
+  fromMsg(t_in.orientation, r);
+
+  tf2::Transform t;
+  fromMsg(transform.transform, t);
+  tf2::Transform v_out = t * tf2::Transform(r, v);
+  toMsg(v_out, t_out);
+}
+}  // namespace tf2
+
+namespace freespace_planning_algorithms
+{
+int discretizeAngle(const double theta, const int theta_size);
+
+struct IndexXYT
+{
+  int x;
+  int y;
+  int theta;
+};
+
+struct IndexXY
+{
+  int x;
+  int y;
+};
+
+IndexXYT pose2index(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local,
+  const int theta_size);
+
+geometry_msgs::msg::Pose index2pose(
+  const nav_msgs::msg::OccupancyGrid & costmap, const IndexXYT & index, const int theta_size);
+
+geometry_msgs::msg::Pose global2local(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_global);
+
+geometry_msgs::msg::Pose local2global(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local);
+
+struct VehicleShape
+{
+  double length;     // X [m]
+  double width;      // Y [m]
+  double base2back;  // base_link to rear [m]
+};
+
+struct PlannerCommonParam
+{
+  // base configs
+  double time_limit;  // planning time limit [msec]
+
+  // robot configs
+  VehicleShape vehicle_shape;
+  double minimum_turning_radius;  // [m]
+  double maximum_turning_radius;  // [m]
+  int turning_radius_size;        // discretized turning radius table size [-]
+
+  // search configs
+  int theta_size;                  // discretized angle table size [-]
+  double curve_weight;             // curve moving cost [-]
+  double reverse_weight;           // backward moving cost [-]
+  double lateral_goal_range;       // reaching threshold, lateral error [m]
+  double longitudinal_goal_range;  // reaching threshold, longitudinal error [m]
+  double angle_goal_range;         // reaching threshold, angle error [deg]
+
+  // costmap configs
+  int obstacle_threshold;  // obstacle threshold on grid [-]
+};
+
+struct PlannerWaypoint
+{
+  geometry_msgs::msg::PoseStamped pose;
+  bool is_back = false;
+};
+
+struct PlannerWaypoints
+{
+  std_msgs::msg::Header header;
+  std::vector<PlannerWaypoint> waypoints;
+};
+
+class AbstractPlanningAlgorithm
+{
+public:
+  explicit AbstractPlanningAlgorithm(const PlannerCommonParam & planner_common_param)
+  : planner_common_param_(planner_common_param)
+  {
+  }
+  virtual void setMap(const nav_msgs::msg::OccupancyGrid & costmap);
+  virtual bool makePlan(
+    const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) = 0;
+  virtual bool hasFeasibleSolution() = 0;  // currently used only in testing
+  void setVehicleShape(const VehicleShape & vehicle_shape)
+  {
+    planner_common_param_.vehicle_shape = vehicle_shape;
+  }
+  bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory);
+  const PlannerWaypoints & getWaypoints() const { return waypoints_; }
+  virtual ~AbstractPlanningAlgorithm() {}
+
+protected:
+  void computeCollisionIndexes(int theta_index, std::vector<IndexXY> & indexes);
+  bool detectCollision(const IndexXYT & base_index);
+  inline bool isOutOfRange(const IndexXYT & index)
+  {
+    if (index.x < 0 || static_cast<int>(costmap_.info.width) <= index.x) {
+      return true;
+    }
+    if (index.y < 0 || static_cast<int>(costmap_.info.height) <= index.y) {
+      return true;
+    }
+    return false;
+  }
+  inline bool isObs(const IndexXYT & index)
+  {
+    // NOTE: Accessing by .at() instead makes 1.2 times slower here.
+    // Also, boundary check is already done in isOutOfRange before calling this function.
+    // So, basically .at() is not necessary.
+    return is_obstacle_table_[index.y][index.x];
+  }
+
+  PlannerCommonParam planner_common_param_;
+
+  // costmap as occupancy grid
+  nav_msgs::msg::OccupancyGrid costmap_;
+
+  // collision indexes cache
+  std::vector<std::vector<IndexXY>> coll_indexes_table_;
+
+  // is_obstacle's table
+  std::vector<std::vector<bool>> is_obstacle_table_;
+
+  // pose in costmap frame
+  geometry_msgs::msg::Pose start_pose_;
+  geometry_msgs::msg::Pose goal_pose_;
+
+  // result path
+  PlannerWaypoints waypoints_;
+};
+
+}  // namespace freespace_planning_algorithms
+
+#endif  // FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_