archive
/
Arducam_tof_camera
mirror of https://github.com/ArduCAM/Arducam_tof_camera
1
0
Fork 0
Code Issues Projects Releases Wiki Activity
Browse Source

Update to 0.1.17

main
antonio-ardu 3 months ago committed by Lee
parent
9f5def05d6
commit
096717c5b8
12 changed files with 94 additions and 67 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      .gitignore
  2. 2
      example/c/preview_depth.c
  3. 8
      example/cpp/capture_raw.cpp
  4. 20
      example/cpp/example/test.cpp
  5. 8
      example/cpp/preview_by_usb.cpp
  6. 8
      example/cpp/preview_depth.cpp
  7. 11
      example/python/capture_raw.py
  8. 18
      example/python/preview_depth.py
  9. 26
      open3d_preview/preview_pointcloud.cpp
  10. 2
      ros2_publisher/build.sh
  11. 3
      ros2_publisher/run.sh
  12. 53
      ros2_publisher/src/arducam/arducam_rclpy_tof_pointcloud/arducam_rclpy_tof_pointcloud/tof_pointcloud.py

2
.gitignore vendored
Unescape View File

@ -12,3 +12,5 @@ build/
/example/python/*.json
*.patch
/*.json
/cali.bin

2
example/c/preview_depth.c
Unescape View File

@ -6,7 +6,7 @@ int main()
{
ArducamDepthCamera tof = createArducamDepthCamera();
ArducamFrameBuffer frame;
if (arducamCameraOpen(tof, CSI, 0)) {
if (arducamCameraOpen(tof, CONNECTION_CSI, 0)) {
printf("Failed to open camera\n");
return -1;
}

8
example/cpp/capture_raw.cpp
Unescape View File

@ -50,8 +50,8 @@ int main()
}
// Modify the range also to modify the MAX_DISTANCE
int max_range = 4000;
// tof.setControl(CameraCtrl::RANGE, 4000);
tof.getControl(CameraCtrl::RANGE, &max_range);
// tof.setControl(Control::RANGE, 4000);
tof.getControl(Control::RANGE, &max_range);
CameraInfo info = tof.getCameraInfo();
std::cout << "open camera with (" << info.width << "x" << info.height << ") with range " << max_range << std::endl;
@ -63,8 +63,8 @@ int main()
if (frame == nullptr) {
continue;
}
FrameDataFormat format;
frame->getFrameDataFormat(FrameType::RAW_FRAME, format);
FrameFormat format;
frame->getFormat(FrameType::RAW_FRAME, format);
std::cout << "frame: (" << format.width << "x" << format.height << ")" << std::endl;
int16_t* raw_ptr = (int16_t*)frame->getData(FrameType::RAW_FRAME);

20
example/cpp/example/test.cpp
Unescape View File

@ -271,8 +271,8 @@ LOCAL bool raw_loop(Arducam::ArducamTOFCamera& tof, opt_data& data)
if (frame == nullptr) {
return checkExit(data);
}
Arducam::FrameDataFormat format;
frame->getFrameDataFormat(FrameType::RAW_FRAME, format);
Arducam::FrameFormat format;
frame->getFormat(FrameType::RAW_FRAME, format);
std::cout << "frame: (" << format.width << "x" << format.height << "), time: " << format.timestamp << std::endl;
data.max_height = format.height;
data.max_width = format.width;
@ -306,8 +306,8 @@ LOCAL bool depth_loop(Arducam::ArducamTOFCamera& tof, opt_data& data)
if (frame == nullptr) {
return checkExit(data);
}
Arducam::FrameDataFormat format;
frame->getFrameDataFormat(FrameType::DEPTH_FRAME, format);
Arducam::FrameFormat format;
frame->getFormat(FrameType::DEPTH_FRAME, format);
std::cout << "frame: (" << format.width << "x" << format.height << "), time: " << format.timestamp << std::endl;
data.max_height = format.height;
data.max_width = format.width;
@ -405,7 +405,7 @@ LOCAL bool depth_loop(Arducam::ArducamTOFCamera& tof, opt_data& data)
} \
} while (0)
#define set_ctl_(tof, ctrl, val) check(tof.setControl(CameraCtrl::ctrl, val), "set control(" #ctrl ", " #val ") failed")
#define set_ctl_(tof, ctrl, val) check(tof.setControl(Control::ctrl, val), "set control(" #ctrl ", " #val ") failed")
#define set_ctl(ctrl, val) set_ctl_(tof, ctrl, val)
int main(int argc, char* argv[])
@ -485,7 +485,7 @@ int main(int argc, char* argv[])
if (opt.fps != -1) {
if (opt.fps != 0) {
int max_fps = 0;
tof.getControl(CameraCtrl::FRAME_RATE, &max_fps);
tof.getControl(Control::FRAME_RATE, &max_fps);
if (opt.fps > max_fps) {
std::cerr << "Invalid fps: " << opt.fps << ", max fps: " << max_fps << std::endl;
return -1;
@ -493,17 +493,17 @@ int main(int argc, char* argv[])
set_ctl(FRAME_RATE, opt.fps);
}
// disable auto frame rate if the camera has the control
(void)tof.setControl(CameraCtrl::AUTO_FRAME_RATE, 0);
(void)tof.setControl(Control::AUTO_FRAME_RATE, 0);
}
// int rang = 0;
// tof.setControl(CameraCtrl::RANGE, MAX_DISTANCE);
// tof.getControl(CameraCtrl::RANGE, &rang);
// tof.setControl(Control::RANGE, MAX_DISTANCE);
// tof.getControl(Control::RANGE, &rang);
// std::cout << rang << std::endl;
info = tof.getCameraInfo();
int max_range;
tof.getControl(CameraCtrl::RANGE, &max_range);
tof.getControl(Control::RANGE, &max_range);
std::cout << "open camera with (" << info.width << "x" << info.height << ") with range " << max_range << std::endl;
if (opt.max_range == 0) {

8
example/cpp/preview_by_usb.cpp
Unescape View File

@ -94,8 +94,8 @@ int main()
return -1;
}
// Modify the range also to modify the MAX_DISTANCE
tof.setControl(CameraCtrl::RANGE, MAX_DISTANCE);
tof.getControl(CameraCtrl::RANGE, &max_range);
tof.setControl(Control::RANGE, MAX_DISTANCE);
tof.getControl(Control::RANGE, &max_range);
auto info = tof.getCameraInfo();
std::cout << "open camera with (" << info.width << "x" << info.height << ")" << std::endl;
@ -106,12 +106,12 @@ int main()
cv::setMouseCallback("preview", onMouse);
for (;;) {
Arducam::FrameDataFormat format;
Arducam::FrameFormat format;
frame = tof.requestFrame(200);
if (frame == nullptr) {
continue;
}
frame->getFrameDataFormat(FrameType::DEPTH_FRAME, format);
frame->getFormat(FrameType::DEPTH_FRAME, format);
std::cout << "frame: (" << format.width << "x" << format.height << ")" << std::endl;
max_height = format.height;
max_width = format.width;

8
example/cpp/preview_depth.cpp
Unescape View File

@ -117,8 +117,8 @@ int main()
return -1;
}
// Modify the range also to modify the MAX_DISTANCE
tof.setControl(CameraCtrl::RANGE, MAX_DISTANCE);
tof.getControl(CameraCtrl::RANGE, &max_range);
tof.setControl(Control::RANGE, MAX_DISTANCE);
tof.getControl(Control::RANGE, &max_range);
auto info = tof.getCameraInfo();
std::cout << "open camera with (" << info.width << "x" << info.height << ")" << std::endl;
@ -127,12 +127,12 @@ int main()
cv::setMouseCallback("preview", onMouse);
for (;;) {
Arducam::FrameDataFormat format;
Arducam::FrameFormat format;
frame = tof.requestFrame(200);
if (frame == nullptr) {
continue;
}
frame->getFrameDataFormat(FrameType::DEPTH_FRAME, format);
frame->getFormat(FrameType::DEPTH_FRAME, format);
std::cout << "frame: (" << format.width << "x" << format.height << ")" << std::endl;
max_height = format.height;
max_width = format.width;

11
example/python/capture_raw.py
Unescape View File

@ -4,19 +4,22 @@ import ArducamDepthCamera as ac
def main():
print("Arducam Depth Camera Demo.")
print(" SDK version:", ac.__version__)
cam = ac.ArducamCamera()
cfg_path = None
cfg_path = "/home/user/Downloads/Configuration/SIF2618RM_Tof_4_Modes/SIF2618RM_Tof_4_Modes_MIPI_2Lane_RAW12_8b_640x4810.cfg"
ret = 0
if cfg_path is not None:
ret = cam.openWithFile(cfg_path, 0)
else:
ret = cam.open(ac.TOFConnect.CSI, 0)
ret = cam.open(ac.Connection.CSI, 0)
if ret != 0:
print("initialization failed. Error code:", ret)
return
ret = cam.start(ac.TOFOutput.RAW)
ret = cam.start(ac.FrameType.RAW)
if ret != 0:
print("Failed to start camera. Error code:", ret)
cam.close()
@ -25,7 +28,7 @@ def main():
while True:
frame = cam.requestFrame(2000)
if frame is not None and isinstance(frame, ac.RawData):
buf = frame.getRawData()
buf = frame.raw_data
cam.releaseFrame(frame)
buf = (buf / (1 << 4)).astype(np.uint8)

18
example/python/preview_depth.py
Unescape View File

@ -68,8 +68,12 @@ def usage(argv0):
def main():
print("Arducam Depth Camera Demo.")
print(" SDK version:", ac.__version__)
cam = ac.ArducamCamera()
cfg_path = None
cfg_path = "/home/user/Downloads/Configuration/SIF2618RM_Tof_4_Modes/SIF2618RM_Tof_4_Modes_MIPI_2Lane_RAW12_8b_640x4810.cfg"
# cfg_path = "/home/user/workspace/arducamusb3.0privariety/result/TOF/0_TOF_MIPI_2Lane_RAW12_8b_240x180_60FPS.cfg"
black_color = (0, 0, 0)
white_color = (255, 255, 255)
@ -77,18 +81,18 @@ def main():
if cfg_path is not None:
ret = cam.openWithFile(cfg_path, 0)
else:
ret = cam.open(ac.TOFConnect.CSI, 0)
ret = cam.open(ac.Connection.CSI, 0)
if ret != 0:
print("Failed to open camera. Error code:", ret)
return
ret = cam.start(ac.TOFOutput.DEPTH)
ret = cam.start(ac.FrameType.DEPTH)
if ret != 0:
print("Failed to start camera. Error code:", ret)
cam.close()
return
r = cam.getControl(ac.TOFControl.RANGE)
r = cam.getControl(ac.Control.RANGE)
info = cam.getCameraInfo()
print(f"Camera resolution: {info.width}x{info.height}")
@ -96,7 +100,7 @@ def main():
cv2.namedWindow("preview", cv2.WINDOW_AUTOSIZE)
cv2.setMouseCallback("preview", on_mouse)
if info.device_type == ac.TOFDeviceType.VGA:
if info.device_type == ac.DeviceType.VGA:
# Only VGA support confidence
cv2.createTrackbar(
"confidence", "preview", confidence_value, 255, on_confidence_changed
@ -105,8 +109,8 @@ def main():
while True:
frame = cam.requestFrame(2000)
if frame is not None and isinstance(frame, ac.DepthData):
depth_buf = frame.getDepthData()
confidence_buf = frame.getConfidenceData()
depth_buf = frame.depth_data
confidence_buf = frame.confidence_data
result_image = (depth_buf * (255.0 / r)).astype(np.uint8)
result_image = cv2.applyColorMap(result_image, cv2.COLORMAP_RAINBOW)

26
open3d_preview/preview_pointcloud.cpp
Unescape View File

@ -108,7 +108,7 @@ void onMouse(int event, int x, int y, int flags, void* param)
}
}
bool getControl(ArducamTOFCamera& tof, Arducam::CameraCtrl mode, float& val, float alpha = 1.0)
bool getControl(ArducamTOFCamera& tof, Arducam::Control mode, float& val, float alpha = 1.0)
{
int tmp = 0;
Arducam::TofErrorCode ret = tof.getControl(mode, &tmp);
@ -143,8 +143,8 @@ int main(int argc, char* argv[])
return -1;
}
// Modify the range also to modify the MAX_DISTANCE
tof.setControl(CameraCtrl::RANGE, MAX_DISTANCE);
tof.getControl(CameraCtrl::RANGE, &max_range);
tof.setControl(Control::RANGE, MAX_DISTANCE);
tof.getControl(Control::RANGE, &max_range);
auto info = tof.getCameraInfo();
std::cout << "open camera with (" << info.width << "x" << info.height << ")" << std::endl;
@ -158,10 +158,10 @@ int main(int argc, char* argv[])
float fx, fy, cx, cy;
getControl(tof, CameraCtrl::INTRINSIC_FX, fx, 100);
getControl(tof, CameraCtrl::INTRINSIC_FY, fy, 100);
getControl(tof, CameraCtrl::INTRINSIC_CX, cx, 100);
getControl(tof, CameraCtrl::INTRINSIC_CY, cy, 100);
getControl(tof, Control::INTRINSIC_FX, fx, 100);
getControl(tof, Control::INTRINSIC_FY, fy, 100);
getControl(tof, Control::INTRINSIC_CX, cx, 100);
getControl(tof, Control::INTRINSIC_CY, cy, 100);
std::cout << "fx: " << fx << " fy: " << fy << " cx: " << cx << " cy: " << cy << std::endl;
@ -178,12 +178,12 @@ int main(int argc, char* argv[])
bool first = true;
for (;;) {
Arducam::FrameDataFormat format;
Arducam::FrameFormat format;
frame = tof.requestFrame(200);
if (frame == nullptr) {
continue;
}
frame->getFrameDataFormat(FrameType::DEPTH_FRAME, format);
frame->getFormat(FrameType::DEPTH_FRAME, format);
std::cout << "frame: (" << format.width << "x" << format.height << ")" << std::endl;
max_height = format.height;
max_width = format.width;
@ -220,10 +220,10 @@ int main(int argc, char* argv[])
memcpy(depth_image.data_.data(), depth_frame.data, depth_image.data_.size());
getControl(tof, CameraCtrl::INTRINSIC_FX, fx, 100);
getControl(tof, CameraCtrl::INTRINSIC_FY, fy, 100);
getControl(tof, CameraCtrl::INTRINSIC_CX, cx, 100);
getControl(tof, CameraCtrl::INTRINSIC_CY, cy, 100);
getControl(tof, Control::INTRINSIC_FX, fx, 100);
getControl(tof, Control::INTRINSIC_FY, fy, 100);
getControl(tof, Control::INTRINSIC_CX, cx, 100);
getControl(tof, Control::INTRINSIC_CY, cy, 100);
auto curr_pcd = open3d::geometry::PointCloud::CreateFromDepthImage( //
depth_image, {max_width, max_height, fx, fy, cx, cy}, Eigen::Matrix4d::Identity(), 1000.0, 5000.0);

2
ros2_publisher/build.sh
Unescape View File

@ -0,0 +1,2 @@
#!/bin/bash
colcon build --merge-install

3
ros2_publisher/run.sh
Unescape View File

@ -0,0 +1,3 @@
#!/bin/bash
source install/setup.bash
ros2 run arducam_rclpy_tof_pointcloud tof_pointcloud

53
ros2_publisher/src/arducam/arducam_rclpy_tof_pointcloud/arducam_rclpy_tof_pointcloud/tof_pointcloud.py
Unescape View File

@ -8,13 +8,20 @@ import numpy as np
from threading import Thread
from ArducamDepthCamera import (ArducamCamera, TOFConnect, TOFDeviceType,
TOFOutput, TOFControl, DepthData, ArducamInfo)
from ArducamDepthCamera import (
ArducamCamera,
CameraInfo,
Connection,
DeviceType,
FrameType,
Control,
DepthData,
)
class TOFPublisher(Node):
def __init__(self, tof: ArducamCamera, camera_info: ArducamInfo):
super().__init__('arducam')
class TOFPublisher(Node):
def __init__(self, tof: ArducamCamera, camera_info: CameraInfo):
super().__init__("arducam")
self.tof_ = tof
if camera_info.device_type == TOFDeviceType.HQVGA:
self.width_ = camera_info.width
@ -25,24 +32,28 @@ class TOFPublisher(Node):
self.pointsize_ = self.width_ * self.height_
self.frame_id = "sensor_frame"
self.depth_msg_ = Float32MultiArray()
self.timer_ = self.create_timer(1/30, self.update)
self.timer_ = self.create_timer(1 / 30, self.update)
self.publisher_ = self.create_publisher(PointCloud2, "point_cloud", 10)
self.publisher_depth_ = self.create_publisher(Float32MultiArray, "depth_frame", 10)
self.publisher_depth_ = self.create_publisher(
Float32MultiArray, "depth_frame", 10
)
self.fx = self.width_ / (2 * tan(0.5 * pi * 64.3 / 180))
self.fy = self.height_ / (2 * tan(0.5 * pi * 50.4 / 180))
self.header = Header()
self.header.frame_id = "map"
self.points = None
self.running_ = True
self.process_point_cloud_thr = Thread(target=self.__generateSensorPointCloud, daemon=True)
self.process_point_cloud_thr = Thread(
target=self.__generateSensorPointCloud, daemon=True
)
self.process_point_cloud_thr.start()
def __generateSensorPointCloud(self):
while self.running_:
frame = self.tof_.requestFrame(200)
if frame is not None and isinstance(frame, DepthData):
depth_buf = frame.getDepthData()
confidence_buf = frame.getConfidenceData()
depth_buf = frame.depth_data
confidence_buf = frame.confidence_data
depth_buf[confidence_buf < 30] = 0
@ -63,10 +74,12 @@ class TOFPublisher(Node):
# Combined point cloud
points = np.stack((x, y, z), axis=-1)
self.points = points[~np.isnan(points).any(axis=-1)] # Filter invalid points
self.points = points[
~np.isnan(points).any(axis=-1)
] # Filter invalid points
self.tof_.releaseFrame(frame)
def update(self):
# self.__generateSensorPointCloud()
if self.points is None:
@ -83,25 +96,25 @@ class TOFPublisher(Node):
self.process_point_cloud_thr.join()
def main(args = None):
def main(args=None):
rclpy.init(args=args)
tof = ArducamCamera()
ret = 0
ret = tof.open(TOFConnect.CSI, 0)
ret = tof.open(Connection.CSI, 0)
if not ret:
print("Failed to open camera. Error code:", ret)
return
ret = tof.start(TOFOutput.DEPTH)
ret = tof.start(FrameType.DEPTH)
if ret != 0:
print("Failed to start camera. Error code:", ret)
tof.close()
return
info = tof.getCameraInfo()
if info.device_type == TOFDeviceType.HQVGA:
tof.setControl(TOFControl.RANGE, 4)
if info.device_type == DeviceType.HQVGA:
tof.setControl(Control.RANGE, 4)
print("pointcloud publisher start")
@ -115,4 +128,4 @@ def main(args = None):
if __name__ == "__main__":
main()
main()

Write Preview
Loading…
Cancel
Save