Merge pull request #1 from dcaiti-robot/main

updating branch

src/dcaiti_control/config/dcaiti_config.yml

@@ -17,12 +17,12 @@ diff_cont:
     left_wheel_names: ['left_wheel_joint']
     right_wheel_names: ['right_wheel_joint']
     wheel_separation: 0.215
-    wheel_radius: 0.0477464829
+    wheel_radius: 0.049338032
 
-    linear:
+#    linear:
-      x:
+#      x:
-        max_acceleration: 0.05
+#        max_acceleration: 0.15
-        has_acceleration_limits: true
+#        has_acceleration_limits: true
 
     use_stamped_vel: false
 

src/dcaiti_control/description/real_robot_control.xacro

@@ -9,21 +9,20 @@
             <param name="device">/dev/ttyUSB0</param>
             <param name="baud_rate">57600</param>
             <param name="timeout">1000</param>
-            <param name="enc_counts_per_rev">3436</param>
         </hardware>
         <!-- Note everything below here is the same as the Gazebo one -->
         <joint name="left_wheel_joint">
             <command_interface name="velocity">
-                <param name="min">-0.005</param>
+                <param name="min">-0.5</param>
-                <param name="max">0.005</param>
+                <param name="max">0.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">-0.005</param>
+                <param name="min">-0.5</param>
-                <param name="max">0.005</param>
+                <param name="max">0.5</param>
             </command_interface>
             <state_interface name="velocity"/>
             <state_interface name="position"/>

src/dcaiti_control/description/robot_core.xacro

@@ -41,17 +41,17 @@
     <link name="left_wheel">
         <visual>
             <geometry>
-                <cylinder radius="${0.3/(2*pi)}" length="0.04"/>
+                <cylinder radius="${0.31/(2*pi)}" length="0.04"/>
             </geometry>
             <material name="blue"/>
         </visual>
         <collision>
             <geometry>
                 <!-- Better collosion behaviour than a cylinder -->
-                <sphere radius="${0.3/(2*pi)}"/>
+                <sphere radius="${0.31/(2*pi)}"/>
             </geometry>
         </collision>
-        <xacro:inertial_cylinder mass="0.1" radius="${0.3/(2*pi)}" length="0.04">
+        <xacro:inertial_cylinder mass="0.1" radius="${0.31/(2*pi)}" length="0.04">
             <origin xyz="0 0 0" rpy="0 0 0"/>
         </xacro:inertial_cylinder>
     </link>
@@ -73,16 +73,16 @@
         <visual>
             <geometry>
                 <!-- Better collosion behaviour than a cylinder -->
-                <cylinder radius="${0.3/(2*pi)}" length="0.04"/>
+                <cylinder radius="${0.31/(2*pi)}" length="0.04"/>
             </geometry>
             <material name="blue"/>
         </visual>
         <collision>
             <geometry>
-                <sphere radius="${0.3/(2*pi)}" />
+                <sphere radius="${0.31/(2*pi)}" />
             </geometry>
         </collision>
-        <xacro:inertial_cylinder mass="0.1" radius="${0.3/(2*pi)}" length="0.04">
+        <xacro:inertial_cylinder mass="0.1" radius="${0.31/(2*pi)}" length="0.04">
             <origin xyz="0 0 0" rpy="0 0 0"/>
         </xacro:inertial_cylinder>
     </link>
@@ -110,16 +110,16 @@
     <link name="caster_wheel_front">
         <visual>
             <geometry>
-                <sphere radius="${0.3/(2*pi)}"/>
+                <sphere radius="${0.31/(2*pi)}"/>
             </geometry>
             <material name="black"/>
         </visual>
         <collision>
             <geometry>
-                <sphere radius="${0.3/(2*pi)}"/>
+                <sphere radius="${0.31/(2*pi)}"/>
             </geometry>
         </collision>
-        <xacro:inertial_sphere mass="0.1" radius="${0.3/(2*pi)}">
+        <xacro:inertial_sphere mass="0.1" radius="${0.31/(2*pi)}">
             <origin xyz="0 0 0" rpy="0 0 0"/>
         </xacro:inertial_sphere>
     </link>
