Imu package integration branch

.gitmodules

@@ -1,3 +1,9 @@
 [submodule "src/rplidar_ros"]
 	path = src/rplidar_ros
 	url = https://github.com/Slamtec/rplidar_ros.git
+[submodule "src/robot_localization"]
+	path = src/robot_localization
+	url = https://github.com/cra-ros-pkg/robot_localization.git
+[submodule "src/atlas_interfaces"]
+	path = src/atlas_interfaces
+	url = https://github.com/movvo/atlas_interfaces.git

src/agl_arduino/AGL_PwmSerial/AGL_PwmSerial.ino

@@ -28,8 +28,7 @@ bool ros2DataRightWheelRead = false;    // Control of wt speed setting.
 bool ros2DataLeftWheelRead = false;
 bool rightWheelGoingForward = true;    // Control of wt speed setting.
 bool leftWheelGoingForward = true;
-  digitalWrite(IN2_ML, HIGH);
-    leftWheelGoingForward = true;
+
 int IN3_MR = 5;
 int IN4_MR = 4;
 
@@ -63,7 +62,8 @@ int pwrL = 0;
 bool firstEncoderReadL = true;                     
 int encoderCounterFilterL = 0;                     
 float lVector[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; 
-float N = 24.0;                                  
+float N = 24.0;                                   // N is the count of servo revolutions while detecting only the falling edge of the channel's signal (12), we use FALLING in encoders.       
+                                                  // We have to leave 24 otherwise the servos behave incorrectly, we multily per 2 in ros2 before sending feedback speeds.                             
 float gear = 74.83;                              
 float diameter = 10;                              
 float length = 35;                                

src/agl_bringup/config/start.sample.yaml

@@ -8,10 +8,21 @@ Start:
   # Por lo tanto en esta lista deberia aparecer unicamente lo siguiente:
 
   packages_list:
-    - agl_lidar:       
-      - Lidar
+    # - agl_lidar:       
+    #   - Lidar
     - agl_description:
       - Description
     - agl_joystick:
       - Joystick
-      - Joy
+      - Joy
+    - agl_odometry:
+      - odometry_node
+    - agl_serial_interface:
+      - serial_server_node
+    - ast_camera:
+      - Camera
+    - ast_imu_filter_madgwick:
+      - imu_filter_madgwick
+    - ast_robot_localization_ekf:
+      - EKF_Node
+

src/agl_bringup/launch/start_launch.py

@@ -10,6 +10,7 @@
 import yaml
 import rclpy
 from typing import Tuple, Dict
+import subprocess
 
 from ament_index_python.packages import get_package_share_directory
 from launch_ros.actions import Node, SetParameter
@@ -42,6 +43,7 @@ def BringupParams(config_file: str):
     return packages_list
 
 def generate_launch_description():
+
     # Configs files
     start = os.path.join(get_package_share_directory(PACKAGE_NAME), "config", "start.sample.yaml")
 

src/agl_description/model/andresito/model.sdf

@@ -1,3 +1,60 @@
 <sdf version='1.7'>
-  <model name='andresito'/>
+  <model name='andresito'>
+    <link name='base_footprint'>
+      <inertial>
+        <pose>0.02885 -0.2175 0.0145 0 -0 0</pose>
+        <mass>0.072</mass>
+        <inertia>
+          <ixx>0.00388124</ixx>
+          <ixy>-1.53418e-09</ixy>
+          <ixz>0</ixz>
+          <iyy>0.00049894</iyy>
+          <iyz>2.71051e-20</iyz>
+          <izz>0.00387926</izz>
+        </inertia>
+      </inertial>
+      <collision name='base_footprint_fixed_joint_lump__base_link_collision'>
+        <pose>0 -0.21 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.42 0.32 0.08</size>
+          </box>
+        </geometry>
+      </collision>
+      <collision name='base_footprint_fixed_joint_lump__camera_link_collision_1'>
+        <pose>0.0373 -0.17 0.0145 0 -0 3.14159</pose>
+        <geometry>
+          <box>
+            <size>0.026 0.124 0.029</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name='base_footprint_fixed_joint_lump__base_link_visual'>
+        <pose>0 -0.21 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.42 0.32 0.08</size>
+          </box>
+        </geometry>
+      </visual>
+      <visual name='base_footprint_fixed_joint_lump__laser_visual_1'>
+        <pose>0 -0.21 0.08 0 -0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.12</length>
+            <radius>0.05</radius>
+          </cylinder>
+        </geometry>
+      </visual>
+      <visual name='base_footprint_fixed_joint_lump__camera_link_visual_2'>
+        <pose>0.0242 -0.17 0.0145 1.5708 -0 -1.5708</pose>
+        <geometry>
+          <mesh>
+            <scale>0.001 0.001 0.001</scale>
+            <uri>file:///opt/ros/humble/share/realsense2_description/meshes/d455.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
 </sdf>

src/agl_description/urdf/andresito.urdf

@@ -5,13 +5,6 @@
 <!-- =================================================================================== -->
 <robot name="andresito">
   <!-- <preserveFixedJoint>true</preserveFixedJoint> -->
-  <!-- CAMERA -->
-  <!-- <xacro:property name="camera_link" value="realsense_link" />
-    <xacro:property name="camera_relative_to" value="puller_base" />
-    <xacro:property name="camera_x" value="-${puller_depth/2-0.03}" />
-    <xacro:property name="camera_y" value="0.0" />
-    <xacro:property name="camera_z" value="0.24" />
-    <xacro:property name="camera_rotation_z" value="${180*deg_to_rad}" /> -->
   <!-- PROPERTIES -->
   <material name="blue">
     <color rgba="0 0 0.9 1"/>
@@ -118,7 +111,12 @@
       </geometry>
     </collision>
   </link>
+  <!-- <odometry_frame>odom</odometry_frame> -->
   <!-- JOINTS -->
+  <!-- <joint name="odom_base_footprint" type="fixed">
+    <parent link="odom"/>
+    <child link="base_footprint" />
+  </joint> -->
   <joint name="base_joint" type="fixed">
     <origin xyz="0 -0.21 0.0"/>
     <parent link="base_footprint"/>
@@ -154,4 +152,135 @@
     <origin xyz="0 0 0.08"/>
     <axis xyz="0 0 1"/>
   </joint>
+  <material name="aluminum">
+    <color rgba="0.5 0.5 0.5 1"/>
+  </material>
+  <material name="plastic">
+    <color rgba="0.1 0.1 0.1 1"/>
+  </material>
+  <link name="realsense_link"/>
+  <!-- camera body, with origin at bottom screw mount -->
+  <joint name="camera_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="realsense_link"/>
+    <child link="camera_bottom_screw_frame"/>
+  </joint>
+  <link name="camera_bottom_screw_frame"/>
+  <joint name="camera_link_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.011150000000000002 0.0475 0.0145"/>
+    <parent link="camera_bottom_screw_frame"/>
+    <child link="camera_link"/>
+  </joint>
+  <link name="camera_link">
+    <visual>
+      <!-- the mesh origin is at front plate in between the two infrared camera axes -->
+      <!-- <origin xyz="${d455_zero_depth_to_glass-d455_cam_depth/2} ${-d455_cam_depth_py} 0" rpy="0 0 0"/> -->
+      <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0.0046500000000000005 -0.0475 0"/>
+      <geometry>
+        <!-- <box size="${d455_cam_depth} ${d455_cam_width} ${d455_cam_height}"/> -->
+        <mesh filename="file:///opt/ros/humble/share/realsense2_description/meshes/d455.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="aluminum"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.00845 -0.0475 0"/>
+      <geometry>
+        <box size="0.026 0.124 0.029"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <!-- The following are not reliable values, and should not be used for modeling -->
+      <mass value="0.072"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257"/>
+    </inertial>
+  </link>
+  <!-- camera depth joints and links -->
+  <joint name="camera_depth_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="camera_link"/>
+    <child link="camera_depth_frame"/>
+  </joint>
+  <link name="camera_depth_frame"/>
+  <joint name="camera_depth_optical_joint" type="fixed">
+    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
+    <parent link="camera_depth_frame"/>
+    <child link="camera_depth_optical_frame"/>
+  </joint>
+  <link name="camera_depth_optical_frame"/>
+  <!-- camera left IR joints and links -->
+  <joint name="camera_infra1_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.0 0"/>
+    <parent link="camera_link"/>
+    <child link="camera_infra1_frame"/>
+  </joint>
+  <link name="camera_infra1_frame"/>
+  <joint name="camera_infra1_optical_joint" type="fixed">
+    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
+    <parent link="camera_infra1_frame"/>
+    <child link="camera_infra1_optical_frame"/>
+  </joint>
+  <link name="camera_infra1_optical_frame"/>
+  <!-- camera right IR joints and links -->
+  <joint name="camera_infra2_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 -0.095 0"/>
+    <parent link="camera_link"/>
+    <child link="camera_infra2_frame"/>
+  </joint>
+  <link name="camera_infra2_frame"/>
+  <joint name="camera_infra2_optical_joint" type="fixed">
+    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
+    <parent link="camera_infra2_frame"/>
+    <child link="camera_infra2_optical_frame"/>
+  </joint>
+  <link name="camera_infra2_optical_frame"/>
+  <!-- camera color joints and links -->
+  <joint name="camera_color_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 -0.059 0"/>
+    <parent link="camera_link"/>
+    <child link="camera_color_frame"/>
+  </joint>
+  <link name="camera_color_frame"/>
+  <joint name="camera_color_optical_joint" type="fixed">
+    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
+    <parent link="camera_color_frame"/>
+    <child link="camera_color_optical_frame"/>
+  </joint>
+  <link name="camera_color_optical_frame"/>
+  <link name="camera_accel_frame"/>
+  <link name="camera_accel_optical_frame"/>
+  <link name="camera_gyro_frame"/>
+  <link name="camera_gyro_optical_frame"/>
+  <link name="camera_imu_optical_frame"/>
+  <joint name="camera_accel_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.01602 -0.03022 0.0074"/>
+    <parent link="camera_link"/>
+    <child link="camera_accel_frame"/>
+  </joint>
+  <joint name="camera_accel_optical_joint" type="fixed">
+    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
+    <parent link="camera_accel_frame"/>
+    <child link="camera_accel_optical_frame"/>
+  </joint>
+  <joint name="camera_gyro_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.01602 -0.03022 0.0074"/>
+    <parent link="camera_link"/>
+    <child link="camera_gyro_frame"/>
+  </joint>
+  <joint name="camera_gyro_optical_joint" type="fixed">
+    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
+    <parent link="camera_gyro_frame"/>
+    <child link="camera_gyro_optical_frame"/>
+  </joint>
+  <joint name="camera_imu_optical_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="camera_gyro_optical_frame"/>
+    <child link="camera_imu_optical_frame"/>
+  </joint>
+  <joint name="base_link_to_realsense_link" type="fixed">
+    <axis xyz="0 1 1"/>
+    <parent link="base_link"/>
+    <child link="realsense_link"/>
+    <origin rpy="0 0 3.1415922" xyz="0.04 0.04 0.0"/>
+  </joint>
 </robot>

src/agl_description/urdf/andresito.xacro

@@ -5,6 +5,7 @@
 
   <!-- PROPERTIES -->
   <xacro:include filename="properties.xacro" />
+  <xacro:property name="deg_to_rad"  value="0.01745329"/>
 
   <!-- MACROS -->
   <xacro:include filename="wheel.xacro" />
@@ -46,7 +47,15 @@
   <xacro:wheel side="right" position="front"/>
   <xacro:wheel side="right" position="back"/>
 
+  <!-- <odometry_frame>odom</odometry_frame> -->
+
   <!-- JOINTS -->
+
+  <!-- <joint name="odom_base_footprint" type="fixed">
+    <parent link="odom"/>
+    <child link="base_footprint" />
+  </joint> -->
+
   <joint name="base_joint" type="fixed">
     <origin xyz="0 -${robot_depth/2} ${wheels_height}"/>
     <parent link="base_footprint"/>
@@ -88,4 +97,19 @@
     <axis xyz="0 0 1"/>
   </joint>
 
+  <!-- CAMERA LINKS and JOINT -->
+  <xacro:include filename="$(find realsense2_description)/urdf/_d455.urdf.xacro" />
+
+  <link name="${camera_link}" />
+  <xacro:sensor_d455 parent="${camera_link}" use_nominal_extrinsics="true">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </xacro:sensor_d455>
+
+  <joint name="base_link_to_${camera_link}" type="fixed">
+    <axis xyz="0 1 1"/>
+    <parent link="base_link"/>
+    <child link="${camera_link}"/>
+    <origin xyz="${camera_x} ${camera_y} ${camera_z}" rpy="0 0 ${camera_rotation_z}"/>
+  </joint>
+
 </robot>

src/agl_description/urdf/properties.xacro

@@ -15,12 +15,13 @@
     <xacro:property name="wheel_radius" value="0.05" />
 
     <!-- CAMERA -->
-    <!-- <xacro:property name="camera_link" value="realsense_link" />
+    <xacro:property name="camera_size" value="0.01" />
+    <xacro:property name="camera_link" value="realsense_link" />
     <xacro:property name="camera_relative_to" value="puller_base" />
-    <xacro:property name="camera_x" value="-${puller_depth/2-0.03}" />
-    <xacro:property name="camera_y" value="0.0" />
-    <xacro:property name="camera_z" value="0.24" />
-    <xacro:property name="camera_rotation_z" value="${180*deg_to_rad}" /> -->
+    <xacro:property name="camera_x" value="0.04" />
+    <xacro:property name="camera_y" value="0.04" />
+    <xacro:property name="camera_z" value="0.0" />
+    <xacro:property name="camera_rotation_z" value="${180*deg_to_rad}" />
 
     <!-- PROPERTIES -->
     <material name="blue">

src/agl_interfaces/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(std_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/TwoAngularSpeeds.msg"
+  "msg/OdometryMsg.msg"
   "srv/GenerateDifferentialSpeeds.srv"
   DEPENDENCIES std_msgs
 )

src/agl_interfaces/msg/OdometryMsg.msg

@@ -0,0 +1,6 @@
+# Standard ROS2 header
+std_msgs/Header header 
+
+float64 orientation
+float64 x_position
+float64 y_position

src/agl_interfaces/package.xml

@@ -9,14 +9,12 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
   <depend>std_msgs</depend>
 
-
   <export>
     <build_type>ament_cmake</build_type>
   </export>