Initial draft of changes for online map / planner updates

car_demo/car_demo/config/reeds_shepp_planner_ros.yaml

@@ -5,17 +5,41 @@ state_space:
   y_lower: -100.0
   y_upper: 100.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: traversability
+  grid_map_state_validity_checking_method: traversability # 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
+  robot_footprint_length_forward:   0.40
+  robot_footprint_length_backward:  0.40
+  robot_footprint_width_left:       0.35
+  robot_footprint_width_right:      0.35
+
 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
+  state_space_bounds_margin: 0.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/include/se2_planning/GridMapLazyStateValidator.hpp

@@ -15,6 +15,8 @@
 
 namespace se2_planning {
 
+enum StateValidityCheckingMethod : int { COLLISION = 0, TRAVERSABILITY = 1, ROBUST_TRAVERSABILITY = 2 };
+
 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,11 +47,11 @@ class GridMapStateValidator : public StateValidator {
   bool isFootprintInitialized_ = false;
   bool isLayerNameInitialized_ = false;
 
-  std::string obstacleLayerName_ = "";
   grid_map::GridMap gridMap_;
   RobotFootprint nominalFootprint_;
+  std::string obstacleLayerName_ = "";
 
- private:
+private:
   mutable RobotFootprint currentFootprint_;
 };
 

se2_planning/include/se2_planning/OmplPlanner.hpp

@@ -24,6 +24,17 @@ class OmplPlanner : public Planner {
   void setStartingState(const State& startingState) final;
   void setGoalState(const State& goalState) final;
   void getPath(Path* path) const final;
+  void getStartingState(State* startingState) const final;
+  void getGoalState(State* goalState) const final;
+
+  void setStateValidator(std::unique_ptr<StateValidator> stateValidator) final;
+  const StateValidator& getStateValidator() const final;
+  void lockStateValidator() final;
+  void unlockStateValidator() final;
+  bool isLocked() const final;
+
+  virtual void setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds);
+  virtual const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const;
 
   void setMaxPlanningDuration(double T);
   void getOmplPath(ompl::geometric::PathGeometric* omplPath) const;
@@ -35,15 +46,17 @@ class OmplPlanner : public Planner {
   void setOmplPlanner(ompl::base::PlannerPtr planner);
 
  protected:
-  virtual void initializeStateSpace() = 0;
-  virtual bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) = 0;
+  virtual bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const = 0;
   virtual ompl::base::ScopedStatePtr convert(const State& state) const = 0;
+  virtual void convert(const ompl::base::ScopedStatePtr omplState, State* state) const = 0;
   virtual void convert(const ompl::geometric::PathGeometric& pathOmpl, Path* path) const = 0;
 
   ompl::base::StateSpacePtr stateSpace_;
+  std::unique_ptr<ompl::base::RealVectorBounds> bounds_;
   ompl::geometric::SimpleSetupPtr simpleSetup_;
   ompl::base::ScopedStatePtr startState_, goalState_;
   std::unique_ptr<ompl::geometric::PathGeometric> path_, interpolatedPath_;
+  std::unique_ptr<StateValidator> stateValidator_;
 
  private:
   double maxPlanningDuration_ = 1.0;

se2_planning/include/se2_planning/OmplReedsSheppPlanner.hpp

@@ -35,10 +35,14 @@ struct ReedsSheppPath : public Path {
 
 struct OmplReedsSheppPlannerParameters {
   double turningRadius_ = 10.0;
+
+  // TODO(christoph): Do we need these parameters? Should be provided by the map. If no map available, no planning
+  //  possible...?
   double xLowerBound_ = -1000.0;
   double xUpperBound_ = 1000.0;
   double yLowerBound_ = -1000.0;
   double yUpperBound_ = 1000.0;
+
   double pathSpatialResolution_ = 0.05;
   double maxPlanningTime_ = 1.0;
   std::string omplPlannerName_ = "RRTstar";
@@ -54,22 +58,20 @@ class OmplReedsSheppPlanner final : public OmplPlanner {
   bool initialize() final;
   bool plan() final;
   void setParameters(const OmplReedsSheppPlannerParameters& parameters);
-  void setStateValidator(std::unique_ptr<StateValidator> stateValidator);
-  const StateValidator& getStateValidator() const;
+  void setStateSpaceBoundaries(const ompl::base::RealVectorBounds& bounds) override;
+  const ompl::base::RealVectorBounds& getStateSpaceBoundaries() const override;
+  bool satisfiesStateSpaceBoundaries(const se2_planning::ReedsSheppState& state) const;
 
  private:
   void createDefaultStateSpace();
-  void initializeStateSpace() final;
-  void setStateSpaceBoundaries();
-  bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) final;
+  bool isStateValid(const ompl::base::SpaceInformation* si, const ompl::base::State* state) const final;
   ompl::base::ScopedStatePtr convert(const State& state) const final;
+  void convert(const ompl::base::ScopedStatePtr omplState, State* state) const final;
   void convert(const ompl::geometric::PathGeometric& pathOmpl, Path* path) const final;
   int getDistanceSignAt(const ompl::geometric::PathGeometric& path, unsigned int id) const;
 
-  std::unique_ptr<ompl::base::RealVectorBounds> bounds_;
   const int reedsSheppStateSpaceDim_ = 2;
   OmplReedsSheppPlannerParameters parameters_;
-  std::unique_ptr<StateValidator> stateValidator_;
 };
 
 std::string toString(ReedsSheppPathSegment::Direction direction);

se2_planning/include/se2_planning/Planner.hpp

@@ -8,6 +8,7 @@
 #pragma once
 
 #include "se2_planning/State.hpp"
+#include "se2_planning/StateValidator.hpp"
 
 #include <boost/concept_check.hpp>
 namespace se2_planning {
@@ -27,6 +28,12 @@ class Planner {
   virtual void getStartingState(State* startingState) const { throw std::runtime_error("Planner: getStartingState() not implemented"); }
   virtual void getGoalState(State* goalState) const { throw std::runtime_error("Planner: getGoalState() not implemented"); }
 
+  virtual void setStateValidator(std::unique_ptr<StateValidator> stateValidator) = 0;
+  virtual const StateValidator& getStateValidator() const { throw std::runtime_error("Planner: getStateValidator() not implemented"); };
+  virtual void lockStateValidator() = 0;
+  virtual void unlockStateValidator() = 0;
+  virtual bool isLocked() const = 0;
+
   template <class T>
   const T* as() const {
     BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));

se2_planning/include/se2_planning/StateValidator.hpp

@@ -17,6 +17,12 @@ class StateValidator {
   virtual bool isStateValid(const State& state) const = 0;
   virtual void initialize();
   virtual bool isInitialized() const;
+  virtual bool isLocked() const;
+  virtual void lock();
+  virtual void unlock();
+
+ private:
+  bool isLocked_ = false;
 };
 
 class SE2stateValidator : public StateValidator {