Taken into account that the footprint does not get out of the costmap to avoid...

Taken into account that the footprint does not get out of the costmap to avoid problems with the obstacle cost function computation.

include/ackermann_planner_util.h

@@ -45,6 +45,7 @@
 #include <costmap_2d/costmap_2d.h>
 #include <tf2_ros/transform_listener.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <base_local_planner/costmap_model.h>
 
 #include <iri_ackermann_local_planner/AckermannLocalPlannerConfig.h>
 /**
@@ -59,6 +60,7 @@ class AckermannPlannerUtil
     std::string global_frame_;
 
     costmap_2d::Costmap2D* costmap_;
+    base_local_planner::CostmapModel *world_model_;
     tf2_ros::Buffer *tf_;
 
     std::vector<geometry_msgs::PoseStamped> global_plan_;
@@ -96,6 +98,8 @@ class AckermannPlannerUtil
 
     costmap_2d::Costmap2D* get_costmap();
 
+    base_local_planner::CostmapModel *get_world_model();
+
     std::string get_global_frame(void);
 
     ~AckermannPlannerUtil();

src/ackermann_planner.cpp

@@ -251,7 +251,7 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
 
   Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf::getYaw(global_pose.pose.orientation));
   Eigen::Vector3f ack_state(ackermann.speed,ackermann.steering_angle,ackermann.steering_angle_velocity);
-  while(!planner_util_->get_costmap()->worldToMap(global_plan_[path_index].pose.position.x,global_plan_[path_index].pose.position.y, cell_x, cell_y) && path_index>0)
+  while(planner_util_->get_world_model()->footprintCost(global_plan_[path_index].pose.position.x,global_plan_[path_index].pose.position.y,tf::getYaw(global_plan_[path_index].pose.orientation),footprint_spec)<0 && path_index>0)
   {
     if(path_index>0)
       path_index--;

src/ackermann_planner_ros.cpp

@@ -179,9 +179,7 @@ bool AckermannPlannerROS::ackermann_compute_velocity_commands(geometry_msgs::Pos
   drive_cmds.header.frame_id = costmap_ros_->getBaseFrameID();
 
   // call with updated footprint
-  std::vector< geometry_msgs::Point > robot_footprint;
-  costmap_ros_->getOrientedFootprint(robot_footprint);
-  base_local_planner::Trajectory path = dp_->find_best_path(global_pose, ackermann, drive_cmds, robot_footprint);
+  base_local_planner::Trajectory path = dp_->find_best_path(global_pose, ackermann, drive_cmds, costmap_ros_->getRobotFootprint());
   //ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);
 
   //pass along drive commands

src/ackermann_planner_util.cpp

@@ -51,6 +51,7 @@ void AckermannPlannerUtil::initialize(tf2_ros::Buffer* tf, costmap_2d::Costmap2D
   {
     tf_ = tf;
     costmap_ = costmap;
+    this->world_model_= new base_local_planner::CostmapModel(*costmap_);
     global_frame_ = global_frame;
     initialized_ = true;
   }
@@ -80,6 +81,11 @@ costmap_2d::Costmap2D* AckermannPlannerUtil::get_costmap()
   return costmap_;
 }
 
+base_local_planner::CostmapModel *AckermannPlannerUtil::get_world_model()
+{
+  return world_model_;
+}
+
 std::string AckermannPlannerUtil::get_global_frame(void)
 {
   return global_frame_;
@@ -237,5 +243,9 @@ bool AckermannPlannerUtil::get_local_plan(geometry_msgs::PoseStamped& global_pos
 
 AckermannPlannerUtil::~AckermannPlannerUtil()
 {
-
+  if(this->world_model_!=NULL)
+  {
+    delete this->world_model_;
+    this->world_model_=NULL;
+  }
 }