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 readme and ros for python

main
antonio-ardu 3 weeks ago committed by Lee
parent
ef190adf68
commit
32249d6d63
2 changed files with 92 additions and 62 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. 59
      ros2_publisher/README.md
  2. 95
      ros2_publisher/src/arducam/arducam_rclpy_tof_pointcloud/arducam_rclpy_tof_pointcloud/tof_pointcloud.py

59
ros2_publisher/README.md
Unescape View File

@ -1,63 +1,68 @@
## Run the example in the ros2_publisher folder
# ros2_publisher
**RaspberryPi:**
## Dependencies
> Please refer to the instructions above for _*a. Driver Install*_ , _*b.Configuration*_ and _*c.SDK install*_ whice run on RaspberryPi
**Raspberry Pi:**
> Please refer to the instructions above for _*a. Driver Install*_, _*b. Configuration*_, and _*c. SDK Install*_ which run on Raspberry Pi.
**Jetson:**
> Please refer to the instructions above for _*project dependencies*_ and _*SDK install*_ whice run on Jetson
> Please refer to the instructions above for _*project dependencies*_ and _*SDK Install*_ which run on Jetson.
### Ros2 installed on RaspberryPI
### ROS2 Installation
**Note:**
#### On Raspberry Pi
This script only supports RaspberryPi OS bullseye 32-bit,If you want to run ros2 on 64-bit os, you can see this [repository](https://github.com/Ar-Ray-code/rpi-bullseye-ros2)
Install ROS2 on Raspberry Pi. See [ROS2 Installation](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html) for details.
Alternatively, you can use a third-party installation script to install ROS2 on Raspberry Pi. See [this repository](https://github.com/Ar-Ray-code/rpi-bullseye-ros2) for details.
> The example is based on ros2,We found the installation script on github, you can install it as follows, or you can install it yourself. author : [Ar-Ray](https://github.com/Ar-Ray-code/rpi-bullseye-ros2)
#### On Jetson
```Shell
# default : (humble, arm7l)
curl -s https://raw.githubusercontent.com/v1ster/rpi-bullseye-ros2/main/install.bash | bash
```
The example is based on ROS2. Please refer to [ROS2 Installation](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html) and install it yourself.
```Shell
# Environment variable configuration,Take the humble version as an example
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
echo "export ROS_DOMAIN_ID=7" >> ~/.bashrc
source ~/.bashrc
```bash
# Install the compilation tool colcon
sudo apt install python3-colcon-common-extensions
```
### Ros2 installed on Jetson
### ROS2 post-installation
> Please refer to the instructions above for _* Driver Install*_ .
Then export the environment variables to the bashrc file.
The example is based on ros2, please refer to [ros2 installation](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) and install it yourself
-----
> for humble version
```Shell
# Install the compilation tool colcon
sudo apt install python3-colcon-common-extensions
# Environment variable configuration,Take the humble version as an example
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
echo "export ROS_DOMAIN_ID=7" >> ~/.bashrc
source ~/.bashrc
```
#### Compilation
-----
> for jazzy version
```Shell
echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc
echo "export ROS_DOMAIN_ID=7" >> ~/.bashrc
source ~/.bashrc
```
## Compilation
```Shell
cd Arducam_tof_camera/ros2_publisher
colcon build --merge-install
```
#### Run C++ examples
### Run C++ examples
```Shell
. install/setup.bash
ros2 run arducam_rclcpp_tof_pointcloud tof_pointcloud
```
#### Run Python examples
### Run Python examples
```shell
. install/setup.bash
@ -66,7 +71,7 @@ source ~/.bashrc
>You can preview by running rviz2 on the host in the LAN
#### RViz2
### RViz2
> If you encounter an error like this:
> ![rviz::RenderSystem: error](../img/rviz_error.png)

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

@ -1,4 +1,5 @@
from math import tan, pi
from argparse import ArgumentParser
from typing import Optional
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
@ -10,7 +11,6 @@ from threading import Thread
from ArducamDepthCamera import (
ArducamCamera,
CameraInfo,
Connection,
DeviceType,
FrameType,
@ -18,40 +18,77 @@ from ArducamDepthCamera import (
DepthData,
)
class Option:
cfg: Optional[str]
class TOFPublisher(Node):
def __init__(self, tof: ArducamCamera, camera_info: CameraInfo):
def __init__(self, options: Option):
super().__init__("arducam")
tof = self.__init_camera(options)
if tof is None:
raise Exception("Failed to initialize camera")
self.tof_ = tof
if camera_info.device_type == TOFDeviceType.HQVGA:
self.width_ = camera_info.width
self.height_ = camera_info.height
else:
self.width_ = camera_info.width
self.height_ = camera_info.height // 10 - 1
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.publisher_ = self.create_publisher(PointCloud2, "point_cloud", 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.fx = tof.getControl(Control.INTRINSIC_FX) / 100
self.fy = tof.getControl(Control.INTRINSIC_FY) / 100
self.header = Header()
self.header.frame_id = "map"
self.points = None
self.running_ = True
self.timer_ = self.create_timer(1 / 30, self.update)
self.process_point_cloud_thr = Thread(
target=self.__generateSensorPointCloud, daemon=True
)
self.process_point_cloud_thr.start()
def __init_camera(self, options: Option):
print("pointcloud publisher init")
tof = ArducamCamera()
ret = 0
if options.cfg is not None:
ret = tof.openWithFile(options.cfg, 0)
else:
ret = tof.open(Connection.CSI, 0)
if ret != 0:
print("Failed to open camera. Error code:", ret)
return
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 == DeviceType.HQVGA:
self.width_ = info.width
self.height_ = info.height
tof.setControl(Control.RANGE, 4)
elif info.device_type == DeviceType.VGA:
self.width_ = info.width
self.height_ = info.height // 10 - 1
print(f"Open camera success, width: {self.width_}, height: {self.height_}")
print("Pointcloud publisher start")
return tof
def __generateSensorPointCloud(self):
while self.running_:
frame = self.tof_.requestFrame(200)
if frame is not None and isinstance(frame, DepthData):
self.fx = self.tof_.getControl(Control.INTRINSIC_FX) / 100
self.fy = self.tof_.getControl(Control.INTRINSIC_FY) / 100
depth_buf = frame.depth_data
confidence_buf = frame.confidence_data
@ -94,37 +131,25 @@ class TOFPublisher(Node):
def stop(self):
self.running_ = False
self.process_point_cloud_thr.join()
self.tof_.stop()
self.tof_.close()
def main(args=None):
rclpy.init(args=args)
tof = ArducamCamera()
ret = 0
ret = tof.open(Connection.CSI, 0)
if not ret:
print("Failed to open camera. Error code:", ret)
return
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 == DeviceType.HQVGA:
tof.setControl(Control.RANGE, 4)
print("pointcloud publisher start")
tof_publisher = TOFPublisher(tof, info)
parser = ArgumentParser()
parser.add_argument("--cfg", type=str, help="Path to camera configuration file")
ns = parser.parse_args()
options = Option()
options.cfg = ns.cfg
tof_publisher = TOFPublisher(options)
rclpy.spin(tof_publisher)
rclpy.shutdown()
tof_publisher.stop()
tof.stop()
tof.close()
if __name__ == "__main__":

Write Preview
Loading…
Cancel
Save