Hopefully fixed wrong odometry

src/dcaiti_control/config/dcaiti_config.yml

@@ -13,18 +13,23 @@ diff_cont:
 
     publish_rate: 30.0 # You can set this higher than the controller manager update rate, but it will be throttled
     base_frame_id: base_link
+    publish_limited_velocity: true
 
     left_wheel_names: ['left_wheel_joint']
     right_wheel_names: ['right_wheel_joint']
     wheel_separation: 0.215
     wheel_radius: 0.049338032
 
-#    linear:
-#      x:
-#        max_acceleration: 0.15
-#        has_acceleration_limits: true
 
     use_stamped_vel: false
 
-# joint_broad:
+joint_limits:
-#   ros__parameters:
+  ros__parameters:
+    left_wheel_joint:
+      has_position_limits: false # Continuous joint
+      has_velocity_limits: true
+      max_velocity: 10.0
+    right_wheel_joint:
+      has_position_limits: false # Continuous joint
+      has_velocity_limits: true
+      max_velocity: 10.0

src/dcaiti_control/config/empty.yaml

@@ -1 +0,0 @@
-# This is an empty file, so that git commits the folder correctly

src/dcaiti_control/description/lidar.xacro

@@ -34,19 +34,19 @@
  
         <sensor name="laser" type="ray">
             <pose> 0 0 0 0 0 0 </pose>
-            <visualize>true</visualize>
+            <visualize>false</visualize>
             <update_rate>10</update_rate>
             <ray>
                 <scan>
                     <horizontal>
-                        <samples>360</samples>
+                        <samples>360*5</samples>
                         <min_angle>-3.14</min_angle>
                         <max_angle>3.14</max_angle>
                     </horizontal>
                 </scan>
                 <range>
                     <min>0.3</min>
-                    <max>25</max>
+                    <max>100</max>
                 </range>
             </ray>
             <plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">

src/dcaiti_control/description/robot.urdf.xacro

@@ -5,6 +5,24 @@
     <xacro:arg name="use_ros2_control" default="true"/>
     <xacro:arg name="use_sim" default="false"/>
 
+    <xacro:arg name="tube_thickness" default="0.024"/>
+    <xacro:arg name="tube_radius" default="false"/>
+
+    <xacro:arg name="camera_height" default="0.201"/>
+    <xacro:arg name="camera_offset_x" default="0.016"/>
+
+    <xacro:arg name="lidar_height" default="0.201"/>
+
+    <xacro:arg name="wheel_radius" default="0.05"/>
+    <xacro:arg name="wheel_width" default="0.02"/>
+
+    <xacro:arg name="base_height" default="0.1"/>
+    <xacro:arg name="base_width" default="0.2"/>
+    <xacro:arg name="base_length" default="0.2"/>
+
+
+
+
     <!-- Include the colours -->
     <xacro:include filename="colours.xacro" />
     <!-- Include the inertial macros -->

src/dcaiti_control/description/ros2_control.xacro

@@ -9,7 +9,7 @@
                 <param name="min">-0.5</param>
                 <param name="max">0.5</param>
             </command_interface>
-            <!-- <state_interface name="position" /> -->
+            <state_interface name="position" />
             <state_interface name="velocity" />
         </joint>
         <joint name="right_wheel_joint">
@@ -18,7 +18,7 @@
                 <param name="max">0.5</param>
             </command_interface>
             <state_interface name="velocity" />
-            <!--  <state_interface name="position" /> -->
+            <state_interface name="position" />
         </joint>
     </ros2_control>
     <gazebo>

src/dcaitirobot/embedded/include/communication/UValue.hpp

@@ -37,7 +37,7 @@ public:
 	 * @param value_left 	encoder value left
 	 * @param value_right 	encoder value right
 	 */
-	static void sendBothEncoderValues(float value_left, float value_right);
+	static void sendBothEncoderValues(float speed_right, float speed_left, float pos_right, float pos_left);
 };
 
 #endif

src/dcaitirobot/embedded/include/misc/Utils.hpp

@@ -13,7 +13,6 @@
  */
 
 #include "Arduino.h"
