51 sensor_msgs::LaserScan temp;
52 temp.intensities.reserve(scan.intensities.size());
53 temp.ranges.reserve(scan.ranges.size());
54 const bool has_intensities = scan.intensities.size() > 0 ? true :
false;
56 for (
int i = scan.ranges.size(); i != 0; i--)
58 temp.ranges.push_back(scan.ranges[i]);
61 temp.intensities.push_back(scan.intensities[i]);
65 scan.ranges = temp.ranges;
66 scan.intensities = temp.intensities;
73 : nh_(nh), tf_(
tf), base_frame_(base_frame)
106 bool is_360_lidar =
false;
107 const float angular_range = std::fabs(
scan_.angle_max -
scan_.angle_min);
108 if (std::fabs(angular_range - 2.0 * M_PI) < (
scan_.angle_increment - (std::numeric_limits<float>::epsilon() * 2.0f*M_PI))) {
114 if (angular_range > 6.10865 && (angular_range /
scan_.angle_increment) + 1 ==
scan_.ranges.size()) {
115 is_360_lidar =
false;
120 double max_laser_range = 25;
122 if (max_laser_range >
scan_.range_max)
124 ROS_WARN(
"maximum laser range setting (%.1f m) exceeds the capabilities "
125 "of the used Lidar (%.1f m)",
126 max_laser_range,
scan_.range_max);
127 max_laser_range =
scan_.range_max;
135 geometry_msgs::TransformStamped laser_ident;
136 laser_ident.header.stamp =
scan_.header.stamp;
137 laser_ident.header.frame_id =
frame_;
138 laser_ident.transform.rotation.w = 1.0;
143 ROS_DEBUG(
"laser %s's pose wrt base: %.3f %.3f %.3f %.3f",
148 geometry_msgs::Vector3Stamped laser_orient;
149 laser_orient.vector.z = laser_orient.vector.y = 0.;
150 laser_orient.vector.z = 1 +
laser_pose_.transform.translation.z;
151 laser_orient.header.stamp =
scan_.header.stamp;
155 if (laser_orient.vector.z <= 0)
157 ROS_DEBUG(
"laser is mounted upside-down");
167 current_scans_ = std::make_unique<std::vector<sensor_msgs::LaserScan> >();