40 #define TIME_FILTER_LENGTH 15 47 user_device_timer_(false),
68 sensor_msgs::ImagePtr image(
new sensor_msgs::Image);
74 image->header.stamp = ros_now;
81 uint64_t device_time =
m_frame.getTimestamp();
83 double device_time_in_sec =
static_cast<double>(device_time)/1000000.0;
84 double ros_time_in_sec = ros_now.
toSec();
86 double time_diff = ros_time_in_sec-device_time_in_sec;
92 double corrected_timestamp = device_time_in_sec+filtered_time_diff;
94 image->header.stamp.fromSec(corrected_timestamp);
101 image->width =
m_frame.getWidth();
102 image->height =
m_frame.getHeight();
104 std::size_t data_size =
m_frame.getDataSize();
106 image->data.resize(data_size);
107 memcpy(&image->data[0],
m_frame.getData(), data_size);
109 image->is_bigendian = 0;
111 const openni::VideoMode& video_mode =
m_frame.getVideoMode();
112 switch (video_mode.getPixelFormat())
116 image->step =
sizeof(
unsigned char) * 2 * image->width;
120 image->step =
sizeof(
unsigned char) * 2 * image->width;
124 image->step =
sizeof(
unsigned char) * 2 * image->width;
128 image->step =
sizeof(
unsigned char) * 2 * image->width;
133 image->step =
sizeof(
unsigned char) * 3 * image->width;
137 image->step =
sizeof(
unsigned char) * 4 * image->width;
141 image->step =
sizeof(
unsigned char) * 1 * image->width;
145 image->step =
sizeof(
unsigned char) * 2 * image->width;
FrameCallbackFunction callback_
openni::VideoFrameRef m_frame
boost::shared_ptr< OpenNI2TimerFilter > timer_filter_
#define TIME_FILTER_LENGTH
const std::string TYPE_16UC1
void setUseDeviceTimer(bool enable)
void onNewFrame(openni::VideoStream &stream)