115 std::vector<std::string> topicList;
125 ROS_ERROR(
"No parameter input_scans on parameter server!! Scan unifier can not subscribe to any scan topic!");
130 ROS_WARN(
"No parameter frame on parameter server. Using default value [base_link].");
138 std::vector<sensor_msgs::LaserScan::ConstPtr> current_scans;
139 current_scans.push_back(scan1);
140 current_scans.push_back(scan2);
142 sensor_msgs::LaserScan unified_scan = sensor_msgs::LaserScan();
153 const sensor_msgs::LaserScan::ConstPtr& scan2,
154 const sensor_msgs::LaserScan::ConstPtr& scan3)
156 std::vector<sensor_msgs::LaserScan::ConstPtr> current_scans;
157 current_scans.push_back(scan1);
158 current_scans.push_back(scan2);
159 current_scans.push_back(scan3);
161 sensor_msgs::LaserScan unified_scan = sensor_msgs::LaserScan();
172 const sensor_msgs::LaserScan::ConstPtr& scan2,
173 const sensor_msgs::LaserScan::ConstPtr& scan3,
174 const sensor_msgs::LaserScan::ConstPtr& scan4)
176 std::vector<sensor_msgs::LaserScan::ConstPtr> current_scans;
177 current_scans.push_back(scan1);
178 current_scans.push_back(scan2);
179 current_scans.push_back(scan3);
180 current_scans.push_back(scan4);
182 sensor_msgs::LaserScan unified_scan = sensor_msgs::LaserScan();
201 sensor_msgs::LaserScan& unified_scan)
208 if(!current_scans.empty())
213 vec_cloud_[i].header.stamp = current_scans[i]->header.stamp;
215 << i <<
", at time: " << current_scans[i]->header.stamp <<
" now: " <<
ros::Time::now());
221 ROS_WARN_STREAM(
"Scan unifier skipped scan with " << current_scans[i]->header.stamp
222 <<
" stamp, because of missing tf transform.");
234 unified_scan.header = current_scans.front()->header;
235 unified_scan.header.frame_id =
frame_;
236 unified_scan.angle_increment = M_PI/180.0/2.0;
237 unified_scan.angle_min = -M_PI + unified_scan.angle_increment*0.01;
238 unified_scan.angle_max = M_PI - unified_scan.angle_increment*0.01;
239 unified_scan.time_increment = 0.0;
240 unified_scan.scan_time = current_scans.front()->scan_time;
241 unified_scan.range_min = current_scans.front()->range_min;
242 unified_scan.range_max = current_scans.front()->range_max;
246 unified_scan.ranges.resize(round((unified_scan.angle_max - unified_scan.angle_min) / unified_scan.angle_increment) + 1, unified_scan.range_max);
247 unified_scan.intensities.resize(round((unified_scan.angle_max - unified_scan.angle_min) / unified_scan.angle_increment) + 1, 0.0);
250 ROS_DEBUG(
"unify scans");
253 for (
unsigned int i = 0; i <
vec_cloud_[j].points.size(); i++)
259 if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
261 ROS_DEBUG(
"rejected for nan in point(%f, %f, %f)\n", x, y, z);
265 if (angle < unified_scan.angle_min || angle > unified_scan.angle_max)
267 ROS_DEBUG(
"rejected for angle %f not in range (%f, %f)\n", angle, unified_scan.angle_min, unified_scan.angle_max);
270 int index = std::floor(0.5 + (angle - unified_scan.angle_min) / unified_scan.angle_increment);
271 if(index<0 || index>=unified_scan.ranges.size())
continue;
273 double range_sq = y*y+x*x;
275 if ((
sqrt(range_sq) <= unified_scan.ranges[index]))
278 unified_scan.ranges[index] =
sqrt(range_sq);
280 unified_scan.intensities[index] =
vec_cloud_[j].channels.front().values[i];
289 int main(
int argc,
char** argv)
291 ROS_DEBUG(
"scan unifier: start scan unifier node");
292 ros::init(argc, argv,
"cob_scan_unifier_node");
void publish(const boost::shared_ptr< M > &message) const
std::vector< sensor_msgs::PointCloud > vec_cloud_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & y() const
message_filters::Synchronizer< message_filters::sync_policies::ApproximateTime< sensor_msgs::LaserScan, sensor_msgs::LaserScan > > * synchronizer2_
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
ROSCPP_DECL void spin(Spinner &spinner)
std::vector< message_filters::Subscriber< sensor_msgs::LaserScan > * > message_filter_subscribers_
message_filters::Synchronizer< message_filters::sync_policies::ApproximateTime< sensor_msgs::LaserScan, sensor_msgs::LaserScan, sensor_msgs::LaserScan > > * synchronizer3_
Connection registerCallback(C &callback)
void messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr &first_scanner, const sensor_msgs::LaserScan::ConstPtr &second_scanner)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
tf::TransformListener listener_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
bool unifyLaserScans(const std::vector< sensor_msgs::LaserScan::ConstPtr > ¤t_scans, sensor_msgs::LaserScan &unified_scan)
unifie the scan information from all laser scans in vec_laser_struct_
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
TFSIMD_FORCE_INLINE const tfScalar & z() const
void getParams()
function to load parameters from ros parameter server
int main(int argc, char **argv)
ros::Publisher topicPub_LaserUnified_
bool getParam(const std::string &key, std::string &s) const
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
std::vector< std::string > input_scan_topics
void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, tf::Transformer &tf, double range_cutoff, int channel_options=channel_option::Default)
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
laser_geometry::LaserProjection projector_
message_filters::Synchronizer< message_filters::sync_policies::ApproximateTime< sensor_msgs::LaserScan, sensor_msgs::LaserScan, sensor_msgs::LaserScan, sensor_msgs::LaserScan > > * synchronizer4_