Initial draft of changes for online map / planner updates

car_demo/car_demo/config/reeds_shepp_planner_ros.yaml

@@ -1,21 +1,46 @@
 
 state_space:
-  x_lower: -100.0
-  x_upper: 100.0
-  y_lower: -100.0
-  y_upper: 100.0
+  boundaries_margin: 3.0
   turning_radius: 10.0
-
+
+state_validator_ros:
+  grid_map_frame: odom
+  grid_map_msg_sub_topic: /se2_grid_map_generator_node/grid_map
+  grid_map_msg_pub_topic: state_validator/grid_map
+  grid_map_obstacle_layer_name: elevation
+  grid_map_state_validity_checking_method: collision # see GridMapLazyStateValidator.hpp for list of available methods
+  grid_map_state_validity_threshold: 0.1
+  grid_map_unsafe_state_validity_threshold: 0.1
+  grid_map_max_number_of_unsafe_cells: 0
+  # grid map dimensions/geometry (only for init, can probably be removed)
+  grid_map_resolution: 0.04
+  grid_map_length: 20.0
+  grid_map_width: 20.0
+  grid_map_position_x: 5.0
+  grid_map_position_y: 0.0
+  grid_map_default_value: 0.0
+  # Robot footprint for collision checking
+  # TODO adapt footprint for car
+  robot_footprint_length_forward:   1.75
+  robot_footprint_length_backward:  1.75
+  robot_footprint_width_left:       1.0
+  robot_footprint_width_right:      1.0
+
+map_ros:
+  layer_name: elevation
+  grid_map_sub_topic: /se2_grid_map_generator_node/grid_map
+  grid_map_pub_topic: /occupancy_map_ros/map_out
+
 planner_ros:
   nav_msgs_path_topic: ompl_rs_planner_ros/nav_msgs_path
   planning_service_name: ompl_rs_planner_ros/planning_service
-  path_msg_topic: ompl_rs_planner_ros/nav_msgs_path
+  path_msg_topic: ompl_rs_planner_ros/path_msgs_path # this is the topic the PriusControllerRos.cpp subscribes to (or any other pure_pursuit_controller)
   path_frame: map
   nav_msg_path_spatial_resolution: 1.5
 
 planner:
   path_spatial_resolution: 0.05
-  ompl_planner: RRTstar # see ompl_planner_creators.hpp for list of available planners
+  ompl_planner: RRTstar # see ompl_planner_creators.hpp for list of available planners, not implemented yet
   max_planning_time: 1.0
 
 ompl_planners:

car_demo/car_demo/launch/demo_autonomous.launch

@@ -28,7 +28,8 @@
 	</include>
 
 	<include file="$(find gazebo_ros)/launch/empty_world.launch">
-		<arg name="verbose" value="true" />
+		<arg name="verbose" value="false" />
+		<arg name="gui" value="false" />
 		<arg name="world_name" value="$(find car_demo)/worlds/empty.world" />
 	</include>
 

car_demo/car_demo/rviz/demo_autonomous.rviz

@@ -1,12 +1,13 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 78
+    Help Height: 85
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
         - /Status1
         - /RobotModel1
+        - /Marker1
       Splitter Ratio: 0.5
     Tree Height: 341
   - Class: rviz/Selection
@@ -567,6 +568,35 @@ Visualization Manager:
       Topic: /se2_planner_node/ompl_rs_planner_ros/nav_msgs_path
       Unreliable: false
       Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /se2_planner_node/state_space
+      Name: Marker
+      Namespaces:
+        state_space: true
+      Queue Size: 100
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Class: grid_map_rviz_plugin/GridMap
+      Color: 200; 200; 200
+      Color Layer: elevation
+      Color Transformer: GridMapLayer
+      Enabled: true
+      Height Layer: elevation
+      Height Transformer: GridMapLayer
+      History Length: 1
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 10
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: GridMap
+      Show Grid Lines: true
+      Topic: /se2_planner_node/state_validator/grid_map
+      Unreliable: false
+      Use Rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 186; 189; 182
@@ -602,18 +632,18 @@ Visualization Manager:
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.24793481826782227
-        Y: 1.0106052160263062
-        Z: -0.06408524513244629
+        X: 4.376317977905273
+        Y: -2.549678325653076
+        Z: 5.444847106933594
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.1503983736038208
+      Pitch: 0.4153985381126404
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.2054100036621094
+      Yaw: 2.095402956008911
     Saved: ~
 Window Geometry:
   Displays:
@@ -634,7 +664,7 @@ Window Geometry:
     collapsed: true
   Width: 1853
   X: 1987
-  Y: 27
+  Y: 34
   back_camera:
     collapsed: false
   front_camera:

car_demo/car_demo/src/PriusControllerRos.cpp

