diff --git a/ros2/src/robobin/robobin/helpers/message_handler.py b/ros2/src/robobin/robobin/helpers/message_handler.py index 857cfa9205951862f80a9900bba3ad47280df4d4..3182da3bd7e274b158b5f3003f77798ee748a734 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 dec8b95597d81d37e8cc8f3d74aef15b9c7702c8..00fc1b498f4e752d4c447257a718e56f52858d9c 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}")