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
00039
00040
00041
00042
00043
00044
00045
00046
00047 #include "octomap_pa/octree_stamped_pa_node.h"
00048
00049
00050 #include <parameter_pa/parameter_pa_ros.h>
00051
00052
00053 #include <string>
00054
00055
00056 int main(int argc, char **argv) {
00057
00058 ros::init(argc, argv, "octree_stamped_pa_node");
00059 cOctreeStampedPaNode octomap;
00060
00061 ros::spin();
00062
00063 return 0;
00064 }
00065
00066
00067 cOctreeStampedPaNode::cOctreeStampedPaNode() : cOctreeStampedPaRos(0.1),
00068 tf_listener_(ros::Duration(20), true) {
00069
00070 cParameterPaRos paramloader;
00071
00072 paramloader.load("~/output_frame", rosparams_base_.output_frame_);
00073
00074
00075 double temp;
00076 temp = 0.1 ;
00077 paramloader.load("~/map_resolution" , temp); setResolution (temp);
00078 temp = 0.5 ;
00079 paramloader.load("~/map_prob_threshold", temp); setOccupancyThres (temp);
00080 temp = 0.12;
00081 paramloader.load("~/map_clamp_min" , temp); setClampingThresMin(temp);
00082 temp = 0.97;
00083 paramloader.load("~/map_clamp_max" , temp); setClampingThresMax(temp);
00084
00085
00086 paramloader.load("~/map_prob_hit" , addparams_.map_prob_hit_ );
00087 paramloader.load("~/map_prob_miss" , addparams_.map_prob_miss_ );
00088 paramloader.load("~/pcd_voxel_active" , addparams_.pcd_voxel_active_ );
00089 paramloader.load("~/pcd_voxel_explicit", addparams_.pcd_voxel_explicit_);
00090 paramloader.load("~/pcd_voxel_explicit_relative_resolution",
00091 addparams_.pcd_voxel_explicit_relative_resolution_);
00092 paramloader.load("~/pcd_explicit_transform",
00093 addparams_.pcd_explicit_transform_);
00094
00095
00096 paramloader.load("~/degrading_time" , rosparams_.degrading_time_);
00097 paramloader.load("~/auto_degrading" , rosparams_.auto_degrading_);
00098 paramloader.load("~/auto_degrading_intervall",
00099 rosparams_.auto_degrading_intervall_);
00100
00101
00102 paramloader.loadTopic("~/topic_in_cloud" ,
00103 nodeparams_.topic_in_cloud_ );
00104 paramloader.loadTopic("~/topic_in_cloud_old",
00105 nodeparams_.topic_in_cloud_old_);
00106 paramloader.loadTopic("~/topic_in_laser" ,
00107 nodeparams_.topic_in_laser_ );
00108
00109
00110 paramloader.loadTopic("~/topic_out_octomap" ,
00111 nodeparams_.topic_out_octomap_);
00112 paramloader.loadTopic("~/topic_out_octomap_full" ,
00113 nodeparams_.topic_out_octomap_full_);
00114 paramloader.loadTopic("~/topic_out_cloud_free" ,
00115 nodeparams_.topic_out_cloud_free_);
00116 paramloader.loadTopic("~/topic_out_cloud_occupied",
00117 nodeparams_.topic_out_cloud_occupied_);
00118
00119
00120 if (nodeparams_.topic_in_cloud_ != "") {
00121 sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>(
00122 nodeparams_.topic_in_cloud_, 10,
00123 &cOctreeStampedPaNode::addPointcloudCallbackSub, this);
00124 }
00125
00126
00127 if (nodeparams_.topic_in_cloud_old_ != "") {
00128 sub_cloud_old_ = nh_.subscribe<sensor_msgs::PointCloud>(
00129 nodeparams_.topic_in_cloud_old_, 10,
00130 &cOctreeStampedPaNode::addPointcloudOldCallbackSub, this);
00131 }
00132
00133 if (nodeparams_.topic_in_laser_ != "") {
00134 sub_laser_ = nh_.subscribe<sensor_msgs::LaserScan>(
00135 nodeparams_.topic_in_laser_, 10,
00136 &cOctreeStampedPaNode::addLaserCallbackSub, this);
00137 }
00138
00139
00140
00141 pub_octomap_ = nh_.advertise<octomap_msgs::Octomap>(
00142 nodeparams_.topic_out_octomap_, 10, true);
00143
00144 pub_octomap_full_ = nh_.advertise<octomap_msgs::Octomap>(
00145 nodeparams_.topic_out_octomap_full_, 10, true);
00146
00147
00148 pub_cloud_free_ = nh_.advertise<sensor_msgs::PointCloud2>(
00149 nodeparams_.topic_out_cloud_free_, 10, true);
00150
00151 pub_cloud_occupied_ = nh_.advertise<sensor_msgs::PointCloud2>(
00152 nodeparams_.topic_out_cloud_occupied_, 10, true);
00153
00154
00155 std::string str_service("~/");
00156 str_service = paramloader.resolveRessourcename(str_service);
00157
00158 srv_clear_ = nh_.advertiseService(str_service + "clear",
00159 &cOctreeStampedPaNode::clearCallbackSrv, this);
00160
00161 srv_getsize_ = nh_.advertiseService(str_service + "getsize",
00162 &cOctreeStampedPaNode::getSizeCallbackSrv, this);
00163
00164 srv_save_ = nh_.advertiseService(str_service + "save",
00165 &cOctreeStampedPaNode::saveCallbackSrv, this);
00166
00167 srv_load_ = nh_.advertiseService(str_service + "load",
00168 &cOctreeStampedPaNode::loadCallbackSrv, this);
00169
00170
00171
00172 count_cloud_ = 0;
00173 count_cloud_old_ = 0;
00174 count_laser_ = 0;
00175 }
00176
00177
00178 cOctreeStampedPaNode::~cOctreeStampedPaNode() {
00179
00180 }
00181
00182
00183 void cOctreeStampedPaNode::publishOctomap() {
00184
00185 if (pub_octomap_.getNumSubscribers() > 0) {
00186 pub_octomap_.publish(getOctomap());
00187 }
00188 if (pub_octomap_full_.getNumSubscribers() > 0) {
00189 pub_octomap_.publish(getOctomapFull());
00190 }
00191
00192 if (pub_cloud_occupied_.getNumSubscribers() > 0) {
00193 pub_cloud_occupied_.publish(getOctomapPcd());
00194 }
00195 if (pub_cloud_free_.getNumSubscribers() > 0) {
00196 pub_cloud_free_.publish(getOctomapPcdFree());
00197 }
00198 }
00199
00200
00201 void cOctreeStampedPaNode::addPointcloudCallbackSub(
00202 const sensor_msgs::PointCloud2ConstPtr &msg) {
00203
00204 if (!updateTime(msg->header.stamp)) {
00205 tf_listener_.clear();
00206 return;
00207 }
00208
00209 tf::StampedTransform transform;
00210
00211 try {
00212 tf_listener_.waitForTransform(rosparams_base_.output_frame_,
00213 msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
00214 tf_listener_.lookupTransform(rosparams_base_.output_frame_,
00215 msg->header.frame_id, msg->header.stamp, transform);
00216 } catch (tf::TransformException &ex){
00217 ROS_WARN("%s",ex.what());
00218 return;
00219 }
00220
00221 if (addCloud(msg, addparams_, transform)) {
00222 count_cloud_++;
00223 checkDegrading();
00224 publishOctomap();
00225 }
00226 }
00227
00228
00229 void cOctreeStampedPaNode::addPointcloudOldCallbackSub(
00230 const sensor_msgs::PointCloudConstPtr &msg) {
00231
00232 if (!updateTime(msg->header.stamp)) {
00233 tf_listener_.clear();
00234 return;
00235 }
00236
00237 tf::StampedTransform transform;
00238
00239 try {
00240 tf_listener_.waitForTransform(rosparams_base_.output_frame_,
00241 msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
00242 tf_listener_.lookupTransform(rosparams_base_.output_frame_,
00243 msg->header.frame_id, msg->header.stamp, transform);
00244 } catch (tf::TransformException &ex){
00245 ROS_WARN("%s",ex.what());
00246 return;
00247 }
00248
00249 if (addCloud(msg, addparams_, transform)) {
00250 count_cloud_old_++;
00251 checkDegrading();
00252 publishOctomap();
00253 }
00254 }
00255
00256
00257 void cOctreeStampedPaNode::addLaserCallbackSub(
00258 const sensor_msgs::LaserScanConstPtr &msg) {
00259
00260 if (!updateTime(msg->header.stamp)) {
00261 tf_listener_.clear();
00262 return;
00263 }
00264
00265 tf::StampedTransform transform;
00266
00267 try {
00268 tf_listener_.waitForTransform(rosparams_base_.output_frame_,
00269 msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
00270 tf_listener_.lookupTransform(rosparams_base_.output_frame_,
00271 msg->header.frame_id, msg->header.stamp, transform);
00272 } catch (tf::TransformException &ex){
00273 ROS_WARN("%s",ex.what());
00274 return;
00275 }
00276
00277 updateTime(msg->header.stamp);
00278 if (addCloud(msg, addparams_, transform)) {
00279 count_laser_++;
00280 checkDegrading();
00281 publishOctomap();
00282 }
00283 }
00284
00285
00286
00287 bool cOctreeStampedPaNode::clearCallbackSrv(std_srvs::Empty::Request &req,
00288 std_srvs::Empty::Response &res) {
00289
00290 ROS_INFO("cOctreeStampedPaNode::clear()");
00291
00292 count_cloud_ = 0;
00293 count_cloud_old_ = 0;
00294 count_laser_ = 0;
00295
00296 clear();
00297 tf_listener_.clear();
00298
00299 return true;
00300 }
00301
00302
00303 bool cOctreeStampedPaNode::getSizeCallbackSrv (
00304 octomap_pa::OctomapPaGetSize::Request &req,
00305 octomap_pa::OctomapPaGetSize::Response &res) {
00306
00307 ROS_INFO("cOctreeStampedPaNode::getsize()");
00308
00309 res.size = size();
00310 res.memoryusage = (int64_t) memoryUsage();
00311
00312 res.count_cloud = count_cloud_ ;
00313 res.count_cloud_old = count_cloud_old_;
00314 res.count_laser = count_laser_ ;
00315
00316 return true;
00317 }
00318
00319
00320 bool cOctreeStampedPaNode::saveCallbackSrv(
00321 octomap_pa::OctomapPaFileName::Request &req,
00322 octomap_pa::OctomapPaFileName::Response &res) {
00323
00324 ROS_INFO_STREAM("cOctreeStampedPaNode::save(" << req.filename << ")");
00325
00326 std::string filename;
00327 filename = req.filename;
00328 cParameterPaRos par;
00329 par.replaceFindpack(filename);
00330
00331
00332 res.ok = this->write(filename);
00333
00334 return res.ok;
00335 }
00336
00337
00338 bool cOctreeStampedPaNode::loadCallbackSrv(
00339 octomap_pa::OctomapPaFileName::Request &req,
00340 octomap_pa::OctomapPaFileName::Response &res) {
00341
00342 ROS_INFO_STREAM("cOctreeStampedPaNode::load(" << req.filename << ")");
00343
00344 std::string filename;
00345 filename = req.filename;
00346 cParameterPaRos par;
00347 par.replaceFindpack(filename);
00348
00349
00350 res.ok = readFull(filename);
00351
00352 if (res.ok) { publishOctomap(); }
00353 return res.ok;
00354 }