123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299 |
- #ifndef NAVIGATION_COSTMAP_ROS_H_
- #define NAVIGATION_COSTMAP_ROS_H_
- #include <queue>
- #include <vector>
- #include <string>
- #include <boost/thread.hpp>
- #include <boost/thread/mutex.hpp>
- #include <ros/ros.h>
- #include <geometry_msgs/Pose2D.h>
- #include <sensor_msgs/PointCloud.h>
- #include <nav_msgs/OccupancyGrid.h>
- #include "common/common.h"
- #include <std_msgs/UInt32.h>
- typedef uint32_t uint32;
- typedef unsigned char uchar;
- namespace nav
- {
- namespace costmap
- {
-
- struct Cell
- {
- Cell(float dist, int index, uint32 x, uint32 y, uint32 sx, uint32 sy) : distance_(dist), index_(index), x_(x), y_(y), sx_(sx), sy_(sy) {}
- Cell() {}
- float distance_;
- uint32 index_;
- uint32 x_, y_, sx_, sy_;
- };
- inline bool operator<(const Cell &a, const Cell &b)
- {
- return a.distance_ > b.distance_;
- }
-
- enum COST_VALUE
- {
- PERDEFINED_PLAN = 0,
- FREE_SPACE = 100,
- INSCRIBED_INFLATED_OBSTACLE = 253,
- LETHAL_OBSTACLE,
- NO_INFORMATION,
- };
- class Pose
- {
- float x;
- float y;
- float theta;
- };
-
- class PoseWithTime : public Pose
- {
- public:
- ros::Time time;
- bool use;
- };
-
- class CostMap
- {
- public:
- CostMap(CostmapInfo costmap_config);
- ~CostMap();
- public:
- void restart();
- void run();
- void stop();
- void deleteAll();
-
-
-
- bool getRobotPose(Pose &pose);
-
-
-
- bool setCostPath(const Path path);
-
-
-
- bool setCostValue(uint32 x, uint32 y, uchar value, bool mandatory = false);
-
-
-
- uchar getCost(uint32 x, uint32 y);
-
-
- uchar *getCostmap();
-
-
- uint32 getSizeInCellsX() { return size_x_; };
-
-
- uint32 getSizeInCellsY() { return size_y_; };
-
-
- float getOriginX() { return origin_x_; };
-
-
- float getOriginY() { return origin_y_; };
-
-
- float getResolution() { return resolution_; };
-
-
-
- bool worldToMap(float wx, float wy, uint32 &mx, uint32 &my);
-
-
-
- bool mapToWorld(uint32 mx, uint32 my, float &wx, float &wy);
- private:
- void initMap();
- void resetMap();
- void pubCostmap();
-
-
-
- int cellDistance(float dist);
-
-
-
- uchar computeCost(float distance);
-
- void getInflationAreaCost();
-
-
-
- uchar *setInflationMap(uchar *map);
-
-
- uchar costLookup(int mx, int my, int src_x, int src_y);
-
-
-
- float distanceLookup(int mx, int my, int src_x, int src_y);
-
-
- void enqueue(int index, int mx, int my, int src_x, int src_y);
-
-
-
- uchar interpretValue(uchar value);
- private:
-
- void incomingMap(const nav_msgs::OccupancyGridConstPtr &map);
-
- void locationCallback(const geometry_msgs::Pose2DConstPtr &pose);
-
- void sensorCloudCallback(const sensor_msgs::PointCloudConstPtr &cloud);
-
- void updateCostMap();
- private:
-
- uchar *costmap_;
-
- uchar *originCostmap_;
- std::vector<ros::Time> obstacleUpdateTimeMap_;
-
- uchar *originmap_;
-
- bool *seen_;
-
- bool run_;
-
- float inflation_radius_;
-
- float inscribe_radius_;
-
- uint32 inflation_cells_;
- uint32 inscribe_cells_;
- float sensor_duration_;
-
- float resolution_;
-
- uint32 size_x_;
-
- uint32 size_y_;
-
- float origin_x_;
-
- float origin_y_;
-
- uint32 frequency_;
-
- boost::shared_mutex locat_mutex_;
-
- boost::shared_mutex sensor_mutex_;
-
- boost::shared_mutex costmap_mutex_;
-
- boost::thread *update_cost_;
-
- PoseWithTime robot_pose_;
-
- uchar **cached_cost_;
- float **cached_dist_;
-
- space::BlockQueue<Cell> bin_cells_;
-
- std::vector<Cell> sensor_cells_;
-
- std::vector<int> cost_path_;
- private:
- ros::NodeHandle nh_;
-
- ros::Publisher costmap_publisher_;
-
- ros::Subscriber map_subscriber_;
-
- ros::Subscriber sensor_subscriber_;
-
- ros::Subscriber location_subscriber_;
- };
- }
- }
- #endif
|