Jesus/#169 camera info yaml

.github/workflows/ci.yml

@@ -29,6 +29,6 @@ jobs:
     - uses: ros-tooling/[email protected]
       id: action_ros_ci_step
       with:
-        package-name: andino_base andino_bringup andino_control andino_description andino_firmware andino_hardware andino_slam andino_gz_classic andino_navigation
+        package-name: andino_base andino_bringup andino_control andino_description andino_firmware andino_hardware andino_slam andino_gz_classic andino_navigation andino_apps
         target-ros2-distro: ${{ env.ROS_DISTRO }}
         import-token: ${{ secrets.GITHUB_TOKEN }}

README.md

@@ -31,13 +31,16 @@ _Note: For videos go to [Media](#selfie-media) section._
 - :computer: [`andino_gz_classic`](./andino_gz_classic/): [Gazebo Classic](https://classic.gazebosim.org/) simulation of the `andino` robot.
 - :world_map: [`andino_slam`](./andino_slam/): Provides support for SLAM with your `andino` robot.
 - :compass: [`andino_navigation`](./andino_navigation/): Navigation stack based on `nav2`.
+- :exclamation: [`andino_apps`](./andino_apps/): Integrated applications with the `andino` robot.
 
 ## :paperclips: Related projects
 
 Other projects built upon Andino! :rocket:
 
 - :computer: [`andino_gz`](https://github.com/Ekumen-OS/andino_gz): [Gazebo](https://gazebosim.org/home)(non-classic) simulation of the `andino` robot.
 - :lady_beetle: [`andino_webots`](https://github.com/Ekumen-OS/andino_webots): [Webots](https://github.com/cyberbotics/webots) simulation of the Andino robot fully integrated with ROS 2.
+- :joystick: [`andino_o3de`](https://github.com/Ekumen-OS/andino_o3de): [O3DE](https://o3de.org/) simulation of the Andino robot.
+- :green_circle: [`andino_isaac`](https://github.com/Ekumen-OS/andino_isaac): [Isaac Sim](https://docs.omniverse.nvidia.com/isaacsim/latest/index.html) simulation of the Andino robot.
 - :test_tube: [`andino_integration_tests`](https://github.com/Ekumen-OS/andino_integration_tests): Extension to the Andino robot showing how to build integration tests.
 
 ## :pick: Robot Assembly
@@ -105,6 +108,12 @@ source install/setup.bash
 
 `Note`: Whether your are installing the packages in your dev machine or in your robot the procedure is the same.
 
+### Install the binaries
+
+The packages have been also released via ROS package manager system for the 'humble' distro. You can check them [here](https://repo.ros2.org/status_page/ros_humble_default.html?q=andino).
+
+These packages can be installed using `apt` (e.g: `sudo apt install ros-humble-andino-description`) or using `rosdep`.
+
 ## :rocket: Usage
 
 ### Robot bringup
@@ -166,20 +175,19 @@ ros2 launch andino_bringup rviz.launch.py
 
 For starting `rviz2` visualization with a provided configuration.
 
-## :computer: Simulation
-
-The [`andino_gz_classic`](./andino_gz_classic/README.MD) package provides a Gazebo simulation fo the Andino robot.
-
-<img src="./andino_gz_classic/docs/andino_gz_classic.png" width=400/>
-
 ## :compass: Navigation
 
 The [`andino_navigation`](./andino_navigation/README.md) package provides a navigation stack based on the great [Nav2](https://github.com/ros-planning/navigation2) package.
 
 https://github.com/Ekumen-OS/andino/assets/53065142/29951e74-e604-4a6e-80fc-421c0c6d8fee
 
-_Important!: At the moment this package is only working with the simulation. The support for the real robot is forthcoming._
+Follow the [`andino_navigation`'s README](./andino_navigation/README.md) instructions for bringing up the Navigation stack in the real robot or in the simulation.
+
+## :computer: Simulation
+
+The [`andino_gz_classic`](./andino_gz_classic/README.MD) package provides a Gazebo simulation for the Andino robot.
 
+<img src="./andino_gz_classic/docs/andino_gz_classic.png" width=400/>
 
 ## :selfie: Media
 
@@ -214,7 +222,7 @@ This section is dedicated to recognizing and expressing gratitude to the open-so
 
 ## :raised_hands: Contributing
 
-Issues or PRs are always welcome! Please refer to [CONTRIBUTING](CONTRIBUTING.md) doc. 
+Issues or PRs are always welcome! Please refer to [CONTRIBUTING](CONTRIBUTING.md) doc.
 
 ## Code development
 

andino_apps/CHANGELOG.rst

@@ -0,0 +1,9 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package andino_apps
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.0 (2024-04-10)
+------------------
+* Initial version of the package
+* Moved launch file to execute Gazebo classic simulation + Nav2 from `andino_navigation` (`#228 <https://github.com/Ekumen-OS/andino/issues/228>`_)
+* Contributors: Jesús Silva

andino_apps/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.8)
+project(andino_apps)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    launch
+  DESTINATION
+    share/${PROJECT_NAME}/
+)
+
+ament_package()

andino_apps/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2024, Ekumen Inc.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

andino_apps/README.md

@@ -0,0 +1,28 @@
+# Andino Apps
+
+This package contains integration applications with the Andino robot.
+
+# Applications
+
+## Gazebo classic simulation + Nav2
+
+A launch file for running the andino_gz_classic simulation and the Nav 2 stack is provided.
+It uses the [turtlebot3_world](https://github.com/ROBOTIS-GIT/turtlebot3_simulations/tree/master) world (_Apache 2 license_) by default.
+
+```
+ ros2 launch andino_apps andino_simulation_navigation.launch.py
+```
+
+To visualize and interact with the Andino robot in RViz:
+
+- Click in 2D pose estimate button and select the initial pose of the robot
+- Click in Nav2 Goal button and select the final point.
+- The robot will start to move to the selected goal.
+
+![Rviz_example_Nav2](docs/Rviz_example_Nav2.gif)
+
+For further information and examples you can check the [Nav2 tutorials](https://navigation.ros.org/tutorials/index.html).
+
+This package has been tested with the Andino robot with `diff drive plugin` in Gazebo-classic.
+
+By changing the world file, make sure to also change map file. Further navigation [parameters](params/nav2_params.yaml) tunning is recommended.

andino_apps/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>andino_apps</name>
+  <version>0.1.0</version>
+  <description>Package for apps created with andino</description>
+  <author email="[email protected]">JesusSilvaUtrera</author>
+  <maintainer email="[email protected]">JesusSilvaUtrera</maintainer>
+  <maintainer email="[email protected]">Franco Cipollone</maintainer>
+  <license file="LICENSE">BSD Clause 3</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>ros2launch</exec_depend>
+  <exec_depend>andino_gz_classic</exec_depend>
+  <exec_depend>nav2_bringup</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

andino_bringup/README.md

@@ -0,0 +1,26 @@
+# andino_bringup
+
+## Description
+This package contains mainly launch files in order to launch all related driver and nodes to be used in the real robot. Some configuration files are also added to set up the nodes launched in the 'config' folder.
+
+## Launch Files
+
+### Main file
+
+The main launch file is the `andino_robot.launch.py` file. This file executes all necessary nodes for Andino Robot to work.
+
+It includes:
+
+- `andino_description.launch.py`: file to load the 3D model of the robot from the URDF.
+- `andino_control.launch.py`: file for the `ros2_control` nodes and drivers.
+- `camera.launch.py`: file to execute the camera drivers, using `v4l2_camera` package. If not necessary, it can be disabled using the `include_camera` parameter.
+- `rplidar.launch.py`: file to execute the drivers of the RP 2D LiDAR attached to the robot. If not necessary, it can be disabled using the `include_rplidar` parameter.
+
+### Other launch files included
+
+These launch files are included just in case you want to command the robot in different ways, or visualize the information:
+
+- `rosbag_record.launch.py`: executes the `ros2 bag record` command to store a '.bag' file with the specified topics. If no topics are specified, it will record all of them.
+- `rviz.launch.py`: executes the RViz visualization tool with the 'andino.rviz' configuration by default, to be able to visualize the 3D model of the robot and information from the topics.
+- `teleop_joystick.launch.py`: this file allows you to command the robot using a controller with a joystick, using the `teleop_twist_joy` and the `joy_linux` packages.
+- `teleop_keyboard.launch.py`: this file allows you to command the robot using a keyboard, using the `teleop_twist_keyboard` package.

andino_bringup/config/camera_params.yaml

@@ -0,0 +1,6 @@
+v4l2_camera_node:
+  ros__parameters:
+    image_size: [640,480]
+    camera_frame_id: "camera_link"
+    camera_info_url: file:///home/jesus-silva/ws/src/andino/andino_bringup/config/webcam.yaml
+# 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

andino_bringup/config/webcam.yaml

@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 480
+camera_name: hd_webcam:_hd_webcam # imposed by the hardware, there is no easy way to change this name
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [752.509200, 0.000000, 301.733582, 0.000000, 715.922468, 269.603484, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.143335, 0.127413, -0.017265, 0.003433, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [737.296875, 0.000000, 302.288860, 0.000000, 0.000000, 706.384155, 265.509636, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

andino_bringup/launch/camera.launch.py

@@ -30,17 +30,28 @@
 
 from launch import LaunchDescription
 from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+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
+    camera_params_file = DeclareLaunchArgument(
+        'camera_params_file',
+        default_value=join(pkg_andino_bringup, 'config', 'camera_params.yaml'),
+        description='Path to camera intrinsics YAML file'
+    )
 
+    return LaunchDescription([
+        camera_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'
-                }]
-    )
+            parameters=[LaunchConfiguration('camera_params_file')],
+        )
     ])

andino_firmware/README.md

@@ -8,8 +8,25 @@ Check `encoder_driver.h` and `motor_driver.h` files to check the expected pins f
 
 ## Installation
 
+### Arduino
 Verify and Upload `andino_firmware.ino` to your arduino board.
 
+### PlatformIO
+1. Install dependencies `sudo apt-get install python3.10-venv`
+2. Install platformio
+```
+curl -fsSL -o /tmp/get-platformio.py https://raw.githubusercontent.com/platformio/platformio-core-installer/master/get-platformio.py
+python3 /tmp/get-platformio.py
+```
+3. Add platformio to your $PATH:
+```
+echo "PATH=\"\$PATH:\$HOME/.platformio/penv/bin\"" >> $HOME/.bashrc
+source $HOME/.bashrc
+```
+4. Build and upload the firmware
+   - If you're using an arduino uno `pio run --target upload -e uno`
+   - If you're using an arduino nano `pio run --target upload -e nanoatmega328`
+
 ## Description
 
 Via `serial` connection (57600 baud) it is possible to interact with the microcontroller. The interface is described in the [commands.h](src/commands.h) file. Here are the most used commands:

andino_firmware/src/app.cpp

@@ -75,10 +75,13 @@
 #include "motor.h"
 #include "pid.h"
 #include "pwm_out_arduino.h"
+#include "serial_stream_arduino.h"
 #include "shell.h"
 
 namespace andino {
 
+SerialStreamArduino App::serial_stream_;
+
 Shell App::shell_;
 
 DigitalOutArduino App::left_motor_enable_digital_out_(Hw::kLeftMotorEnableGpioPin);
@@ -116,7 +119,7 @@ void App::setup() {
   // Required by Arduino libraries to work.
   init();
 
-  Serial.begin(Constants::kBaudrate);
+  serial_stream_.begin(Constants::kBaudrate);
 
   left_encoder_.begin();
   right_encoder_.begin();
@@ -130,7 +133,7 @@ void App::setup() {
   right_pid_controller_.reset(right_encoder_.read());
 
   // Initialize command shell.
-  shell_.begin(Serial);
+  shell_.set_serial_stream(&serial_stream_);
   shell_.set_default_callback(cmd_unknown_cb);
   shell_.register_command(Commands::kReadAnalogGpio, cmd_read_analog_gpio_cb);
   shell_.register_command(Commands::kReadDigitalGpio, cmd_read_digital_gpio_cb);

andino_firmware/src/app.h

@@ -35,6 +35,7 @@
 #include "motor.h"
 #include "pid.h"
 #include "pwm_out_arduino.h"
+#include "serial_stream_arduino.h"
 #include "shell.h"
 
 namespace andino {
@@ -82,6 +83,9 @@ class App {
   /// Callback method for the `Commands::kSetPidsTuningGains` command.
   static void cmd_set_pid_tuning_gains_cb(int argc, char** argv);
 
+  /// Serial stream.
+  static SerialStreamArduino serial_stream_;
+
   /// Application command shell.
   static Shell shell_;
 

andino_firmware/src/shell.cpp

@@ -33,7 +33,7 @@
 
 namespace andino {
 
-void Shell::begin(Stream& stream) { stream_ = &stream; }
+void Shell::set_serial_stream(const SerialStream* serial_stream) { serial_stream_ = serial_stream; }
 
 void Shell::set_default_callback(CommandCallback callback) { default_callback_ = callback; }
 
@@ -49,8 +49,8 @@ void Shell::register_command(const char* name, CommandCallback callback) {
 }
 
 void Shell::process_input() {
-  while (stream_->available() > 0) {
-    const char input = stream_->read();
+  while (serial_stream_->available() > 0) {
+    const char input = serial_stream_->read();
 
     switch (input) {
       case '\r':