Merge branch 'humble' into jesus/#237_assembly_process

.gitignore

@@ -6,3 +6,6 @@
 
 # Visual Studio Code files
 .vscode
+
+# PyCache
+*.pyc

andino_description/CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.8)
 project(andino_description)
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 
 install(
   DIRECTORY
@@ -14,4 +15,19 @@ install(
     share/${PROJECT_NAME}/
 )
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+  set(_pytest_tests
+    test/test_xacro_processing.py
+  )
+  foreach(_test_path ${_pytest_tests})
+    get_filename_component(_test_name ${_test_path} NAME_WE)
+    ament_add_pytest_test(${_test_name} ${_test_path}
+      APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
+      TIMEOUT 60
+      WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+    )
+  endforeach()
+endif()
+
 ament_package()

andino_description/README.md

@@ -5,6 +5,14 @@ This package holds the urdf description of the robot.
 
 <img src="docs/robot_rviz.png">
 
+In the `urdf` folder you have the URDF files that contain the description of the robot, divided in different modules and merged into `andino.urdf.xacro` file.
+
+## Configuration
+
+In case you want to change the physical properties of some of the components of the robot, you can do it modifying the default YAML files inside the `config/andino` folder.
+
+You can even add your own configuration files in another directory in the `config` folder, and pass this directory to the main file using the `yaml_config_dir` xacro argument on the launch files.
+
 ## Launch Files
 
 For launching robot state publisher for filling up static tf information and serving the description of the robot. Typically used during robot bringup.
@@ -16,5 +24,3 @@ For launching the robot state publisher and providing some visualization with rv
 ```
 ros2 launch andino_description view_andino.launch.py
 ```
-
-

andino_description/config/andino/hardware.yaml

@@ -0,0 +1,6 @@
+left_wheel_name: left_wheel_joint
+right_wheel_name: right_wheel_joint
+serial_device: /dev/ttyUSB_ARDUINO
+baud_rate: 57600
+timeout: 1000
+enc_ticks_per_rev: 585

andino_description/launch/andino_description.launch.py

@@ -31,7 +31,7 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
+from launch import LaunchDescription, LaunchContext
 from launch.actions import DeclareLaunchArgument
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration
@@ -49,7 +49,8 @@ def generate_launch_description():
     pkg_andino_description = get_package_share_directory('andino_description')
 
     # Obtain urdf from xacro files.
-    doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro'))
+    arguments = {'yaml_config_dir': os.path.join(pkg_andino_description, 'config', 'andino')}
+    doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro'), mappings = arguments)
     robot_desc = doc.toprettyxml(indent='  ')
     params = {'robot_description': robot_desc,
               'publish_frequency': 30.0}

andino_description/launch/view_andino.launch.py

@@ -53,7 +53,8 @@ def generate_launch_description():
     pkg_andino_description = get_package_share_directory('andino_description')
 
     # Obtain urdf from xacro files.
-    doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro'))
+    arguments = {'yaml_config_dir': os.path.join(pkg_andino_description, 'config', 'andino')}
+    doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro'), mappings = arguments)
     robot_desc = doc.toprettyxml(indent='  ')
     params = {'robot_description': robot_desc,
               'publish_frequency': 30.0}

andino_description/package.xml

@@ -9,13 +9,17 @@
   <license file="LICENSE">BSD Clause 3</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <exec_depend>joint_state_publisher_gui</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>ros2launch</exec_depend>
   <exec_depend>rviz2</exec_depend>
   <exec_depend>xacro</exec_depend>
 
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>ament_index_python</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

andino_description/test/test_xacro_processing.py

@@ -0,0 +1,45 @@
+# 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.
+
+import os
+import pytest
+import xacro
+from ament_index_python.packages import get_package_share_directory
+
+def test_xacro_processing():
+    """Test main xacro file (andino.urdf.xacro) processing"""
+    # Get the file path.
+    xacro_file_path = os.path.join(get_package_share_directory("andino_description"), 'urdf', 'andino.urdf.xacro')
+
+    # Test xacro processing.
+    try:
+        xacro.process_file(xacro_file_path)
+    except Exception as e:
+        pytest.fail(f"Xacro processing failed: {e}")

andino_description/urdf/andino.urdf.xacro

@@ -30,6 +30,10 @@
   <xacro:property name="sensor_yaml" value="$(arg yaml_config_dir)/sensors.yaml" />
   <xacro:property name="sensor_prop" value="${xacro.load_yaml(sensor_yaml)}"/>
 
+  <xacro:property name="hardware_yaml" value="$(arg yaml_config_dir)/hardware.yaml" />
+  <xacro:property name="hardware_props" value="${xacro.load_yaml(hardware_yaml)}"/>
+
+
   <!-- Footprint link -->
   <xacro:footprint wheel_props="${wheel_props}" />
 
@@ -89,6 +93,7 @@
   <!-- ros2_control -->
   <xacro:if value="$(arg use_real_ros_control)">
     <xacro:include filename="$(find ${package_name})/urdf/include/andino_control.urdf.xacro" />
+    <xacro:control hardware_props="${hardware_props}" />
   </xacro:if>
 
 

andino_description/urdf/include/andino_control.urdf.xacro

@@ -1,34 +1,43 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <!-- TODO(francocipollone): This xacro should be part of andino_control package instead.-->
-  <ros2_control name="RealRobot" type="system">
-    <hardware>
-      <plugin>andino_base/DiffDriveAndino</plugin>
-      <!-- TODO(francocipollone): Parameters like loop_rate, device, baud_rate, etc should be loaded from a file or passed via the launch file as xacro args -->
-      <param name="left_wheel_name">left_wheel_joint</param>
-      <param name="right_wheel_name">right_wheel_joint</param>
-      <param name="serial_device">/dev/ttyUSB_ARDUINO</param>
-      <param name="baud_rate">57600</param>
-      <param name="timeout">1000</param>
-      <param name="enc_ticks_per_rev">585</param>
-    </hardware>
-    <joint name="left_wheel_joint">
-      <command_interface name="velocity">
-        <param name="min">-5</param>
-        <param name="max">5</param>
-      </command_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="position"/>
-    </joint>
-    <joint name="right_wheel_joint">
-      <command_interface name="velocity">
-        <param name="min">-5</param>
-        <param name="max">5</param>
-      </command_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="position"/>
-    </joint>
-  </ros2_control>
+<!-- ===================== Control xacro =========================================
+
+  params:
+  - hardware_props [dictionary]: hardware properties loaded from the YAML file.
+-->
+
+  <xacro:macro name="control" params="hardware_props" >
+
+    <!-- TODO(francocipollone): This xacro should be part of andino_control package instead.-->
+    <ros2_control name="RealRobot" type="system">
+      <hardware>
+        <plugin>andino_base/DiffDriveAndino</plugin>
+        <param name="left_wheel_name">${hardware_props['left_wheel_name']}</param>
+        <param name="right_wheel_name">${hardware_props['right_wheel_name']}</param>
+        <param name="serial_device">${hardware_props['serial_device']}</param>
+        <param name="baud_rate">${hardware_props['baud_rate']}</param>
+        <param name="timeout">${hardware_props['timeout']}</param>
+        <param name="enc_ticks_per_rev">${hardware_props['enc_ticks_per_rev']}</param>
+      </hardware>
+      <joint name="left_wheel_joint">
+        <command_interface name="velocity">
+          <param name="min">-5</param>
+          <param name="max">5</param>
+        </command_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+      </joint>
+      <joint name="right_wheel_joint">
+        <command_interface name="velocity">
+          <param name="min">-5</param>
+          <param name="max">5</param>
+        </command_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+      </joint>
+    </ros2_control>
+
+  </xacro:macro>
 
 </robot>

andino_firmware/README.md

@@ -9,6 +9,9 @@ Check `encoder_driver.h` and `motor_driver.h` files to check the expected pins f
 ## Installation
 
 ### Arduino
+In Arduino IDE, go to `tools->Manage Libraries ...` and install:
+- "Adafruit BNO055"
+
 Verify and Upload `andino_firmware.ino` to your arduino board.
 
 ### PlatformIO

andino_firmware/platformio.ini

@@ -22,6 +22,12 @@ platform = atmelavr
 framework = arduino
 monitor_speed = 57600
 test_ignore = desktop/*
+lib_deps =
+  Wire
+  SPI
+  adafruit/Adafruit BNO055
+  adafruit/Adafruit BusIO
+  adafruit/Adafruit Unified Sensor
 
 ; Base configuration for desktop platforms (for unit testing).
 [base_desktop]

andino_firmware/src/app.cpp

@@ -64,7 +64,11 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "app.h"
 
+#include <Adafruit_BNO055.h>
+#include <Adafruit_Sensor.h>
 #include <Arduino.h>
+#include <Wire.h>
+#include <utility/imumaths.h>
 
 #include "commands.h"
 #include "constants.h"
@@ -115,6 +119,10 @@ unsigned long App::last_pid_computation_{0};
 
 unsigned long App::last_set_motors_speed_cmd_{0};
 
+bool App::is_imu_connected{false};
+
+Adafruit_BNO055 App::bno055_imu_{/*sensorID=*/55, BNO055_ADDRESS_A, &Wire};
+
 void App::setup() {
   // Required by Arduino libraries to work.
   init();
@@ -142,6 +150,14 @@ void App::setup() {
   shell_.register_command(Commands::kSetMotorsSpeed, cmd_set_motors_speed_cb);
   shell_.register_command(Commands::kSetMotorsPwm, cmd_set_motors_pwm_cb);
   shell_.register_command(Commands::kSetPidsTuningGains, cmd_set_pid_tuning_gains_cb);
+  shell_.register_command(Commands::kGetIsImuConnected, cmd_get_is_imu_connected_cb);
+  shell_.register_command(Commands::kReadEncodersAndImu, cmd_read_encoders_and_imu_cb);
+
+  // Initialize IMU sensor.
+  if (bno055_imu_.begin()) {
+    bno055_imu_.setExtCrystalUse(true);
+    is_imu_connected = true;
+  }
 }
 
 void App::loop() {
@@ -171,10 +187,10 @@ void App::adjust_motors_speed() {
   int right_motor_speed = 0;
   left_pid_controller_.compute(left_encoder_.read(), left_motor_speed);
   right_pid_controller_.compute(right_encoder_.read(), right_motor_speed);
-  if (left_pid_controller_.enabled()){
+  if (left_pid_controller_.enabled()) {
     left_motor_.set_speed(left_motor_speed);
   }
-  if (right_pid_controller_.enabled()){
+  if (right_pid_controller_.enabled()) {
     right_motor_.set_speed(right_motor_speed);
   }
 }
@@ -299,4 +315,47 @@ void App::cmd_set_pid_tuning_gains_cb(int argc, char** argv) {
   Serial.println("OK");
 }
 
+void App::cmd_get_is_imu_connected_cb(int, char**) { Serial.println(is_imu_connected); }
+
+void App::cmd_read_encoders_and_imu_cb(int, char**) {
+  Serial.print(left_encoder_.read());
+  Serial.print(" ");
+  Serial.print(right_encoder_.read());
+  Serial.print(" ");
+
+  // Retrieve absolute orientation (quaternion). See
+  // https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/overview for further
+  // information.
+  imu::Quaternion orientation = bno055_imu_.getQuat();
+  Serial.print(orientation.x(), 4);
+  Serial.print(" ");
+  Serial.print(orientation.y(), 4);
+  Serial.print(" ");
+  Serial.print(orientation.z(), 4);
+  Serial.print(" ");
+  Serial.print(orientation.w(), 4);
+  Serial.print(" ");
+
+  // Retrieve angular velocity (rad/s). See
+  // https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/overview for further
+  // information.
+  imu::Vector<3> angular_velocity = bno055_imu_.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
+  Serial.print(angular_velocity.x());
+  Serial.print(" ");
+  Serial.print(angular_velocity.y());
+  Serial.print(" ");
+  Serial.print(angular_velocity.z());
+  Serial.print(" ");
+
+  // Retrieve linear acceleration (m/s^2). See
+  // https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/overview for further
+  // information.
+  imu::Vector<3> linear_acceleration = bno055_imu_.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
+  Serial.print(linear_acceleration.x());
+  Serial.print(" ");
+  Serial.print(linear_acceleration.y());
+  Serial.print(" ");
+  Serial.print(linear_acceleration.z());
+}
+
 }  // namespace andino