From 8aa4aaab2149ed52496a611ae8d56cb5924ae202 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jes=C3=BAs=20Silva?= <79662866+JesusSilvaUtrera@users.noreply.github.com> Date: Mon, 17 Jun 2024 09:51:59 +0200 Subject: [PATCH] Jesus/#169 camera info yaml (#231) * Improves andino_navigation package. (#224) Signed-off-by: Franco Cipollone Signed-off-by: JesusSilvaUtrera * Add basic Platform IO instructions (#229) Signed-off-by: Gary Servin Signed-off-by: JesusSilvaUtrera * Modify Shell class API to allow dependency injection (#221) Signed-off-by: Javier Balloffet Signed-off-by: JesusSilvaUtrera * Fixed wheel diameter in URDF (#93) Signed-off-by: JesusSilvaUtrera * Added YAML file for default camera intrinsics (#169) Signed-off-by: JesusSilvaUtrera * Adds references to o3de and isaac_sim simulations. (#232) Signed-off-by: Franco Cipollone Signed-off-by: JesusSilvaUtrera * Added README for andino_bringup (#233) Signed-off-by: JesusSilvaUtrera * Added binaries installation docs (#234) Signed-off-by: JesusSilvaUtrera * andino_apps package created and andino_navigation package updated (#235) * andino_apps package created and andino_navigation package updated Signed-off-by: JesusSilvaUtrera * Minor changes from the PR Signed-off-by: JesusSilvaUtrera * Update andino_navigation README with changes from PR Signed-off-by: JesusSilvaUtrera * Add andino_apps package to ci.yaml Signed-off-by: JesusSilvaUtrera * Fixed minor issues from the PR Signed-off-by: JesusSilvaUtrera * Added 'andino_apps' to the general README Signed-off-by: JesusSilvaUtrera --------- Signed-off-by: JesusSilvaUtrera * Minimal change to test launch file Signed-off-by: JesusSilvaUtrera * Added webcam.yaml file and intrinsics are now publishing Signed-off-by: JesusSilvaUtrera * Added raspicam.yaml with camera intrinsics and updated instructions in andino_hardware/README Signed-off-by: JesusSilvaUtrera * Changed the 'camera_info_url' to be relative path Signed-off-by: JesusSilvaUtrera --------- Signed-off-by: Franco Cipollone Signed-off-by: JesusSilvaUtrera Signed-off-by: Gary Servin Signed-off-by: Javier Balloffet Co-authored-by: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Co-authored-by: Gary Servin Co-authored-by: Javier Balloffet --- andino_bringup/config/raspicam.yaml | 29 ++++++++++++++++++++++++++ andino_bringup/launch/camera.launch.py | 25 +++++++++++++++++----- andino_firmware/README.md | 2 +- andino_hardware/README.md | 27 ++++++++++++++++++++++++ 4 files changed, 77 insertions(+), 6 deletions(-) create mode 100644 andino_bringup/config/raspicam.yaml diff --git a/andino_bringup/config/raspicam.yaml b/andino_bringup/config/raspicam.yaml new file mode 100644 index 00000000..c344de09 --- /dev/null +++ b/andino_bringup/config/raspicam.yaml @@ -0,0 +1,29 @@ +# Calibration tutorial (from Nav2 docs): https://docs.nav2.org/tutorials/docs/camera_calibration.html +# Other info for this Raspberry Pi Camera Module V2 can be found here: https://elinux.org/Rpi_Camera_Module#Technical_Parameters_.28v.2_board.29 + +image_width: 640 +image_height: 480 +camera_name: narrow_stereo +camera_matrix: + rows: 3 + cols: 3 + data: [502.10979, 0. , 315.89371, + 0. , 499.35622, 233.15895, + 0. , 0. , 1. ] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [0.150091, -0.263342, 0.002136, -0.004508, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1., 0., 0., + 0., 1., 0., + 0., 0., 1.] +projection_matrix: + rows: 3 + cols: 4 + data: [510.31363, 0. , 312.43595, 0. , + 0. , 510.29425, 233.43648, 0. , + 0. , 0. , 1. , 0. ] diff --git a/andino_bringup/launch/camera.launch.py b/andino_bringup/launch/camera.launch.py index 5d60f9f7..0f1eab1d 100644 --- a/andino_bringup/launch/camera.launch.py +++ b/andino_bringup/launch/camera.launch.py @@ -30,17 +30,32 @@ from launch import LaunchDescription from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution +from ament_index_python.packages import get_package_share_directory +from os.path import join + +pkg_andino_bringup = get_package_share_directory('andino_bringup') def generate_launch_description(): - return LaunchDescription([ + # Declare launch argument for the path to the camera params YAML file (the 'file://' part is mandatory, you can't skip it) + intrinsic_params_file = DeclareLaunchArgument( + 'intrinsic_params_file', + default_value='file://' + join(pkg_andino_bringup, 'config', 'raspicam.yaml'), + description='Path to camera intrinsics YAML file' + ) + return LaunchDescription([ + intrinsic_params_file, Node( package='v4l2_camera', executable='v4l2_camera_node', + name='v4l2_camera_node', output='screen', parameters=[{ - 'image_size': [640,480], - 'camera_frame_id': 'camera_link' - }] - ) + 'image_size': [640, 480], + 'camera_frame_id': 'camera_link', + 'camera_info_url': LaunchConfiguration('intrinsic_params_file'), + }], + ) ]) diff --git a/andino_firmware/README.md b/andino_firmware/README.md index cb1e2d8e..0cc3b5b5 100644 --- a/andino_firmware/README.md +++ b/andino_firmware/README.md @@ -37,7 +37,7 @@ Via `serial` connection (57600 baud) it is possible to interact with the microco - Get encoder values: `'e'` - Set open-loop speed for the motors[pwm] `'o '` - - Example to move forward full speed: `'o 255 255'` + - Example to move forward full speed: `'o 255 255'` - Range `[-255 -> 255]` - Set closed-loop speed for the motors[ticks/sec] `'m '` - Important!: See the `Test it!` section. diff --git a/andino_hardware/README.md b/andino_hardware/README.md index cadafd32..52a9e53c 100644 --- a/andino_hardware/README.md +++ b/andino_hardware/README.md @@ -151,6 +151,33 @@ Check camera status: vcgencmd get_camera ``` +If the output of the previous command is `supported=1 detected=1', everything is fine. If not, your camera won't work correctly, you need to perform some configuration first. + +Modify the `config.txt` file for the boot: + +```sh + sudo nano /boot/firmware/config.txt +``` + +And add these lines: + +``` +# Autoload overlays for any recognized cameras or displays that are attached +# to the CSI/DSI ports. Please note this is for libcamera support, *not* for +# the legacy camera stack +start_x=1 +gpu_mem=128 +``` + +Save and close the file. Then we need to enable the camera support for the raspberry: + +```sh +sudo raspi-config +``` + +Go to `Interface Options`, select `camera` and enable it. + +Finally, you just need to reboot and the camera should be working fine. #### RPLidar installation