Ivanpauno/andino imu rebased

andino_base/include/andino_base/diffdrive_andino.h

@@ -40,6 +40,7 @@
 #include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
 #include <rclcpp_lifecycle/state.hpp>
 
+#include "andino_base/imu.h"
 #include "andino_base/motor_driver.h"
 #include "andino_base/wheel.h"
 
@@ -70,6 +71,7 @@ class DiffDriveAndino : public hardware_interface::SystemInterface {
   hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
  private:
+  const std::string kImuSensorName{"imu_sensor_name"};
   const std::string kLeftWheelNameParam{"left_wheel_name"};
   const std::string kRightWheelNameParam{"right_wheel_name"};
   const std::string kSerialDeviceParam{"serial_device"};
@@ -80,6 +82,7 @@ class DiffDriveAndino : public hardware_interface::SystemInterface {
   // Configuration parameters for the DiffDriveAndino class.
   struct Config {
     // Name of the left and right wheels.
+    std::string imu_sensor_name = "imu_sensor";
     std::string left_wheel_name = "left_wheel";
     std::string right_wheel_name = "right_wheel";
     // Encoder parameters.
@@ -98,6 +101,8 @@ class DiffDriveAndino : public hardware_interface::SystemInterface {
   Wheel left_wheel_;
   // Right wheel of the robot.
   Wheel right_wheel_;
+  // Imu sensor
+  Imu imu_;
   // Logger.
   rclcpp::Logger logger_{rclcpp::get_logger("DiffDriveAndino")};
 };

andino_base/include/andino_base/imu.h

@@ -0,0 +1,57 @@
+// BSD 3-Clause License
+//
+// Copyright (c) 2023, 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.
+#pragma once
+
+#include <string>
+
+namespace andino_base {
+
+struct Imu {
+  /// @brief Default constructor for the Wheel class
+  Imu() = default;
+
+  /// @brief Setup the wheel.
+  /// @param imu_name name of the imu sensor.
+  void Setup(const std::string& imu_name);
+
+  std::string name_ = "";
+  double orientation_x = 0.;
+  double orientation_y = 0.;
+  double orientation_z = 0.;
+  double orientation_w = 0.;
+  double angular_velocity_x_ = 0;
+  double angular_velocity_y_ = 0;
+  double angular_velocity_z_ = 0;
+  double linear_acceleration_x_ = 0;
+  double linear_acceleration_y_ = 0;
+  double linear_acceleration_z_ = 0;
+};
+
+}  // namespace andino_base

andino_base/include/andino_base/motor_driver.h

@@ -45,6 +45,12 @@ class MotorDriver {
  public:
   /// @brief Type to store the encoder values.
   using Encoders = std::array<int, 2>;
+  using Imu = std::array<double, 10>;
+
+  struct EncodersAndImu {
+    Encoders encoders;
+    Imu imu;
+  };
 
   /// @brief Default constructor.
   MotorDriver() = default;
@@ -58,6 +64,17 @@ class MotorDriver {
   ///       ensure that the motor driver is ready to receive a new command.
   void SendEmptyMsg();
 
+  /// @brief Checks if there is an Imu.
+  /// @returns True if Imu is present.
+  bool HasImu();
+
+  /// @brief Read the encoder and Imu values from the motor driver.
+  ///       First value is the left encoder, second value is the right encoder, next values
+  ///       are IMU orientation (qx, qy, qz, qw), angular velocity (x, y, z), 
+  ///       linear acceleration (x, y, z)
+  /// @returns The encoder values.
+  EncodersAndImu ReadEncoderAndImuValues();
+
   /// @brief Read the encoder values from the motor driver.
   ///       First value is the left encoder, second value is the right encoder.
   /// @returns The encoder values.

andino_base/src/diffdrive_andino.cpp

@@ -41,6 +41,7 @@ hardware_interface::CallbackReturn DiffDriveAndino::on_init(const hardware_inter
 
   RCLCPP_INFO(logger_, "On init...");
 
+  config_.imu_sensor_name = info_.hardware_parameters[kImuSensorName];
   config_.left_wheel_name = info_.hardware_parameters[kLeftWheelNameParam];
   RCLCPP_DEBUG(logger_, (kLeftWheelNameParam + static_cast<std::string>(": ") + config_.left_wheel_name).c_str());
   config_.right_wheel_name = info_.hardware_parameters[kRightWheelNameParam];
@@ -106,6 +107,26 @@ std::vector<hardware_interface::StateInterface> DiffDriveAndino::export_state_in
       hardware_interface::StateInterface(right_wheel_.name_, hardware_interface::HW_IF_VELOCITY, &right_wheel_.vel_));
   state_interfaces.emplace_back(
       hardware_interface::StateInterface(right_wheel_.name_, hardware_interface::HW_IF_POSITION, &right_wheel_.pos_));
+  state_interfaces.emplace_back(
+        hardware_interface::StateInterface(config_.imu_sensor_name, "orientation.x", &imu_.orientation_x));
+  state_interfaces.emplace_back(
+        hardware_interface::StateInterface(config_.imu_sensor_name, "orientation.y", &imu_.orientation_y));
+  state_interfaces.emplace_back(
+        hardware_interface::StateInterface(config_.imu_sensor_name, "orientation.z", &imu_.orientation_z));
+  state_interfaces.emplace_back(
+        hardware_interface::StateInterface(config_.imu_sensor_name, "orientation.w", &imu_.orientation_w));
+  state_interfaces.emplace_back(
+        hardware_interface::StateInterface(config_.imu_sensor_name, "angular_velocity.x", &imu_.angular_velocity_x_));
+  state_interfaces.emplace_back(
+        hardware_interface::StateInterface(config_.imu_sensor_name, "angular_velocity.y", &imu_.angular_velocity_y_));
+  state_interfaces.emplace_back(
+        hardware_interface::StateInterface(config_.imu_sensor_name, "angular_velocity.z", &imu_.angular_velocity_z_));
+  state_interfaces.emplace_back(
+        hardware_interface::StateInterface(config_.imu_sensor_name, "linear_acceleration.x", &imu_.linear_acceleration_x_));
+  state_interfaces.emplace_back(
+        hardware_interface::StateInterface(config_.imu_sensor_name, "linear_acceleration.y", &imu_.linear_acceleration_y_));
+  state_interfaces.emplace_back(
+        hardware_interface::StateInterface(config_.imu_sensor_name, "linear_acceleration.z", &imu_.linear_acceleration_z_));
 
   return state_interfaces;
 }
@@ -145,10 +166,25 @@ hardware_interface::return_type DiffDriveAndino::read(const rclcpp::Time& /* tim
     return hardware_interface::return_type::ERROR;
   }
 
-  const MotorDriver::Encoders encoders = motor_driver_.ReadEncoderValues();
-
-  left_wheel_.enc_ = encoders[0];
-  right_wheel_.enc_ = encoders[1];
+  if (motor_driver_.HasImu()) {
+    const MotorDriver::EncodersAndImu eai = motor_driver_.ReadEncoderAndImuValues();
+    left_wheel_.enc_ = eai.encoders[0];
+    right_wheel_.enc_ = eai.encoders[1];
+    imu_.orientation_x = eai.imu[0];
+    imu_.orientation_y = eai.imu[1];
+    imu_.orientation_z = eai.imu[2];
+    imu_.orientation_w = eai.imu[3];
+    imu_.angular_velocity_x_ = eai.imu[4];
+    imu_.angular_velocity_y_ = eai.imu[5];
+    imu_.angular_velocity_z_ = eai.imu[6];
+    imu_.linear_acceleration_x_ = eai.imu[7];
+    imu_.linear_acceleration_y_ = eai.imu[8];
+    imu_.linear_acceleration_z_ = eai.imu[9];
+  } else {
+    const MotorDriver::Encoders encoders = motor_driver_.ReadEncoderValues();
+    left_wheel_.enc_ = encoders[0];
+    right_wheel_.enc_ = encoders[1];
+  }
 
   const double left_pos_prev = left_wheel_.pos_;
   left_wheel_.pos_ = left_wheel_.Angle();

andino_base/src/motor_driver.cpp

@@ -62,14 +62,34 @@ bool MotorDriver::is_connected() const { return serial_port_.IsOpen(); }
 
 void MotorDriver::SendEmptyMsg() { std::string response = SendMsg(""); }
 
+bool MotorDriver::HasImu() {
+  const std::string response = SendMsg("h");
+  return response != "0";
+}
+
+MotorDriver::EncodersAndImu MotorDriver::ReadEncoderAndImuValues() {
+  std::istringstream is(SendMsg("i"));
+
+  MotorDriver::EncodersAndImu eai;
+  for(int &val : eai.encoders) {
+    is >> val;
+  }
+  for(double &val : eai.imu) {
+    is >> val;
+  }
+
+  return eai;
+}
+
 MotorDriver::Encoders MotorDriver::ReadEncoderValues() {
-  static const std::string delimiter = " ";
+  std::istringstream is(SendMsg("e"));
+
+  MotorDriver::Encoders enc;
+  for(int &val : enc) {
+    is >> val;
+  }
 
-  const std::string response = SendMsg("e");
-  const size_t del_pos = response.find(delimiter);
-  const std::string token_1 = response.substr(0, del_pos).c_str();
-  const std::string token_2 = response.substr(del_pos + delimiter.length()).c_str();
-  return {std::atoi(token_1.c_str()), std::atoi(token_2.c_str())};
+  return enc;
 }
 
 void MotorDriver::SetMotorValues(int val_1, int val_2) {

andino_control/config/andino_controllers.yaml

@@ -8,6 +8,17 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: 'imu_sensor'
+    frame_id: 'base_link'
+    static_covariance_orientation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+    static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+    static_covariance_linear_acceleration: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+
 diff_controller:
   ros__parameters:
     # See https://github.com/ros-controls/ros2_controllers/blob/humble/diff_drive_controller/src/diff_drive_controller_parameter.yaml

andino_control/launch/andino_control.launch.py

@@ -75,11 +75,17 @@ def generate_launch_description():
         arguments=["diff_controller", "--controller-manager", "/controller_manager"],
     )
 
+    imu_sensor_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["imu_sensor_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
     # Delay start of diff_drive_controller_spawner after `joint_state_broadcaster`
     delay_diff_drive_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
         event_handler=OnProcessExit(
             target_action=joint_state_broadcaster_spawner,
-            on_exit=[diff_drive_controller_spawner],
+            on_exit=[diff_drive_controller_spawner, imu_sensor_broadcaster_spawner],
         )
     )
 

andino_control/package.xml

@@ -17,6 +17,7 @@
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>diff_drive_controller</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>imu_sensor_broadcaster</exec_depend>
   <exec_depend>ros2controlcli</exec_depend>
 
   <test_depend>ament_cmake_clang_format</test_depend>

andino_description/config/andino/sensors.yaml

@@ -14,3 +14,11 @@ camera:
   dx: 0.085
   dy: 0.0
   dz: 0.025
+
+imu:
+  mass: 0.05          # mass in Kg
+  box_size: 0.01
+  box_size_height: 0.0025
+  dx: 0.0
+  dy: 0.0
+  dz: 0.0513

andino_description/urdf/andino.urdf.xacro

@@ -83,13 +83,16 @@
   </xacro:motor>
 
   <!-- Sensors -->
-  <xacro:laser prefix="rplidar" parent_link="second_base_link"
+  <xacro:laser prefix="rplidar" parent_link="base_link"
                    sensor_prop="${sensor_prop}">
   </xacro:laser>
 
   <xacro:camera_sensor parent_link="base_link" sensor_prop="${sensor_prop}">
   </xacro:camera_sensor>
 
+  <xacro:imu_sensor parent_link="base_link" sensor_prop="${sensor_prop}">
+  </xacro:imu_sensor>
+
   <!-- ros2_control -->
   <xacro:if value="$(arg use_real_ros_control)">
     <xacro:include filename="$(find ${package_name})/urdf/include/andino_control.urdf.xacro" />

andino_description/urdf/include/andino_control.urdf.xacro

@@ -1,43 +1,45 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-<!-- ===================== 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>
-
+  <!-- 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>
+      <param name="imu_sensor_name">imu_sensor</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>
+    <sensor name="imu_sensor">
+      <state_interface name="orientation.x"/>
+      <state_interface name="orientation.y"/>
+      <state_interface name="orientation.z"/>
+      <state_interface name="orientation.w"/>
+      <state_interface name="angular_velocity.x"/>
+      <state_interface name="angular_velocity.y"/>
+      <state_interface name="angular_velocity.z"/>
+      <state_interface name="linear_acceleration.x"/>
+      <state_interface name="linear_acceleration.y"/>
+      <state_interface name="linear_acceleration.z"/>
+    </sensor>
+  </ros2_control>
 </robot>