Commit 2ceec071 authored by Alex Kushleyev's avatar Alex Kushleyev
Browse files

changes to support selecting tof mode and fps

parent 1401822d
Pipeline #249843197 failed with stages
in 41 seconds
......@@ -51,8 +51,12 @@
class BufferManager;
class PerCameraMgr;
#define TOF_RAW_FRAME_WIDTH 224
#define TOF_RAW_FRAME_HEIGHT 1557
#define TOF_RAW_FRAME_MODE5_WIDTH 224
#define TOF_RAW_FRAME_MODE5_HEIGHT 865
#define TOF_RAW_FRAME_MODE9_WIDTH 224
#define TOF_RAW_FRAME_MODE9_HEIGHT 1557
#define TOF_RAW_FRAME_MODE11_WIDTH 224
#define TOF_RAW_FRAME_MODE11_HEIGHT 1903
// -----------------------------------------------------------------------------------------------------------------------------
// Thread Data for camera request and result thread
......@@ -136,6 +140,7 @@ public:
int cameraid,
int width,
int height,
int framerate,
PreviewFormat format,
CameraMode mode,
int tofdatatype,
......@@ -168,9 +173,20 @@ private:
// Return if this resolution corresponds to a TOF camera
void DetermineTOFCamera(int width, int height)
{
if ((width == TOF_RAW_FRAME_WIDTH) && (height == TOF_RAW_FRAME_HEIGHT))
if ((width == TOF_RAW_FRAME_MODE5_WIDTH) && (height == TOF_RAW_FRAME_MODE5_HEIGHT))
{
m_isTOF = true;
m_tofRange = RoyaleDistanceRange::SHORT_RANGE;
}
else if ((width == TOF_RAW_FRAME_MODE9_WIDTH) && (height == TOF_RAW_FRAME_MODE9_HEIGHT))
{
m_isTOF = true;
m_tofRange = RoyaleDistanceRange::LONG_RANGE;
}
else if ((width == TOF_RAW_FRAME_MODE11_WIDTH) && (height == TOF_RAW_FRAME_MODE11_HEIGHT))
{
m_isTOF = true;
m_tofRange = RoyaleDistanceRange::EXTRA_LONG_RANGE;
}
else
{
......@@ -219,7 +235,7 @@ private:
static const uint32_t MaxPreviewBuffers = 16;
static const uint32_t MaxVideoBuffers = 16;
static const uint8_t FrameRate = 30;
// camera3_callback_ops is returned to us in every result callback. We piggy back any private information we may need at
// the time of processing the frame result. When we register the callbacks with the camera module, we register the starting
......@@ -249,6 +265,8 @@ private:
const camera_info* m_pCameraInfo; ///< Camera info
int m_width; ///< Width
int m_height; ///< Height
uint32_t m_frameRate;
RoyaleDistanceRange m_tofRange;
int m_tofdatatype; ///< TOF datatype
int m_tofnumframes; ///< Number of TOF frames to dump
const char* m_pVideoFilename; ///< Video filename
......@@ -301,6 +319,7 @@ public:
int Start(int cameraid,
int width,
int height,
int framerate,
PreviewFormat format,
CameraMode mode,
int tofdatatype,
......
......@@ -28,6 +28,7 @@
<arg name="namespace" default="tof" />
<arg name="camera_id" default="$(env TOF_CAM_ID)" />
<arg name="frame_rate" default="30" />
<arg name="tof_mode" default="9" />
<arg name="auto_exposure" default="false" />
<arg name="exposure_time" default="800" />
......@@ -56,6 +57,7 @@
<node name="voxl_hal3_tof_cam_ros_node" type="voxl_hal3_tof_cam_ros_node" pkg="voxl_hal3_tof_cam_ros" output="screen">
<param name="camera_id" value="$(arg camera_id)" />
<param name="frame_rate" value="$(arg frame_rate)" />
<param name="tof_mode" value="$(arg tof_mode)" />
<param if="$(arg auto_exposure)" name="auto_exposure" value="1" />
<param unless="$(arg auto_exposure)" name="auto_exposure" value="0" />
<param unless="$(arg auto_exposure)" name="exposure_time" value="$(arg exposure_time)"/>
......@@ -77,4 +79,3 @@
</group>
</launch>
......@@ -74,7 +74,8 @@ int CameraHAL3::FindTofCamera()
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))
//if ((width==TOF_RAW_FRAME_WIDTH) && (height==TOF_RAW_FRAME_HEIGHT))
if (TOFIsTofCam(width,height))
{
tofCameraId=camid;
return tofCameraId;
......@@ -113,10 +114,17 @@ void CameraHAL3::PrintCameraCapabilities(camera_info * pCameraInfo)
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
//HAL_PIXEL_FORMAT_BLOB
if ((format == 0x21) && TOFIsTofCam(width,height))
{
printf("\n raw size: %dx%d (TOF?)",width,height);
}
/*
if ((format == 0x21) && (width==TOF_RAW_FRAME_MODE9_WIDTH) && (height==TOF_RAW_FRAME_MODE9_HEIGHT))
{
printf("\n raw size: %dx%d (TOF?)",width,height);
}
*/
//printf("STREAM: format %d, width %d, height %d\n",format,width,height);
}
......@@ -185,6 +193,7 @@ int CameraHAL3::Initialize()
int CameraHAL3::Start(int cameraid, ///< Camera id to open
int width, ///< Image buffer width
int height, ///< Image buffer height
int framerate, ///< Desired frame rate
PreviewFormat format, ///< Image buffer format
CameraMode mode, ///< Preview / Video
int tofdatatype, ///< Tof data type
......@@ -205,6 +214,7 @@ int CameraHAL3::Start(int cameraid, ///< Camera id to open
cameraid,
width,
height,
framerate,
format,
mode,
tofdatatype,
......
......@@ -67,6 +67,8 @@ int Hal3MainEnter(int argc, char* const argv[], ros::NodeHandle rosNodeHandle)
int cameraid = 0;
int width = 224;
int height = 1557;
int framerate = 30;
int tofmode = 9; //5 or 9
int format = PreviewFormatBLOB;
int mode = CameraModePreview;
int tofdatatype = 0; // 1 << modalai::RoyaleListenerType::LISTENER_IR_IMAGE;
......@@ -78,11 +80,23 @@ int Hal3MainEnter(int argc, char* const argv[], ros::NodeHandle rosNodeHandle)
bool publishLaserScan;
rosNodeHandle.param<int>("camera_id", cameraid, -1);
rosNodeHandle.param<int>("frame_rate", framerate, 30);
rosNodeHandle.param<int>("tof_mode", tofmode, 9);
rosNodeHandle.param<bool>("publish_depth_image", publishDepthImage, false);
rosNodeHandle.param<bool>("publish_ir_image", publishIRImage, false);
rosNodeHandle.param<bool>("publish_point_cloud", publishPointCloud, false);
rosNodeHandle.param<bool>("publish_laser_scan", publishLaserScan, false);
if (TOFGetFrameSize((RoyaleDistanceRange)tofmode, width, height))
{
printf("\n\nERROR: Could not get TOF frame size from mode (%d) - it should be 5 (short range) or 9 (long range)\n",tofmode);
return -1;
}
printf("\nUsing TOF mode %d, fps=%d, will request frame size %dx%d",tofmode,framerate,width,height);
if (publishDepthImage == true)
{
printf("\n\tDEPTH_IMAGE");
......@@ -131,6 +145,7 @@ int Hal3MainEnter(int argc, char* const argv[], ros::NodeHandle rosNodeHandle)
status = g_pCameraHAL3->Start(cameraid,
width,
height,
framerate,
(PreviewFormat)format,
(CameraMode)mode,
tofdatatype,
......
......@@ -76,6 +76,8 @@ PerCameraMgr::PerCameraMgr(ros::NodeHandle rosNodeHandle) ///< ROS node han
m_previewFormat = 0;
m_width = 0;
m_height = 0;
m_frameRate = 30;
m_tofRange = RoyaleDistanceRange::LONG_RANGE;
for (uint32_t i = 0; i < StreamTypeMax; i++)
{
......@@ -104,6 +106,7 @@ int PerCameraMgr::Initialize(const camera_module_t* pCameraModule, ///< Cam
int cameraid, ///< Camera id managed
int width, ///< Image buffer width that will be streamed
int height, ///< Image buffer height that will be streamed
int framerate, ///< Desired frame rate
PreviewFormat format, ///< Image buffer format that will be streamed
CameraMode mode, ///< Preview / Video mode
int tofdatatype, ///< TOF datatype
......@@ -119,6 +122,7 @@ int PerCameraMgr::Initialize(const camera_module_t* pCameraModule, ///< Cam
m_cameraMode = mode;
m_width = width;
m_height = height;
m_frameRate = framerate;
m_tofdatatype = tofdatatype;
m_tofnumframes = tofnumframes;
m_pVideoFilename = pVideoFilename;
......@@ -231,7 +235,8 @@ int PerCameraMgr::Initialize(const camera_module_t* pCameraModule, ///< Cam
initializationData.pDataTypes = &dataTypes[0];
initializationData.numDataTypes = numDataTypes;
initializationData.pListener = this;
initializationData.frameRate = FrameRate;
initializationData.frameRate = m_frameRate;
initializationData.range = m_tofRange;
status = TOFInitialize(&initializationData);
}
......
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