@@ -217,7 +217,7 @@ void PriusControllerRos::initRos()
   controllerCommandService_ = nh_->advertiseService("/prius/controller_command_service",
                                                     &PriusControllerRos::controllerCommandService,
                                                     this);
-  pathSub_ = nh_->subscribe("/se2_planner_node/ompl_rs_planner_ros/path", 1,
+  pathSub_ = nh_->subscribe("/se2_planner_node/ompl_rs_planner_ros/path_msgs_path", 1,
                             &PriusControllerRos::pathCallback, this);
 }
 

se2_grid_map_generator/config/default.yaml

@@ -1,5 +1,5 @@
 map:
-  frame_id:                     odom
+  frame_id:                     map # TODO(christoph): planning breaks if start and goal are not set using map as reference frame in rviz
   layers:                       [elevation, traversability]
   default_values:               [0.0, 1.0]
   resolution:                   0.1

se2_navigation_msgs/msg/PathRequestMsg.msg

@@ -1,2 +1,3 @@
+std_msgs/Header header
 geometry_msgs/Pose startingPose
 geometry_msgs/Pose goalPose

se2_planning/CMakeLists.txt

@@ -6,14 +6,17 @@ add_compile_options(-Wall -Wextra -Wpedantic)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
 set(SRC_FILES
-  src/ompl_planner_creators.cpp
+  src/Bounds.cpp
   src/GridMapLazyStateValidator.cpp
   src/GridMapStateValidator.cpp
+  src/HeightMap.cpp
+  src/Map.cpp
+  src/OccupancyMap.cpp
   src/OmplPlanner.cpp
   src/OmplReedsSheppPlanner.cpp
   src/State.cpp
   src/StateValidator.cpp
-  src/HeightMap.cpp
+  src/ompl_planner_creators.cpp
 )
 
 set(CATKIN_PACKAGE_DEPENDENCIES
@@ -32,7 +35,7 @@ if(NOT EIGEN3_FOUND)
   set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS})
 endif()
 
