Commit 1fc2e182 authored by Hitendra Gangani's avatar Hitendra Gangani
Browse files

Merge branch 'fix/timestamp' into 'dev'

Fix/timestamp

See merge request voxl-public/voxl-hal3-tof-cam-ros!3
parents 9c4bf8ea 009b9ce7
Pipeline #173940366 passed with stages
in 1 minute and 24 seconds
......@@ -68,6 +68,11 @@
<param name="publish_laser_scan" value="$(arg publish_laser_scan)" />
<!-- for now and to stay compatible with older version, we keep the images upside down -->
<param name="flip_image" value="true" />
<remap from="~voxl_ir_image_raw" to="/voxl_ir_image_raw"/>
<remap from="~voxl_depth_image_raw" to="/voxl_depth_image_raw"/>
<remap from="~voxl_point_cloud" to="/voxl_point_cloud"/>
<remap from="~voxl_laser_scan" to="/voxl_laser_scan"/>
</node>
</group>
......
......@@ -143,7 +143,7 @@ int PerCameraMgr::Initialize(const camera_module_t* pCameraModule, ///< Cam
else if (format == PreviewFormatBLOB)
{
m_previewFormat = HAL_PIXEL_FORMAT_BLOB;
}
}
// Check if the stream configuration is supported by the camera or not. If cameraid doesnt support the stream configuration
// we just exit. The stream configuration is checked into the static metadata associated with every camera.
......@@ -263,10 +263,10 @@ int PerCameraMgr::Initialize(const camera_module_t* pCameraModule, ///< Cam
m_rosNodeHandle.param<bool>("publish_point_cloud", m_publishPointCloud, false);
m_rosNodeHandle.param<bool>("publish_laser_scan", m_publishLaserScan, false);
m_rosIRImagePublisher = m_rosImageTransport.advertiseCamera("/voxl_ir_image_raw", ImageQueueSize, false);
m_rosDepthImagePublisher = m_rosImageTransport.advertiseCamera("/voxl_depth_image_raw", ImageQueueSize, false);
m_rosPointCloudPublisher = m_rosNodeHandle.advertise<sensor_msgs::PointCloud2>("/voxl_point_cloud", PointCloudChannels);
m_rosLaserScanPublisher = m_rosNodeHandle.advertise<sensor_msgs::LaserScan>("/voxl_laser_scan", LaseScanQueueSize);
m_rosIRImagePublisher = m_rosImageTransport.advertiseCamera("voxl_ir_image_raw", ImageQueueSize, false);
m_rosDepthImagePublisher = m_rosImageTransport.advertiseCamera("voxl_depth_image_raw", ImageQueueSize, false);
m_rosPointCloudPublisher = m_rosNodeHandle.advertise<sensor_msgs::PointCloud2>("voxl_point_cloud", PointCloudChannels);
m_rosLaserScanPublisher = m_rosNodeHandle.advertise<sensor_msgs::LaserScan>("voxl_laser_scan", LaseScanQueueSize);
// Example of how to use params
// m_rosNodeHandle.param<int>("scan_width_degrees", scan, 11);
......@@ -319,7 +319,7 @@ int PerCameraMgr::ConfigureStreams()
streamConfig.num_streams = 2; // Implies preview + video
streamConfig.operation_mode = CAMERA3_STREAM_CONFIGURATION_NORMAL_MODE;
}
camera3_stream_t* pStreams[] = { &m_streams[0], &m_streams[1] };
streamConfig.streams = &pStreams[0];
......@@ -452,7 +452,7 @@ int PerCameraMgr::Start()
pthread_condattr_t attr;
pthread_condattr_init(&attr);
pthread_condattr_setclock(&attr, CLOCK_MONOTONIC);
pthread_mutex_init(&m_requestThread.mutex, NULL);
pthread_mutex_init(&m_requestThread.mutex, NULL);
pthread_mutex_init(&m_resultThread.mutex, NULL);
pthread_cond_init(&m_requestThread.cond, &attr);
pthread_cond_init(&m_resultThread.cond, &attr);
......@@ -543,15 +543,15 @@ bool PerCameraMgr::IsStreamConfigSupported(int width, int height, int format)
// Get the list of all stream resolutions supported and then go through each one of them looking for a match
int status = find_camera_metadata_ro_entry(pStaticMetadata, ANDROID_SCALER_AVAILABLE_STREAM_CONFIGURATIONS, &entry);
if ((0 == status) && (0 == (entry.count % 4)))
if ((0 == status) && (0 == (entry.count % 4)))
{
for (size_t i = 0; i < entry.count; i+=4)
for (size_t i = 0; i < entry.count; i+=4)
{
if (ANDROID_SCALER_AVAILABLE_STREAM_CONFIGURATIONS_OUTPUT == entry.data.i32[i + 3])
{
if ((format == entry.data.i32[i]) &&
(width == entry.data.i32[i+1]) &&
(height == entry.data.i32[i+2]))
(height == entry.data.i32[i+2]))
{
isStreamSupported = true;
break;
......@@ -605,7 +605,7 @@ void PerCameraMgr::ProcessOneCaptureResult(const camera3_capture_result* pHalRes
int64_t imageTimestampNsecs;
static struct timespec temp;
clock_gettime(CLOCK_REALTIME, &temp);
imageTimestampNsecs = (temp.tv_sec*1e9) + temp.tv_nsec;
imageTimestampNsecs = (temp.tv_sec*1e9) + temp.tv_nsec;
memset(pCaptureResultData, 0, sizeof(CaptureResultFrameData));
......@@ -733,7 +733,7 @@ void* ThreadPostProcessResult(void* pData)
pid_t tid = syscall(SYS_gettid);
int which = PRIO_PROCESS;
int nice = -10;
setpriority(which, tid, nice);
ThreadData* pThreadData = (ThreadData*)pData;
......@@ -748,14 +748,14 @@ void* ThreadPostProcessResult(void* pData)
{
pthread_mutex_lock(&pThreadData->mutex);
if (pThreadData->msgQueue.empty())
if (pThreadData->msgQueue.empty())
{
struct timespec tv;
clock_gettime(CLOCK_MONOTONIC, &tv);
tv.tv_sec += 1;
// Go to a temporary small sleep waiting for the result frame to arrive
if(pthread_cond_timedwait(&pThreadData->cond, &pThreadData->mutex, &tv) != 0)
if(pthread_cond_timedwait(&pThreadData->cond, &pThreadData->mutex, &tv) != 0)
{
pthread_mutex_unlock(&pThreadData->mutex);
continue;
......@@ -782,7 +782,7 @@ void* ThreadPostProcessResult(void* pData)
{
TOFProcessRAW16(pTOFInterface,
(uint16_t*)pCaptureResultData->pPreviewBufferInfo->vaddr,
pCaptureResultData->timestampNsecs/1000);
pCaptureResultData->timestampNsecs);
}
if (dumpPreviewFrames > 0)
......@@ -911,7 +911,7 @@ void* ThreadIssueCaptureRequests(void* data)
int nice = -10;
int frame_number = -1;
setpriority(which, tid, nice);
while (!pThreadData->stop)
......@@ -992,7 +992,7 @@ bool PerCameraMgr::RoyaleDataDone(const void* pData,
fp = fopen(fileName, "w");
fclose(fp);
fp = fopen(fileName, "a");
const royale::DepthImage *data = static_cast<const royale::DepthImage *> (pData);
fwrite((uint8_t*)&data->timestamp, sizeof (data->timestamp), 1, fp);
......@@ -1016,7 +1016,7 @@ bool PerCameraMgr::RoyaleDataDone(const void* pData,
fp = fopen(fileName, "w");
fclose(fp);
fp = fopen(fileName, "a");
const royale::SparsePointCloud *data = static_cast<const royale::SparsePointCloud *> (pData);
fwrite((uint8_t*)&data->timestamp, sizeof (data->timestamp), 1, fp);
......@@ -1044,7 +1044,7 @@ bool PerCameraMgr::RoyaleDataDone(const void* pData,
fp = fopen(fileName, "w");
fclose(fp);
fp = fopen(fileName, "a");
const royale::DepthData *data = static_cast<const royale::DepthData *> (pData);
int size_points = data->points.size() * sizeof(royale::DepthPoint);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment