From 0453d1df583896754322d5f87cb67ff6608d6a4f Mon Sep 17 00:00:00 2001
From: Paul-Winpenny <92634321+Paul-Winpenny@users.noreply.github.com>
Date: Mon, 23 Dec 2024 09:17:58 +0000
Subject: [PATCH] IMU needs to be powered on and then the offset should be
looked at. NEeds to be tested in the lab
---
.../robobin/robobin/helpers/message_handler.py | 16 ++++++++--------
ros2/src/robobin/robobin/uwb_pathing_node.py | 2 +-
2 files changed, 9 insertions(+), 9 deletions(-)
diff --git a/ros2/src/robobin/robobin/helpers/message_handler.py b/ros2/src/robobin/robobin/helpers/message_handler.py
index 857cfa92..3182da3b 100644
--- a/ros2/src/robobin/robobin/helpers/message_handler.py
+++ b/ros2/src/robobin/robobin/helpers/message_handler.py
@@ -114,10 +114,10 @@ class MessageHandler:
# D: linear.x = 0, angular.z = -0.5
directions = {
- "W": (0.5, 0), # Move forward
- "A": (0, 0.5), # Turn left
- "S": (-0.5, 0), # Move backward
- "D": (0, -0.5) # Turn right
+ "W": (0.25, 0), # Move forward
+ "A": (0, 0.25), # Turn left
+ "S": (-0.25, 0), # Move backward
+ "D": (0, -0.25) # Turn right
}
# Get direction data and ensure it's a tuple of floats
@@ -195,10 +195,10 @@ if __name__ == "__main__":
message_handler = MessageHandler(None, testing=True)
assert message_handler.handle_message(None, "PING") is None
assert message_handler.handle_message(None, "TIME") is None
- assert message_handler.handle_message(None, "MANUALCTRL W") == ("cmd_vel", (0.5, 0))
- assert message_handler.handle_message(None, "MANUALCTRL A") == ("cmd_vel", (0, 0.5))
- assert message_handler.handle_message(None, "MANUALCTRL S") == ("cmd_vel", (-0.5, 0))
- assert message_handler.handle_message(None, "MANUALCTRL D") == ("cmd_vel", (0, -0.5))
+ assert message_handler.handle_message(None, "MANUALCTRL W") == ("cmd_vel", (0.25, 0))
+ assert message_handler.handle_message(None, "MANUALCTRL A") == ("cmd_vel", (0, 0.25))
+ assert message_handler.handle_message(None, "MANUALCTRL S") == ("cmd_vel", (-0.25, 0))
+ assert message_handler.handle_message(None, "MANUALCTRL D") == ("cmd_vel", (0, -0.25))
assert message_handler.handle_message(None, "STOP") == ("cmd_vel", (0.0, 0.0))
assert message_handler.handle_message(None, "CALLME (212.9638, 283.98108)") == ("nav_command", "(212.9638, 283.98108)")
diff --git a/ros2/src/robobin/robobin/uwb_pathing_node.py b/ros2/src/robobin/robobin/uwb_pathing_node.py
index dec8b955..00fc1b49 100644
--- a/ros2/src/robobin/robobin/uwb_pathing_node.py
+++ b/ros2/src/robobin/robobin/uwb_pathing_node.py
@@ -73,7 +73,7 @@ class UWBPathingNode(Node):
return
# Calculate yaw error (assuming robot orientation 0 = facing x-axis)
- # If you don't have actual orientation, you might assume the robot always faces the target directly
+
yaw_error = angle_to_target
yaw_error = atan2(sin(yaw_error), cos(yaw_error)) # Normalize angle
self.get_logger().info(f"Current Position: x={self.current_x:.2f}, y={self.current_y:.2f}")
--
GitLab