Extend costmap_2d interface with methods required by Hybrid A* planner

.gitignore

@@ -1,4 +1,6 @@
 *~
+.idea
+cmake-build-debug
 *.project
 *.cproject
 

costmap_2d/include/costmap_2d/costmap_2d.h

@@ -115,6 +115,13 @@ class Costmap2D
    */
   unsigned char getCost(unsigned int mx, unsigned int my) const;
 
+  /**
+   * @brief  Get the cost of a cell in the costmap
+   * @param index The index of the cell
+   * @return The cost of the cell
+   */
+  unsigned char getCost(unsigned int index) const;
+
   /**
    * @brief  Set the cost of a cell in the costmap
    * @param mx The x coordinate of the cell
@@ -142,6 +149,16 @@ class Costmap2D
    */
   bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
 
+  /**
+   * @brief  Convert from world coordinates to map coordinates
+   * @param  wx The x world coordinate
+   * @param  wy The y world coordinate
+   * @param  mx Will be set to the associated map x coordinate
+   * @param  my Will be set to the associated map y coordinate
+   * @return True if the conversion was successful (legal bounds) false otherwise
+   */
+  bool worldToMapContinuous(double wx, double wy, float& mx, float& my) const;
+
   /**
    * @brief  Convert from world coordinates to map coordinates without checking for legal bounds
    * @param  wx The x world coordinate

costmap_2d/include/costmap_2d/costmap_2d_ros.h

@@ -235,6 +235,14 @@ class Costmap2DROS
    * getUnpaddedRobotFootprint(). */
   void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint);
 
+  /**
+   * @brief  Get the costmap's use_radius_ parameter, corresponding to
+   * whether the footprint for the robot is a circle with radius robot_radius_
+   * or an arbitrarily defined footprint in footprint_.
+   * @return  use_radius_
+   */
+  bool getUseRadius() const noexcept { return use_radius_; }
+
 protected:
   LayeredCostmap* layered_costmap_;
   std::string name_;
@@ -273,6 +281,7 @@ class Costmap2DROS
 
   ros::Subscriber footprint_sub_;
   ros::Publisher footprint_pub_;
+  bool use_radius_;
   std::vector<geometry_msgs::Point> unpadded_footprint_;
   std::vector<geometry_msgs::Point> padded_footprint_;
   float footprint_padding_;

costmap_2d/include/costmap_2d/footprint.h

@@ -120,8 +120,11 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
 /**
  * @brief Read the ros-params "footprint" and/or "robot_radius" from
  * the given NodeHandle using searchParam() to go up the tree.
+ * Optionally, it can return whether we used the robot_radius to create the footprint.
+ * If that's the case, the footprint will be a circle of "robot_radius".
+ * @return The footprint as a list of points.
  */
-std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh);
+std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh, bool* use_radius = nullptr);
 
 /**
  * @brief Create the footprint from the given XmlRpcValue.

costmap_2d/include/costmap_2d/inflation_layer.h

@@ -126,6 +126,16 @@ class InflationLayer : public Layer
    */
   void setInflationParameters(double inflation_radius, double cost_scaling_factor);
 
+  double getCostScalingFactor()
+  {
+    return weight_;
+  }
+
+  double getInflationRadius()
+  {
+    return inflation_radius_;
+  }
+
 protected:
   virtual void onFootprintChanged();
   boost::recursive_mutex* inflation_access_;

costmap_2d/src/costmap_2d.cpp

@@ -194,6 +194,11 @@ unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
   return costmap_[getIndex(mx, my)];
 }
 
+unsigned char Costmap2D::getCost(unsigned int index) const
+{
+  return costmap_[index];
+}
+
 void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
 {
   costmap_[getIndex(mx, my)] = cost;
@@ -219,6 +224,19 @@ bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int&
   return false;
 }
 
+bool Costmap2D::worldToMapContinuous(double wx, double wy, float& mx, float& my) const
+{
+  if (wx < origin_x_ || wy < origin_y_)
+  {
+    return false;
+  }
+
+  mx = static_cast<float>((wx - origin_x_) / resolution_) + 0.5f;
+  my = static_cast<float>((wy - origin_y_) / resolution_) + 0.5f;
+
+  return mx < size_x_ && my < size_y_;
+}
+
 void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const
 {
   mx = (int)((wx - origin_x_) / resolution_);

costmap_2d/src/costmap_2d_ros.cpp

@@ -76,6 +76,7 @@ Costmap2DROS::Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf) :
     plugin_loader_("costmap_2d", "costmap_2d::Layer"),
     publisher_(NULL),
     dsrv_(NULL),
+    use_radius_(false),
     footprint_padding_(0.0)
 {
   // Initialize old pose with something
@@ -157,7 +158,7 @@ Costmap2DROS::Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf) :
   private_nh.param(topic_param, topic, std::string("footprint"));  // TODO: revert to oriented_footprint in N-turtle
   footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>(topic, 1);
 
-  setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
+  setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh, &use_radius_));
 
   publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
                                       always_send_full_costmap);
@@ -389,11 +390,13 @@ void Costmap2DROS::readFootprintFromConfig(const costmap_2d::Costmap2DConfig &ne
     {
         ROS_ERROR("Invalid footprint string from dynamic reconfigure");
     }
+    use_radius_ = false;
   }
   else
   {
     // robot_radius may be 0, but that must be intended at this point.
     setUnpaddedRobotFootprint(makeFootprintFromRadius(new_config.robot_radius));
+    use_radius_ = true;
   }
 }
 

costmap_2d/src/footprint.cpp

@@ -209,7 +209,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
 
 
 
-std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
+std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh, bool* use_radius)
 {
   std::string full_param_name;
   std::string full_radius_param_name;
@@ -225,13 +225,21 @@ std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
       if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
       {
         writeFootprintToParam(nh, points);
+        if (use_radius)
+        {
+          *use_radius = false;
+        }
         return points;
       }
     }
     else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
     {
       points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
       writeFootprintToParam(nh, points);
+      if (use_radius)
+      {
+        *use_radius = false;
+      }
       return points;
     }
   }
@@ -242,6 +250,10 @@ std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
     nh.param(full_radius_param_name, robot_radius, 1.234);
     points = makeFootprintFromRadius(robot_radius);
     nh.setParam("robot_radius", robot_radius);
+    if (use_radius)
+    {
+      *use_radius = true;
+    }
   }
   // Else neither param was found anywhere this knows about, so
   // defaults will come from dynamic_reconfigure stuff, set in