update folder plugins

include/costmap_2d/costmap_2d_robot.h

@@ -0,0 +1,282 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
+#ifndef COSTMAP_2D_COSTMAP_2D_ROS_H_
+#define COSTMAP_2D_COSTMAP_2D_ROS_H_
+
+#include <costmap_2d/layered_costmap.h>
+#include <costmap_2d/layer.h>
+#include <costmap_2d/costmap_2d_publisher.h>
+#include <costmap_2d/Costmap2DConfig.h>
+#include <costmap_2d/footprint.h>
+#include <geometry_msgs/Polygon.h>
+#include <geometry_msgs/PolygonStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <dynamic_reconfigure/server.h>
+#include <pluginlib/class_loader.hpp>
+#include <tf2/LinearMath/Transform.h>
+
+// class SuperValue : public XmlRpc::XmlRpcValue
+// {
+// public:
+//   void setStruct(XmlRpc::XmlRpcValue::ValueStruct* a)
+//   {
+//     _type = TypeStruct;
+//     _value.asStruct = new XmlRpc::XmlRpcValue::ValueStruct(*a);
+//   }
+//   void setArray(XmlRpc::XmlRpcValue::ValueArray* a)
+//   {
+//     _type = TypeArray;
+//     _value.asArray = new std::vector<XmlRpc::XmlRpcValue>(*a);
+//   }
+// };
+
+namespace costmap_2d
+{
+
+/** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
+ * topics that provide observations about obstacles in either the form
+ * of PointCloud or LaserScan messages. */
+class Costmap2DRobot
+{
+public:
+  /**
+   * @brief  Constructor for the wrapper
+   * @param name The name for this costmap
+   * @param tf A reference to a TransformListener
+   */
+  Costmap2DRobot(const std::string &name, tf2::BufferCore& tf);
+  ~Costmap2DRobot();
+
+  /**
+   * @brief  Subscribes to sensor topics if necessary and starts costmap
+   * updates, can be called to restart the costmap after calls to either
+   * stop() or pause()
+   */
+  void start();
+
+  /**
+   * @brief  Stops costmap updates and unsubscribes from sensor topics
+   */
+  void stop();
+
+  /**
+   * @brief  Stops the costmap from updating, but sensor data still comes in over the wire
+   */
+  void pause();
+
+  /**
+   * @brief  Resumes costmap updates
+   */
+  void resume();
+
+  void updateMap();
+
+  /**
+   * @brief Reset each individual layer
+   */
+  void resetLayers();
+
+  /** @brief Same as getLayeredCostmap()->isCurrent(). */
+  bool isCurrent() const
+    {
+      return layered_costmap_->isCurrent();
+    }
+
+  /**
+   * @brief Is the costmap stopped
+   */
+  bool isStopped() const
+    {
+      return stopped_;
+    }
+
+  /**
+   * @brief Get the pose of the robot in the global frame of the costmap
+   * @param global_pose Will be set to the pose of the robot in the global frame of the costmap
+   * @return True if the pose was set successfully, false otherwise
+   */
+  bool getRobotPose(geometry_msgs::PoseStamped& global_pose) const;
+
+  /** @brief Returns costmap name */
+  inline const std::string& getName() const noexcept
+    {
+      return name_;
+    }
+
+  /** @brief Returns the delay in transform (tf) data that is tolerable in seconds */
+  double getTransformTolerance() const
+    {
+      return transform_tolerance_;
+    }
+
+  /** @brief Return a pointer to the "master" costmap which receives updates from all the layers.
+   *
+   * Same as calling getLayeredCostmap()->getCostmap(). */
+  Costmap2D* getCostmap() const
+    {
+      return layered_costmap_->getCostmap();
+    }
+
+  /**
+   * @brief  Returns the global frame of the costmap
+   * @return The global frame of the costmap
+   */
+  inline const std::string& getGlobalFrameID() const noexcept
+    {
+      return global_frame_;
+    }
+
+  /**
+   * @brief  Returns the local frame of the costmap
+   * @return The local frame of the costmap
+   */
+  inline const std::string& getBaseFrameID() const noexcept
+    {
+      return robot_base_frame_;
+    }
+  LayeredCostmap* getLayeredCostmap() const
+    {
+      return layered_costmap_;
+    }
+
+  /** @brief Returns the current padded footprint as a geometry_msgs::Polygon. */
+  geometry_msgs::Polygon getRobotFootprintPolygon() const
+  {
+    return costmap_2d::toPolygon(padded_footprint_);
+  }
+
+  /** @brief Return the current footprint of the robot as a vector of points.
+   *
+   * This version of the footprint is padded by the footprint_padding_
+   * distance, set in the rosparam "footprint_padding".
+   *
+   * The footprint initially comes from the rosparam "footprint" but
+   * can be overwritten by dynamic reconfigure or by messages received
+   * on the "footprint" topic. */
+  inline const std::vector<geometry_msgs::Point>& getRobotFootprint() const noexcept
+  {
+    return padded_footprint_;
+  }
+
+  /** @brief Return the current unpadded footprint of the robot as a vector of points.
+   *
+   * This is the raw version of the footprint without padding.
+   *
+   * The footprint initially comes from the rosparam "footprint" but
+   * can be overwritten by dynamic reconfigure or by messages received
+   * on the "footprint" topic. */
+  inline const std::vector<geometry_msgs::Point>& getUnpaddedRobotFootprint() const noexcept
+  {
+    return unpadded_footprint_;
+  }
+
+  /**
+   * @brief  Build the oriented footprint of the robot at the robot's current pose
+   * @param  oriented_footprint Will be filled with the points in the oriented footprint of the robot
+   */
+  void getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const;
+
+  /** @brief Set the footprint of the robot to be the given set of
+   * points, padded by footprint_padding.
+   *
+   * Should be a convex polygon, though this is not enforced.
+   *
+   * First expands the given polygon by footprint_padding_ and then
+   * sets padded_footprint_ and calls
+   * layered_costmap_->setFootprint().  Also saves the unpadded
+   * footprint, which is available from
+   * getUnpaddedRobotFootprint(). */
+  void setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points);
+
+  /** @brief Set the footprint of the robot to be the given polygon,
+   * padded by footprint_padding.
+   *
+   * Should be a convex polygon, though this is not enforced.
+   *
+   * First expands the given polygon by footprint_padding_ and then
+   * sets padded_footprint_ and calls
+   * layered_costmap_->setFootprint().  Also saves the unpadded
+   * footprint, which is available from
+   * getUnpaddedRobotFootprint(). */
+  void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint);
+
+protected:
+  LayeredCostmap* layered_costmap_;
+  std::string name_;
+  tf2_ros::Buffer& tf_;  ///< @brief Used for transforming point clouds
+  std::string global_frame_;  ///< @brief The global frame for the costmap
+  std::string robot_base_frame_;  ///< @brief The frame_id of the robot base
+  double transform_tolerance_;  ///< timeout before transform errors
+
+private:
+  /** @brief Set the footprint from the new_config object.
+   *
+   * If the values of footprint and robot_radius are the same in
+   * new_config and old_config, nothing is changed. */
+  void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
+                               const costmap_2d::Costmap2DConfig &old_config);
+
+  void loadOldParameters(ros::NodeHandle& nh);
+  void warnForOldParameters(ros::NodeHandle& nh);
+  void checkOldParam(ros::NodeHandle& nh, const std::string &param_name);
+  void copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh);
+  // void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level);
+  void movementCB(const ros::TimerEvent &event);
+  void mapUpdateLoop(double frequency);
+  bool map_update_thread_shutdown_;
+  bool stop_updates_, initialized_, stopped_;
+  boost::thread* map_update_thread_;  ///< @brief A thread for updating the map
+  // ros::Time last_publish_;
+  // ros::Duration publish_cycle;
+  // pluginlib::ClassLoader<costmap_2d::Layer> plugin_loader_;
+  // Costmap2DPublisher* publisher_;
+  // dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
+
+  boost::recursive_mutex configuration_mutex_;
+
+  // ros::Subscriber footprint_sub_;
+  // ros::Publisher footprint_pub_;
+  std::vector<geometry_msgs::Point> unpadded_footprint_;
+  std::vector<geometry_msgs::Point> padded_footprint_;
+  float footprint_padding_;
+  // costmap_2d::Costmap2DConfig old_config_;
+};
+// class Costmap2DRobot
+}  // namespace costmap_2d
+
+#endif  // COSTMAP_2D_COSTMAP_2D_ROS_H