@@ -142,16 +142,16 @@
     <link name="caster_wheel_back">
         <visual>
             <geometry>
-                <sphere radius="${0.3/(2*pi)}"/>
+                <sphere radius="${0.31/(2*pi)}"/>
             </geometry>
             <material name="black"/>
         </visual>
         <collision>
             <geometry>
-                <sphere radius="${0.3/(2*pi)}"/>
+                <sphere radius="${0.31/(2*pi)}"/>
             </geometry>
         </collision>
-        <xacro:inertial_sphere mass="0.1" radius="${0.3/(2*pi)}">
+        <xacro:inertial_sphere mass="0.1" radius="${0.31/(2*pi)}">
             <origin xyz="0 0 0" rpy="0 0 0"/>
         </xacro:inertial_sphere>
     </link>

src/dcaitirobot/dcaitirobot/twist.py

@@ -7,54 +7,98 @@ from sensor_msgs.msg import Joy
 
 import numpy as np
 
-SCALE_LINEAR_VELOCITY = 0.5
+SCALE_LINEAR_VELOCITY = 0.1
-SCALE_ANGULAR_VELOCITY = 0.4
+SCALE_ANGULAR_VELOCITY = 0.1
-PUBLISH_FREQUENCY_HZ = 10
+PUBLISH_FREQUENCY_HZ = 30
 
+SMOOTH_WEIGHTS = np.array([1, 2, 4, 10, 20])
 
-def calc_speed_from_joy(x, y):    
-    linear = -y * SCALE_LINEAR_VELOCITY
-    angular = x * SCALE_ANGULAR_VELOCITY
-
-    return linear, angular
 
 class TwistPublisher(Node):
+    def __init__(
+        self,
+        smooth_weights=np.ones(5),
+        linear_scale=1.0,
+        angular_scale=1.0,
+        publish_frequency_hz=30,
+    ):
+        super().__init__("twist_publisher")
+
+        self.linear_window = np.zeros(smooth_weights.shape[0])
+        self.angular_window = np.zeros(smooth_weights.shape[0])
+        self.linear_recent = 0.0
+        self.angular_recent = 0.0
+
+        self.linear_out = 0.0
+        self.angular_out = 0.0
+
+        self.weights = smooth_weights / smooth_weights.sum()
+        self.window_size = smooth_weights.shape[0]
+
+        self.linear_scale = linear_scale
+        self.angular_scale = angular_scale
+
+        self.publisher_ = self.create_publisher(
+            Twist, "/diff_cont/cmd_vel_unstamped", 1
+        )
+
+        self.publisher_2 = self.create_publisher(Twist, "/cmd_vel", 1)
 
-    def __init__(self):
-        super().__init__('twist_publisher')
-        self.linear = 0.0
-        self.angular = 0.0
-        self.publisher_ = self.create_publisher(Twist, '/diff_cont/cmd_vel_unstamped', 1)
         self.create_subscription(Joy, "/joy", self.joy_callback, 1)
-        self.timer = self.create_timer(1/PUBLISH_FREQUENCY_HZ, self.timer_callback)
+        self.timer = self.create_timer(1 / publish_frequency_hz, self.timer_callback)
-
 
     def joy_callback(self, joy_message):
         x, y = joy_message.axes[:2]
+        self.linear_recent = -y * SCALE_LINEAR_VELOCITY
+        self.angular_recent = x * SCALE_ANGULAR_VELOCITY
 
-        self.linear, self.angular = calc_speed_from_joy(x,y)
+    def update_smooth_window(self):
+        self.linear_window = np.append(self.linear_window, self.linear_recent)[
+            -self.window_size :
+        ]
+        self.angular_window = np.append(self.angular_window, self.angular_recent)[
+            -self.window_size :
+        ]
+
+    def calc_smooth_speed(self):
+        self.linear_out = np.average(self.linear_window, weights=self.weights)
+        self.angular_out = np.average(self.angular_window, weights=self.weights)
 
     def timer_callback(self):
         msg = Twist()
