| 145 | } |
| 146 | |
| 147 | void ObstacleLayer::LaserScanValidInfoCallback(const sensor_msgs::LaserScanConstPtr &raw_message, |
| 148 | const std::shared_ptr<ObservationBuffer> &buffer) { |
| 149 | float epsilon = 0.0001, range; |
| 150 | sensor_msgs::LaserScan message = *raw_message; |
| 151 | for (size_t i = 0; i < message.ranges.size(); i++) { |
| 152 | range = message.ranges[i]; |
| 153 | if (!std::isfinite(range) && range > 0) { |
| 154 | message.ranges[i] = message.range_max - epsilon; |
| 155 | } |
| 156 | } |
| 157 | sensor_msgs::PointCloud2 cloud; |
| 158 | cloud.header = message.header; |
| 159 | try { |
| 160 | projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_); |
| 161 | } |
| 162 | catch (tf::TransformException &ex) { |
| 163 | ROS_ERROR("High fidelity enabled, but TF returned a transform exception to frame %s: %s", \ |
| 164 | global_frame_.c_str(), ex.what()); |
| 165 | projector_.projectLaser(message, cloud); |
| 166 | } |
| 167 | buffer->Lock(); |
| 168 | buffer->BufferCloud(cloud); |
| 169 | buffer->Unlock(); |
| 170 | } |
| 171 | |
| 172 | void ObstacleLayer::UpdateBounds(double robot_x, |
| 173 | double robot_y, |
nothing calls this directly
no test coverage detected