-#include "R2WD.h"
 
 class Utils {
 private:

src/dcaitirobot/embedded/src/communication/UARTCom.cpp

@@ -3,6 +3,7 @@
 #include "communication/UValue.hpp"
 #include "misc/Utils.hpp"
 #include "constants.h"
+#include "MotorWheel.h"
 
 uint8_t UARTCom::data[MAX_PACKET_LENGTH];
 uint8_t currentParseIndex;
@@ -170,7 +171,7 @@ void handleRequest (uint8_t* d)
 			break;
 		}
 		case DRIVE_ENCODER: {
-			UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM());
+			UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM(), wheelLeft.getPosition(), wheelRight.getPosition());
 			break;
 		}
 		case PID_PARAMETER: {

src/dcaitirobot/embedded/src/communication/UCommands.cpp

@@ -1,5 +1,7 @@
 #include "communication/UCommands.hpp"
 #include "misc/Utils.hpp"
+#include "MotorWheel.h"
+
 extern MotorWheel wheelLeft;
 extern MotorWheel wheelRight;
 

src/dcaitirobot/embedded/src/communication/UValue.cpp

@@ -19,20 +19,24 @@ void UValue::sendEncoderValue (U_Component cp, float value)
 	UARTCom::send(VALUE, 4, cp, switched_b);
 }
 
-void UValue::sendBothEncoderValues (float value_right, float value_left)
+void UValue::sendBothEncoderValues (float speed_right, float speed_left, float pos_right, float pos_left)
 {
 	union {
-		float f[2];
+		float f[4];
-		uint8_t b[8];
+		uint8_t b[16];
 	} u;
-	u.f[0] = value_right;
+	u.f[0] = speed_right;
-	u.f[1] = value_left;
+	u.f[1] = speed_left;
+	u.f[2] = pos_right;
+	u.f[3] = pos_left;
 
-	uint8_t switched_byte_array[8];
+	uint8_t switched_byte_array[16];
 	for (int i = 0; i < 4; i++) {
 		switched_byte_array[i] = u.b[3 - i];
 		switched_byte_array[i+4] = u.b[7 - i];
+		switched_byte_array[i+8] = u.b[11 - i];
+		switched_byte_array[i+12] = u.b[15 - i];
 	}
 
-	UARTCom::send(VALUE, 8, ALL_MOTORS, switched_byte_array);
+	UARTCom::send(VALUE, 16, ALL_MOTORS, switched_byte_array);
 }

src/diffdrive_arduino/include/diffdrive_arduino/arduino_comms.h

@@ -34,11 +34,11 @@ public:
 
   void setup(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms);
   void sendEmptyMsg();
-  void readEncoderValues(double &val_1, double &val_2);
+  void readEncoderValues(double &left_speed, double &right_speed, double &left_pos, double &right_pos);
   void setMotorValues(float val_1, float val_2);
   void setPidValues(float k_p, float k_d, float k_i, float k_o);
   const std::vector<uint8_t> buildMsg(uint8_t frame_type, uint8_t frame_component, long time,  std::vector<uint8_t> payload);
-  void readSpeed(double &left_speed, double &right_speed);
+  void readSpeedandPosition(double &left_speed, double &right_speed, double &left_pos, double &right_pos);
 
   bool connected() const { return serial_conn_.isOpen(); }
 

src/diffdrive_arduino/src/arduino_comms.cpp

@@ -21,12 +21,12 @@ void ArduinoComms::sendEmptyMsg()
 {
 }
 
-void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed)
+void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed, double &left_pos, double &right_pos)
 {   
     std::vector<uint8_t> payload = {DRIVE_ENCODER};
     std::vector<uint8_t> msg = buildMsg(REQUEST, BOTH_MOTORS, 0, payload);
     sendMsg(msg);
-    readSpeed(left_speed, right_speed);
+    readSpeedandPosition(left_speed, right_speed, left_pos, right_pos);
 }
 
 void ArduinoComms::setMotorValues(float left_speed, float right_speed)
@@ -49,7 +49,7 @@ void ArduinoComms::setPidValues(float k_p, float k_d, float k_i, float k_o)
     ss << "u " << k_p << ":" << k_d << ":" << k_i << ":" << k_o << "\r";
 }
 
