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 #include <ros/ros.h>
00031
00032 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00033 #include <tf2_ros/transform_listener.h>
00034 #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
00035 #include <nav_msgs/OccupancyGrid.h>
00036 #include <sensor_msgs/PointCloud2.h>
00037
00038 #include <string>
00039 #include <vector>
00040
00041 #include <costmap_cspace/pointcloud_accumulator/accumulator.h>
00042 #include <neonavigation_common/compatibility.h>
00043
00044 class Pointcloud2ToMapNode
00045 {
00046 private:
00047 ros::NodeHandle nh_;
00048 ros::NodeHandle pnh_;
00049 ros::Publisher pub_map_;
00050 ros::Subscriber sub_cloud_;
00051 ros::Subscriber sub_cloud_single_;
00052
00053 nav_msgs::OccupancyGrid map_;
00054 tf2_ros::Buffer tfbuf_;
00055 tf2_ros::TransformListener tfl_;
00056 ros::Time published_;
00057 ros::Duration publish_interval_;
00058
00059 double z_min_, z_max_;
00060 std::string global_frame_;
00061 std::string robot_frame_;
00062
00063 unsigned int width_;
00064 unsigned int height_;
00065 float origin_x_;
00066 float origin_y_;
00067
00068 std::vector<PointcloudAccumurator<sensor_msgs::PointCloud2>> accums_;
00069
00070 public:
00071 Pointcloud2ToMapNode()
00072 : nh_()
00073 , pnh_("~")
00074 , tfl_(tfbuf_)
00075 , accums_(2)
00076 {
00077 neonavigation_common::compat::checkCompatMode();
00078 pnh_.param("z_min", z_min_, 0.1);
00079 pnh_.param("z_max", z_max_, 1.0);
00080 pnh_.param("global_frame", global_frame_, std::string("map"));
00081 pnh_.param("robot_frame", robot_frame_, std::string("base_link"));
00082
00083 double accum_duration;
00084 pnh_.param("accum_duration", accum_duration, 1.0);
00085 accums_[0].reset(ros::Duration(accum_duration));
00086 accums_[1].reset(ros::Duration(0.0));
00087
00088 pub_map_ = neonavigation_common::compat::advertise<nav_msgs::OccupancyGrid>(
00089 nh_, "map_local",
00090 pnh_, "map", 1, true);
00091 sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>(
00092 "cloud", 100,
00093 boost::bind(&Pointcloud2ToMapNode::cbCloud, this, _1, false));
00094 sub_cloud_single_ = nh_.subscribe<sensor_msgs::PointCloud2>(
00095 "cloud_singleshot", 100,
00096 boost::bind(&Pointcloud2ToMapNode::cbCloud, this, _1, true));
00097
00098 int width_param;
00099 pnh_.param("width", width_param, 30);
00100 height_ = width_ = width_param;
00101 map_.header.frame_id = global_frame_;
00102
00103 double resolution;
00104 pnh_.param("resolution", resolution, 0.1);
00105 map_.info.resolution = resolution;
00106 map_.info.width = width_;
00107 map_.info.height = height_;
00108 map_.data.resize(map_.info.width * map_.info.height);
00109
00110 double hz;
00111 pnh_.param("hz", hz, 1.0);
00112 publish_interval_ = ros::Duration(1.0 / hz);
00113 }
00114
00115 private:
00116 void cbCloud(const sensor_msgs::PointCloud2::ConstPtr& cloud, const bool singleshot)
00117 {
00118 sensor_msgs::PointCloud2 cloud_global;
00119 geometry_msgs::TransformStamped trans;
00120 try
00121 {
00122 trans = tfbuf_.lookupTransform(global_frame_, cloud->header.frame_id,
00123 cloud->header.stamp, ros::Duration(0.5));
00124 }
00125 catch (tf2::TransformException& e)
00126 {
00127 ROS_WARN("%s", e.what());
00128 return;
00129 }
00130 tf2::doTransform(*cloud, cloud_global, trans);
00131
00132 const int buffer = singleshot ? 1 : 0;
00133 accums_[buffer].push(PointcloudAccumurator<sensor_msgs::PointCloud2>::Points(
00134 cloud_global, cloud_global.header.stamp));
00135
00136 ros::Time now = cloud->header.stamp;
00137 if (published_ + publish_interval_ > now)
00138 return;
00139 published_ = now;
00140
00141 float robot_z;
00142 try
00143 {
00144 tf2::Stamped<tf2::Transform> trans;
00145 tf2::fromMsg(tfbuf_.lookupTransform(global_frame_, robot_frame_, ros::Time(0)), trans);
00146
00147 auto pos = trans.getOrigin();
00148 float x = static_cast<int>(pos.x() / map_.info.resolution) * map_.info.resolution;
00149 float y = static_cast<int>(pos.y() / map_.info.resolution) * map_.info.resolution;
00150 map_.info.origin.position.x = x - map_.info.width * map_.info.resolution * 0.5;
00151 map_.info.origin.position.y = y - map_.info.height * map_.info.resolution * 0.5;
00152 map_.info.origin.position.z = 0.0;
00153 map_.info.origin.orientation.w = 1.0;
00154 origin_x_ = x - width_ * map_.info.resolution * 0.5;
00155 origin_y_ = y - height_ * map_.info.resolution * 0.5;
00156 robot_z = pos.z();
00157 }
00158 catch (tf2::TransformException& e)
00159 {
00160 ROS_WARN("%s", e.what());
00161 return;
00162 }
00163 for (auto& cell : map_.data)
00164 cell = 0;
00165
00166 for (auto& accum : accums_)
00167 {
00168 for (auto& pc : accum)
00169 {
00170 sensor_msgs::PointCloud2Iterator<float> iter_x(pc, "x");
00171 sensor_msgs::PointCloud2Iterator<float> iter_y(pc, "y");
00172 sensor_msgs::PointCloud2Iterator<float> iter_z(pc, "z");
00173 for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
00174 {
00175 if (*iter_z - robot_z < z_min_ || z_max_ < *iter_z - robot_z)
00176 continue;
00177 unsigned int x = int(
00178 (*iter_x - map_.info.origin.position.x) / map_.info.resolution);
00179 unsigned int y = int(
00180 (*iter_y - map_.info.origin.position.y) / map_.info.resolution);
00181 if (x >= map_.info.width || y >= map_.info.height)
00182 continue;
00183 map_.data[x + y * map_.info.width] = 100;
00184 }
00185 }
00186 }
00187
00188 pub_map_.publish(map_);
00189 }
00190 };
00191
00192 int main(int argc, char** argv)
00193 {
00194 ros::init(argc, argv, "pointcloud2_to_map");
00195
00196 Pointcloud2ToMapNode conv;
00197 ros::spin();
00198
00199 return 0;
00200 }