Jesus/#169 camera info yaml (#231)

JesusSilvaUtrera · francocipollone · garyservin · web-flow · commit 8aa4aaab2149 · 2024-06-17T04:51:59.000-03:00
* Improves andino_navigation package. (#224) Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com> Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Add basic Platform IO instructions (#229) Signed-off-by: Gary Servin <gary@servin.dev> Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Modify Shell class API to allow dependency injection (#221) Signed-off-by: Javier Balloffet <javier.balloffet@gmail.com> Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Fixed wheel diameter in URDF (#93) Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Added YAML file for default camera intrinsics (#169) Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Adds references to o3de and isaac_sim simulations. (#232) Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com> Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Added README for andino_bringup (#233) Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Added binaries installation docs (#234) Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * andino_apps package created and andino_navigation package updated (#235) * andino_apps package created and andino_navigation package updated Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Minor changes from the PR Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Update andino_navigation README with changes from PR Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Add andino_apps package to ci.yaml Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Fixed minor issues from the PR Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Added 'andino_apps' to the general README Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> --------- Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Minimal change to test launch file Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Added webcam.yaml file and intrinsics are now publishing Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Added raspicam.yaml with camera intrinsics and updated instructions in andino_hardware/README Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> * Changed the 'camera_info_url' to be relative path Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> --------- Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com> Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com> Signed-off-by: Gary Servin <gary@servin.dev> Signed-off-by: Javier Balloffet <javier.balloffet@gmail.com> Co-authored-by: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Co-authored-by: Gary Servin <gary@servin.dev> Co-authored-by: Javier Balloffet <javier.balloffet@gmail.com>
diff --git a/andino_bringup/config/raspicam.yaml 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
@@ -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
@@ -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 <left> <right>'`
-   - 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 <left> <right>'`
    - Important!: See the `Test it!` section.
diff --git 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
 