include/costmap_2d/critical_layer.h

@@ -11,8 +11,8 @@ public:
   CriticalLayer();
   virtual ~CriticalLayer();
 private:
-  unsigned char interpretValue(unsigned char value);
-  void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  unsigned char interpretValue(unsigned char value) override;
+  void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;
 };
 }
 

include/costmap_2d/data_convert.h

@@ -2,12 +2,60 @@
 #define DATA_CONVERT_H
 
 #include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <tf2/utils.h>
 #include <tf2/compat.h>
 #include <robot/time.h>
+#include <cmath>
 
 namespace costmap_2d
 {
+    template <typename T>
+    bool isEqual(const T& a, const T& b, double eps = 1e-5)
+    {
+        return std::fabs(a - b) < eps;
+    }
+
+    inline bool operator==(const geometry_msgs::Quaternion& a, const geometry_msgs::Quaternion& b)
+    {
+        return isEqual(a.x, b.x) && isEqual(a.y, b.y) && isEqual(a.z, b.z) && isEqual(a.w, b.w);
+    }
+
+    inline bool operator==(const geometry_msgs::Point& a, const geometry_msgs::Point& b)
+    {
+        return isEqual(a.x, b.x) && isEqual(a.y, b.y) && isEqual(a.z, b.z);
+    }
+
+    inline bool operator==(const geometry_msgs::Pose& a, const geometry_msgs::Pose& b)
+    {
+        return (a.position == b.position) && (a.orientation == b.orientation);
+    }
+
+    inline bool operator==(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b)
+    {
+        return (a.header.frame_id == b.header.frame_id) &&
+            (a.pose == b.pose);
+    }
+
+    inline double getYaw(const geometry_msgs::Quaternion& q)
+    {
+        double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
+        double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
+        return std::atan2(siny_cosp, cosy_cosp);
+    }
+
+    inline geometry_msgs::Quaternion getQuaternion(const double& yaw)
+    {
+        geometry_msgs::Quaternion q;
+        q.x = 0.0;
+        q.y = 0.0;
+        q.z = std::sin(yaw / 2.0);
+        q.w = std::cos(yaw / 2.0);
+        return q;
+    }
+
     robot::Time convertTime(tf2::Time time)
     {
         robot::Time time_tmp;
@@ -24,6 +72,16 @@ namespace costmap_2d
         return time_tmp;
     }
 
+    tf2::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q)
+    {
+        tf2::Quaternion out(q.x,q.y,q.z,q.w);
+        return out;
+    }
+    
+    geometry_msgs::Quaternion convertQuaternion(const tf2::Quaternion& q)
+    {
+        return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
+    }
     
     tf2::Transform convertToTf2Transform(const geometry_msgs::Transform& msg)
     {

include/costmap_2d/directional_layer.h

@@ -2,8 +2,7 @@
 #define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
 
 #include <costmap_2d/static_layer.h>
-// #include <tf/transform_listener.h>
-// #include <nav_msgs/Path.h>
+#include <nav_msgs/Path.h>
 
 namespace costmap_2d
 {
@@ -12,14 +11,14 @@ namespace costmap_2d
   public:
     DirectionalLayer();
     virtual ~DirectionalLayer();
-    // bool laneFilter(const std::vector<geometry_msgs::PoseStamped> plan);
+    bool laneFilter(const std::vector<geometry_msgs::PoseStamped> plan);
     void resetMap();
 
 	private:
-    // void incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map);
-		// bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path);
+    void incomingMap(const nav_msgs::OccupancyGrid &new_map);
+		bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path);
     unsigned char laneFilter(unsigned int x, unsigned int y, double yaw_robot);
-		// void laneFilter(std::vector<unsigned int> x, std::vector<unsigned int> y, std::vector<double> yaw_robot);
+		void laneFilter(std::vector<unsigned int> x, std::vector<unsigned int> y, std::vector<double> yaw_robot);
     void laneFilter(std::vector<std::pair<unsigned int, double>> x, std::vector<std::pair<unsigned int, double>> y, std::vector<double> yaw_robot);
     bool inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance);
     void convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th);
