//////////////////////////////////////////////////////////////////////////////// // Store an image frame into the 'frame' buffer
| 182 | // //////////////////////////////////////////////////////////////////////////////// |
| 183 | // // Store an image frame into the 'frame' buffer |
| 184 | void SR::readData(sensor_msgs::PointCloud &cloud, |
| 185 | sensor_msgs::PointCloud2 &cloud2, |
| 186 | sensor_msgs::Image &image_d, |
| 187 | sensor_msgs::Image &image_i, |
| 188 | sensor_msgs::Image &image_c, |
| 189 | sensor_msgs::Image &image_d16) { |
| 190 | |
| 191 | if (srCam_ == NULL) { |
| 192 | SR_EXCEPT(sr::Exception, "Read attempted on NULL SwissRanger port!"); |
| 193 | return; |
| 194 | } |
| 195 | |
| 196 | |
| 197 | int res; |
| 198 | |
| 199 | double time1 = ros::Time::now().toSec(); |
| 200 | res = SR_Acquire (srCam_); |
| 201 | double time2 = ros::Time::now().toSec(); |
| 202 | if (res < 0) |
| 203 | { |
| 204 | SR_EXCEPT(sr::Exception, "Unable to capture data"); |
| 205 | return; |
| 206 | } |
| 207 | |
| 208 | double timestamp=(time1+time2)/2; |
| 209 | cloud.header.stamp=cloud2.header.stamp=image_d.header.stamp=image_d16.header.stamp= |
| 210 | image_i.header.stamp=image_c.header.stamp=ros::Time(timestamp); |
| 211 | |
| 212 | |
| 213 | size_t image_size = imgEntryArray_->width * imgEntryArray_->height ; |
| 214 | |
| 215 | // Pointers to data |
| 216 | uint8_t *distance_image = (unsigned char*)SR_GetImage (srCam_, 0); |
| 217 | uint8_t *intensity_image = (unsigned char*)SR_GetImage (srCam_, 1); |
| 218 | uint8_t *confidence_image = (unsigned char*)SR_GetImage (srCam_, 2); |
| 219 | |
| 220 | // Points array |
| 221 | res = SR_CoordTrfFlt (srCam_, xp_, yp_, zp_, sizeof (float), |
| 222 | sizeof (float), sizeof (float)); |
| 223 | |
| 224 | cloud.points.resize(image_size); |
| 225 | cloud.channels.resize (2); |
| 226 | cloud.channels[1].name = "confidence"; |
| 227 | cloud.channels[1].values.resize(image_size); |
| 228 | cloud.channels[0].name = "intensity"; |
| 229 | cloud.channels[0].values.resize(image_size); |
| 230 | |
| 231 | cloud2.height = imgEntryArray_->height; |
| 232 | cloud2.width = imgEntryArray_->width; |
| 233 | cloud2.fields.resize (5); |
| 234 | cloud2.fields[0].name = "x"; |
| 235 | cloud2.fields[0].offset = 0; |
| 236 | cloud2.fields[0].count = 1; |
| 237 | cloud2.fields[0].datatype = sensor_msgs::PointField::FLOAT32; |
| 238 | cloud2.fields[1].name = "y"; |
| 239 | cloud2.fields[1].offset = 4; |
| 240 | cloud2.fields[1].count = 1; |
| 241 | cloud2.fields[1].datatype = sensor_msgs::PointField::FLOAT32; |