Global costmaps taken into account

rotational_recovery/include/rotational_recovery/rotational_recovery.h

@@ -51,6 +51,7 @@ namespace rotational_recovery
       // Memory owned by this object
       // Mutable because footprintCost is not declared const
       mutable base_local_planner::CostmapModel* world_model_;
+      mutable base_local_planner::CostmapModel* local_model_;
 
       geometry_msgs::Twist base_frame_twist_;
 

rotational_recovery/src/rotational_recovery.cpp

@@ -27,6 +27,7 @@ namespace rotational_recovery
   RotationalRecovery::~RotationalRecovery ()
   {
     delete world_model_;
+    delete local_model_;
   }
 
   // Initialize the recovery reading all necessary params
@@ -38,7 +39,8 @@ namespace rotational_recovery
       tf_ = tf;
       local_costmap_ = local_cmap;
       global_costmap_ = global_cmap;
-      world_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap());
+      world_model_ = new base_local_planner::CostmapModel(*global_costmap_->getCostmap());
+      local_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap());
 
       pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 10);
       ros::NodeHandle private_nh("~/" + name);
@@ -101,8 +103,11 @@ namespace rotational_recovery
   // Return the cost of a pose, modified so that -1 does not equal infinity; instead 1e9 does.
   double RotationalRecovery::normalizedPoseCost (const geometry_msgs::Pose2D& pose) const
   {
-    const double c = world_model_->footprintCost(pose.x, pose.y, pose.theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
-    return c < 0 ? 1e9 : c;
+    const double world_c = world_model_->footprintCost(pose.x, pose.y, pose.theta, global_costmap_->getRobotFootprint(), 0.0, 0.0);
+    const double local_c = local_model_->footprintCost(pose.x, pose.y, pose.theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
+    world_c < 0 ? 1e9 : world_c;
+    local_c < 0 ? 1e9 : local_c;
+    return max(world_c, local_c);
   }
 
   double linearSpeed (const geometry_msgs::Twist& twist)

twist_recovery/include/twist_recovery/twist_recovery.h

@@ -78,6 +78,7 @@ namespace twist_recovery
       // Memory owned by this object
       // Mutable because footprintCost is not declared const
       mutable base_local_planner::CostmapModel* world_model_;
+      mutable base_local_planner::CostmapModel* local_model_;
 
       geometry_msgs::Twist base_frame_twist_;
 

twist_recovery/src/twist_recovery.cpp

@@ -57,6 +57,7 @@ namespace twist_recovery
   TwistRecovery::~TwistRecovery ()
   {
     delete world_model_;
+    delete local_model_;
   }
 
   // Initialize the recovery reading all necessary params
@@ -68,7 +69,8 @@ namespace twist_recovery
       tf_ = tf;
       local_costmap_ = local_cmap;
       global_costmap_ = global_cmap;
-      world_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap());
+      world_model_ = new base_local_planner::CostmapModel(*global_costmap_->getCostmap());
+      local_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap());
 
       pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 10);
       ros::NodeHandle private_nh("~/" + name);
@@ -125,8 +127,11 @@ namespace twist_recovery
   // Return the cost of a pose, modified so that -1 does not equal infinity; instead 1e9 does.
   double TwistRecovery::normalizedPoseCost (const geometry_msgs::Pose2D& pose) const
   {
-    const double c = world_model_->footprintCost(pose.x, pose.y, pose.theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
-    return c < 0 ? 1e9 : c;
+    const double world_c = world_model_->footprintCost(pose.x, pose.y, pose.theta, global_costmap_->getRobotFootprint(), 0.0, 0.0);
+    const double local_c = local_model_->footprintCost(pose.x, pose.y, pose.theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
+    world_c < 0 ? 1e9 : world_c;
+    local_c < 0 ? 1e9 : local_c;
+    return max(world_c, local_c);
   }