From 5cb0be703fe11b5452392e18bfdc9cbe3dc75a71 Mon Sep 17 00:00:00 2001 From: Daniel Neurath Date: Tue, 20 Jun 2023 11:52:23 +0200 Subject: [PATCH 1/3] twist message smoothing (needs to be tested) --- src/dcaitirobot/dcaitirobot/twist.py | 86 ++++++++++++++++++++-------- 1 file changed, 63 insertions(+), 23 deletions(-) diff --git a/src/dcaitirobot/dcaitirobot/twist.py b/src/dcaitirobot/dcaitirobot/twist.py index 7fd9f5a..fab6d26 100644 --- a/src/dcaitirobot/dcaitirobot/twist.py +++ b/src/dcaitirobot/dcaitirobot/twist.py @@ -7,54 +7,94 @@ from sensor_msgs.msg import Joy import numpy as np -SCALE_LINEAR_VELOCITY = 0.5 -SCALE_ANGULAR_VELOCITY = 0.4 -PUBLISH_FREQUENCY_HZ = 10 +SCALE_LINEAR_VELOCITY = 0.1 +SCALE_ANGULAR_VELOCITY = 0.1 +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") - 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.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.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() + + self.update_smooth_window() + self.calc_smooth_speed() + msg.linear.x = self.linear msg.angular.z = self.angular self.publisher_.publish(msg) - self.get_logger().info('Publishing: "%s"' % 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__': - main() \ No newline at end of file +if __name__ == "__main__": + main() From ebb17be5af97421d52a44bf746f270fa9d11848b Mon Sep 17 00:00:00 2001 From: Tim Korjakow Date: Thu, 22 Jun 2023 21:03:35 +0200 Subject: [PATCH 2/3] Fixed real <> sim transfer --- src/dcaiti_control/config/dcaiti_config.yml | 10 ++++---- .../description/real_robot_control.xacro | 9 ++++--- .../description/robot_core.xacro | 24 +++++++++---------- .../include/communication/UARTCom.hpp | 1 - .../embedded/src/communication/UARTCom.cpp | 5 ++-- .../embedded/src/communication/UCommands.cpp | 24 ++++++++++++------- src/dcaitirobot/embedded/src/main.cpp | 9 ++++--- .../include/diffdrive_arduino/wheel.h | 5 ++-- src/diffdrive_arduino/src/arduino_comms.cpp | 1 + .../src/diffdrive_arduino.cpp | 23 +++++++++++++----- src/diffdrive_arduino/src/fake_robot.cpp | 4 ++-- src/diffdrive_arduino/src/wheel.cpp | 12 +++------- 12 files changed, 68 insertions(+), 59 deletions(-) diff --git a/src/dcaiti_control/config/dcaiti_config.yml b/src/dcaiti_control/config/dcaiti_config.yml index e72efb9..2a07f76 100644 --- a/src/dcaiti_control/config/dcaiti_config.yml +++ b/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: - x: - max_acceleration: 0.05 - has_acceleration_limits: true +# linear: +# x: +# max_acceleration: 0.15 +# has_acceleration_limits: true use_stamped_vel: false diff --git a/src/dcaiti_control/description/real_robot_control.xacro b/src/dcaiti_control/description/real_robot_control.xacro index fbf6f23..18e52cc 100644 --- a/src/dcaiti_control/description/real_robot_control.xacro +++ b/src/dcaiti_control/description/real_robot_control.xacro @@ -9,21 +9,20 @@ /dev/ttyUSB0 57600 1000 - 3436 - -0.005 - 0.005 + -0.5 + 0.5 - -0.005 - 0.005 + -0.5 + 0.5 diff --git a/src/dcaiti_control/description/robot_core.xacro b/src/dcaiti_control/description/robot_core.xacro index fd4ed23..e7f1ec8 100644 --- a/src/dcaiti_control/description/robot_core.xacro +++ b/src/dcaiti_control/description/robot_core.xacro @@ -41,17 +41,17 @@ - + - + - + @@ -73,16 +73,16 @@ - + - + - + @@ -110,16 +110,16 @@ - + - + - + @@ -142,16 +142,16 @@ - + - + - + diff --git a/src/dcaitirobot/embedded/include/communication/UARTCom.hpp b/src/dcaitirobot/embedded/include/communication/UARTCom.hpp index bb64930..50ae844 100644 --- a/src/dcaitirobot/embedded/include/communication/UARTCom.hpp +++ b/src/dcaitirobot/embedded/include/communication/UARTCom.hpp @@ -14,7 +14,6 @@ #include "Arduino.h" #include "constants.h" -#include "R2WD.h" class UARTCom { private: diff --git a/src/dcaitirobot/embedded/src/communication/UARTCom.cpp b/src/dcaitirobot/embedded/src/communication/UARTCom.cpp index 975ba38..358d3bf 100644 --- a/src/dcaitirobot/embedded/src/communication/UARTCom.cpp +++ b/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: { diff --git a/src/dcaitirobot/embedded/src/communication/UCommands.cpp b/src/dcaitirobot/embedded/src/communication/UCommands.cpp index 17201f0..ac675ed 100644 --- a/src/dcaitirobot/embedded/src/communication/UCommands.cpp +++ b/src/dcaitirobot/embedded/src/communication/UCommands.cpp @@ -1,7 +1,7 @@ #include "communication/UCommands.hpp" #include "misc/Utils.hpp" -#include "R2WD.h" -extern R2WD _2WD; +extern MotorWheel wheelLeft; +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); - _2WD.wheelRightSetSpeedMMPS(abs(speed2*1000), speed2 > 0 ? DIR_ADVANCE : DIR_BACKOFF); + wheelLeft.setGearedRPM(speed1, speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE); + 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: { diff --git a/src/dcaitirobot/embedded/src/main.cpp b/src/dcaitirobot/embedded/src/main.cpp index cabab23..814d0c3 100644 --- a/src/dcaitirobot/embedded/src/main.cpp +++ b/src/dcaitirobot/embedded/src/main.cpp @@ -10,7 +10,6 @@ int speed = 0; #include #include #include -#include #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); } \ No newline at end of file diff --git a/src/diffdrive_arduino/include/diffdrive_arduino/wheel.h b/src/diffdrive_arduino/include/diffdrive_arduino/wheel.h index 712f25e..3c9deab 100644 --- a/src/diffdrive_arduino/include/diffdrive_arduino/wheel.h +++ b/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(); diff --git a/src/diffdrive_arduino/src/arduino_comms.cpp b/src/diffdrive_arduino/src/arduino_comms.cpp index 5d03e04..fdeb06d 100644 --- a/src/diffdrive_arduino/src/arduino_comms.cpp +++ b/src/diffdrive_arduino/src/arduino_comms.cpp @@ -31,6 +31,7 @@ void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed) void ArduinoComms::setMotorValues(float left_speed, float right_speed) { + std::vector payload = {DRIVE_MOTOR}; auto bytes_left_speed = marshalling(left_speed); payload.insert(payload.end(), bytes_left_speed.begin(), bytes_left_speed.end()); diff --git a/src/diffdrive_arduino/src/diffdrive_arduino.cpp b/src/diffdrive_arduino/src/diffdrive_arduino.cpp index ea8192c..b80283e 100644 --- a/src/diffdrive_arduino/src/diffdrive_arduino.cpp +++ b/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 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); - r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev); + l_wheel_.setup(cfg_.left_wheel_name); + r_wheel_.setup(cfg_.right_wheel_name); // Set up the Arduino arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout); @@ -106,9 +106,16 @@ 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); + // 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 + + // Force the wheel position l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds; r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds; return return_type::OK; @@ -120,7 +127,11 @@ return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcp { 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; } diff --git a/src/diffdrive_arduino/src/fake_robot.cpp b/src/diffdrive_arduino/src/fake_robot.cpp index 6794043..979259a 100644 --- a/src/diffdrive_arduino/src/fake_robot.cpp +++ b/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); - r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev); + l_wheel_.setup(cfg_.left_wheel_name); + r_wheel_.setup(cfg_.right_wheel_name); RCLCPP_INFO(logger_, "Finished Configuration"); diff --git a/src/diffdrive_arduino/src/wheel.cpp b/src/diffdrive_arduino/src/wheel.cpp index 60d8ede..e18884b 100644 --- a/src/diffdrive_arduino/src/wheel.cpp +++ b/src/diffdrive_arduino/src/wheel.cpp @@ -3,19 +3,13 @@ #include -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; } \ No newline at end of file From 4d41485c4718d7fa53d1b23b54e5fe447bdd2a99 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 5 Jul 2023 11:36:16 +0200 Subject: [PATCH 3/3] working turtlebot3 sim --- src/dcaitirobot/dcaitirobot/twist.py | 8 +++- .../launch/remote_control_launch.py | 27 +++++++----- .../launch/start_turtle_slam_sim.py | 44 +++++++++++++++++++ 3 files changed, 65 insertions(+), 14 deletions(-) create mode 100644 src/dcaitirobot/launch/start_turtle_slam_sim.py diff --git a/src/dcaitirobot/dcaitirobot/twist.py b/src/dcaitirobot/dcaitirobot/twist.py index fab6d26..5cbcbbb 100644 --- a/src/dcaitirobot/dcaitirobot/twist.py +++ b/src/dcaitirobot/dcaitirobot/twist.py @@ -41,6 +41,9 @@ class TwistPublisher(Node): self.publisher_ = self.create_publisher( Twist, "/diff_cont/cmd_vel_unstamped", 1 ) + + self.publisher_2 = self.create_publisher(Twist, "/cmd_vel", 1) + self.create_subscription(Joy, "/joy", self.joy_callback, 1) self.timer = self.create_timer(1 / publish_frequency_hz, self.timer_callback) @@ -67,9 +70,10 @@ class TwistPublisher(Node): self.update_smooth_window() self.calc_smooth_speed() - msg.linear.x = self.linear - msg.angular.z = self.angular + msg.linear.x = self.linear_out + msg.angular.z = self.angular_out self.publisher_.publish(msg) + self.publisher_2.publish(msg) self.get_logger().info( "Linear_window: {}, Linear_out: {}, Angular_window: {}, Angular_out: {}".format( self.linear_window, diff --git a/src/dcaitirobot/launch/remote_control_launch.py b/src/dcaitirobot/launch/remote_control_launch.py index 193078a..6a5af93 100644 --- a/src/dcaitirobot/launch/remote_control_launch.py +++ b/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([ - Node( - package='dcaitirobot', - executable='joy', - name='joystick', - ), - Node( - package='dcaitirobot', - executable='twistcalc', - name='twistcalc', - ) - ]) + return LaunchDescription( + [ + Node( + package="dcaitirobot", + executable="joy", + name="joystick", + ), + Node( + package="dcaitirobot", + executable="twistcalc", + name="twistcalc", + ), + ] + ) diff --git a/src/dcaitirobot/launch/start_turtle_slam_sim.py b/src/dcaitirobot/launch/start_turtle_slam_sim.py new file mode 100644 index 0000000..3a2804f --- /dev/null +++ b/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")], + ), + ] + )