Hiep update

include/robot_costmap_2d/directional_layer.h

@@ -0,0 +1,35 @@
+#ifndef ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
+#define ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
+
+#include <robot_costmap_2d/static_layer.h>
+#include <robot_nav_msgs/Path.h>
+
+namespace robot_costmap_2d
+{
+  class DirectionalLayer : public StaticLayer
+  {
+  public:
+    DirectionalLayer();
+    virtual ~DirectionalLayer();
+    bool laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan);
+    void resetMap();
+
+	private:
+    void incomingMap(const robot_nav_msgs::OccupancyGrid &new_map);
+		bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_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<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, robot_nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th);
+    bool getRobotPose(double &x, double &y, double &yaw);
+
+    std::vector<std::array<uint16_t, 2>> directional_areas_;
+    double pose_x_, pose_y_, pose_yaw_;
+    double distance_skip_ = 1.0;
+    // ros::Publisher lane_mask_pub_;
+    robot_nav_msgs::OccupancyGrid new_map_;
+	};
+}
+
+#endif // ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_H_