-        msg.linear.x = self.linear
+
-        msg.angular.z = self.angular
+        self.update_smooth_window()
+        self.calc_smooth_speed()
+
+        msg.linear.x = self.linear_out
+        msg.angular.z = self.angular_out
         self.publisher_.publish(msg)
-        self.get_logger().info('Publishing: "%s"' % msg)
+        self.publisher_2.publish(msg)
+        self.get_logger().info(
+            "Linear_window: {}, Linear_out: {}, Angular_window: {}, Angular_out: {}".format(
+                self.linear_window,
+                self.linear_out,
+                self.angular_window,
+                self.angular_out,
+            )
+        )
 
 
 def main(args=None):
     rclpy.init(args=args)
 
-    minimal_publisher = TwistPublisher()
+    minimal_publisher = TwistPublisher(
+        SMOOTH_WEIGHTS,
+        SCALE_LINEAR_VELOCITY,
+        SCALE_ANGULAR_VELOCITY,
+        PUBLISH_FREQUENCY_HZ,
+    )
 
     rclpy.spin(minimal_publisher)
 
-    # Destroy the node explicitly
-    # (optional - otherwise it will be done automatically
-    # when the garbage collector destroys the node object)
     minimal_publisher.destroy_node()
     rclpy.shutdown()
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     main()

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

@@ -14,7 +14,6 @@
 
 #include "Arduino.h"
 #include "constants.h"
