Timed Costmap

base_local_planner/include/base_local_planner/local_planner_util.h

@@ -61,7 +61,7 @@ class LocalPlannerUtil {
   std::string name_;
   std::string global_frame_;
 
-  costmap_2d::Costmap2D* costmap_;
+  costmap_2d::LayeredCostmap* layered_costmap_;
   tf2_ros::Buffer* tf_;
 
 
@@ -86,8 +86,7 @@ class LocalPlannerUtil {
   ~LocalPlannerUtil() {
   }
 
-  void initialize(tf2_ros::Buffer* tf,
-      costmap_2d::Costmap2D* costmap,
+  void initialize(tf2_ros::Buffer* tf, costmap_2d::LayeredCostmap* layered_costmap,
       std::string global_frame);
 
   bool getGoal(geometry_msgs::PoseStamped& goal_pose);
@@ -98,7 +97,9 @@ class LocalPlannerUtil {
 
   bool getLocalPlan(const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan);
 
-  costmap_2d::Costmap2D* getCostmap();
+  costmap_2d::Costmap2D* getCostmap(double t = 0);
+
+  costmap_2d::LayeredCostmap* getLayeredCostmap();
 
   LocalPlannerLimits getCurrentLimits();
 

base_local_planner/include/base_local_planner/obstacle_cost_function.h

@@ -42,6 +42,7 @@
 
 #include <base_local_planner/costmap_model.h>
 #include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/layered_costmap.h>
 
 namespace base_local_planner {
 
@@ -53,8 +54,7 @@ namespace base_local_planner {
 class ObstacleCostFunction : public TrajectoryCostFunction {
 
 public:
-  ObstacleCostFunction(costmap_2d::Costmap2D* costmap);
-  ~ObstacleCostFunction();
+  ObstacleCostFunction(costmap_2d::LayeredCostmap* layered_costmap);
 
   ExePathOutcome prepare(const geometry_msgs::PoseStamped& current_pose);
   double scoreTrajectory(Trajectory &traj);
@@ -72,13 +72,12 @@ class ObstacleCostFunction : public TrajectoryCostFunction {
       const double& y,
       const double& th,
       const std::vector<geometry_msgs::Point>& scaled_footprint,
-      costmap_2d::Costmap2D* costmap,
-      base_local_planner::WorldModel* world_model);
+      costmap_2d::LayeredCostmap* layered_costmap,
+      double t = 0.0);
 
 private:
-  costmap_2d::Costmap2D* costmap_;
+  costmap_2d::LayeredCostmap* layered_costmap_;
   std::vector<geometry_msgs::Point> footprint_spec_;
-  base_local_planner::WorldModel* world_model_;
   double max_trans_vel_;
   bool sum_scores_;
   //footprint scaling with velocity;

base_local_planner/src/local_planner_util.cpp

@@ -43,11 +43,11 @@ namespace base_local_planner {
 
 void LocalPlannerUtil::initialize(
     tf2_ros::Buffer* tf,
-    costmap_2d::Costmap2D* costmap,
+    costmap_2d::LayeredCostmap* layered_costmap,
     std::string global_frame) {
   if(!initialized_) {
     tf_ = tf;
-    costmap_ = costmap;
+    layered_costmap_ = layered_costmap;    
     global_frame_ = global_frame;
     initialized_ = true;
   }
@@ -70,8 +70,12 @@ void LocalPlannerUtil::reconfigureCB(LocalPlannerLimits &config, bool restore_de
   limits_ = LocalPlannerLimits(config);
 }
 
-costmap_2d::Costmap2D* LocalPlannerUtil::getCostmap() {
-  return costmap_;
+costmap_2d::Costmap2D* LocalPlannerUtil::getCostmap(double t) {
+  return layered_costmap_->getCostmap(t);
+}
+
+costmap_2d::LayeredCostmap* LocalPlannerUtil::getLayeredCostmap() {
+  return layered_costmap_;
 }
 
 LocalPlannerLimits LocalPlannerUtil::getCurrentLimits() {
@@ -108,7 +112,7 @@ bool LocalPlannerUtil::getLocalPlan(const geometry_msgs::PoseStamped& global_pos
       *tf_,
       global_plan_,
       global_pose,
-      *costmap_,
+      *layered_costmap_->getCostmap(),
       global_frame_,
       transformed_plan)) {
     ROS_WARN("Could not transform the global plan to the frame of the controller");

base_local_planner/src/obstacle_cost_function.cpp

@@ -44,24 +44,15 @@
 
 namespace base_local_planner {
 
-ObstacleCostFunction::ObstacleCostFunction(costmap_2d::Costmap2D* costmap)
-    : costmap_(costmap), sum_scores_(false), sideward_inflation_scale_(1.0) {
-  if (costmap != NULL) {
-    world_model_ = new base_local_planner::CostmapModel(*costmap_);
-  }
+ObstacleCostFunction::ObstacleCostFunction(costmap_2d::LayeredCostmap* layered_costmap)
+    : layered_costmap_(layered_costmap), sum_scores_(false), sideward_inflation_scale_(1.0) {
 
   ros::NodeHandle pnh("~");
   sideward_inflation_scale_sub_ = pnh.subscribe<std_msgs::Float32>("sideward_inflation_scale", 1, [&](const std_msgs::Float32ConstPtr& msg){
     sideward_inflation_scale_ = msg->data;
   });
 }
 
-ObstacleCostFunction::~ObstacleCostFunction() {
-  if (world_model_ != NULL) {
-    delete world_model_;
-  }
-}
-
 
 void ObstacleCostFunction::setParams(double max_trans_vel, double max_forward_inflation, double max_sideward_inflation, double scaling_speed, bool occdist_use_footprint) {
   // TODO: move this to prepare if possible
@@ -119,7 +110,7 @@ double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
     traj.getPoint(i, px, py, pth);
     double f_cost = footprintCost(px, py, pth,
         scaled_footprint,
-        costmap_, world_model_);
+        layered_costmap_, i * traj.time_delta_); 
 
     if(f_cost < 0){
         return f_cost;
@@ -148,13 +139,20 @@ double ObstacleCostFunction::footprintCost (
     const double& y,
     const double& th,
     const std::vector<geometry_msgs::Point>& scaled_footprint,
-    costmap_2d::Costmap2D* costmap,
-    base_local_planner::WorldModel* world_model) {
+    costmap_2d::LayeredCostmap* layered_costmap,
+    double t) {
 
   //check if the footprint is legal
   // TODO: Cache inscribed radius
-  double footprint_cost = world_model->footprintCost(x, y, th, scaled_footprint);
+  costmap_2d::Costmap2D* costmap = layered_costmap->getCostmap(t);
 
+  if (costmap == NULL) {
+    return -10.0; // What to return ???????????
+  }
+
+  base_local_planner::CostmapModel world_model(*costmap);
+  double footprint_cost = world_model.footprintCost(x, y, th, scaled_footprint);
+
   if (footprint_cost < 0) {
     return -6.0;
   }

costmap_2d/include/costmap_2d/costmap_2d_ros.h

@@ -267,6 +267,8 @@ class Costmap2DROS
   pluginlib::ClassLoader<Layer> plugin_loader_;
   geometry_msgs::PoseStamped old_pose_;
   Costmap2DPublisher* publisher_;
+  Costmap2DPublisher* timed_publisher_;
+  double timestep_, prediction_time_;   
   dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
 
   boost::recursive_mutex configuration_mutex_;

costmap_2d/include/costmap_2d/layer.h

@@ -64,12 +64,53 @@ class Layer
   virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                             double* max_x, double* max_y) {}
 
+  /**
+   * @brief Modified updateBounds() to use time argument t for timed layers. 
+   *        Calls the above function without t by default.
+   */
+  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
+                            double* max_x, double* max_y, double t)
+  {
+    updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
+  }
+
   /**
    * @brief Actually update the underlying costmap, only within the bounds
    *        calculated during UpdateBounds().
    */
   virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {}
 
+  /**
+   * @brief Modified updateCosts() to use time argument t for timed layers.
+   *        Calls the above function without t by default.
+   */
+  virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j, double t)
+  {
+    updateCosts(master_grid, min_i, min_j, max_i, max_j);
+  }
+
+  /**
+   * @brief Returns true for layers that have a timed logic.
+   */
+  bool isTimed() const
+  {
+    return timed_;
+  }
+
+  /**
+   * @brief Returns true for layers that are time dependent but don't have a timed logic yet.
+   *        These layers should not be part of the static costmap but should be painted in the
+   *        first timed costmap (where t = 0). 
+   *        This includes layers like the obstacle or stvl layer, which paint observations that 
+   *        might include dynamic obstacles and are thus timed but should not be painted in 
+   *        every timestep.
+   *        To-Do: Change logic and remove this function (ideally make all layers either timed or not timed)
+   */
+  bool paintOnlyCurrentTime() const
+  {
+    return paint_only_current_time_;
+  }
+
   /** @brief Stop publishers. */
   virtual void deactivate() {}
 
@@ -139,6 +180,7 @@ class Layer
   LayeredCostmap* layered_costmap_;
   bool current_;
   bool enabled_;
+  bool timed_, paint_only_current_time_;
   std::string name_;
   tf2_ros::Buffer *tf_;
 

costmap_2d/include/costmap_2d/layered_costmap.h

@@ -56,9 +56,9 @@ class LayeredCostmap
 {
 public:
   /**
-   * @brief  Constructor for a costmap
+   * @brief  Constructor for a timed costmap
    */
-  LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown);
+  LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown, double prediction_time = 0.0, double timestep = 0.0);
 
   /**
    * @brief  Destructor
@@ -79,19 +79,27 @@ class LayeredCostmap
   void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y,
                  bool size_locked = false);
 
-  void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy)
+  void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy, double t = 0)
   {
-    minx = minx_;
-    miny = miny_;
-    maxx = maxx_;
-    maxy = maxy_;
+    unsigned int i = getTimeIndex(t);
+    minx = timed_bounds_[i].minx;
+    miny = timed_bounds_[i].miny;
+    maxx = timed_bounds_[i].maxx;
+    maxy = timed_bounds_[i].maxy;
   }
 
   bool isCurrent();
 
-  Costmap2D* getCostmap()
+  costmap_2d::Costmap2D* getCostmap(double t = 0.0);
+
+  unsigned int getTimeIndex(double t)
   {
-    return &costmap_;
+    unsigned int i;
+    if (timestep_ <= 0 || prediction_time_ <= 0)
+      i = 0;
+    else
+      i = std::min((int)timed_bounds_.size()-1, (int)round(t/timestep_));
+    return i;
   }
 
   bool isRolling()
@@ -101,7 +109,7 @@ class LayeredCostmap
 
   bool isTrackingUnknown()
   {
-    return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION;
+    return getCostmap()->getDefaultValue() == costmap_2d::NO_INFORMATION;
   }
 
   std::vector<boost::shared_ptr<Layer> >* getPlugins()
@@ -119,12 +127,13 @@ class LayeredCostmap
     return size_locked_;
   }
 
-  void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn)
+  void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn, double t = 0.0)
   {
-    *x0 = bx0_;
-    *xn = bxn_;
-    *y0 = by0_;
-    *yn = byn_;
+    unsigned int i = getTimeIndex(t);
+    *x0 = timed_bounds_[i].bx0;
+    *xn = timed_bounds_[i].bxn;
+    *y0 = timed_bounds_[i].by0;
+    *yn = timed_bounds_[i].byn;
   }
 
   bool isInitialized()
@@ -154,15 +163,63 @@ class LayeredCostmap
    * This is updated by setFootprint(). */
   double getInscribedRadius() { return inscribed_radius_; }
 
+protected:
+  /**
+   * ! Function copied from Costmap2D !
+   * To-Do: Modify function for this use-case
+   * @brief  Copy a region of a source map into a destination map
+   * @param  source_map The source map
+   * @param sm_lower_left_x The lower left x point of the source map to start the copy
+   * @param sm_lower_left_y The lower left y point of the source map to start the copy
+   * @param sm_size_x The x size of the source map
+   * @param  dest_map The destination map
+   * @param dm_lower_left_x The lower left x point of the destination map to start the copy
+   * @param dm_lower_left_y The lower left y point of the destination map to start the copy
+   * @param dm_size_x The x size of the destination map
+   * @param region_size_x The x size of the region to copy
+   * @param region_size_y The y size of the region to copy
+   */
+  template<typename data_type>
+    void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y,
+                       unsigned int sm_size_x, data_type* dest_map, unsigned int dm_lower_left_x,
+                       unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
+                       unsigned int region_size_y)
+    {
+      // we'll first need to compute the starting points for each map
+      data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
+      data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);
+
+      // now, we'll copy the source map into the destination map
+      for (unsigned int i = 0; i < region_size_y; ++i)
+      {
+        memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
+        sm_index += sm_size_x;
+        dm_index += dm_size_x;
+      }
+    }
+
 private:
-  Costmap2D costmap_;
+
+  // Struct to store the bounds for each timed costmaps
+  struct Costmap2DBounds
+  {
+    double minx, miny, maxx, maxy;
+    unsigned int bx0, bxn, by0, byn;
+  };
+
+  std::vector<Costmap2D> timed_costmaps_;
+  std::vector<Costmap2DBounds> timed_bounds_;
+
+  Costmap2D static_costmap_;
   std::string global_frame_;
 
   bool rolling_window_;  /// < @brief Whether or not the costmap should roll with the robot
 
   bool current_;
-  double minx_, miny_, maxx_, maxy_;
-  unsigned int bx0_, bxn_, by0_, byn_;
+
+  double static_minx_, static_miny_, static_maxx_, static_maxy_;  // Bounds for static costmap
+  int static_bx0_, static_bxn_, static_by0_, static_byn_;
+  double timestep_, prediction_time_; 
 
   std::vector<boost::shared_ptr<Layer> > plugins_;
 

costmap_2d/plugins/obstacle_layer.cpp

@@ -69,6 +69,14 @@ void ObstacleLayer::onInitialize()
   ObstacleLayer::matchSize();
   current_ = true;
 
+  // The obstacle layer is considered time dependent as it paints also dynamic obstacles that are 
+  // observed by sensord. However, it does not yet have a timed logic, so we don't want to paint these
+  // Observations in all timed snapshots
+  // To-Do: Add timed logic (--> Dynamic Obstacle Layer)
+  // How to implement this depends on how perception component provides information...
+  timed_ = true;
+  paint_only_current_time_ = true;
+
   global_frame_ = layered_costmap_->getGlobalFrameID();
   double transform_tolerance;
   nh.param("transform_tolerance", transform_tolerance, 0.2);