40 lines
1.5 KiB
C++
40 lines
1.5 KiB
C++
#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();
|
|
|
|
LayerType getType() const override
|
|
{
|
|
return LayerType::DIRECTIONAL_LAYER;
|
|
}
|
|
|
|
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_
|