diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 862243a..fbfb06e 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -13,22 +13,25 @@ import os import sys import threading import glob -from math import sqrt from std_msgs.msg import String from sensor_msgs.msg import Imu, NavSatFix from geometry_msgs.msg import TwistStamped, Twist -from ros2_interfaces_pkg.msg import CoreFeedback -from ros2_interfaces_pkg.msg import CoreControl +from ros2_interfaces_pkg.msg import CoreControl, CoreFeedback +from ros2_interfaces_pkg.msg import NewCoreFeedback, CoreCtrlState serial_pub = None thread = None -CORE_WHEELBASE = 0.84 # meters -- TODO: verify -CORE_WHEEL_RADIUS = 0.015 # meters -- TODO: verify +CORE_WHEELBASE = 0.836 # meters +CORE_WHEEL_RADIUS = 0.171 # meters CORE_GEAR_RATIO = 100 # Clucky: 100:1, Testbed: 64:1 +WALKING_SPEED = 0.5 # duty cycle +FAST_SPEED = 0.9 # duty cycle +SLOW_SPEED = 0.25 # duty cycle + control_qos = qos.QoSProfile( history=qos.QoSHistoryPolicy.KEEP_LAST, depth=2, @@ -60,6 +63,8 @@ class SerialRelay(Node): self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10) self.cmd_vel_sub_ = self.create_subscription(TwistStamped, '/cmd_vel', self.cmd_vel_callback, qos_profile=control_qos) self.twist_man_sub_ = self.create_subscription(Twist, '/core/twist', self.twist_man_callback, qos_profile=control_qos) + self.control_state_sub_ = self.create_subscription(CoreCtrlState, '/core/control/state', self.control_state_callback, qos_profile=control_qos) + self.twist_speed_mode = 0 # -1 = slow, 0 = walk, 1 = fast # Feedback self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10) @@ -197,10 +202,10 @@ class SerialRelay(Node): self.send_cmd(command) def twist_man_callback(self, msg: Twist): - linear = msg.linear.x # [-1 1] for forward/back from left joy y - angular = -msg.angular.z # [-1 1] for left/right from right joy x + linear = msg.linear.x # [-1 1] for forward/back from left stick y + angular = msg.angular.z # [-1 1] for left/right from right stick x - if (linear < 0): # reverse turning direction when going backwards + if (linear < 0): # reverse turning direction when going backwards (WIP) angular *= -1 if abs(linear) > 1 or abs(angular) > 1: @@ -214,9 +219,28 @@ class SerialRelay(Node): scale = max(1, abs(duty_left), abs(duty_right)) duty_left /= scale duty_right /= scale + + # Apply speed mode + if self.twist_speed_mode == -1: # slow + duty_left = map_range(duty_left, -1, 1, -SLOW_SPEED, SLOW_SPEED) + duty_right = map_range(duty_right, -1, 1, -SLOW_SPEED, SLOW_SPEED) + elif self.twist_speed_mode == 0: # walk + duty_left = map_range(duty_left, -1, 1, -WALKING_SPEED, WALKING_SPEED) + duty_right = map_range(duty_right, -1, 1, -WALKING_SPEED, WALKING_SPEED) + elif self.twist_speed_mode == 1: # fast + duty_left = map_range(duty_left, -1, 1, -FAST_SPEED, FAST_SPEED) + duty_right = map_range(duty_right, -1, 1, -FAST_SPEED, FAST_SPEED) command = f"can_relay_tovic,core,19,{duty_left},{duty_right}\n" self.send_cmd(command) + + def control_state_callback(self, msg: CoreCtrlState): + # Brake mode + command = f"can_relay_tovic,core,18,{str(int(msg.brake_mode))}\n" + self.send_cmd(command) + + # Speed mode + self.twist_speed_mode = msg.speed_mode # twist_man_callback will handle this def send_cmd(self, msg: str): if self.launch_mode == 'anchor': @@ -321,6 +345,9 @@ def myexcepthook(type, value, tb): if serial_pub: serial_pub.cleanup() +def map_range(value: float, in_min: float, in_max: float, out_min: float, out_max: float): + return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min + def main(args=None): rclpy.init(args=args) diff --git a/src/headless_pkg/src/headless_node.py b/src/headless_pkg/src/headless_node.py index 930f257..1837296 100755 --- a/src/headless_pkg/src/headless_node.py +++ b/src/headless_pkg/src/headless_node.py @@ -16,6 +16,7 @@ import glob from std_msgs.msg import String from geometry_msgs.msg import Twist from ros2_interfaces_pkg.msg import CoreControl, ArmManual +from ros2_interfaces_pkg.msg import CoreCtrlState import pygame @@ -49,9 +50,6 @@ arm_stop_msg.gripper = 0 arm_stop_msg.linear_actuator = 0 arm_stop_msg.laser = 0 -ctrl_mode = "core" -CORE_MODE = "twist" # "twist" or "duty" - control_qos = qos.QoSProfile( history=qos.QoSHistoryPolicy.KEEP_LAST, depth=2, @@ -63,6 +61,12 @@ control_qos = qos.QoSProfile( liveliness_lease_duration=Duration(seconds=5) ) +ctrl_mode = "core" +CORE_MODE = "twist" # "twist" or "duty" + +core_brake_mode = False +core_speed_mode = 0 # -1 = slow, 0 = walk, 1 = fast + class Headless(Node): def __init__(self): @@ -93,6 +97,7 @@ class Headless(Node): self.core_publisher = self.create_publisher(CoreControl, '/core/control', 10) self.arm_publisher = self.create_publisher(ArmManual, '/arm/control/manual', 10) self.core_twist_pub_ = self.create_publisher(Twist, '/core/twist', qos_profile=control_qos) + self.core_state_pub_ = self.create_publisher(CoreCtrlState, '/core/control/state', qos_profile=control_qos) def run(self): # This thread makes all the update processes run in the background @@ -168,21 +173,42 @@ class Headless(Node): # Collect controller state left_stick_y = deadzone(self.gamepad.get_axis(1)) right_stick_x = deadzone(self.gamepad.get_axis(3)) + button_a = self.gamepad.get_button(0) + left_bumper = self.gamepad.get_button(4) + right_bumper = self.gamepad.get_button(5) + # Forward/back and Turn + input.linear.x = -left_stick_y + input.angular.z = -right_stick_x - # Forward/back - input.linear.x = float(round(-1 * left_stick_y, 2)) - - # Turn - input.angular.z = float(round(right_stick_x, 2)) - - - # Debug - output = f'Lin X: {input.linear.x}, Ang Z: {input.angular.z}' - self.get_logger().info(f"[Ctrl] {output}") - + # Publish self.core_twist_pub_.publish(input) self.arm_publisher.publish(arm_stop_msg) + self.get_logger().info(f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}") + + # State (brake and speed) + global core_brake_mode + global core_speed_mode + + # Brake mode + new_brake_mode = button_a + # Speed mode + if left_bumper: + new_speed_mode = -1 + elif right_bumper: + new_speed_mode = 1 + else: + new_speed_mode = 0 + + # Only publish if needed + if new_brake_mode != core_brake_mode or new_speed_mode != core_speed_mode: + core_brake_mode = new_brake_mode + core_speed_mode = new_speed_mode + state_msg = CoreCtrlState() + state_msg.brake_mode = core_brake_mode + state_msg.speed_mode = core_speed_mode + self.core_state_pub_.publish(state_msg) + self.get_logger().info(f"[Core State] Brake: {core_brake_mode}, Speed: {core_speed_mode}") # ARM diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index 7ae068d..821cddd 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit 7ae068d5705547cd3cef918e2db350397f0e5314 +Subproject commit 821cdddee0a100beca9bc243549ad361a3effaa3