-#include "R2WD.h"
 
 class UARTCom {
 private:

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

@@ -8,7 +8,8 @@ uint8_t UARTCom::data[MAX_PACKET_LENGTH];
 uint8_t currentParseIndex;
 int payloadLength;
 uint32_t currentTimestamp = 0;
-extern R2WD _2WD;
+extern MotorWheel wheelLeft;
+extern MotorWheel wheelRight;
 
 /**
  * @brief extracts frametype from data package
@@ -169,7 +170,7 @@ void handleRequest (uint8_t* d)
 			break;
 		}
 		case DRIVE_ENCODER: {
-			UValue::sendBothEncoderValues(_2WD.wheelLeftGetSpeedMMPS()/1000.0, _2WD.wheelRightGetSpeedMMPS()/1000.0);
+			UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM());
 			break;
 		}
 		case PID_PARAMETER: {

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

@@ -1,7 +1,7 @@
 #include "communication/UCommands.hpp"
 #include "misc/Utils.hpp"
-#include "R2WD.h"
+extern MotorWheel wheelLeft;
-extern R2WD _2WD;
+extern MotorWheel wheelRight;
 
 void UCommands::setSetpoint (uint8_t* d, U_Component c)
 {
@@ -15,16 +15,17 @@ void UCommands::setSetpoint (uint8_t* d, U_Component c)
 											d[COMMAND_POS + 6],
 											d[COMMAND_POS + 7],
 											d[COMMAND_POS + 8]);
-			_2WD.wheelLeftSetSpeedMMPS(abs(speed1*1000), speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE);
+			wheelLeft.setGearedRPM(speed1, speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE);
-			_2WD.wheelRightSetSpeedMMPS(abs(speed2*1000), speed2 > 0 ? DIR_ADVANCE : DIR_BACKOFF);
+			wheelRight.setGearedRPM(speed2, speed2 > 0 ? DIR_ADVANCE : DIR_BACKOFF);
+			
 			break;
 		}
 		case RIGHT_MOTOR: {
-			_2WD.wheelLeftSetSpeedMMPS(abs(speed1*1000), speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE);
+			wheelRight.setGearedRPM(speed1, speed1 > 0 ? DIR_ADVANCE : DIR_BACKOFF);
 			break;
 		}
 		case LEFT_MOTOR: {
-			_2WD.wheelRightSetSpeedMMPS(abs(speed1*1000), speed1 > 0 ? DIR_ADVANCE : DIR_BACKOFF);
+			wheelLeft.setGearedRPM(speed1, speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE);
 			break;
 		}
 		default: {
@@ -47,17 +48,22 @@ void UCommands::setPIDParameter (uint8_t* d, U_Component c)
 									  d[COMMAND_POS + 10],
 									  d[COMMAND_POS + 11],
 									  d[COMMAND_POS + 12]);
+	int T = Utils::bytesToInt(d[COMMAND_POS + 9],
+									  d[COMMAND_POS + 10],
+									  d[COMMAND_POS + 11],
+									  d[COMMAND_POS + 12]);
 	switch (c) {
 		case ALL_MOTORS: {
-			_2WD.PIDTune(P,I,D,10);
+			wheelLeft.PIDSetup(P,I,D,T);
+			wheelRight.PIDSetup(P,I,D,T);
 			break;
 		}
 		case RIGHT_MOTOR: {
-			_2WD.wheelLeftPIDTune(P,I,D,10);
+			wheelLeft.PIDSetup(P,I,D,T);
 			break;
 		}
 		case LEFT_MOTOR: {
-			_2WD.wheelRightPIDTune(P,I,D,10);
+			wheelRight.PIDSetup(P,I,D,T);
 			break;
 		}
 		default: {

src/dcaitirobot/embedded/src/main.cpp

@@ -10,7 +10,6 @@ int speed = 0;
 #include <PinChangeIntConfig.h>
 #include <PID_Beta6.h>
 #include <MotorWheel.h>
-#include <R2WD.h>
 #ifndef MICROS_PER_SEC
 #define MICROS_PER_SEC 1000000
 #endif
@@ -18,14 +17,13 @@ irqISR(irq1, isr1);
 irqISR(irq2, isr2);
 MotorWheel wheelLeft(9, 8, 4, 5, &irq1, REDUCTION_RATIO, 300);	// Motor PWM:Pin9, DIR:Pin8, Encoder A:Pin6, B:Pin7
 MotorWheel wheelRight(10, 11, 6, 7, &irq2, REDUCTION_RATIO, 300); // Motor PWM:Pin9, DIR:Pin8, Encoder A:Pin6, B:Pin7
-R2WD _2WD(&wheelLeft, &wheelRight, WHEELSPAN);
 
 void setup(void)
 {
 	TCCR1B = TCCR1B & 0xf8 | 0x01; // Pin9,Pin10 PWM 31250Hz, Silent PWM
-	_2WD.PIDEnable(0.26,0.02,0.02,10);// Enable PID
+	wheelLeft.PIDEnable(0.26,0.02,0.02,10);// Enable PID
+	wheelRight.PIDEnable(0.26,0.02,0.02,10);// Enable PID
 
-	//_2WD.PIDEnable(KC, TAUI, TAUD, 10);
 	UARTCom::init(UARTCom::data);
 	controlTimer = millis();
 	uartTransmitTimer = millis();
@@ -38,6 +36,7 @@ void setup(void)
 
 void loop(void)
 {
-	_2WD.PIDRegulate();
+	wheelLeft.PIDRegulate();
+	wheelRight.PIDRegulate();
 	UARTCom::read(UARTCom::data);
 }

src/dcaitirobot/launch/remote_control_launch.py

@@ -1,16 +1,19 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
+
 def generate_launch_description():
-    return LaunchDescription([
+    return LaunchDescription(
+        [
             Node(
-            package='dcaitirobot',
+                package="dcaitirobot",
-            executable='joy',
+                executable="joy",
-            name='joystick',
+                name="joystick",
             ),
             Node(
-            package='dcaitirobot',
+                package="dcaitirobot",
-            executable='twistcalc',
+                executable="twistcalc",
-            name='twistcalc',
+                name="twistcalc",
+            ),
+        ]
     )
-    ])

src/dcaitirobot/launch/start_turtle_slam_sim.py

@@ -0,0 +1,44 @@
+from pathlib import Path
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from ament_index_python import get_package_share_directory
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    str(Path(__file__).parent.absolute() / "remote_control_launch.py")
+                )
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    str(
+                        (
+                            Path(
+                                get_package_share_directory("turtlebot3_gazebo")
+                            ).absolute()
+                            / "launch"
+                            / "turtlebot3_world.launch.py"
+                        )
+                    )
+                )
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    str(
+                        (
+                            Path(get_package_share_directory("slam_toolbox")).absolute()
+                            / "launch"
+                            / "online_async_launch.py"
+                        )
+                    )
+                ),
+                launch_arguments=[("use_sim_time", "True")],
+            ),
+        ]
+    )

src/diffdrive_arduino/include/diffdrive_arduino/wheel.h

@@ -16,13 +16,12 @@ class Wheel
     double vel = 0;
     double eff = 0;
     double velSetPt = 0;
-    double rads_per_count = 0;
 
     Wheel() = default;
 
-    Wheel(const std::string &wheel_name, int counts_per_rev);
+    Wheel(const std::string &wheel_name);
     
-    void setup(const std::string &wheel_name, int counts_per_rev);
+    void setup(const std::string &wheel_name);
 
     double calcEncAngle();
 

src/diffdrive_arduino/src/arduino_comms.cpp

@@ -30,6 +30,7 @@ void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed)
 
 void ArduinoComms::setMotorValues(float left_speed, float right_speed)
 {
+    
     std::vector<uint8_t> payload = {DRIVE_MOTOR};
     auto bytes_left_speed = marshalling(left_speed);
     payload.insert(payload.end(), bytes_left_speed.begin(), bytes_left_speed.end());

src/diffdrive_arduino/src/diffdrive_arduino.cpp

@@ -5,6 +5,7 @@
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 
 #include "rclcpp/rclcpp.hpp"
+#include <math.h>
 
 namespace diffdrive_arduino
 {
@@ -31,11 +32,10 @@ CallbackReturn DiffDriveArduino::on_init(const hardware_interface::HardwareInfo
   cfg_.device = info_.hardware_parameters["device"];
   cfg_.baud_rate = std::stoi(info_.hardware_parameters["baud_rate"]);
   cfg_.timeout = std::stoi(info_.hardware_parameters["timeout"]);
-  cfg_.enc_counts_per_rev = std::stoi(info_.hardware_parameters["enc_counts_per_rev"]);
 
   // Set up the wheels
-  l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev);
+  l_wheel_.setup(cfg_.left_wheel_name);
-  r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev);
+  r_wheel_.setup(cfg_.right_wheel_name);
 
   // Set up the Arduino
   arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout);  
@@ -107,6 +107,13 @@ return_type DiffDriveArduino::read()
   }
 
   arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel);
+  // 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 ;
+  r_wheel_.vel = (r_wheel_.vel / 60) * 2 * M_PI * 0.049 ;
+
     
   // Force the wheel position
   l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds;
@@ -120,7 +127,11 @@ return_type DiffDriveArduino::write()
   {
     return return_type::ERROR;
   }
-  arduino_.setMotorValues(l_wheel_.cmd, r_wheel_.cmd);
+  // convert rad/s to rpm and write to motors
+  arduino_.setMotorValues(l_wheel_.cmd *  30 / M_PI , r_wheel_.cmd * 30 / M_PI );
+  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;  
 }
 

src/diffdrive_arduino/src/fake_robot.cpp

@@ -26,8 +26,8 @@ CallbackReturn FakeRobot::on_init(const hardware_interface::HardwareInfo & info)
   // Note: It doesn't matter that we haven't set encoder counts per rev
   // since the fake robot bypasses the encoder code completely
 
-  l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev);
+  l_wheel_.setup(cfg_.left_wheel_name);
-  r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev);
+  r_wheel_.setup(cfg_.right_wheel_name);
 
   RCLCPP_INFO(logger_, "Finished Configuration Fakerobot");
 

src/diffdrive_arduino/src/wheel.cpp

@@ -3,19 +3,13 @@
 #include <cmath>
 
 
-Wheel::Wheel(const std::string &wheel_name, int counts_per_rev)
+Wheel::Wheel(const std::string &wheel_name)
 {
-  setup(wheel_name, counts_per_rev);
+  setup(wheel_name);
 }
 
 
-void Wheel::setup(const std::string &wheel_name, int counts_per_rev)
+void Wheel::setup(const std::string &wheel_name)
 {
   name = wheel_name;
-  rads_per_count = (2*M_PI)/counts_per_rev;
-}
-
-double Wheel::calcEncAngle()
-{
-  return enc * rads_per_count;
 }