Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <boost/bind.hpp>
00039 #include <costmap_2d/costmap_2d_publisher.h>
00040 #include <costmap_2d/cost_values.h>
00041
00042 namespace costmap_2d
00043 {
00044
00045 char* Costmap2DPublisher::cost_translation_table_ = NULL;
00046
00047 Costmap2DPublisher::Costmap2DPublisher(ros::NodeHandle * ros_node, Costmap2D* costmap, std::string global_frame,
00048 std::string topic_name, bool always_send_full_costmap) :
00049 node(ros_node), costmap_(costmap), global_frame_(global_frame), active_(false),
00050 always_send_full_costmap_(always_send_full_costmap)
00051 {
00052 costmap_pub_ = ros_node->advertise<nav_msgs::OccupancyGrid>(topic_name, 1,
00053 boost::bind(&Costmap2DPublisher::onNewSubscription, this, _1));
00054 costmap_update_pub_ = ros_node->advertise<map_msgs::OccupancyGridUpdate>(topic_name + "_updates", 1);
00055
00056 if (cost_translation_table_ == NULL)
00057 {
00058 cost_translation_table_ = new char[256];
00059
00060
00061 cost_translation_table_[0] = 0;
00062 cost_translation_table_[253] = 99;
00063 cost_translation_table_[254] = 100;
00064 cost_translation_table_[255] = -1;
00065
00066
00067
00068 for (int i = 1; i < 253; i++)
00069 {
00070 cost_translation_table_[ i ] = char(1 + (97 * (i - 1)) / 251);
00071 }
00072 }
00073
00074 xn_ = yn_ = 0;
00075 x0_ = costmap_->getSizeInCellsX();
00076 y0_ = costmap_->getSizeInCellsY();
00077 }
00078
00079 Costmap2DPublisher::~Costmap2DPublisher()
00080 {
00081 }
00082
00083 void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& pub)
00084 {
00085 prepareGrid();
00086 pub.publish(grid_);
00087 }
00088
00089
00090 void Costmap2DPublisher::prepareGrid()
00091 {
00092 boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
00093 double resolution = costmap_->getResolution();
00094
00095 grid_.header.frame_id = global_frame_;
00096 grid_.header.stamp = ros::Time::now();
00097 grid_.info.resolution = resolution;
00098
00099 grid_.info.width = costmap_->getSizeInCellsX();
00100 grid_.info.height = costmap_->getSizeInCellsY();
00101
00102 double wx, wy;
00103 costmap_->mapToWorld(0, 0, wx, wy);
00104 grid_.info.origin.position.x = wx - resolution / 2;
00105 grid_.info.origin.position.y = wy - resolution / 2;
00106 grid_.info.origin.position.z = 0.0;
00107 grid_.info.origin.orientation.w = 1.0;
00108 saved_origin_x_ = costmap_->getOriginX();
00109 saved_origin_y_ = costmap_->getOriginY();
00110
00111 grid_.data.resize(grid_.info.width * grid_.info.height);
00112
00113 unsigned char* data = costmap_->getCharMap();
00114 for (unsigned int i = 0; i < grid_.data.size(); i++)
00115 {
00116 grid_.data[i] = cost_translation_table_[ data[ i ]];
00117 }
00118 }
00119
00120 void Costmap2DPublisher::publishCostmap()
00121 {
00122 if (costmap_pub_.getNumSubscribers() == 0)
00123 {
00124
00125 return;
00126 }
00127
00128 float resolution = costmap_->getResolution();
00129
00130 if (always_send_full_costmap_ || grid_.info.resolution != resolution ||
00131 grid_.info.width != costmap_->getSizeInCellsX() ||
00132 grid_.info.height != costmap_->getSizeInCellsY() ||
00133 saved_origin_x_ != costmap_->getOriginX() ||
00134 saved_origin_y_ != costmap_->getOriginY())
00135 {
00136 prepareGrid();
00137 costmap_pub_.publish(grid_);
00138 }
00139 else if (x0_ < xn_)
00140 {
00141 boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
00142
00143 map_msgs::OccupancyGridUpdate update;
00144 update.header.stamp = ros::Time::now();
00145 update.header.frame_id = global_frame_;
00146 update.x = x0_;
00147 update.y = y0_;
00148 update.width = xn_ - x0_;
00149 update.height = yn_ - y0_;
00150 update.data.resize(update.width * update.height);
00151
00152 unsigned int i = 0;
00153 for (unsigned int y = y0_; y < yn_; y++)
00154 {
00155 for (unsigned int x = x0_; x < xn_; x++)
00156 {
00157 unsigned char cost = costmap_->getCost(x, y);
00158 update.data[i++] = cost_translation_table_[ cost ];
00159 }
00160 }
00161 costmap_update_pub_.publish(update);
00162 }
00163
00164 xn_ = yn_ = 0;
00165 x0_ = costmap_->getSizeInCellsX();
00166 y0_ = costmap_->getSizeInCellsY();
00167 }
00168
00169 }
costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:15