@@ -30,7 +29,6 @@ namespace costmap_2d
     double distance_skip_ = 1.0;
     // ros::Publisher lane_mask_pub_;
     nav_msgs::OccupancyGrid new_map_;
-    // tf::TransformListener listener_;
 	};
 }
 

include/costmap_2d/obstacle_layer.h

@@ -76,38 +76,6 @@ public:
   virtual void deactivate();
   virtual void reset();
 
-  // /**
-  //  * @brief  A callback to handle buffering LaserScan messages
-  //  * @param message The message returned from a message notifier
-  //  * @param buffer A pointer to the observation buffer to update
-  //  */
-  // void laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
-  //                        const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
-
-  //  /**
-  //   * @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max.
-  //   * @param message The message returned from a message notifier
-  //   * @param buffer A pointer to the observation buffer to update
-  //   */
-  // void laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& message,
-  //                                const boost::shared_ptr<ObservationBuffer>& buffer);
-
-  // /**
-  //  * @brief  A callback to handle buffering PointCloud messages
-  //  * @param message The message returned from a message notifier
-  //  * @param buffer A pointer to the observation buffer to update
-  //  */
-  // void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
-  //                         const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
-
-  // /**
-  //  * @brief  A callback to handle buffering PointCloud2 messages
-  //  * @param message The message returned from a message notifier
-  //  * @param buffer A pointer to the observation buffer to update
-  //  */
-  // void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
-  //                          const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
-
   // for testing purposes
   void addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing);
   void clearStaticObservations(bool marking, bool clearing);
