Implement map interface and occupancy map & adapt height map

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/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 */

se2_planning/include/se2_planning/OccupancyMap.hpp

@@ -0,0 +1,32 @@
+/*
+ * OccupancyMap.hpp
+ *
+ *  Created on: May 16, 2021
+ *      Author: meyerc
+ */
+
+#pragma once
+
+#include "grid_map_core/GridMap.hpp"
+#include "se2_planning/Map.hpp"
+
+namespace se2_planning {
+
+class OccupancyMap : public Map {
+ public:
+  OccupancyMap() = default;
+  ~OccupancyMap() 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& occupancyLayer);
+
+ private:
+  grid_map::GridMap impl_;
+  std::string occupancyLayer_ = "";
+  bool isInitialized_ = false;
+};
+
+} /* namespace se2_planning */

se2_planning/src/Bounds.cpp

@@ -0,0 +1,16 @@
+/*
+ * Bounds.cpp
+ *
+ *  Created on: May 29, 2021
+ *      Author: meyerc
+ */
+
+#include "se2_planning/Bounds.hpp"
+
+#include <cmath>
+
+namespace se2_planning {
+
+Bounds::Bounds(XYstate low, XYstate high) : low_(low), high_(high) {}
+
+} /* namespace se2_planning */

se2_planning/src/HeightMap.cpp

@@ -5,13 +5,6 @@
  *      Author: jelavice
  */
 
-/*
- * HeightMap.hpp
- *
- *  Created on: Feb 17, 2021
- *      Author: jelavice
- */
-
 #include "se2_planning/HeightMap.hpp"
 
 namespace se2_planning {
@@ -20,7 +13,15 @@ bool HeightMap::isInitialized() const {
   return isInitialized_;
 }
 
-double HeightMap::getHeightAt(double x, double y) const {
+void HeightMap::initializeBounds(const Bounds& bounds) {
+  (void)bounds;
+}
+
+Bounds HeightMap::getBounds() const {
+  return Bounds{};
+}
+
+double HeightMap::getValueAt(double x, double y) const {
   return impl_.atPosition(heightLayer_, grid_map::Position(x, y));
 }
 
@@ -30,8 +31,4 @@ void HeightMap::setGridMap(const grid_map::GridMap& gm, const std::string& heigh
   isInitialized_ = true;
 }
 
-const grid_map::GridMap& HeightMap::getGridMap() const {
-  return impl_;
-}
-
 } /* namespace se2_planning*/

se2_planning/src/Map.cpp

@@ -0,0 +1,38 @@
+/*
+ * Map.cpp
+ *
+ *  Created on: May 30, 2021
+ *      Author: meyerc
+ */
+
+#include "se2_planning/Map.hpp"
+
+namespace se2_planning {
+
+bool Map::isInitialized() const {
+  return true;
+}
+
+void Map::initializeBounds(const Bounds& bounds) {
+  (void)bounds;
+  throw std::runtime_error("Planner: initializeBounds() not implemented");
+}
+
+Bounds Map::getBounds() const {
+  throw std::runtime_error("Planner: getBounds() not implemented");
+}
+
+double Map::getValueAt(double x, double y) const {
+  (void)x;
+  (void)y;
+  throw std::runtime_error("Planner: getValueAt() not implemented");
+}
+
+void Map::lock() {
+  mtx_.lock();
+}
+
+void Map::unlock() {
+  mtx_.unlock();
+}
+}  // namespace se2_planning

se2_planning/src/OccupancyMap.cpp

@@ -0,0 +1,40 @@
+/*
+ * OccupancyMap.cpp
+ *
+ *  Created on: May 16, 2021
+ *      Author: meyerc
+ */
+
+#include "se2_planning/OccupancyMap.hpp"
+
+namespace se2_planning {
+
+bool OccupancyMap::isInitialized() const {
+  return isInitialized_;
+}
+
+void OccupancyMap::initializeBounds(const Bounds& bounds) {
+  // TODO(christoph): When grid map is set, bounds are set anyways, do we need a separate init option? Can lead to
+  //  mismatch between bounds and map.
+  (void)bounds;
+}
+
+Bounds OccupancyMap::getBounds() const {
+  const auto position = impl_.getPosition();
+  const auto length = impl_.getLength();
+  const Bounds bounds = {{position.x() - length.x() / 2.0, position.y() - length.y() / 2.0},
+                         {position.x() + length.x() / 2.0, position.y() + length.y() / 2.0}};
+  return bounds;
+}
+
+double OccupancyMap::getValueAt(double x, double y) const {
+  return impl_.atPosition(occupancyLayer_, grid_map::Position(x, y));
+}
+
+void OccupancyMap::setGridMap(const grid_map::GridMap& gm, const std::string& occupancyLayer) {
+  occupancyLayer_ = occupancyLayer;
+  impl_ = gm;
+  isInitialized_ = true;
+}
+
+} /* namespace se2_planning*/