Commit 7e915f70 authored by Alex Kushleyev's avatar Alex Kushleyev
Browse files

add option to autodetect TOF camera id

parent 08279f33
Pipeline #241427752 passed with stage
in 57 seconds
......@@ -51,6 +51,9 @@
class BufferManager;
class PerCameraMgr;
#define TOF_RAW_FRAME_WIDTH 224
#define TOF_RAW_FRAME_HEIGHT 1557
// -----------------------------------------------------------------------------------------------------------------------------
// Thread Data for camera request and result thread
// -----------------------------------------------------------------------------------------------------------------------------
......@@ -83,7 +86,7 @@ enum PreviewFormat
{
PreviewFormatNV21 = 0, ///< NV21
PreviewFormatRAW8 = 1, ///< RAW8 (If the camera doesnt support RAW8, RAW10 will be requested and converted to RAW8)
PreviewFormatBLOB = 2, ///< BLOB (TOF camera uses this for preview format)
PreviewFormatBLOB = 2, ///< BLOB (TOF camera uses this for preview format)
};
// -----------------------------------------------------------------------------------------------------------------------------
......@@ -104,7 +107,7 @@ typedef enum
// RAW only mode for devices that will simultaneously use more than two cameras.
// This mode has following limitations: Back end 3A, Face Detect or any additional functionality depending on image/sensor
// statistics and YUV streams will be disabled
QCAMERA3_VENDOR_STREAM_CONFIGURATION_RAW_ONLY_MODE = 0x8000,
} QCamera3VendorStreamConfiguration;
......@@ -165,7 +168,7 @@ private:
// Return if this resolution corresponds to a TOF camera
void DetermineTOFCamera(int width, int height)
{
if ((width == 224) && (height == 1557))
if ((width == TOF_RAW_FRAME_WIDTH) && (height == TOF_RAW_FRAME_HEIGHT))
{
m_isTOF = true;
}
......@@ -308,6 +311,9 @@ public:
// Stop the opened camera and close it
void Stop();
// Automatically determine ID of TOF camera based on output stream dimensions
int FindTofCamera();
private:
// Callback to indicate device status change
static void CameraDeviceStatusChange(const struct camera_module_callbacks* callbacks, int camera_id, int new_status)
......@@ -321,6 +327,8 @@ private:
// Load the camera module to start communicating with it
int LoadCameraModule();
void PrintCameraCapabilities(camera_info * pCameraInfo);
static const uint32_t MaxCameras = 4;
camera_module_t* m_pCameraModule; ///< Camera module
......
......@@ -47,6 +47,82 @@ CameraHAL3::CameraHAL3()
}
}
int CameraHAL3::FindTofCamera()
{
int tofCameraId = -1;
for (int camid=0; camid<m_numCameras; camid++)
{
camera_metadata_t* pStaticMetadata = (camera_metadata_t *)m_cameraInfo[camid].static_camera_characteristics;
camera_metadata_ro_entry entry;
// 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 (status)
{
printf("ERROR: could not find ANDROID_SCALER_AVAILABLE_STREAM_CONFIGURATIONS entry\n");
continue;
}
for (size_t i = 0; i < entry.count; i+=4)
{
if (ANDROID_SCALER_AVAILABLE_STREAM_CONFIGURATIONS_OUTPUT == entry.data.i32[i + 3])
{
//int32_t format = entry.data.i32[i];
int32_t width = entry.data.i32[i+1];
int32_t height = entry.data.i32[i+2];
//printf("STREAM: format %d, width %d, height %d\n",format,width,height);
if ((width==TOF_RAW_FRAME_WIDTH) && (height==TOF_RAW_FRAME_HEIGHT))
{
tofCameraId=camid;
return tofCameraId;
}
}
}
}
return tofCameraId;
}
void CameraHAL3::PrintCameraCapabilities(camera_info * pCameraInfo)
{
camera_metadata_t* pStaticMetadata = (camera_metadata_t *)pCameraInfo->static_camera_characteristics;
camera_metadata_ro_entry entry;
// 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 (status)
{
printf("ERROR: could not find ANDROID_SCALER_AVAILABLE_STREAM_CONFIGURATIONS entry\n");
return;
}
for (size_t i = 0; i < entry.count; i+=4)
{
if (ANDROID_SCALER_AVAILABLE_STREAM_CONFIGURATIONS_OUTPUT == entry.data.i32[i + 3])
{
int32_t format = entry.data.i32[i];
int32_t width = entry.data.i32[i+1];
int32_t height = entry.data.i32[i+2];
if (format == 0x20) //HAL_PIXEL_FORMAT_RAW_SENSOR
{
printf("\n raw size: %dx%d",width,height);
}
if ((format == 0x21) && (width==TOF_RAW_FRAME_WIDTH) && (height==TOF_RAW_FRAME_HEIGHT)) //HAL_PIXEL_FORMAT_BLOB
{
printf("\n raw size: %dx%d (TOF?)",width,height);
}
//printf("STREAM: format %d, width %d, height %d\n",format,width,height);
}
}
}
// -----------------------------------------------------------------------------------------------------------------------------
// Performs any one time initialization. This function should only be called once.
// -----------------------------------------------------------------------------------------------------------------------------
......@@ -83,7 +159,9 @@ int CameraHAL3::Initialize()
if (status == 0)
{
printf("\nCamera Id: %d Facing: %d", i, m_cameraInfo[i].facing);
//printf("\nCamera Id: %d Facing: %d", i, m_cameraInfo[i].facing);
printf("\nCamera Id: %d .. Available raw sizes:", i);
PrintCameraCapabilities(&m_cameraInfo[i]);
}
else
{
......@@ -144,7 +222,7 @@ int CameraHAL3::Start(int cameraid, ///< Camera id to open
{
printf("\nInvalid camera id %d. There are only %d cameras", cameraid, m_numCameras);
}
return status;
}
......@@ -161,4 +239,4 @@ void CameraHAL3::Stop()
m_pPerCameraMgr[i] = NULL;
}
}
}
\ No newline at end of file
}
......@@ -77,7 +77,7 @@ int Hal3MainEnter(int argc, char* const argv[], ros::NodeHandle rosNodeHandle)
bool publishPointCloud;
bool publishLaserScan;
rosNodeHandle.param<int>("camera_id", cameraid, 0);
rosNodeHandle.param<int>("camera_id", cameraid, -1);
rosNodeHandle.param<bool>("publish_depth_image", publishDepthImage, false);
rosNodeHandle.param<bool>("publish_ir_image", publishIRImage, false);
rosNodeHandle.param<bool>("publish_point_cloud", publishPointCloud, false);
......@@ -108,6 +108,18 @@ int Hal3MainEnter(int argc, char* const argv[], ros::NodeHandle rosNodeHandle)
status = g_pCameraHAL3->Initialize();
}
if (cameraid == -1) //autodetect TOF camera id
{
printf("\nUser specified TOF camera id = -1.. Autodetecting..");
cameraid = g_pCameraHAL3->FindTofCamera();
printf("\nFound TOF camera with ID %d\n",cameraid);
}
if (cameraid == -1)
{
status = -1;
}
if (status == 0)
{
// Making this function call will start the camera (cameraid) and it will start streaming frames
......
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