-find_package(Boost REQUIRED)
+find_package(Boost REQUIRED COMPONENTS thread)
 find_package(ompl REQUIRED)
 
 catkin_package(
@@ -88,8 +91,8 @@ add_executable(planning_benchmark
   src/planning_benchmark.cpp
 )
 target_link_libraries(planning_benchmark
-  ${catkin_LIBRARIES}
   ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
   ${OMPL_LIBRARIES}
   test_helpers
 )

se2_planning/include/se2_planning/Bounds.hpp

@@ -0,0 +1,22 @@
+/*
+ * Bounds.hpp
+ *
+ *  Created on: May 29, 2021
+ *      Author: meyerc
+ */
+
+#pragma once
+#include <boost/concept_check.hpp>
+#include <se2_planning/State.hpp>
+
+namespace se2_planning {
+
+struct Bounds {
+  Bounds() = default;
+  Bounds(XYstate low, XYstate high);
+  ~Bounds() = default;
+  XYstate low_ = {0.0, 0.0};
+  XYstate high_ = {0.0, 0.0};
+};
+
+}  // namespace se2_planning

se2_planning/include/se2_planning/GridMapLazyStateValidator.hpp

@@ -15,6 +15,8 @@
 
 namespace se2_planning {
 
+enum StateValidityCheckingMethod : int { COLLISION = 0, TRAVERSABILITY = 1, TRAVERSABILITY_ITERATOR = 2, ROBUST_TRAVERSABILITY = 3 };
+
 class GridMapLazyStateValidator : public GridMapStateValidator {
   using BASE = GridMapStateValidator;
 
@@ -23,32 +25,56 @@ class GridMapLazyStateValidator : public GridMapStateValidator {
   ~GridMapLazyStateValidator() override = default;
 
   bool isStateValid(const State& state) const final;
-  void initialize() final;
+  void initialize() override;
   bool isInitialized() const final;
 
   void setIsUseRandomizedStrategy(bool value);
   bool getIsUseRandomizedStrategy() const;
 
   void setIsUseEarlyStoppingHeuristic(bool value);
-  bool setIsUseEarlyStoppingHeuristic() const;
+  bool getIsUseEarlyStoppingHeuristic() const;
 
   void setSeed(int value);
   int getSeed() const;
 
+  void setStateValidityCheckingMethod(StateValidityCheckingMethod value);
+  StateValidityCheckingMethod getStateValidityCheckingMethod() const;
+
+  void setStateValidityThreshold(double value);
+  double getStateValidityThreshold() const;
+
+  void setUnsafeStateValidityThreshold(double value);
+  double getUnsafeStateValidityThreshold() const;
+
+  void setMaxNumberOfUnsafeCells(int value);
+  int getMaxNumberOfUnsafeCells() const;
+
  private:
   std::vector<Vertex> nominalFootprintPoints_;
   bool isInitializeCalled_ = false;
   bool isUseRandomizedStrategy_ = false;
   bool isUseEarlyStoppingHeuristic_ = false;
+  StateValidityCheckingMethod stateValidityCheckingMethod_ = StateValidityCheckingMethod::COLLISION;
+  double minStateValidityThreshold_ = 0.5;
+  double unsafeStateValidityThreshold_ = 0.5;
+  int maxNumberOfUnsafeCells_ = 0;
   int seed_ = 0;
 };
 
 void computeFootprintPoints(const grid_map::GridMap& gridMap, const RobotFootprint& footprint, std::vector<Vertex>* footprintPoints);
-bool isInCollision(const SE2state& state, const std::vector<Vertex>& nominalFootprintPoints, const grid_map::GridMap& gridMap,
-                   const std::string& obstacleLayer);
+bool isInCollision(const SE2state& state, const std::vector<Vertex>& footprint, const grid_map::GridMap& gridMap,
+                   const std::string& obstacleLayer, const double collisionThreshold);
+bool isTraversable(const SE2state& state, const std::vector<Vertex>& footprint, const grid_map::GridMap& gridMap,
+                   const std::string& traversabilityLayer, const double traversabilityThreshold);
+bool isTraversableIterator(const SE2state& state, const RobotFootprint& footprint, const grid_map::GridMap& gridMap,
+                           const std::string& traversabilityLayer, const double traversabilityThreshold);
+bool isTraversableRobustIterator(const SE2state& state, const RobotFootprint& footprint, const grid_map::GridMap& gridMap,
+                                 const std::string& traversabilityLayer, const double minTraversabilityThreshold,
+                                 const double unsafeTraversabilityThreshold, const int maxNumberOfUnsafeCells);
 void addExtraPointsForEarlyStopping(const RobotFootprint& footprint, std::vector<Vertex>* points, int seed);
-std::unique_ptr<GridMapLazyStateValidator> createGridMapLazyStateValidator(const grid_map::GridMap& gridMap,
-                                                                           const RobotFootprint& footprint,
-                                                                           const std::string& obstacleLayer);
+std::unique_ptr<GridMapLazyStateValidator> createGridMapLazyStateValidator(
+    const grid_map::GridMap& gridMap, const RobotFootprint& footprint, const std::string& obstacleLayer,
+    const StateValidityCheckingMethod& stateValidityCheckingMethod, const double stateValidityThreshold,
+    const double unsafeStateValidityThreshold, const int maxNumberOfUnsafeCells);
 
 } /* namespace se2_planning */

se2_planning/include/se2_planning/GridMapStateValidator.hpp

@@ -47,9 +47,9 @@ class GridMapStateValidator : public StateValidator {
   bool isFootprintInitialized_ = false;
   bool isLayerNameInitialized_ = false;
 
-  std::string obstacleLayerName_ = "";
   grid_map::GridMap gridMap_;
   RobotFootprint nominalFootprint_;
+  std::string obstacleLayerName_ = "";
 
  private:
   mutable RobotFootprint currentFootprint_;

se2_planning/include/se2_planning/HeightMap.hpp

@@ -7,22 +7,21 @@
 
 #pragma once
 #include "grid_map_core/GridMap.hpp"
+#include "se2_planning/Map.hpp"
 
 namespace se2_planning {
 
-class HeightMap {
+class HeightMap : public Map {
  public:
   HeightMap() = default;
-  virtual ~HeightMap() = default;
-
-  bool isInitialized() const;
-
-  double getHeightAt(double x, double y) const;
+  ~HeightMap() override = default;
 
+  bool isInitialized() const override;
+  void initializeBounds(const Bounds& bounds) override;
+  Bounds getBounds() const override;
+  double getValueAt(double x, double y) const override;
   void setGridMap(const grid_map::GridMap& gm, const std::string& heightLayer);
 
-  const grid_map::GridMap& getGridMap() const;
-
  private:
   grid_map::GridMap impl_;
   std::string heightLayer_ = "";

se2_planning/include/se2_planning/Map.hpp

@@ -0,0 +1,33 @@
+/*
+ * Map.hpp
+ *
+ *  Created on: May 16, 2021
+ *      Author: meyerc
+ */
+
+#pragma once
+
+#include "se2_planning/Bounds.hpp"
+#include "se2_planning/State.hpp"
+
+#include <boost/thread/mutex.hpp>
+
+namespace se2_planning {
+
+class Map {
+ public:
+  Map() = default;
+  virtual ~Map() = default;
+
+  virtual bool isInitialized() const = 0;
+  virtual void initializeBounds(const Bounds& bounds) = 0;
+  virtual Bounds getBounds() const = 0;
+  virtual double getValueAt(double x, double y) const = 0;
+  virtual void lock();
+  virtual void unlock();
+
+ private:
+  boost::mutex mtx_;
+};
+
+} /* namespace se2_planning */