41 #define TIME_FILTER_LENGTH 15 48 user_device_timer_(false),
69 sensor_msgs::ImagePtr image(
new sensor_msgs::Image);
75 image->header.stamp = ros_now;
84 double device_time_in_sec =
static_cast<double>(device_time)/1000000.0;
85 double ros_time_in_sec = ros_now.
toSec();
87 double time_diff = ros_time_in_sec-device_time_in_sec;
93 double corrected_timestamp = device_time_in_sec+filtered_time_diff;
95 image->header.stamp.fromSec(corrected_timestamp);
107 image->data.resize(data_size);
110 image->is_bigendian = 0;
117 image->step =
sizeof(
unsigned char) * 2 * image->width;
121 image->step =
sizeof(
unsigned char) * 2 * image->width;
125 image->step =
sizeof(
unsigned char) * 2 * image->width;
129 image->step =
sizeof(
unsigned char) * 2 * image->width;
134 image->step =
sizeof(
unsigned char) * 3 * image->width;
138 image->step =
sizeof(
unsigned char) * 4 * image->width;
142 image->step =
sizeof(
unsigned char) * 1 * image->width;
146 image->step =
sizeof(
unsigned char) * 2 * image->width;
openni::VideoFrameRef m_frame
FrameCallbackFunction callback_
const void * getData() const
boost::shared_ptr< AstraTimerFilter > timer_filter_
void onNewFrame(openni::VideoStream &stream)
void setUseDeviceTimer(bool enable)
const std::string TYPE_16UC1
Status readFrame(VideoFrameRef *pFrame)
PixelFormat getPixelFormat() const
uint64_t getTimestamp() const
#define TIME_FILTER_LENGTH
const VideoMode & getVideoMode() const