@@ -165,8 +133,6 @@ protected:
 
   laser_geometry::LaserProjection projector_;  ///< @brief Used to project laser scans into point clouds
 
-  // std::vector<boost::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_;  ///< @brief Used for the observation message filters
-  // std::vector<boost::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_;  ///< @brief Used to make sure that transforms are available for each sensor
   std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_;  ///< @brief Used to store observations from various sensors
   std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_;  ///< @brief Used to store observation buffers used for marking obstacles
   std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_;  ///< @brief Used to store observation buffers used for clearing obstacles
@@ -183,6 +149,8 @@ private:
   bool getParams();
 };
 
+using PluginObstacleLayerPtr = std::shared_ptr<ObstacleLayer>;
+
 }  // namespace costmap_2d
 
 #endif  // COSTMAP_2D_OBSTACLE_LAYER_H_

include/costmap_2d/static_layer.h

@@ -27,7 +27,7 @@ public:
 
   virtual void matchSize();
 protected:
-  void handleImpl(const void* data, 
+  void handleImpl(const void* data,
                   const std::type_info& type,
                   const std::string& topic) override;
   void incomingMap(const nav_msgs::OccupancyGrid& new_map);

include/costmap_2d/voxel_grid.h

@@ -1,30 +0,0 @@
-// Header header
-// uint32_t[] data
-// geometry_msgs/Point32 origin
-// geometry_msgs/Vector3 resolutions
-// uint32_t size_x
-// uint32_t size_y
-// uint32_t size_z
-#ifndef COSTMAP_2D_VOXEL_GRID_H_
-#define COSTMAP_2D_VOXEL_GRID_H_
-
-#include <std_msgs/Header.h>
-#include <geometry_msgs/Point32.h>
-#include <geometry_msgs/Vector3.h>
-
-namespace costmap_2d
-{
-    struct VoxelGrid
-    {
-        std_msgs::Header header;
-        std::vector<uint32_t> data;
-        geometry_msgs::Point32 origin;
-        geometry_msgs::Vector3 resolutions;
-        uint32_t size_x;
-        uint32_t size_y;
-        uint32_t size_z;
-    };
-}
-
-
-#endif // COSTMAP_2D_VOXEL_GRID_H_

include/costmap_2d/voxel_layer.h

@@ -42,7 +42,7 @@
 #include <costmap_2d/layer.h>
 #include <costmap_2d/layered_costmap.h>
 #include <costmap_2d/observation_buffer.h>
-#include <costmap_2d/voxel_grid.h>
+// #include <costmap_2d/voxel_grid.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <sensor_msgs/LaserScan.h>
 // #include <laser_geometry/laser_geometry.h>