-void ArduinoComms::readSpeed(double &left_speed, double &right_speed)
+void ArduinoComms::readSpeedandPosition(double &left_speed, double &right_speed, double &left_pos, double &right_pos)
 {
     std::vector<uint8_t> buffer;
     int read_bytes = 0;
@@ -91,13 +91,21 @@ void ArduinoComms::readSpeed(double &left_speed, double &right_speed)
     if (frame_type == VALUE && frame_component == BOTH_MOTORS)
     {
             std::vector<uint8_t> left_speed_bytes(payload.begin(), payload.begin() + 4);
-            std::vector<uint8_t> right_speed_bytes(payload.begin() + 4, payload.end());
+            std::vector<uint8_t> right_speed_bytes(payload.begin() + 4, payload.begin()+8);
+            std::vector<uint8_t> left_pos_bytes(payload.begin() + 8, payload.begin()+12);
+            std::vector<uint8_t> right_pos_bytes(payload.begin() + 12, payload.end());
             std::reverse(left_speed_bytes.begin(), left_speed_bytes.end());
             std::reverse(right_speed_bytes.begin(), right_speed_bytes.end());
+            std::reverse(left_pos_bytes.begin(), left_pos_bytes.end());
+            std::reverse(right_pos_bytes.begin(), right_pos_bytes.end());
             float left_speed_float = *reinterpret_cast<float*>(&left_speed_bytes[0]);
             float right_speed_float = *reinterpret_cast<float*>(&right_speed_bytes[0]);
+            float left_pos_float = *reinterpret_cast<float*>(&left_pos_bytes[0]);
+            float right_pos_float = *reinterpret_cast<float*>(&right_pos_bytes[0]);
             left_speed = left_speed_float;
             right_speed = right_speed_float;
+            left_pos = left_pos_float;
+            right_pos = right_pos_float;
     }
 }
 

src/diffdrive_arduino/src/diffdrive_arduino.cpp

@@ -7,6 +7,11 @@
 #include "rclcpp/rclcpp.hpp"
 #include <math.h>
 
+#define RADSTORPM(a)(a*60.0/(2.0*M_PI))
+#define RPMSTORADS(a)(a*2.0*M_PI/60.0)
+#define REVSTORAD(a)(a*2.0*M_PI)
+#define RADSTOREV(a)(a/(2.0*M_PI))
+
 namespace diffdrive_arduino
 {
 
@@ -106,18 +111,15 @@ return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp
     return return_type::ERROR;
   }
 
-  arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel);
+  arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel, l_wheel_.pos, r_wheel_.pos);
-  // write l_wheel_.cmd to string and print via RCLCPP_INFO
-  std::string msg = "Left Wheel Read: " + std::to_string(l_wheel_.vel);
-  RCLCPP_INFO(logger_, msg.c_str());
   // convert rpm to rad/s
-  l_wheel_.vel = (l_wheel_.vel / 60) * 2 * M_PI * 0.049 ;
+  l_wheel_.vel = RPMSTORADS(l_wheel_.vel);
-  r_wheel_.vel = (r_wheel_.vel / 60) * 2 * M_PI * 0.049 ;
+  r_wheel_.vel = RPMSTORADS(r_wheel_.vel);
 
     
-  // Force the wheel position
+  // convert rev to rad
-  l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds;
+  l_wheel_.pos = REVSTORAD(l_wheel_.pos);
-  r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds;
+  r_wheel_.pos = REVSTORAD(r_wheel_.pos);
   return return_type::OK;  
 }
 
@@ -128,9 +130,7 @@ return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcp
     return return_type::ERROR;
   }
   // convert rad/s to rpm and write to motors
-  arduino_.setMotorValues(l_wheel_.cmd *  30 / M_PI , r_wheel_.cmd * 30 / M_PI );
+  arduino_.setMotorValues(RADSTORPM(l_wheel_.cmd), RADSTORPM(r_wheel_.cmd));
-  std::string msg = "Left Wheel Command: " + std::to_string(l_wheel_.cmd* 30 / M_PI);
-  RCLCPP_INFO(logger_, msg.c_str());
 
   return return_type::OK;  
 }