nearfield_map_degrading_pa_node.cpp
Go to the documentation of this file.
1 /******************************************************************************
2 * *
3 * nearfield_map_degrading_pa_node.cpp *
4 * =================================== *
5 * *
6 *******************************************************************************
7 * *
8 * github repository *
9 * https://github.com/TUC-ProAut/ros_nearfield_map *
10 * *
11 * Chair of Automation Technology, Technische Universität Chemnitz *
12 * https://www.tu-chemnitz.de/etit/proaut *
13 * *
14 *******************************************************************************
15 * *
16 * New BSD License *
17 * *
18 * Copyright (c) 2015-2020, Peter Weissig, Technische Universität Chemnitz *
19 * All rights reserved. *
20 * *
21 * Redistribution and use in source and binary forms, with or without *
22 * modification, are permitted provided that the following conditions are met: *
23 * * Redistributions of source code must retain the above copyright *
24 * notice, this list of conditions and the following disclaimer. *
25 * * Redistributions in binary form must reproduce the above copyright *
26 * notice, this list of conditions and the following disclaimer in the *
27 * documentation and/or other materials provided with the distribution. *
28 * * Neither the name of the Technische Universität Chemnitz nor the *
29 * names of its contributors may be used to endorse or promote products *
30 * derived from this software without specific prior written permission. *
31 * *
32 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" *
33 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
34 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
35 * ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY *
36 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
37 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
38 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
39 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
40 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
41 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH *
42 * DAMAGE. *
43 * *
44 ******************************************************************************/
45 
46 // local headers
48 
49 // ros headers
51 
52 // standard headers
53 #include <string>
54 
55 //**************************[main]*********************************************
56 int main(int argc, char **argv) {
57 
58  ros::init(argc, argv, "nearfield_map_pa_node");
60 
61  ros::spin();
62 
63  return 0;
64 }
65 
66 //**************************[cNearfieldMapDegradingPaNode]*********************
68 
69  cParameterPaRos paramloader;
70  paramloader.load("~/degrading_time" , rosparams_.degrading_time_);
71  paramloader.load("~/auto_degrading" , rosparams_.auto_degrading_);
72  paramloader.load("~/auto_degrading_intervall",
74 
75  // Subscriber for depth images from camera
76  if (nodeparams_.topic_in_camera_ != "") {
77  sub_camera_ = nh_.subscribe<sensor_msgs::PointCloud2>(
80  }
81  // Subscriber for single laserscans
83  sub_laser_scan_ = nh_.subscribe<sensor_msgs::PointCloud2>(
86  }
87  // Subscriber for full laserscans
89  sub_laser_full_ = nh_.subscribe<sensor_msgs::PointCloud2>(
92  }
93 }
94 
95 //**************************[~cNearfieldMapDegradingPaNode]********************
97 
98 }
99 
100 //**************************[addPcdCameraCallbackSub]**************************
102  const sensor_msgs::PointCloud2ConstPtr &msg) {
103 
104  if (!updateTime(msg->header.stamp)) {
106  return;
107  }
108 
109  tf::StampedTransform transform;
110  if (! checkTF(msg->header, transform)) { return;}
111 
112  if (addCloud(msg, params_addcloud_camera_, transform)) {
113  count_camera_++;
114  checkDegrading();
115  publish();
116  }
117 }
118 
119 //**************************[addPcdLaserScanCallbackSub]***********************
121  const sensor_msgs::PointCloud2ConstPtr &msg) {
122 
123  if (!updateTime(msg->header.stamp)) {
125  return;
126  }
127 
128  tf::StampedTransform transform;
129  if (! checkTF(msg->header, transform)) { return;}
130 
131  if (addCloud(msg, params_addcloud_laser_scan_, transform)) {
133  checkDegrading();
134  publish();
135  }
136 }
137 
138 //**************************[addPcdLaserFullCallbackSub]***********************
140  const sensor_msgs::PointCloud2ConstPtr &msg) {
141 
142  if (!updateTime(msg->header.stamp)) {
144  return;
145  }
146 
147  tf::StampedTransform transform;
148  if (! checkTF(msg->header, transform)) { return;}
149 
150  if (addCloud(msg, params_addcloud_laser_full_, transform)) {
152  checkDegrading();
153  publish();
154  }
155 }
cOctreeStampedPaRosParameter rosparams_
ros::NodeHandle nh_
node handler for topic subscription and advertising
ros::Subscriber sub_laser_scan_
subscriber for a laserscan
cNearfieldMapPaNodeParameter nodeparams_
parameters
int count_camera_
number of inserted depth images from camera (pointclouds)
ros::Subscriber sub_laser_full_
subscriber for single laserscans
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool updateTime(const ros::Time &time)
tf::TransformListener tf_listener_
transformation of different frames
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool checkTF(const std_msgs::Header &header, tf::StampedTransform &transform)
cAddCloudParameter params_addcloud_laser_scan_
parameter for insertion of single laserscans
void checkDegrading(void)
int count_laser_full_
number of inserted full laserscans (pointclouds)
cAddCloudParameter params_addcloud_camera_
parameter for insertion of depth images from camera
bool load(const std::string name, bool &value, const bool print_default=true) const
ROSCPP_DECL void spin(Spinner &spinner)
std::string topic_in_laser_scan_
name of the topic for subscribing single laserscans ("in_laser_scan")
bool addCloud(const sensor_msgs::PointCloud2ConstPtr &cloud, const cAddCloudParameter &params, const tf::Transform transform=tf::Transform::getIdentity())
int main(int argc, char **argv)
int count_laser_scan_
number of inserted single laserscans
std::string topic_in_laser_full_
name of the topic for subscribing full laserscans ("in_laser_full")
void addPcdLaserScanCallbackSub(const sensor_msgs::PointCloud2ConstPtr &msg)
callback function for receiving a laserscan
cAddCloudParameter params_addcloud_laser_full_
parameter for insertion of full laserscans
void publish(void)
function for publishing all topics (nearfield map and debugging)
void addPcdCameraCallbackSub(const sensor_msgs::PointCloud2ConstPtr &msg)
callback function for receiving a depth images from camera
void addPcdLaserFullCallbackSub(const sensor_msgs::PointCloud2ConstPtr &msg)
callback function for single laserscans
ros::Subscriber sub_camera_
subscriber for depth images from camera


nearfield_map
Author(s):
autogenerated on Sat Feb 27 2021 03:09:46