diff --git a/README.md b/README.md index 20ef4533..1f42efab 100644 --- a/README.md +++ b/README.md @@ -99,6 +99,8 @@ devcontainer build --push true --workspace-folder . --platform="linux/amd64,linu
How to Run Inside the ISAAC ROS Container on Linux/Jetson
+** FOR MORE INFO ON UPDATING AND USING DOCKER STUFF, SEE THE README INSIDE THE `docker` directory ** + First, you will need to log in to Nvidia NGC and get an API Key here: https://org.ngc.nvidia.com/setup Then install Nvidia ngc CLI and make sure it is present in path: https://org.ngc.nvidia.com/setup/installers/cli diff --git a/arduino/sensors/sensors.ino b/arduino/sensors/sensors.ino index dc9ac952..ffab8bd2 100644 --- a/arduino/sensors/sensors.ino +++ b/arduino/sensors/sensors.ino @@ -1,13 +1,12 @@ // Define a struct to hold the sensor data struct SensorData { - int leftMotorPotentiometer; - int rightMotorPotentiometer; - bool bottomLimitSwitch; + bool dumpLimitSwitch; + bool extensionLimitSwitch; }; // Define the sensor pins here -#define LEFT_MOTOR_POT_PIN A0 -#define RIGHT_MOTOR_POT_PIN A1 +#define DUMP_LIMIT_SWITCH 12 +#define EXTENSION_LIMIT_SWITCH 13 #define RELAY_PIN 7 void setup() { @@ -17,6 +16,9 @@ void setup() { pinMode(RELAY_PIN, OUTPUT); // Set relay pin as an output digitalWrite(RELAY_PIN, LOW); // Ensure the motor is off at the start + pinMode(DUMP_LIMIT_SWITCH, INPUT_PULLUP); + pinMode(EXTENSION_LIMIT_SWITCH, INPUT_PULLUP); + // No need to configure analog pins explicitly for potentiometers // as they are used as inputs by default. } @@ -26,10 +28,8 @@ void loop() { SensorData data; // Read from the analog inputs (potentiometers) - data.leftMotorPotentiometer = analogRead(LEFT_MOTOR_POT_PIN); // Read left motor potentiometer value - data.rightMotorPotentiometer = analogRead(RIGHT_MOTOR_POT_PIN); // Read right motor potentiometer value - - data.bottomLimitSwitch = analogRead(bottom_limit_switch); //bottom limit switch value + data.dumpLimitSwitch = digitalRead(DUMP_LIMIT_SWITCH); + data.extensionLimitSwitch = digitalRead(EXTENSION_LIMIT_SWITCH); // Send the struct over the serial bus to the Nvidia Jetson Serial.write((byte *)&data, sizeof(SensorData)); diff --git a/config/auger_config.yaml b/config/auger_config.yaml index 5d2a7153..d88c84ea 100644 --- a/config/auger_config.yaml +++ b/config/auger_config.yaml @@ -1,21 +1,17 @@ /**: ros__parameters: #TODO: Figure out all of these values - SCREW_SPEED: 4000 # TODO: update + SCREW_SPEED: 20000 # TODO: update POWER_LIMIT: 1 #TODO: update - MAX_SCREW_SPEED: 4000 # in RPM - MIN_SCREW_SPEED: 2000 # in RPM TODO: This is a guess - MAX_RETRACT_PUSH_MOTOR_VELOCITY: -600 # in RPM, potentially could be faster - MAX_EXTEND_PUSH_MOTOR_VELOCITY: 600 # in RPM - push_motor_position: 0 - tilt_actuator_position: 0 - TILT_ACTUATOR_CURRENT_THRESHOLD: 0 - MAX_PUSH_MOTOR_POSITION: 0 - MIN_PUSH_MOTOR_POSITION: 0 + FAST_SCREW_SPEED: 20000 #0.21 + extension_limit_switch: true + STOWED: true + MAX_PUSH_MOTOR_POSITION: 10000 + MIN_PUSH_MOTOR_POSITION: 1000 PUSH_MOTOR_POS_TOLERANCE: 0 MAX_PUSH_MOTOR_VELOCITY: 0 - TILT_ACTUATOR_SPEED: 0 + TILT_ACTUATOR_SPEED: 1.0 TILT_ACTUATOR_MIN_EXTENSION: 0 - TILT_ACTUATOR_ID: 0 - PUSH_MOTOR_ID: 0 - SPIN_MOTOR_ID: 0 \ No newline at end of file + TILT_ACTUATOR_ID: 1 + PUSH_MOTOR_ID: 3 + SPIN_MOTOR_ID: 4 \ No newline at end of file diff --git a/config/dumper_config.yaml b/config/dumper_config.yaml index 7013fd33..b300a393 100644 --- a/config/dumper_config.yaml +++ b/config/dumper_config.yaml @@ -1,3 +1,3 @@ /**: ros__parameters: - DUMPER_POWER: 0.75 # The power the dumper needs to go + DUMPER_POWER: -0.75 # The power the dumper needs to go diff --git a/config/motor_control.yaml b/config/motor_control.yaml index a756c74c..599ee9d4 100644 --- a/config/motor_control.yaml +++ b/config/motor_control.yaml @@ -2,10 +2,11 @@ ros__parameters: CAN_INTERFACE_TRANSMIT: "can0" # can0, can1, slcan0, slcan1 are common values CAN_INTERFACE_RECEIVE: "can0" # can0, can1, slcan0, slcan1 are common values - BACK_LEFT_DRIVE: 5 # CAN ID of the back left drive motor - FRONT_LEFT_DRIVE: 3 # CAN ID of the front left drive motor - BACK_RIGHT_DRIVE: 1 # CAN ID of the back right drive motor - FRONT_RIGHT_DRIVE: 6 # CAN ID of the front right drive motor + CAN_INTERFACE: "can0" # can0, can1, slcan0, slcan1 are common values + BACK_LEFT_DRIVE: 6 # CAN ID of the back left drive motor + FRONT_LEFT_DRIVE: 8 # CAN ID of the front left drive motor + BACK_RIGHT_DRIVE: 7 # CAN ID of the back right drive motor + FRONT_RIGHT_DRIVE: 5 # CAN ID of the front right drive motor DIGGER_MOTOR: 10 # CAN ID of the digger chain motor DIGGER_LEFT_LINEAR_ACTUATOR: 8 # CAN ID of the left linear actuator DIGGER_RIGHT_LINEAR_ACTUATOR: 7 # CAN ID of the right linear actuator @@ -16,7 +17,7 @@ DIGGER_ACTUATORS_kP_coupling: 0.004 # kP for digger actuator position syncing DIGGER_PITCH_kP: 2.5 # kP for tipping while digging TIPPING_SPEED_ADJUSTMENT: false # determines if pitch speed adjustment is used - DIGGER_SAFETY_ZONE: 120 # Measured in potentiometer units (0 to 1023) + EXTENSION_SAFETY_ZONE: 0 # Measured in potentiometer units (0 to 1023) CURRENT_SPIKE_THRESHOLD: 1.8 # Threshold for current spike detection (measured in Amps) CURRENT_SPIKE_TIME: 0.2 # Time in seconds to consider it a current spike (measured in seconds) BUCKETS_CURRENT_SPIKE_THRESHOLD: 12.0 # Threshold for current spike detection (measured in Amps) diff --git a/config/rovr_control.yaml b/config/rovr_control.yaml index cb6e909e..4cefe568 100644 --- a/config/rovr_control.yaml +++ b/config/rovr_control.yaml @@ -7,7 +7,7 @@ digger_lift_manual_power_up: 0.5 # Measured in Duty Cycle (0.0-1.0) autonomous_field_type: "cosmic" # The type of field ("cosmic", "top", "bottom", "nasa") tilt_digging_start_position: 125.0 # Measured in potentiometer units (0 to 1023) - fast_screw_speed: 4000 + fast_screw_speed: 20000 slow_screw_speed: 2000 # Auto Dig cost starts at max_dig_cost, and increases to absolute_max_dig_cost absolute_max_dig_cost: 200 # Measured as a Costmap grid value (0-255) # TODO: tune this diff --git a/docker/Dockerfile.deepstream b/docker/Dockerfile.deepstream index 44ac7782..8c10570c 100644 --- a/docker/Dockerfile.deepstream +++ b/docker/Dockerfile.deepstream @@ -5,7 +5,9 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o COPY deepstream/deepstream.deb deepstream.deb -RUN --mount=type=cache,target=/var/cache/apt apt-get update && apt-get install -y \ +RUN --mount=type=cache,target=/var/cache/apt \ + apt-get update --allow-releaseinfo-change && \ + apt-get install -y --no-install-recommends \ gstreamer1.0-tools \ gstreamer1.0-plugins-base \ gstreamer1.0-plugins-good \ diff --git a/docker/Dockerfile.umn b/docker/Dockerfile.umn index ff532cef..9543d6c5 100644 --- a/docker/Dockerfile.umn +++ b/docker/Dockerfile.umn @@ -43,11 +43,14 @@ RUN --mount=type=cache,target=/var/cache/apt apt-get update && apt-get install libudev-dev \ libusb-1.0-0-dev \ libhidapi-libusb0 \ + libxcb-cursor0 \ && rm -rf /var/lib/apt/lists/* RUN python3 -m pip install -U \ pyvista \ - streamdeck + streamdeck \ + PySide6 \ + av RUN echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="0fd9", MODE="0666"' | sudo tee /etc/udev/rules.d/99-streamdeck.rules diff --git a/docker/README.md b/docker/README.md index 6307fa40..41633b54 100644 --- a/docker/README.md +++ b/docker/README.md @@ -9,6 +9,9 @@ Find the highest level image of this build with docker image list. The highest l Tag this image as `umnrobotics/:_` At time of writing, we were tagging images as 'umnrobotics/isaac_ros3.1:x86_64-ros2_humble-realsense-deepstream-user-zed-umn-gazebo_d000f8df5f3859fd56c7459b2ad3a718' +to find the hash, run the build command again, and find the first line starting with "Checking if base image ... exists on remote repository. +The image ID listed here is the one you should use. It is generated based on the dockerfiles in mysterious ways, so just copy paste it from the terminal log. + If you are logged in to docker, running docker image push will push this image to the cloud, and you are done. If you changed the name of/created a new dockerhub repo, update BASE_DOCKER_REGISTRY_NAMES in /scripts/build_image.sh to reflect the new name. diff --git a/scripts/can_reset.sh b/scripts/can_reset.sh new file mode 100755 index 00000000..f7a3edab --- /dev/null +++ b/scripts/can_reset.sh @@ -0,0 +1,5 @@ +#!/bin/bash +sudo ip link set can0 down +sudo ip link set can0 type can restart-ms 100 +sudo ip link set can0 up +candump can0 \ No newline at end of file diff --git a/scripts/enter_isaac_ros_container.sh b/scripts/enter_isaac_ros_container.sh index 03185f19..ceaa73f2 100755 --- a/scripts/enter_isaac_ros_container.sh +++ b/scripts/enter_isaac_ros_container.sh @@ -1,7 +1,21 @@ #!/bin/bash printf 'CONFIG_DOCKER_SEARCH_DIRS="$HOME/Lunabotics/src/isaac_ros/isaac_ros_common/scripts/../../../../docker"\n BASE_DOCKER_REGISTRY_NAMES="umnrobotics/isaac_ros3.1"\n'> ~/.isaac_ros_common-config image_key="ros2_humble.deepstream.user.zed.umn.gazebo" -docker_arg="-v /usr/local/zed/resources:/usr/local/zed/resources -v $HOME/rosbags:/rosbags -v /usr/local/zed/settings:/usr/local/zed/settings" + +# Start with your existing volumes +docker_arg="-v /usr/local/zed/resources:/usr/local/zed/resources \ +-v $HOME/rosbags:/rosbags \ +-v /usr/local/zed/settings:/usr/local/zed/settings \ +-v /dev/v4l:/dev/v4l \ +-v /dev/video0:/dev/video0 \ +-v /dev/video1:/dev/video1 \ +-v /dev/video2:/dev/video2 \ +-v /dev/video3:/dev/video3 \ +-v /dev/video4:/dev/video4 \ +-v /dev/video5:/dev/video5 \ +-v /dev/video6:/dev/video6 \ +-v /dev/video7:/dev/video7 \ +-v /dev/video8:/dev/video8" USE_CACHED_IMAGE=${1:-true} diff --git a/scripts/joy_reset.sh b/scripts/joy_reset.sh new file mode 100755 index 00000000..326677e9 --- /dev/null +++ b/scripts/joy_reset.sh @@ -0,0 +1,5 @@ +#!/bin/bash +source /opt/ros/humble/setup.bash +ros2 daemon stop +ros2 daemon start +ros2 topic echo joy \ No newline at end of file diff --git a/src/auger/auger/auger_node.py b/src/auger/auger/auger_node.py index d91f98c2..59936260 100644 --- a/src/auger/auger/auger_node.py +++ b/src/auger/auger/auger_node.py @@ -18,8 +18,8 @@ SetScrewMotorSpeed, ) from rovr_interfaces.srv import SetExtension -from rovr_interfaces.msg import Potentiometers from std_srvs.srv import Trigger +from std_msgs.msg import Bool, Float32 class Auger(Node): @@ -30,6 +30,7 @@ def __init__(self): # Callback group for anything that changes motor speeds # There should not be a need for a separate one because all services # should eventually terminate timely + self.stop_service_cb_group = MutuallyExclusiveCallbackGroup() self.service_cb_group = MutuallyExclusiveCallbackGroup() # TODO Define service clients here @@ -37,30 +38,32 @@ def __init__(self): self.cli_motor_get = self.create_client(MotorCommandGet, "motor/get") # Define parameters here + self.declare_parameter("AUGER_STOWED", True) + self.declare_parameter("DUMPER_STOWED", True) + self.declare_parameter("extension_limit_switch", True) self.declare_parameter("POWER_LIMIT", 1) - self.declare_parameter("SCREW_SPEED", 4000) + self.declare_parameter("SCREW_SPEED", 20000) self.declare_parameter( "MAX_SCREW_SPEED", 4_000 ) # in RPM for both negative and positive direction - self.declare_parameter("MIN_SCREW_DIG_SPEED", 2_000) + self.declare_parameter("MIN_SCREW_DIG_SPEED", 2000) self.declare_parameter("MAX_SPIN_MOTOR_CURRENT", 0) self.declare_parameter("push_motor_position", 0) - self.declare_parameter("tilt_actuator_position", 0) # The error range to consider the current 0 - self.declare_parameter("TILT_ACTUATOR_CURRENT_THRESHOLD", 0) + self.declare_parameter("TILT_ACTUATOR_CURRENT_THRESHOLD", 0.3) # TODO: Find real value for this - self.declare_parameter("MAX_PUSH_MOTOR_POSITION", 0) - self.declare_parameter("MIN_PUSH_MOTOR_POSITION", 0) - self.declare_parameter("DEFAULT_PUSH_MOTOR_SPEED", 0) + self.declare_parameter("MAX_PUSH_MOTOR_POSITION", 10000) + self.declare_parameter("MIN_PUSH_MOTOR_POSITION", 1000) + self.declare_parameter("DEFAULT_PUSH_MOTOR_SPEED", 3000) self.declare_parameter("MAX_PUSH_MOTOR_CURRENT", 0) self.declare_parameter("PUSH_MOTOR_POS_TOLERANCE", 0) # Could potentially be faster self.declare_parameter("MAX_RETRACT_PUSH_MOTOR_VELOCITY", -600) self.declare_parameter("MAX_EXTEND_PUSH_MOTOR_VELOCITY", 600) # Since we only care if the tilt actuator is fully extened or fully - # retracted then we should only need to care about the speed it moves + #m retracted then we should only need to care about the speed it moves # make sure to verify direction of this velocity - self.declare_parameter("TILT_ACTUATOR_SPEED", 0) + self.declare_parameter("TILT_ACTUATOR_SPEED", 1.0) # Minimum amount the tilt actuator needs to be extened to safely extend # push motor self.declare_parameter("TILT_ACTUATOR_MIN_EXTENSION", 0) @@ -68,21 +71,27 @@ def __init__(self): # tilt actuator self.declare_parameter("PUSH_MOTOR_MIN_RETRACTION", 0) # TODO Get real can ids - self.declare_parameter("TILT_ACTUATOR_ID", 0) + self.declare_parameter("TILT_ACTUATOR_ID", 1) self.declare_parameter("PUSH_MOTOR_ID", 0) self.declare_parameter("SPIN_MOTOR_ID", 0) + self.declare_parameter("FAST_SCREW_SPEED", 20000) + self.declare_parameter("SLOW_SCREW_SPEED", 0) + self.declare_parameter("LINEAR_ACTUATOR_CURRENT", 0.0) # Local variables here - self.POWER_LIMIT = self.get_parameter("POWER_LIMIT").value - self.SCREW_SPEED = self.get_parameter("SCREW_SPEED").value - self.MAX_SCREW_SPEED = self.get_parameter("MAX_SCREW_SPEED").value self.MIN_SCREW_DIG_SPEED = self.get_parameter("MIN_SCREW_DIG_SPEED").value + self.POWER_LIMIT = self.get_parameter("POWER_LIMIT").value + self.FAST_SCREW_SPEED = self.get_parameter("FAST_SCREW_SPEED").value + self.SLOW_SCREW_SPEED = self.get_parameter("SLOW_SCREW_SPEED").value self.MAX_SPIN_MOTOR_CURRENT = self.get_parameter("MAX_SPIN_MOTOR_CURRENT").value self.push_motor_position = self.get_parameter("push_motor_position").value - self.tilt_actuator_position = self.get_parameter("tilt_actuator_position").value + self.extension_limit_switch = self.get_parameter("extension_limit_switch").value + self.auger_stowed = self.get_parameter("AUGER_STOWED").value + self.dumper_stowed = self.get_parameter("DUMPER_STOWED").value + self.linear_actuator_current = self.get_parameter("LINEAR_ACTUATOR_CURRENT").value self.TILT_ACTUATOR_CURRENT_THRESHOLD = self.get_parameter( "TILT_ACTUATOR_CURRENT_THRESHOLD" - ) + ).value self.MAX_PUSH_MOTOR_POSITION = self.get_parameter( "MAX_PUSH_MOTOR_POSITION" ).value @@ -113,6 +122,15 @@ def __init__(self): self.PUSH_MOTOR_ID = self.get_parameter("PUSH_MOTOR_ID").value self.SPIN_MOTOR_ID = self.get_parameter("SPIN_MOTOR_ID").value + self.linear_actuator_tilted: bool = False + self.cancel_linear_actuator: bool = False + self.linear_actuator_running: bool = False + + self.cancel_motor_push: bool = False + self.motor_push_running: bool = False + + + # TODO Define services (methods callable from the outside) here self.srv_set_tilt_extension = self.create_service( SetExtension, @@ -125,21 +143,21 @@ def __init__(self): Trigger, "auger/tilt_actuator/stop", self.stop_tilt_callback, - callback_group=self.service_cb_group, + callback_group=self.stop_service_cb_group, ) - self.srv_set_push_position = self.create_service( - AugerSetPushMotor, - "auger/push_motor/setPosition", - self.set_push_position_callback, - callback_group=self.service_cb_group, - ) + # self.srv_set_push_position = self.create_service( + # AugerSetPushMotor, + # "auger/push_motor/setPosition", + # self.set_push_position_callback, + # callback_group=self.service_cb_group, + # ) self.srv_stop_push = self.create_service( Trigger, "auger/push_motor/stop", self.stop_push_callback, - callback_group=self.service_cb_group, + callback_group=self.stop_service_cb_group, ) self.srv_extend_push = self.create_service( @@ -167,7 +185,7 @@ def __init__(self): Trigger, "auger/screw/stop", self.stop_spin_callback, - callback_group=self.service_cb_group, + callback_group=self.stop_service_cb_group, ) self.srv_extend_digger = self.create_service( @@ -184,20 +202,30 @@ def __init__(self): callback_group=self.service_cb_group, ) - self.srv_retract_digger = self.create_service( + self.srv_stop_all = self.create_service( Trigger, "auger/control/stop_all", - self.stop_all, - callback_group=self.service_cb_group, + self.stop_all_callback, + callback_group=self.stop_service_cb_group, ) - # TODO Define subscribers here - need to subscribe to potentiometer - # readings? - self.potentiometer_sub = self.create_subscription( - Potentiometers, "potentiometer", self.push_motor_position_callback, 10 + + self.limit_switch_sub = self.create_subscription( + Bool, "ExtensionLimitSwitch", self.limit_switch_callback, 10 + ) + + self.dumper_stowed_sub = self.create_subscription( + Bool, "dumper_stowed", self.dumper_stowed_callback, 10 + ) + + self.tilt_linear_actuator_current = self.create_subscription( + Float32, "TiltActuatorCurrent", self.tilt_actuator_current_callback, 10 ) # TODO Define publishers here + self.auger_stowed_pub = self.create_publisher(Bool, "auger_stowed", 10) + + self.extension_pos_pub = self.create_publisher(Float32, "extension_pos", 10) # Define subsystem methods here @@ -208,21 +236,25 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: This will return false and do nothing if the push motor is currently extended. Caller is responsible for timeouts. """ - push_motor_pos_future = self.cli_motor_get.call_async( - MotorCommandGet.Request(type="position", can_id=self.PUSH_MOTOR_ID) - ) - rclpy.spin_until_future_complete(self, push_motor_pos_future) - push_motor_pos = push_motor_pos_future.result() - if not push_motor_pos.success: - self.get_logger().info( - "WARNING: Failed to move the tilt actuator because the push motor position could not be determined" - ) - return False - if push_motor_pos.data > self.PUSH_MOTOR_MIN_RETRACTION: - self.get_logger().info( - "WARNING: Failed to move the tilt actuator because the push motor is extended too far" - ) - return False + self.linear_actuator_running = True + lastPowerTime = time.time() + # push_motor_pos_future = self.cli_motor_get.call_async( + # MotorCommandGet.Request(type="position", can_id=self.PUSH_MOTOR_ID) + # ) + # rclpy.spin_until_future_complete(self, push_motor_pos_future) + # push_motor_pos = push_motor_pos_future.result() + # if not push_motor_pos.success: + # self.get_logger().info( + # "WARNING: Failed to move the tilt actuator because the push motor position could not be determined" + # ) + # self.linear_actuator_running = False + # return False + # if push_motor_pos.data > self.PUSH_MOTOR_MIN_RETRACTION: + # self.get_logger().info( + # "WARNING: Failed to move the tilt actuator because the push motor is extended too far" + # ) + # self.linear_actuator_running = False + # return False if tilt: self.get_logger().info("Extending tilt actuator") @@ -231,43 +263,56 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: speed = self.TILT_ACTUATOR_SPEED * (1 if tilt else -1) - motor_set_future = self.cli_motor_set.call_async( + self.cli_motor_set.call_async( MotorCommandSet.Request( - type="velocity", can_id=self.TILT_ACTUATOR_ID, value=speed + type="duty_cycle", can_id=self.TILT_ACTUATOR_ID, value=float(speed) ) ) - rclpy.spin_until_future_complete(self, motor_set_future) - if not motor_set_future.result().success: - self.get_logger().info("WARNING: Failed to set tilt motor velocity") - return False + self.get_logger().info("set the duty cycle") + time.sleep(1.5) + if tilt: + self.auger_stowed = False + msg = Bool() + msg.data = self.auger_stowed + self.auger_stowed_pub.publish(msg) + + while True:#time.time() - lastPowerTime < 0.5: + if self.cancel_linear_actuator: + self.cancel_linear_actuator = False + break + #TODO figure out current publisher + # if not self.linear_actuator_current < self.current_threshold: + # lastPowerTime = time.time() + time.sleep(0.1) + + - # gets motor current until it is 0 which means it has hit an limit - # switch - while True: - motor_get_future = self.cli_motor_get.call_async( - MotorCommandGet.Request( - type="current", - can_id=self.TILT_ACTUATOR_ID, - ) + + self.cli_motor_set.call_async( + MotorCommandSet.Request( + type="duty_cycle", can_id=self.TILT_ACTUATOR_ID, value=0.0 ) - rclpy.spin_until_future_complete(self, motor_get_future) - - if motor_get_future.result().success: - if ( - abs(motor_get_future.result().data) - < self.TILT_ACTUATOR_CURRENT_THRESHOLD - ): - break - else: - self.get_logger().info("WARNING: Failed to read tilt actuator position") - - time.sleep(0.1) + ) + self.linear_actuator_running = False + if not tilt: + self.auger_stowed = True + self.get_logger().info("Set auger_stowed") + msg = Bool() + msg.data = self.auger_stowed + self.auger_stowed_pub.publish(msg) + + self.linear_actuator_tilted = tilt + self.get_logger().info(f"linear actuator tilt set to {self.linear_actuator_tilted}") return True def stop_actuator_tilt(self) -> bool: """Stop the auger angular position of the auger motor.""" - self.get_logger().info("Stopping tilt actuator") + self.get_logger().info("In the stop acutator tilt function") + if self.linear_actuator_running: + self.cancel_linear_actuator = True + return True + motor_set_future = self.cli_motor_set.call_async( MotorCommandSet.Request( type="duty_cycle", @@ -275,146 +320,188 @@ def stop_actuator_tilt(self) -> bool: value=0.0, ) ) - rclpy.spin_until_future_complete(self, motor_set_future) - return motor_set_future.result().success + return True - def set_motor_push_position( - self, speed: float, desired_position: float, power_limit: float - ) -> bool: + def set_motor_push_extend(self) -> bool: """ Set the target position of the motor that pushes the auger into the ground. This will spin until the motor reaches given setpoint. This will fail if the screw is not spinning fast enough Caller is responsible for timeouts. """ - if self.tilt_actuator_position < self.TILT_ACTUATOR_MIN_EXTENSION: - self.get_logger().warn( - "WARNING: Push motor will not move because the tilt actuator is not extended" - ) - return False - - get_screw_speed_future = self.cli_motor_get.call_async( - MotorCommandGet.Request( + self.motor_push_running = True + power_limit = 0.5 + # if not self.linear_actuator_tilted: + # self.get_logger().warn( + # "WARNING: Push motor will not move because the tilt actuator is not extended" + # ) + # self.motor_push_running = False + # return False + + # get_screw_speed_future = self.cli_motor_get.call_async( + # MotorCommandGet.Request( + # type="velocity", + # can_id=self.SPIN_MOTOR_ID, + # ) + # ) + # rclpy.spin_until_future_complete(self, get_screw_speed_future) + + # if not get_screw_speed_future.result().success: + # self.get_logger().warn( + # "WARNING: Push motor will not move because the screw speed could not be determined" + # ) + # self.motor_push_running = False + # return False + + # elif get_screw_speed_future.result().data < self.MIN_SCREW_DIG_SPEED: + # self.get_logger().warn( + # "WARNING: Push motor will not move because the screw is not spinning fast enough" + # ) + # self.motor_push_running = False + # return False + + # if ( + # self.MAX_PUSH_MOTOR_POSITION > self.MAX_PUSH_MOTOR_POSITION + # or self.MAX_PUSH_MOTOR_POSITION < self.MIN_PUSH_MOTOR_POSITION + # ): + # self.get_logger().warn( + # f"WARNING: Requested push motor position is out of range, clamping value; requested: {self.MAX_PUSH_MOTOR_POSITION}" + # ) + # self.MAX_PUSH_MOTOR_POSITION = max( + # self.MIN_PUSH_MOTOR_POSITION, + # min(self.MAX_PUSH_MOTOR_POSITION, self.MAX_PUSH_MOTOR_POSITION), + # ) # clamp the value to be within range + # self.get_logger().info( + # "Setting auger push motor position to: " + str(self.MAX_PUSH_MOTOR_POSITION) + # ) + + self.get_logger().info("Setting the plunge motor velocity") + self.cli_motor_set.call_async( + MotorCommandSet.Request( type="velocity", - can_id=self.SPIN_MOTOR_ID, + can_id=self.PUSH_MOTOR_ID, + value=float(self.DEFAULT_PUSH_MOTOR_SPEED), ) ) - rclpy.spin_until_future_complete(self, get_screw_speed_future) - - if not get_screw_speed_future.result().success: - self.get_logger().warn( - "WARNING: Push motor will not move because the screw speed could not be determined" - ) - return False - elif get_screw_speed_future.result().data < self.MIN_SCREW_DIG_SPEED: - self.get_logger().warn( - "WARNING: Push motor will not move because the screw is not spinning fast enough" - ) - return False - - if ( - desired_position > self.MAX_PUSH_MOTOR_POSITION - or desired_position < self.MIN_PUSH_MOTOR_POSITION - ): - self.get_logger().warn( - f"WARNING: Requested push motor position is out of range, clamping value; requested: {desired_position}" - ) - desired_position = max( - self.MIN_PUSH_MOTOR_POSITION, - min(desired_position, self.MAX_PUSH_MOTOR_POSITION), - ) # clamp the value to be within range self.get_logger().info( - "Setting auger push motor position to: " + str(desired_position) + f"Set the plunge velocity to {self.DEFAULT_PUSH_MOTOR_SPEED}" ) - motor_set_future = self.cli_motor_set.call_async( + # wait till motor reaches desired position + while True: + if self.cancel_motor_push: + break + # self.get_logger().info("Getting the plunge motor position") + # motor_get_pos_future = self.cli_motor_get.call_async( + # MotorCommandGet.Request( + # type="position", + # can_id=self.PUSH_MOTOR_ID, + # ) + # ) + # rclpy.spin_until_future_complete(self, motor_get_pos_future) + + # if motor_get_pos_future.result().success: + # current_pos = motor_get_pos_future.result().data + # self.get_logger().info(f"Plunge pos: {current_pose}") + # msg = Float32() + # msg.data = current_pos + # self.extension_pos_pub.publish(msg) + # if ( + # (self.DEFAULT_PUSH_MOTOR_SPEED <= 0 and current_pos <= self.MIN_PUSH_MOTOR_POSITION) + # or (self.DEFAULT_PUSH_MOTOR_SPEED > 0 and current_pos >= self.MAX_PUSH_MOTOR_POSITION) + # ): + # break + # else: + # self.get_logger().warn("WARNING: Failed to read push motor position") + + time.sleep(0.1) + + self.get_logger().info("out of the loop") + stop_motor_future = self.cli_motor_set.call_async( MotorCommandSet.Request( type="velocity", power_limit=power_limit, can_id=self.PUSH_MOTOR_ID, - value=float(speed), + value=0.0, ) ) - rclpy.spin_until_future_complete(self, motor_set_future) - - if not motor_set_future.result().success: - self.get_logger().warn("WARNING: Failed to set push motor voltage") - return False + self.motor_push_running = False + self.cancel_motor_push = False + return True - # wait till motor reaches desired position - while True: - motor_get_pos_future = self.cli_motor_get.call_async( - MotorCommandGet.Request( - type="position", + def set_motor_push_retract(self) -> bool: + """ + Set the target position of the motor that pushes the auger into the ground. + This will spin until the motor reaches given setpoint. + This will fail if the screw is not spinning fast enough + Caller is responsible for timeouts. + """ + self.motor_push_running = True + power_limit = 0.5 + if not self.extension_limit_switch: + motor_set_future = self.cli_motor_set.call_async( + MotorCommandSet.Request( + type="velocity", + power_limit=power_limit, can_id=self.PUSH_MOTOR_ID, + value=float(-self.DEFAULT_PUSH_MOTOR_SPEED), ) ) - rclpy.spin_until_future_complete(self, motor_get_pos_future) - - if motor_get_pos_future.result().success: - current_pos = motor_get_pos_future.result().data - if (speed <= 0 and current_pos <= desired_position) or ( - speed > 0 and current_pos >= desired_position - ): - break - else: - self.get_logger().warn("WARNING: Failed to read push motor position") + + while not self.extension_limit_switch: + if self.cancel_motor_push: + break + + + # current_pos = motor_get_pos_future.result().data + # msg = Float32() + # msg.data = current_pos + # self.extension_pos_pub.publish(msg) time.sleep(0.1) - stop_motor_future = self.cli_motor_set.call_async( + self.cli_motor_set.call_async( MotorCommandSet.Request( type="velocity", power_limit=power_limit, can_id=self.PUSH_MOTOR_ID, - value=0, + value=0.0, ) ) - stop_motor_response = rclpy.spin_until_future_complete(self, stop_motor_future) - if not stop_motor_response.result().success: - self.get_logger().warn("WARNING: Failed to stop the auger screw") - return False - + self.motor_push_running = False + self.cancel_motor_push = False return True - def extend_motor_push(self) -> bool: - """ - Extends the push motor at max extend speed, returns false - if it is not able to due to the tilt actuator not being fully extended. - """ - return self.set_motor_push_position( - self.DEFAULT_PUSH_MOTOR_SPEED, self.MAX_PUSH_MOTOR_POSITION, 0.5 - ) - - def retract_motor_push(self) -> bool: - """ - Run the push motor at max retract speed. - Returns false if it is not able to due to the tilt actuator not being fully extended. - """ - return self.set_motor_push_position( - -self.DEFAULT_PUSH_MOTOR_SPEED, self.MIN_PUSH_MOTOR_POSITION, 0.5 - ) - def stop_motor_push(self) -> bool: """Stop the motor that pushes the auger into the ground.""" - motor_set_future = self.cli_motor_set.call_async( + if self.motor_push_running: + self.cancel_motor_push = True + self.get_logger().info(f"cancel_motor_push is {self.cancel_motor_push}") + return True + self.cli_motor_set.call_async( MotorCommandSet.Request( type="duty_cycle", can_id=self.PUSH_MOTOR_ID, value=0.0, ) ) - rclpy.spin_until_future_complete(self, motor_set_future) - return motor_set_future.result().success + self.get_logger().info("stopping plunge") + return True def run_auger_spin_velocity(self, desired_speed: float, power_limit: float) -> bool: """Set the auger spin velocity of the auger motor.""" - if abs(desired_speed) > self.MAX_SCREW_SPEED: + if desired_speed < 0: self.get_logger().info( - f"WARNING: Requested auger screw speed is too fast: {desired_speed}" + f"WARNING: Requested auger screw speed backwards, instead setting to 0" ) - # This is the same as signum(desired_speed) * self.MAX_SCREW_SPEED - desired_speed = math.copysign(self.MAX_SCREW_SPEED, desired_speed) + + desired_speed = 0 + elif desired_speed > self.FAST_SCREW_SPEED: + self.get_logger().info( + f"WARNING: Requested auger screw speed is too fast, clamping it" + ) + desired_speed = self.FAST_SCREW_SPEED + self.get_logger().info(f"Running auger spin at velocity: {desired_speed}") @@ -423,39 +510,42 @@ def run_auger_spin_velocity(self, desired_speed: float, power_limit: float) -> b type="velocity", can_id=self.SPIN_MOTOR_ID, value=float(desired_speed), - power_limit=power_limit, + power_limit=float(power_limit), ) ) - rclpy.spin_until_future_complete(self, motor_set_future) - return motor_set_future.result().success + return True def stop_auger_spin(self) -> bool: """Stop the auger motor from spinning.""" self.get_logger().info("Stopping auger spin") - motor_set_future = self.cli_motor_set.call_async( + self.cli_motor_set.call_async( MotorCommandSet.Request( type="duty_cycle", can_id=self.SPIN_MOTOR_ID, value=0.0, ) ) - rclpy.spin_until_future_complete(self, motor_set_future) - return motor_set_future.result().success + + return True def extend_digger(self) -> bool: """Tilt and extend""" + if not self.dumper_stowed: + return False tilt_success = self.set_actuator_tilt_extension(True) - if not tilt_success: + + if not tilt_success or self.cancel_linear_actuator: + self.cancel_linear_actuator = False return False - spin_success = self.run_auger_spin_velocity(self.SCREW_SPEED, self.POWER_LIMIT) + spin_success = self.run_auger_spin_velocity(self.FAST_SCREW_SPEED, self.POWER_LIMIT) if not spin_success: return False - extend_success = self.extend_motor_push() + extend_success = self.set_motor_push_extend() if not extend_success: return False @@ -464,7 +554,7 @@ def extend_digger(self) -> bool: def retract_digger(self) -> bool: """Tilt and retract""" - retract_success = self.retract_motor_push() + retract_success = self.set_motor_push_retract() if not retract_success: return False @@ -480,6 +570,7 @@ def retract_digger(self) -> bool: def stop_all(self) -> bool: # This does not short circit but still returns false if any do + self.get_logger().info("in the stop all function") return ( self.stop_actuator_tilt() & self.stop_auger_spin() & self.stop_motor_push() ) @@ -496,15 +587,15 @@ def stop_tilt_callback(self, request, response): response.success = True return response - def set_push_position_callback(self, request, response): - """ - This service request sets position of the motor that pushes the auger into the ground. - It will fail if the tilt actuator is not fully extended - """ - response.success = self.set_motor_push_position( - request.speed, request.position, request.power_limit - ) - return response + # def set_push_position_callback(self, request, response): + # """ + # This service request sets position of the motor that pushes the auger into the ground. + # It will fail if the tilt actuator is not fully extended + # """ + # response.success = self.set_motor_push_position( + # request.speed, request.position, request.power_limit + # ) + # return response def stop_push_callback(self, request, response): """This service request stops the motor that pushes the auger into the ground.""" @@ -523,15 +614,21 @@ def stop_spin_callback(self, request, response): response.success = self.stop_auger_spin() return response - def push_motor_position_callback(self, msg): - self.tilt_actuator_position = msg.data + def limit_switch_callback(self, msg): + self.extension_limit_switch = msg.data + + def dumper_stowed_callback(self, msg): + self.dumper_stowed = msg.data + + def tilt_actuator_current_callback(self, msg): + self.linear_actuator_current = msg.data def extend_push_callback(self, request, response): """ This service requests to extend the push motor at full speed. It will fail if the tilt actuator is not fully extended """ - response.success = self.extend_motor_push() + response.success = self.set_motor_push_extend() return response def retract_push_callback(self, request, response): @@ -539,7 +636,7 @@ def retract_push_callback(self, request, response): This service requests to retract the push motor at full speed. It will fail if the tilt actuator is not fully extended """ - response.success = self.retract_motor_push() + response.success = self.set_motor_push_retract() return response def extend_digger_callback(self, request, response): @@ -554,6 +651,7 @@ def retract_digger_callback(self, request, response): def stop_all_callback(self, request, response): """This Service will stop all three motors""" + self.get_logger().info("In the stop all callback") response.success = self.stop_all() return response @@ -564,19 +662,16 @@ def main(args=None): node = Auger() executor = MultiThreadedExecutor() - executor.add_node(node) - + executor.add_node(node + ) node.get_logger().info("Initializing the Auger subsystem!") - try: - executor.spin() - # TODO this exception needed? Could this ever happen? - except KeyboardInterrupt: - node.get_logger().info("Shutting down the Auger subsystem.") - finally: - node.destroy_node() + executor.spin() - rclpy.shutdown() + node.get_logger().info(" subsystem!") + + node.destroy_node() + rclpy.shutdown() # This code does NOT run if this file is imported as a module if __name__ == "__main__": diff --git a/src/camera_interface/camera_interface/__init__.py b/src/camera_interface/camera_interface/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/camera_interface/camera_interface/compression.py b/src/camera_interface/camera_interface/compression.py new file mode 100644 index 00000000..45fe3957 --- /dev/null +++ b/src/camera_interface/camera_interface/compression.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from rcl_interfaces.msg import ParameterDescriptor, ParameterType +from sensor_msgs.msg import CompressedImage +import cv2 +import subprocess +import threading +import time + +class VideoCompressionNode(Node): + def __init__(self): + super().__init__('video_compression_node') + + self.initalize_parameters() + self.topic_name = self.get_parameter('topic_name').get_parameter_value().string_value + self.device_id = self.get_parameter('device_id').get_parameter_value().string_value + + # Determine camera source + if self.device_id.isdigit(): + self.camera_source = int(self.device_id) + else: + self.camera_source = self.device_id + + self.fps = 30 + self.cap = None + self.process = None + self.is_running = True + + # Codec Settings + self.codec = 'hevc_nvenc' + self.stream_fmt = 'hevc' + + self.publisher_ = self.create_publisher(CompressedImage, self.topic_name, 10) + + # Start the background thread for FFmpeg output + self.output_thread = threading.Thread(target=self.read_encoded_stream, daemon=True) + self.output_thread.start() + + # Timer for capturing frames + self.input_timer = self.create_timer(1.0 / self.fps, self.feed_encoder) + + self.get_logger().info(f"Node started for device: {self.camera_source}") + + def open_camera(self): + """Attempts to open the camera and configure it.""" + if self.cap is not None: + self.cap.release() + + self.cap = cv2.VideoCapture(self.camera_source, cv2.CAP_V4L2) + if not self.cap.isOpened(): + return False + + self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G')) + self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) + self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) + self.cap.set(cv2.CAP_PROP_FPS, self.fps) + + w = self.cap.get(cv2.CAP_PROP_FRAME_WIDTH) or 640 + h = self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT) or 480 + self.width = int(w) if int(w) % 2 == 0 else int(w) - 1 + self.height = int(h) if int(h) % 2 == 0 else int(h) - 1 + + return True + + def setup_ffmpeg(self): + """Initializes the FFmpeg subprocess.""" + if self.process: + self.process.kill() + + ffmpeg_cmd = [ + 'ffmpeg', '-y', '-hide_banner', '-loglevel', 'error', + '-f', 'rawvideo', '-vcodec', 'rawvideo', + '-s', f'{self.width}x{self.height}', '-pix_fmt', 'bgr24', + '-r', str(self.fps), '-i', '-', + '-c:v', self.codec, '-preset', 'p1', '-tune', 'ull', + '-zerolatency', '1', '-g', '30', '-bf', '0', + '-f', self.stream_fmt, '-' + ] + + self.process = subprocess.Popen( + ffmpeg_cmd, stdin=subprocess.PIPE, + stdout=subprocess.PIPE, stderr=subprocess.PIPE, bufsize=0 + ) + + def feed_encoder(self): + """Capture frame and push to FFmpeg. Handles reconnection if camera is lost.""" + if self.cap is None or not self.cap.isOpened(): + self.get_logger().warn(f"Camera {self.camera_source} non-existent. Retrying...", throttle_duration_sec=2.0) + if self.open_camera(): + self.setup_ffmpeg() + self.get_logger().info(f"Camera {self.camera_source} reconnected!") + return + + ret, frame = self.cap.read() + + if not ret: + self.get_logger().error("Camera unplugged or read failed.") + self.cap.release() + return + + # Ensure correct dimensions for FFmpeg + if frame.shape[1] != self.width or frame.shape[0] != self.height: + frame = cv2.resize(frame, (self.width, self.height)) + + try: + if self.process and self.process.stdin: + self.process.stdin.write(frame.tobytes()) + self.process.stdin.flush() + except (BrokenPipeError, AttributeError): + self.get_logger().error("FFmpeg pipe broken. Resetting...") + self.setup_ffmpeg() + + def read_encoded_stream(self): + """Continuously read from FFmpeg stdout and publish.""" + chunk_size = 256 * 1024 + while rclpy.ok() and self.is_running: + if self.process and self.process.poll() is None: + try: + data = self.process.stdout.read(chunk_size) + if data: + msg = CompressedImage() + msg.header.stamp = self.get_clock().now().to_msg() + msg.format = self.stream_fmt + msg.data = data + self.publisher_.publish(msg) + else: + time.sleep(0.005) + except Exception: + time.sleep(0.1) + else: + time.sleep(0.1) + + def initalize_parameters(self): + self.declare_parameter('topic_name', value='', + descriptor=ParameterDescriptor(type=ParameterType.PARAMETER_STRING)) + self.declare_parameter('device_id', value='0', + descriptor=ParameterDescriptor(type=ParameterType.PARAMETER_STRING)) + + def destroy_node(self): + self.is_running = False + if self.process: + self.process.kill() + if self.cap: + self.cap.release() + super().destroy_node() + +def main(args=None): + rclpy.init(args=args) + node = VideoCompressionNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/camera_interface/camera_interface/qt_user_interface.py b/src/camera_interface/camera_interface/qt_user_interface.py new file mode 100644 index 00000000..887e0d96 --- /dev/null +++ b/src/camera_interface/camera_interface/qt_user_interface.py @@ -0,0 +1,393 @@ +import sys +import threading +import time +import math +import signal +import av # PyAV + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy +from sensor_msgs.msg import Image, CompressedImage +from cv_bridge import CvBridge + +from PySide6.QtWidgets import ( + QApplication, QMainWindow, QLabel, QPushButton, + QVBoxLayout, QHBoxLayout, QWidget, + QListWidget, QListWidgetItem, + QGridLayout, QSizePolicy # <--- Added QSizePolicy import +) +from PySide6.QtGui import QImage, QPixmap, QPainter, QColor, QFont +from PySide6.QtCore import Qt, Signal, QObject + +# -------------------- Dark Theme -------------------- + +dark_stylesheet = """ +QWidget { background-color: #121212; color: #e0e0e0; } +QPushButton { background-color: #1f1f1f; border: 1px solid #333; padding: 6px; } +QPushButton:hover { background-color: #333; } +QListWidget { background-color: #1c1c1c; border: 1px solid #333; } +""" + +# -------------------- Decoding Logic -------------------- + +class ImageSignal(QObject): + image_ready = Signal(object) + +class StreamDecoder: + """Stateful decoder using PyAV for AV1/HEVC/H264 streams.""" + def __init__(self, codec_name='av1'): + try: + self.codec = av.CodecContext.create(codec_name, 'r') + except Exception as e: + print(f"Error creating codec {codec_name}: {e}") + self.codec = None + + def decode(self, binary_data): + if not self.codec: + return None + + try: + packets = self.codec.parse(binary_data) + for packet in packets: + frames = self.codec.decode(packet) + for frame in frames: + return frame.to_ndarray(format='rgb24') + except Exception as e: + print(f"Decode error: {e}") + return None + +# -------------------- ROS Node -------------------- + +class UserInterfaceNode(Node): + def __init__(self): + super().__init__('multi_camera_qt_interface') + self.bridge = CvBridge() + self.subscriptions_map = {} + self.image_signals = {} + self.decoders = {} + self.last_frame_time = {} + self.max_fps = 60.0 + + def subscribe(self, topic, msg_type_str): + if topic in self.subscriptions_map: + return + + qos = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1 + ) + self.last_frame_time[topic] = 0.0 + + # Map string type to class type + if 'CompressedImage' in str(msg_type_str): + msg_type = CompressedImage + else: + msg_type = Image + + self.subscriptions_map[topic] = self.create_subscription( + msg_type, + topic, + lambda msg, t=topic: self.callback(msg, t), + qos + ) + + def unsubscribe(self, topic): + if topic in self.subscriptions_map: + self.destroy_subscription(self.subscriptions_map[topic]) + del self.subscriptions_map[topic] + del self.last_frame_time[topic] + if topic in self.decoders: + del self.decoders[topic] + + def callback(self, msg, topic): + if topic not in self.image_signals: + return + + now = time.time() + if now - self.last_frame_time.get(topic, 0.0) < 1.0 / self.max_fps: + return + self.last_frame_time[topic] = now + + cv_img = None + try: + if isinstance(msg, CompressedImage): + if topic not in self.decoders: + fmt = msg.format.lower() + codec = 'hevc' if 'hevc' in fmt else 'av1' if 'av1' in fmt else 'h264' + self.decoders[topic] = StreamDecoder(codec) + + cv_img = self.decoders[topic].decode(bytes(msg.data)) + + elif isinstance(msg, Image): + if "av1" in msg.encoding.lower(): + if topic not in self.decoders: + self.decoders[topic] = StreamDecoder('av1') + cv_img = self.decoders[topic].decode(bytes(msg.data)) + else: + cv_img = self.bridge.imgmsg_to_cv2(msg, 'rgb8') + + if cv_img is not None: + signal = self.image_signals.get(topic) + if signal: + signal.image_ready.emit(cv_img) + + except Exception as e: + self.get_logger().error(f"Processing error on {topic}: {e}") + +# -------------------- UI Components -------------------- + +class CameraWidget(QWidget): + def __init__(self, topic): + super().__init__() + self.topic = topic + self.last_frame = None + self.last_received_time = time.time() + self.start_time = time.time() # Track when we started waiting + self.is_disconnected = True + + self.label = QLabel("Initializing...") + self.label.setAlignment(Qt.AlignCenter) + self.label.setStyleSheet("background-color: #000; color: #555;") + self.label.setSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored) + + title = QLabel(topic) + title.setAlignment(Qt.AlignCenter) + title.setStyleSheet("font-size: 10px; color: #888; background: #1a1a1a; padding: 2px;") + + layout = QVBoxLayout(self) + layout.setContentsMargins(0, 0, 0, 0) + layout.setSpacing(0) + layout.addWidget(self.label, 1) + layout.addWidget(title) + + from PySide6.QtCore import QTimer + self.watchdog = QTimer(self) + self.watchdog.timeout.connect(self.check_connection) + self.watchdog.start(500) + + self.show_no_signal() + + def check_connection(self): + """Checks if the frame rate has stalled.""" + # Check if we are timed out (either from a stall or initial wait) + if time.time() - self.last_received_time > 2.0: + self.show_no_signal() + else: + self.is_disconnected = False + + def show_no_signal(self): + """Displays status overlay based on connection history and elapsed time.""" + self.is_disconnected = True + elapsed_since_start = time.time() - self.start_time + + base_pixmap = QPixmap(self.label.size()) + + # Decide which text and color to use + if self.last_frame is not None: + # SCENARIO 1: We had a frame, but it stopped. + h, w, ch = self.last_frame.shape + bytes_per_line = ch * w + qimg = QImage(self.last_frame.data, w, h, bytes_per_line, QImage.Format_RGB888) + base_pixmap = QPixmap.fromImage(qimg).scaled( + self.label.size(), Qt.KeepAspectRatio, Qt.SmoothTransformation + ) + overlay_color = QColor(0, 0, 0, 140) + text_str = "NO SIGNAL" + text_color = QColor(255, 50, 50) # Red + else: + # SCENARIO 2: We have never received a frame. + base_pixmap.fill(Qt.black) + overlay_color = QColor(0, 0, 0, 0) + + # If we've been waiting more than 10 seconds, call it 'No Signal' + if elapsed_since_start > 10.0: + text_str = "NO SIGNAL (TIMEOUT)" + text_color = QColor(255, 50, 50) # Switch to Red + else: + text_str = "WAITING FOR CAMERA..." + text_color = QColor(180, 180, 180) # Neutral Grey + + painter = QPainter(base_pixmap) + if overlay_color.alpha() > 0: + painter.fillRect(base_pixmap.rect(), overlay_color) + + painter.setPen(text_color) + painter.setFont(QFont("Arial", 14, QFont.Bold)) + painter.drawText(base_pixmap.rect(), Qt.AlignCenter, text_str) + painter.end() + + self.label.setPixmap(base_pixmap) + + def update_image(self, cv_img): + self.last_received_time = time.time() + self.is_disconnected = False + self.last_frame = cv_img.copy() + + h, w, ch = self.last_frame.shape + bytes_per_line = ch * w + qimg = QImage(self.last_frame.data, w, h, bytes_per_line, QImage.Format_RGB888) + + pix = QPixmap.fromImage(qimg) + self.label.setPixmap(pix.scaled( + self.label.size(), Qt.KeepAspectRatio, Qt.SmoothTransformation + )) + + def resizeEvent(self, event): + if self.is_disconnected: + self.show_no_signal() + elif self.last_frame is not None: + self.update_image(self.last_frame) + super().resizeEvent(event) + +# -------------------- Updated UI Window logic -------------------- + +class MultiCameraWindow(QMainWindow): + def __init__(self, ros_node): + super().__init__() + self.ros_node = ros_node + self.camera_widgets = {} + self.setWindowTitle("ROS2 Stream Viewer") + + main_widget = QWidget() + self.layout = QHBoxLayout(main_widget) + + side_panel = QWidget() + side_layout = QVBoxLayout(side_panel) + self.topic_list = QListWidget() + self.topic_list.itemChanged.connect(self.topic_toggled) + + btn_refresh = QPushButton("Refresh Topics") + btn_refresh.clicked.connect(self.refresh_topics) + + side_layout.addWidget(QLabel("Available Topics:")) + side_layout.addWidget(self.topic_list) + side_layout.addWidget(btn_refresh) + + self.grid_widget = QWidget() + self.grid_layout = QGridLayout(self.grid_widget) + self.grid_layout.setContentsMargins(0,0,0,0) + + self.layout.addWidget(side_panel, 1) + self.layout.addWidget(self.grid_widget, 4) + self.setCentralWidget(main_widget) + + self.refresh_topics() + + def refresh_topics(self): + self.topic_list.blockSignals(True) + checked = {self.topic_list.item(i).text() for i in range(self.topic_list.count()) + if self.topic_list.item(i).checkState() == Qt.Checked} + + self.topic_list.clear() + + try: + topic_names_and_types = self.ros_node.get_topic_names_and_types() + except Exception as e: + print(f"Error fetching topics: {e}") + topic_names_and_types = [] + + for name, types in topic_names_and_types: + # Check for relevant message types + if any(t in ['sensor_msgs/msg/Image', 'sensor_msgs/msg/CompressedImage'] for t in types): + item = QListWidgetItem(name) + item.setFlags(item.flags() | Qt.ItemIsUserCheckable) + item.setCheckState(Qt.Checked if name in checked else Qt.Unchecked) + # Store the type string for later use + item.setData(Qt.UserRole, types[0]) + self.topic_list.addItem(item) + + self.topic_list.blockSignals(False) + + def topic_toggled(self, item): + topic = item.text() + msg_type = item.data(Qt.UserRole) + + if item.checkState() == Qt.Checked: + self.add_camera(topic, msg_type) + else: + self.remove_camera(topic) + + def add_camera(self, topic, msg_type): + if topic in self.camera_widgets: return + + widget = CameraWidget(topic) + # Using a lambda to ensure the widget receives the frame + sig = ImageSignal() + sig.image_ready.connect(widget.update_image) + + self.ros_node.image_signals[topic] = sig + self.ros_node.subscribe(topic, msg_type) + self.camera_widgets[topic] = widget + self.rebuild_grid() + + def remove_camera(self, topic): + if topic in self.camera_widgets: + self.ros_node.unsubscribe(topic) + if topic in self.ros_node.image_signals: + del self.ros_node.image_signals[topic] + + widget = self.camera_widgets.pop(topic) + self.grid_layout.removeWidget(widget) + widget.deleteLater() + self.rebuild_grid() + + def rebuild_grid(self): + for i in reversed(range(self.grid_layout.count())): + item = self.grid_layout.itemAt(i) + if item.widget(): + item.widget().setParent(None) + + widgets = list(self.camera_widgets.values()) + if not widgets: return + + count = len(widgets) + cols = math.ceil(math.sqrt(count)) + + for i, widget in enumerate(widgets): + row, col = divmod(i, cols) + self.grid_layout.addWidget(widget, row, col) + +# -------------------- Execution -------------------- + +def main(): + rclpy.init() + + node = UserInterfaceNode() + # Ensure this thread is a 'daemon' so it dies when the main process dies + ros_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) + ros_thread.start() + + app = QApplication(sys.argv) + app.setStyleSheet(dark_stylesheet) + + # --- SIGINT Handling --- + # This allows Ctrl+C in terminal to close the Qt Window + signal.signal(signal.SIGINT, lambda *args: QApplication.quit()) + + # A tiny timer that runs every 500ms just to give the + # Python interpreter a chance to catch the signal + from PySide6.QtCore import QTimer + timer = QTimer() + timer.start(500) + timer.timeout.connect(lambda: None) + # ----------------------- + + gui = MultiCameraWindow(node) + gui.resize(1280, 720) + gui.show() + + try: + app.exec() + except KeyboardInterrupt: + pass + finally: + # Clean shutdown + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + print("\nUI and ROS Node shut down successfully.") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/camera_interface/camera_interface/webcam.py b/src/camera_interface/camera_interface/webcam.py new file mode 100644 index 00000000..7f4ec84c --- /dev/null +++ b/src/camera_interface/camera_interface/webcam.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CompressedImage +import cv2 +import subprocess +import threading +import time + +class GpuWebcamPublisher(Node): + def __init__(self): + super().__init__('webcam_gpu_node') + + # Configuration + self.device_id = 0 + self.fps = 30 + + # 1. Setup Camera with V4L2 Backend + # 'cv2.CAP_V4L2' prevents the GStreamer "Internal data stream error" + self.cap = cv2.VideoCapture(self.device_id, cv2.CAP_V4L2) + + # CRITICAL: Force MJPEG. + # Most USB cams cannot do 1280x720 @ 30fps in raw YUYV format (bandwidth limit). + self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G')) + + # Force Resolution + self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) + self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) + self.cap.set(cv2.CAP_PROP_FPS, self.fps) + + # Read actual parameters (in case camera rejected our request) + w = self.cap.get(cv2.CAP_PROP_FRAME_WIDTH) or 640 + h = self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT) or 480 + + # NVENC requires even dimensions + self.width = int(w) if int(w) % 2 == 0 else int(w) - 1 + self.height = int(h) if int(h) % 2 == 0 else int(h) - 1 + + # Codec Settings + self.codec = 'hevc_nvenc' # NVIDIA Hardware Encoder + self.stream_fmt = 'hevc' # Raw HEVC stream + + # 2. Setup FFmpeg + self.ffmpeg_cmd = [ + 'ffmpeg', '-y', + '-hide_banner', '-loglevel', 'error', + '-f', 'rawvideo', + '-vcodec', 'rawvideo', + '-s', f'{self.width}x{self.height}', + '-pix_fmt', 'bgr24', + '-r', str(self.fps), + '-i', '-', # Input from pipe + '-c:v', self.codec, + '-preset', 'p1', # Low latency preset + '-tune', 'ull', # Ultra Low Latency + '-zerolatency', '1', + '-g', '30', # Keyframe every 1s + '-bf', '0', # No B-frames + '-f', self.stream_fmt, + '-' # Output to pipe + ] + + try: + self.process = subprocess.Popen( + self.ffmpeg_cmd, + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + bufsize=0 # Unbuffered IO + ) + except FileNotFoundError: + self.get_logger().error("FFmpeg not found. Ensure ffmpeg is installed.") + return + + self.publisher_ = self.create_publisher(CompressedImage, '/webcam/gpu_stream', 10) + + # 3. Start Threads + self.input_timer = self.create_timer(1.0 / self.fps, self.feed_encoder) + + self.output_thread = threading.Thread(target=self.read_encoded_stream, daemon=True) + self.output_thread.start() + + self.get_logger().info(f"Streaming {self.width}x{self.height} via {self.codec} (MJPG Input)...") + + def feed_encoder(self): + """Capture frame and push to FFmpeg stdin""" + if not self.cap.isOpened(): + return + + ret, frame = self.cap.read() + if ret: + # Resize if the camera ignores our resolution request + if frame.shape[1] != self.width or frame.shape[0] != self.height: + frame = cv2.resize(frame, (self.width, self.height)) + + try: + self.process.stdin.write(frame.tobytes()) + self.process.stdin.flush() + except BrokenPipeError: + self.get_logger().error("FFmpeg stdin broken pipe") + rclpy.shutdown() + else: + self.get_logger().warn("Camera frame read failed.") + + def read_encoded_stream(self): + """Continuously read from FFmpeg stdout and publish""" + # Buffer size large enough for 1080p frames to prevent artifacting + chunk_size = 256 * 1024 + + while rclpy.ok() and self.process.poll() is None: + try: + # Read whatever is available + data = self.process.stdout.read(chunk_size) + + if data: + msg = CompressedImage() + msg.header.stamp = self.get_clock().now().to_msg() + msg.format = self.stream_fmt + msg.data = data + + self.publisher_.publish(msg) + else: + time.sleep(0.002) + except Exception as e: + self.get_logger().error(f"Read error: {e}") + break + + def destroy_node(self): + if self.process: + self.process.kill() + self.cap.release() + super().destroy_node() + +def main(args=None): + rclpy.init(args=args) + node = GpuWebcamPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/camera_interface/camera_interface/webcam_raw.py b/src/camera_interface/camera_interface/webcam_raw.py new file mode 100644 index 00000000..ec0b3a10 --- /dev/null +++ b/src/camera_interface/camera_interface/webcam_raw.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image + +import cv2 +from cv_bridge import CvBridge + +class WebcamPublisher(Node): + def __init__(self): + super().__init__('webcam_node_raw') + self.publisher_ = self.create_publisher(Image, '/webcam/image_raw', 10) + self.publisher1_ = self.create_publisher(Image, '/webcam/image_raw_1', 10) + self.publisher2_ = self.create_publisher(Image, '/webcam/image_raw_2', 10) + self.publisher3_ = self.create_publisher(Image, '/webcam/image_raw_3', 10) + self.timer = self.create_timer(0.01, self.timer_callback) + self.bridge = CvBridge() + self.cap = cv2.VideoCapture(0) + + def timer_callback(self): + msg = Image() + + if not self.cap.isOpened(): + self.get_logger().error('Error: Could not open webcam.') + self.destroy_node() + + success, frame = self.cap.read() + + # If frame is not read successfully, break the loop + if not success: + self.get_logger().warn("Error: Could not read frame.") + return + + msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8') + + self.get_logger().info('Publishing: laptop webcam frame') + + self.publisher_.publish(msg) + self.publisher1_.publish(self.bridge.cv2_to_imgmsg(cv2.flip(frame, 0), encoding='bgr8')) + self.publisher2_.publish(self.bridge.cv2_to_imgmsg(cv2.flip(frame, 1), encoding='bgr8')) + self.publisher3_.publish(self.bridge.cv2_to_imgmsg(cv2.flip(frame, -1), encoding='bgr8')) + +def main(args=None): + rclpy.init(args=args) + webcam_publisher = WebcamPublisher() + + rclpy.spin(webcam_publisher) + webcam_publisher.destroy_node() + + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/camera_interface/launch/camera_system_launch.py b/src/camera_interface/launch/camera_system_launch.py new file mode 100644 index 00000000..2b8eaa72 --- /dev/null +++ b/src/camera_interface/launch/camera_system_launch.py @@ -0,0 +1,32 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + # NOTE: Instead of an index, you can use the stable path found in /dev/v4l/by-id/ or /dev/v4l/by-path/ + # To get the list of ids, we do : ls -l /dev/v4l/by-id/ + # (e.g.) -> /dev/v4l/by-id/usb-Logitech_Webcam_C920_ABC123-video-index0 + camera_configs = [ + {"name": "right", "topic": "/webcam/right", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index0"}, + {"name": "left", "topic": "/webcam/left", "id": "/dev/v4l/by-id/usb-OmniVision_Technologies__Inc._2000-video-index0"}, # + {"name": "back", "topic": "/webcam/back", "id": "/dev/v4l/by-id/usb-OmniVision_Technologies__Inc._USB_Camera-B4.09.24.1-video-index0"}, + {"name": "digger", "topic": "/webcam/digger", "id": "/dev/v4l/by-id/usb-Technologies__Inc._ZED_2i_OV0001-video-index0"}, + {"name": "digger", "topic": "/webcam/digger_2", "id": "/dev/v4l/by-id/usb-Technologies__Inc._ZED_2i_OV0001-video-index1"}, + # {"name": "dumper", "topic": "/webcam/dumper", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index1"}, + # {"name": "front", "topic": "/webcam/front", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index1"}, + ] + + nodes = [ + Node( + package='camera_interface', + executable='compression', + name=f"compressor_{config['name']}", + parameters=[{ + 'topic_name': config['topic'], + 'device_id': config['id'] + }], + output='screen' + ) + for config in camera_configs + ] + + return LaunchDescription(nodes) \ No newline at end of file diff --git a/src/camera_interface/package.xml b/src/camera_interface/package.xml new file mode 100644 index 00000000..3fd290a8 --- /dev/null +++ b/src/camera_interface/package.xml @@ -0,0 +1,22 @@ + + + + camera_interface + 0.0.0 + TODO: Package description + adam + TODO: License declaration + + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + tkinter + + + ament_python + + diff --git a/src/camera_interface/resource/camera_interface b/src/camera_interface/resource/camera_interface new file mode 100644 index 00000000..e69de29b diff --git a/src/camera_interface/setup.cfg b/src/camera_interface/setup.cfg new file mode 100644 index 00000000..115fcfc4 --- /dev/null +++ b/src/camera_interface/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/camera_interface +[install] +install_scripts=$base/lib/camera_interface diff --git a/src/camera_interface/setup.py b/src/camera_interface/setup.py new file mode 100644 index 00000000..ae92bfe3 --- /dev/null +++ b/src/camera_interface/setup.py @@ -0,0 +1,35 @@ +from setuptools import find_packages, setup + +package_name = 'camera_interface' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', + ['launch/camera_system_launch.py']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='adam', + maintainer_email='hwvxyeej@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'qt_user_interface = camera_interface.qt_user_interface:main', + 'webcam = camera_interface.webcam:main', + 'webcam_raw = camera_interface.webcam_raw:main', + 'compression = camera_interface.compression:main' + ], + }, +) diff --git a/src/camera_interface/test/test_copyright.py b/src/camera_interface/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/camera_interface/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/camera_interface/test/test_flake8.py b/src/camera_interface/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/camera_interface/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/camera_interface/test/test_pep257.py b/src/camera_interface/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/camera_interface/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/drivetrain/drivetrain/drivetrain_node.py b/src/drivetrain/drivetrain/drivetrain_node.py index 5910eb0c..812fb4fb 100644 --- a/src/drivetrain/drivetrain/drivetrain_node.py +++ b/src/drivetrain/drivetrain/drivetrain_node.py @@ -30,7 +30,9 @@ def __init__(self): self.declare_parameter("GAZEBO_SIMULATION", False) self.declare_parameter("MAX_DRIVETRAIN_RPM", 10000) self.declare_parameter("DRIVETRAIN_TYPE", "velocity") - self.declare_parameter("DIGGER_SAFETY_ZONE", 120) # Measured in potentiometer units (0 to 1023) + self.declare_parameter( + "EXTENSION_SAFETY_ZONE", 0 + ) # Measured in potentiometer units (0 to 1023) # Assign the ROS Parameters to member variables below # self.FRONT_LEFT_DRIVE = self.get_parameter("FRONT_LEFT_DRIVE").value @@ -40,43 +42,76 @@ def __init__(self): self.GAZEBO_SIMULATION = self.get_parameter("GAZEBO_SIMULATION").value self.MAX_DRIVETRAIN_RPM = self.get_parameter("MAX_DRIVETRAIN_RPM").value self.DRIVETRAIN_TYPE = self.get_parameter("DRIVETRAIN_TYPE").value - self.DIGGER_SAFETY_ZONE = self.get_parameter("DIGGER_SAFETY_ZONE").value + self.EXTENSION_SAFETY_ZONE = self.get_parameter("EXTENSION_SAFETY_ZONE").value # Define publishers and subscribers here - self.cmd_vel_sub = self.create_subscription(Twist, "cmd_vel", self.cmd_vel_callback, 10) - self.lift_pose_subscription = self.create_subscription(Float32, "lift_pose", self.lift_pose_callback, 10) + self.cmd_vel_sub = self.create_subscription( + Twist, "cmd_vel", self.cmd_vel_callback, 10 + ) + self.extension_sub = self.create_subscription( + Float32, "extension_pos", self.extension_callback, 10 + ) if self.GAZEBO_SIMULATION: - self.gazebo_front_left_wheel_pub = self.create_publisher(Float64, "front_left_wheel/cmd_vel", 10) - self.gazebo_back_left_wheel_pub = self.create_publisher(Float64, "back_left_wheel/cmd_vel", 10) - self.gazebo_front_right_wheel_pub = self.create_publisher(Float64, "front_right_wheel/cmd_vel", 10) - self.gazebo_back_right_wheel_pub = self.create_publisher(Float64, "back_right_wheel/cmd_vel", 10) + self.gazebo_front_left_wheel_pub = self.create_publisher( + Float64, "front_left_wheel/cmd_vel", 10 + ) + self.gazebo_back_left_wheel_pub = self.create_publisher( + Float64, "back_left_wheel/cmd_vel", 10 + ) + self.gazebo_front_right_wheel_pub = self.create_publisher( + Float64, "front_right_wheel/cmd_vel", 10 + ) + self.gazebo_back_right_wheel_pub = self.create_publisher( + Float64, "back_right_wheel/cmd_vel", 10 + ) # Define service clients here self.cli_motor_set = self.create_client(MotorCommandSet, "motor/set") # Define services (methods callable from the outside) here - self.srv_stop = self.create_service(Trigger, "drivetrain/stop", self.stop_callback) - self.srv_drive = self.create_service(Drive, "drivetrain/drive", self.drive_callback) + self.srv_stop = self.create_service( + Trigger, "drivetrain/stop", self.stop_callback + ) + self.srv_drive = self.create_service( + Drive, "drivetrain/drive", self.drive_callback + ) # Print the ROS Parameters to the terminal below # - self.get_logger().info("FRONT_LEFT_DRIVE has been set to: " + str(self.FRONT_LEFT_DRIVE)) - self.get_logger().info("FRONT_RIGHT_DRIVE has been set to: " + str(self.FRONT_RIGHT_DRIVE)) - self.get_logger().info("BACK_LEFT_DRIVE has been set to: " + str(self.BACK_LEFT_DRIVE)) - self.get_logger().info("BACK_RIGHT_DRIVE has been set to: " + str(self.BACK_RIGHT_DRIVE)) - self.get_logger().info("GAZEBO_SIMULATION has been set to: " + str(self.GAZEBO_SIMULATION)) - self.get_logger().info("MAX_DRIVETRAIN_RPM has been set to: " + str(self.MAX_DRIVETRAIN_RPM)) - self.get_logger().info("DIGGER_SAFETY_ZONE has been set to: " + str(self.DIGGER_SAFETY_ZONE)) + self.get_logger().info( + "FRONT_LEFT_DRIVE has been set to: " + str(self.FRONT_LEFT_DRIVE) + ) + self.get_logger().info( + "FRONT_RIGHT_DRIVE has been set to: " + str(self.FRONT_RIGHT_DRIVE) + ) + self.get_logger().info( + "BACK_LEFT_DRIVE has been set to: " + str(self.BACK_LEFT_DRIVE) + ) + self.get_logger().info( + "BACK_RIGHT_DRIVE has been set to: " + str(self.BACK_RIGHT_DRIVE) + ) + self.get_logger().info( + "GAZEBO_SIMULATION has been set to: " + str(self.GAZEBO_SIMULATION) + ) + self.get_logger().info( + "MAX_DRIVETRAIN_RPM has been set to: " + str(self.MAX_DRIVETRAIN_RPM) + ) + self.get_logger().info( + "EXTENSION_SAFETY_ZONE has been set to: " + str(self.EXTENSION_SAFETY_ZONE) + ) # Current position of the lift motor in potentiometer units (0 to 1023) - self.current_lift_position = None # We don't know the current position yet + self.extension_pos = 0 # We don't know the current position yet # Define subsystem methods here def drive(self, forward_power: float, turning_power: float) -> None: """This method drives the robot with the desired forward and turning power.""" if ( - (self.current_lift_position is None or self.current_lift_position > self.DIGGER_SAFETY_ZONE) + ( + self.extension_pos is None + or self.extension_pos > self.EXTENSION_SAFETY_ZONE + ) and not self.GAZEBO_SIMULATION and (forward_power != 0 or turning_power != 0) ): @@ -104,25 +139,45 @@ def drive(self, forward_power: float, turning_power: float) -> None: # Send velocity (not duty cycle) motor commands to the motor_control_node self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.FRONT_LEFT_DRIVE, type=self.DRIVETRAIN_TYPE, value=leftPower) + MotorCommandSet.Request( + can_id=self.FRONT_LEFT_DRIVE, type=self.DRIVETRAIN_TYPE, value=leftPower + ) ) self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.BACK_LEFT_DRIVE, type=self.DRIVETRAIN_TYPE, value=leftPower) + MotorCommandSet.Request( + can_id=self.BACK_LEFT_DRIVE, type=self.DRIVETRAIN_TYPE, value=leftPower + ) ) self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.FRONT_RIGHT_DRIVE, type=self.DRIVETRAIN_TYPE, value=rightPower) + MotorCommandSet.Request( + can_id=self.FRONT_RIGHT_DRIVE, + type=self.DRIVETRAIN_TYPE, + value=rightPower, + ) ) self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.BACK_RIGHT_DRIVE, type=self.DRIVETRAIN_TYPE, value=rightPower) + MotorCommandSet.Request( + can_id=self.BACK_RIGHT_DRIVE, + type=self.DRIVETRAIN_TYPE, + value=rightPower, + ) ) # Publish the wheel speeds to the gazebo simulation if self.GAZEBO_SIMULATION: if self.DRIVETRAIN_TYPE == "velocity": - self.gazebo_front_left_wheel_pub.publish(Float64(data=leftPower / self.MAX_DRIVETRAIN_RPM)) - self.gazebo_back_left_wheel_pub.publish(Float64(data=leftPower / self.MAX_DRIVETRAIN_RPM)) - self.gazebo_front_right_wheel_pub.publish(Float64(data=rightPower / self.MAX_DRIVETRAIN_RPM)) - self.gazebo_back_right_wheel_pub.publish(Float64(data=rightPower / self.MAX_DRIVETRAIN_RPM)) + self.gazebo_front_left_wheel_pub.publish( + Float64(data=leftPower / self.MAX_DRIVETRAIN_RPM) + ) + self.gazebo_back_left_wheel_pub.publish( + Float64(data=leftPower / self.MAX_DRIVETRAIN_RPM) + ) + self.gazebo_front_right_wheel_pub.publish( + Float64(data=rightPower / self.MAX_DRIVETRAIN_RPM) + ) + self.gazebo_back_right_wheel_pub.publish( + Float64(data=rightPower / self.MAX_DRIVETRAIN_RPM) + ) else: self.gazebo_front_left_wheel_pub.publish(Float64(data=leftPower)) self.gazebo_back_left_wheel_pub.publish(Float64(data=leftPower)) @@ -133,16 +188,24 @@ def drive(self, forward_power: float, turning_power: float) -> None: def stop(self) -> None: """This method stops the drivetrain.""" self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.FRONT_LEFT_DRIVE, type="duty_cycle", value=0.0) + MotorCommandSet.Request( + can_id=self.FRONT_LEFT_DRIVE, type="duty_cycle", value=0.0 + ) ) self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.BACK_LEFT_DRIVE, type="duty_cycle", value=0.0) + MotorCommandSet.Request( + can_id=self.BACK_LEFT_DRIVE, type="duty_cycle", value=0.0 + ) ) self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.FRONT_RIGHT_DRIVE, type="duty_cycle", value=0.0) + MotorCommandSet.Request( + can_id=self.FRONT_RIGHT_DRIVE, type="duty_cycle", value=0.0 + ) ) self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.BACK_RIGHT_DRIVE, type="duty_cycle", value=0.0) + MotorCommandSet.Request( + can_id=self.BACK_RIGHT_DRIVE, type="duty_cycle", value=0.0 + ) ) # Define service callback methods here @@ -165,9 +228,9 @@ def cmd_vel_callback(self, msg: Twist) -> None: self.drive(msg.linear.x, msg.angular.z) # Define the subscriber callback for the lift pose topic - def lift_pose_callback(self, msg: Float32): + def extension_callback(self, msg: Float32): # Average the two potentiometer values - self.current_lift_position = msg.data + self.extension_pos = msg.data def main(args=None): diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index 48aa0470..ce6783de 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -34,10 +34,16 @@ def __init__(self): # Define services (methods callable from the outside) here self.srv_toggle = self.create_service( - Trigger, "dumper/toggle", self.toggle_callback, callback_group=self.service_cb_group + Trigger, + "dumper/toggle", + self.toggle_callback, + callback_group=self.service_cb_group, ) self.srv_stop = self.create_service( - Trigger, "dumper/stop", self.stop_callback, callback_group=self.stop_service_cb_group + Trigger, + "dumper/stop", + self.stop_callback, + callback_group=self.stop_service_cb_group, ) self.srv_setPower = self.create_service( SetPower, @@ -47,38 +53,48 @@ def __init__(self): ) self.srv_dumpDumper = self.create_service( - Trigger, "dumper/storeDumper", self.dump_callback, callback_group=self.service_cb_group + Trigger, "dumper/dumpDumper", self.dump_callback, callback_group=self.service_cb_group ) self.srv_storeDumper = self.create_service( - Trigger, "dumper/dumpDumper", self.store_callback, callback_group=self.service_cb_group + Trigger, "dumper/storeDumper", self.store_callback, callback_group=self.service_cb_group ) # Define default values for our ROS parameters below # - self.declare_parameter("DUMPER_MOTOR", 11) - self.declare_parameter("DUMPER_POWER", 0.3) + self.declare_parameter("DUMPER_MOTOR", 2) + self.declare_parameter("DUMPER_POWER", 0.5) + self.declare_parameter("DUMPER_VELOCITY", (1200*7)) + self.declare_parameter("DUMPER_POS", 1000) # Assign the ROS Parameters to member variables below # self.DUMPER_MOTOR = self.get_parameter("DUMPER_MOTOR").value self.DUMPER_POWER = self.get_parameter("DUMPER_POWER").value - + self.DUMPER_VEL = self.get_parameter("DUMPER_VELOCITY").value + self.DUMP_POS = self.get_parameter("DUMPER_POS").value # Print the ROS Parameters to the terminal below # - self.get_logger().info("DUMPER_MOTOR has been set to: " + str(self.DUMPER_MOTOR)) + self.get_logger().info( + "DUMPER_MOTOR has been set to: " + str(self.DUMPER_MOTOR) + ) # Current state of the dumper self.dumped_state = False # Dumper Current Threshold self.current_threshold = 0.3 - self.dumper_current = 0.0 - self.dumper_current_sub = self.create_subscription( - Float32, "Dumper_Current", self.dumper_current_callback, 10 - ) + self.KillSwitch_sub = self.create_subscription( Bool, "DumperLimitSwitch", self.killSwitch_callback, 10 ) + + self.auger_stowed_sub = self.create_subscription( + Bool, "auger_stowed", self.auger_stowed_callback, 10 + ) + + self.dumper_stowed_pub = self.create_publisher(Bool, "dumper_stowed", 10) + + self.dumper_stowed = True self.limitSwitchBottom = False - # self.auger_stowed = True + self.auger_stowed = True # Define subsystem methods here def set_power(self, dumper_power: float) -> None: @@ -89,11 +105,13 @@ def set_power(self, dumper_power: float) -> None: ) ) - def stop(self) -> None: + def stop(self) -> bool: """This method stops the dumper.""" self.cli_motor_set.call_async( - MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=0.0) + MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=float(0.0)) ) + self.get_logger().info("Waiting to stop the dumper...") + return True def toggle(self) -> None: """This method toggles the dumper.""" @@ -111,8 +129,11 @@ def set_power_callback(self, request, response): def stop_callback(self, request, response): """This service request stops the dumper.""" + self.get_logger().info("In the stop acutator tilt function") if self.long_service_running: self.cancel_current_srv = True + self.get_logger().info(f"inside if statement, cancel_current_srv: {self.cancel_current_srv}") + self.stop() response.success = True return response @@ -124,26 +145,35 @@ def toggle_callback(self, request, response): return response def dump_dumper(self) -> None: - # if not self.auger_stowed: - # self.get_logger().info("The auger is already extended") - # return + if not self.auger_stowed: + self.get_logger().info("The auger is already extended") + return self.get_logger().info("Extending the dumper") self.dumped_state = True self.long_service_running = True future = self.cli_motor_set.call_async( - MotorCommandSet.Request(type="position", can_id=self.DUMPER_MOTOR, value=90) + MotorCommandSet.Request(type="velocity", can_id=self.DUMPER_MOTOR, value=float(self.DUMPER_VEL)) ) - while not future.done(): # While loop makes the motor keep going till limit switch is hit + self.dumper_stowed = False + msg = Bool() + msg.data = self.dumper_stowed + self.dumper_stowed_pub.publish(msg) + + + while True: + self.get_logger().info(str(self.cancel_current_srv)) if self.cancel_current_srv: - self.cancel_current_srv = False + self.get_logger().info("cancel current srv is true") break time.sleep(0.1) - + + self.get_logger().info("Finished dump dumper service") self.stop() self.long_service_running = False + self.cancel_current_srv = False self.get_logger().info("Done dumping the dumper") def dump_callback(self, request, response): @@ -151,25 +181,36 @@ def dump_callback(self, request, response): response.success = True return response - def store_dumper(self) -> None: # get the variables - # if not self.auger_stowed: - # self.get_logger().info("The Auger is already extended") - # return + def store_dumper(self) -> bool: # get the variables + if not self.auger_stowed: + self.get_logger().info("The Auger is already extended") + return self.get_logger().info("Retracting the dumper") - self.dumped_state = False + self.long_service_running = True - self.cli_motor_set.call_async( - MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=self.DUMPER_POWER) - ) + if not self.limitSwitchBottom: + store_dumper = self.cli_motor_set.call_async( + MotorCommandSet.Request(type="velocity", can_id=self.DUMPER_MOTOR, value=float(-self.DUMPER_VEL)) + ) + while not self.limitSwitchBottom: + self.get_logger().info(f"Dumper limit switch is {self.limitSwitchBottom}") if self.cancel_current_srv: - self.cancel_current_srv = False break time.sleep(0.1) self.stop() + self.dumper_stowed = True + msg = Bool() + msg.data = self.dumper_stowed + self.dumper_stowed_pub.publish(msg) self.long_service_running = False + self.cancel_current_srv = False self.get_logger().info("Done storing the dumper") + self.dumped_state = False + + return True + # the storage bin can only be dumped back # when the auger is completely stowed (both actuators fully stored) @@ -178,12 +219,13 @@ def store_callback(self, request, response): response.success = True return response - def dumper_current_callback(self, msg): - self.dumper_current = msg.data def killSwitch_callback(self, msg): # position control... - self.LimitSwitchBottom = msg.data + self.limitSwitchBottom = msg.data + + def auger_stowed_callback(self, msg): + self.auger_stowed = msg.data def main(args=None): diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index 13321d18..efcfb48e 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -246,7 +246,12 @@ class MotorControlNode : public rclcpp::Node { // Get the motor controller's current duty cycle command std::optional vesc_get_duty_cycle(uint32_t id) { if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { + try{ return this->can_data[id].dutyCycle; + } + catch(...) { + return std::nullopt; + } } else { return std::nullopt; // The data is too stale } @@ -254,7 +259,12 @@ class MotorControlNode : public rclcpp::Node { // Get the current velocity of the motor in RPM (Rotations Per Minute) std::optional vesc_get_velocity(uint32_t id) { if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { + try { return this->can_data[id].velocity; + } + catch(...) { + return std::nullopt; + } } else { return std::nullopt; // The data is too stale } @@ -262,7 +272,15 @@ class MotorControlNode : public rclcpp::Node { // Get the current position (tachometer reading) of the motor std::optional vesc_get_position(uint32_t id) { if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { - return (static_cast(this->can_data[id].tachometer) / static_cast(this->pid_controllers[id]->getCountsPerRevolution())) * 360.0; + try{ + float n = static_cast(this->can_data[id].tachometer); + + float d = 14.0 * 360.0; + return (n/ d); + } + catch(...) { + return std::nullopt; + } } else { return std::nullopt; // The data is too stale } @@ -270,7 +288,12 @@ class MotorControlNode : public rclcpp::Node { // Get the current draw of the motor in amps std::optional vesc_get_current(uint32_t id) { if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { + try{ return this->can_data[id].current; + } + catch(...) { + return std::nullopt; + } } else { return std::nullopt; // The data is too stale } @@ -410,7 +433,6 @@ class MotorControlNode : public rclcpp::Node { void get_callback(const std::shared_ptr request, std::shared_ptr response) { std::optional data = std::nullopt; - if (strcmp(request->type.c_str(), "velocity") == 0) { data = vesc_get_velocity(request->can_id); } else if (strcmp(request->type.c_str(), "duty_cycle") == 0) { diff --git a/src/rovr_control/package.xml b/src/rovr_control/package.xml index cdb94d41..7adee727 100644 --- a/src/rovr_control/package.xml +++ b/src/rovr_control/package.xml @@ -26,7 +26,7 @@ ros2launch motor_control drivetrain - digger + auger dumper joy rovr_interfaces diff --git a/src/rovr_control/rovr_control/auto_dig_server.py b/src/rovr_control/rovr_control/auto_dig_server.py index cd41d8ee..221941c3 100644 --- a/src/rovr_control/rovr_control/auto_dig_server.py +++ b/src/rovr_control/rovr_control/auto_dig_server.py @@ -35,9 +35,9 @@ def __init__(self): ) # /actuator_tilt/stop # extend - self.set_extension = self.create_client( - AugerSetPushMotor, "auger/push_motor/setPosition" - ) + # self.set_extension = self.create_client( + # AugerSetPushMotor, "auger/push_motor/setPosition" + # ) self.stop_extension = self.create_client(Trigger, "auger/push_motor/stop") self.retract_extender = self.create_client(Trigger, "auger/push_motor/retract") @@ -85,10 +85,10 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): self.get_logger().error("Tilt stop service not available") goal_handle.abort() return result - if not self.set_extension.wait_for_service(timeout_sec=1.0): - self.get_logger().error("Extension set power service not available") - goal_handle.abort() - return result + # if not self.set_extension.wait_for_service(timeout_sec=1.0): + # self.get_logger().error("Extension set power service not available") + # goal_handle.abort() + # return result if not self.retract_extender.wait_for_service(timeout_sec=1.0): self.get_logger().error("Extension retract service not available") goal_handle.abort() diff --git a/src/rovr_control/rovr_control/main_control_node.py b/src/rovr_control/rovr_control/main_control_node.py index 4d3b988a..2f8cad0f 100644 --- a/src/rovr_control/rovr_control/main_control_node.py +++ b/src/rovr_control/rovr_control/main_control_node.py @@ -35,7 +35,7 @@ from rovr_interfaces.msg import StreamDeckState # Import custom ROS 2 interfaces -from rovr_interfaces.srv import SetPower, SetScrewMotorSpeed +from rovr_interfaces.srv import SetPower, SetScrewMotorSpeed, SetExtension # Uncomment the line below to use the Xbox controller mappings instead # from rovr_control import xbox_controller_constants as bindings @@ -93,8 +93,10 @@ def __init__(self) -> None: "tilt_digging_start_position", 125.0 ) # Measured in encoder counts self.declare_parameter( - "fast_screw_speed", 4000 + "fast_screw_speed", 20000 ) + self.screwing = False + # Measured in potentiometer units (0 to 1023) self.declare_parameter("DIGGER_SAFETY_ZONE", 120) # The power the dumper needs to go @@ -159,6 +161,8 @@ def __init__(self) -> None: self.cli_dumper_toggle = self.create_client(Trigger, "dumper/toggle") self.cli_dumper_setPower = self.create_client(SetPower, "dumper/setPower") self.cli_dumper_stop = self.create_client(Trigger, "dumper/stop") + self.cli_dumper_dump = self.create_client(Trigger, "dumper/dumpDumper") + self.cli_dumper_store = self.create_client(Trigger, "dumper/storeDumper") # self.cli_digger_toggle = self.create_client(SetPower, "digger/toggle") self.cli_auger_stop = self.create_client(Trigger, "auger/control/stop_all") self.cli_auger_extend = self.create_client( @@ -173,6 +177,21 @@ def __init__(self) -> None: self.cli_screw_start = self.create_client( SetScrewMotorSpeed, "auger/screw/run" ) + self.cli_plunge_extend = self.create_client( + Trigger, "auger/push_motor/extend" + ) + self.cli_plunge_retract = self.create_client( + Trigger, "auger/push_motor/retract" + ) + self.cli_plunge_stop = self.create_client( + Trigger, "auger/push_motor/stop" + ) + self.cli_linear_actuator_tilt = self.create_client( + SetExtension, "auger/tilt_actuator/setExtension" + ) + self.cli_linear_actuator_stop = self.create_client( + Trigger, "auger/tilt_actuator/stop" + ) self.cli_big_agitator_on_off = self.create_client( SetBool, "big_agitator_on_off" ) @@ -204,9 +223,6 @@ def __init__(self) -> None: 10, callback_group=ReentrantCallbackGroup(), ) - self.lift_pose_subscription = self.create_subscription( - Float32, "lift_pose", self.lift_pose_callback, 10 - ) self.act_calibrate_field_coordinates = ActionClient( self, CalibrateFieldCoordinates, "calibrate_field_coordinates" @@ -226,9 +242,6 @@ def __init__(self) -> None: None, None, None ) - # Current position of the lift motor in potentiometer units (0 to 1023) - self.current_lift_position = None # We don't know the current position yet - # Add watchdog parameters self.declare_parameter("watchdog_timeout", 0.5) # Timeout in seconds self.watchdog_timeout = self.get_parameter("watchdog_timeout").value @@ -388,54 +401,51 @@ async def joystick_callback(self, msg: Joy) -> None: # Check if the digger button is pressed # if msg.buttons[bindings.X_BUTTON] == 1 and buttons[bindings.X_BUTTON] == 0: - self.cli_screw_start.call_async( - SetScrewMotorSpeed.Request(speed=self.screw_speed) - ) + if self.screwing: + self.cli_screw_stop.call_async(Trigger.Request()) + self.screwing = False + elif not self.screwing: + self.cli_screw_start.call_async( + SetScrewMotorSpeed.Request(speed=float(self.screw_speed)) + ) + self.screwing=True - if msg.buttons[bindings.X_BUTTON] == 0 and buttons[bindings.X_BUTTON] == 1: - self.cli_screw_stop.call_async(Trigger.Request()) + # if msg.buttons[bindings.X_BUTTON] == 0 and buttons[bindings.X_BUTTON] == 1: + # self.cli_screw_stop.call_async(Trigger.Request()) # Check if the dumper button is pressed # if msg.buttons[bindings.B_BUTTON] == 1 and buttons[bindings.B_BUTTON] == 0: - self.cli_dumper_stop.call_async( - Trigger.Request() - ) # Stop whatever the dumper is doing - # Toggle the dumper (extended or retracted) - self.cli_dumper_toggle.call_async(Trigger.Request()) - + self.cli_plunge_extend.call_async(Trigger.Request()) + elif msg.buttons[bindings.B_BUTTON] == 0 and buttons[bindings.B_BUTTON] == 1: + self.cli_plunge_stop.call_async(Trigger.Request()) # Check if the agitator button is pressed # - if msg.buttons[bindings.Y_BUTTON] == 1 and buttons[bindings.Y_BUTTON] == 0: - self.cli_big_agitator_toggle.call_async( - Trigger.Request() - ) # Toggle the agitator motor - # self.cli_small_agitator_toggle.call_async(Trigger.Request()) - # # Toggle the agitator motor - + elif msg.buttons[bindings.Y_BUTTON] == 1 and buttons[bindings.Y_BUTTON] == 0: + self.cli_plunge_retract.call_async(Trigger.Request()) + elif msg.buttons[bindings.Y_BUTTON] == 1 and buttons[bindings.Y_BUTTON] == 0: + self.cli_plunge_stop.call_async(Trigger.Request()) # Manually adjust the dumper position with the left and right # bumpers if ( msg.buttons[bindings.RIGHT_BUMPER] == 1 and buttons[bindings.RIGHT_BUMPER] == 0 ): - self.cli_dumper_setPower.call_async( - SetPower.Request(power=self.dumper_power) - ) + self.cli_dumper_dump.call_async(Trigger.Request()) elif ( msg.buttons[bindings.RIGHT_BUMPER] == 0 and buttons[bindings.RIGHT_BUMPER] == 1 ): + self.get_logger().info("Right bumper releases") self.cli_dumper_stop.call_async(Trigger.Request()) elif ( msg.buttons[bindings.LEFT_BUMPER] == 1 and buttons[bindings.LEFT_BUMPER] == 0 ): - self.cli_dumper_setPower.call_async( - SetPower.Request(power=-self.dumper_power) - ) + self.cli_dumper_store.call_async(Trigger.Request()) elif ( msg.buttons[bindings.LEFT_BUMPER] == 0 and buttons[bindings.LEFT_BUMPER] == 1 ): + self.get_logger().info("Left bumper released") self.cli_dumper_stop.call_async(Trigger.Request()) # Manually adjust the height of the digger with the left and right @@ -444,22 +454,24 @@ async def joystick_callback(self, msg: Joy) -> None: msg.buttons[bindings.LEFT_TRIGGER] == 1 and buttons[bindings.LEFT_TRIGGER] == 0 ): - self.cli_auger_extend.call_async(Trigger.Request()) + self.cli_linear_actuator_tilt.call_async(SetExtension.Request(extension=False)) elif ( msg.buttons[bindings.LEFT_TRIGGER] == 0 and buttons[bindings.LEFT_TRIGGER] == 1 ): - self.cli_auger_stop.call_async(Trigger.Request()) + self.get_logger().info("Left Trigger released") + self.cli_linear_actuator_stop.call_async(Trigger.Request()) elif ( msg.buttons[bindings.RIGHT_TRIGGER] == 1 and buttons[bindings.RIGHT_TRIGGER] == 0 ): - self.cli_auger_retract.call_async(Trigger.Request()) + self.cli_linear_actuator_tilt.call_async(SetExtension.Request(extension=True)) elif ( msg.buttons[bindings.RIGHT_TRIGGER] == 0 and buttons[bindings.RIGHT_TRIGGER] == 1 ): - self.cli_auger_stop.call_async(Trigger.Request()) + self.get_logger().info("Right trigger released") + self.cli_linear_actuator_stop.call_async(Trigger.Request()) # THE CONTROLS BELOW ALWAYS WORK # @@ -557,10 +569,6 @@ async def stream_deck_callback(self, msg: StreamDeckState) -> None: # self.get_logger().warn("Joystick messages received! Functionality of the robot has been restored.") # self.connection_active = True - # Define the subscriber callback for the lift pose topic - def lift_pose_callback(self, msg: Float32): - # Average the two potentiometer values - self.current_lift_position = msg.data def main(args=None) -> None: diff --git a/src/rovr_control/rovr_control/read_serial.py b/src/rovr_control/rovr_control/read_serial.py index 1d5e83fe..517e0a88 100644 --- a/src/rovr_control/rovr_control/read_serial.py +++ b/src/rovr_control/rovr_control/read_serial.py @@ -12,8 +12,10 @@ class read_serial(Node): def __init__(self): super().__init__("read_serial") - self.potentiometerPub = self.create_publisher(Int16, "potentiometer", 10) - self.LimitSwitchPub = self.create_publisher(Bool, "DumperLimitSwitch", 10) + self.DumperLimitSwitchPub = self.create_publisher(Bool, "DumperLimitSwitch", 10) + self.ExtensionLimitSwitchPub = self.create_publisher( + Bool, "ExtensionLimitSwitch", 10 + ) # Services to control the relay-driven agitator motor self.srv_bigonoff = self.create_service( @@ -50,17 +52,19 @@ def timer_callback(self): self.get_logger().fatal("Killing read_serial node") self.destroy_node() return - data = self.arduino.read(4) # Pause until 4 bytes are read + data = self.arduino.read(2) # Pause until 4 bytes are read # Use h for integers and ? for booleans - decoded = struct.unpack("h?", data) + decoded = struct.unpack("??", data) + + dumper_bool_msg = Bool() + dumper_bool_msg.data = not decoded[0] + self.DumperLimitSwitchPub.publish(dumper_bool_msg) - potentiometer_msg = Int16() - potentiometer_msg.data = decoded[0] - self.potentiometerPub.publish(potentiometer_msg) + extension_bool_msg = Bool() + extension_bool_msg.data = not decoded[1] + self.ExtensionLimitSwitchPub.publish(extension_bool_msg) - bool_msg = Bool() - bool_msg.data = decoded[1] - self.LimitSwitchPub.publish(bool_msg) + # self.get_logger().info(f"dumper bool: {not decoded[0]}, extension bool: {not decoded[1]}") def big_on_off_callback(self, request, response): # request.data == True → ON, False → OFF diff --git a/src/rovr_control/rovr_control/xbox_controller_constants.py b/src/rovr_control/rovr_control/xbox_controller_constants.py index 30ed9517..08577bf8 100644 --- a/src/rovr_control/rovr_control/xbox_controller_constants.py +++ b/src/rovr_control/rovr_control/xbox_controller_constants.py @@ -1,8 +1,8 @@ # Define all the buttons here X_BUTTON = 2 # toggle screw A_BUTTON = 0 # auto_dig_nav_offload -B_BUTTON = 1 # toggle dumper -Y_BUTTON = 3 # toggle big agitator +B_BUTTON = 1 # plunge extend +Y_BUTTON = 3 # plunge retract LEFT_BUMPER = 4 # dumper retract RIGHT_BUMPER = 5 # dumper extend BACK_BUTTON = 6 # auto_offload @@ -16,7 +16,7 @@ LEFT_JOYSTICK_VERTICAL_AXIS = 1 # FREE RIGHT_JOYSTICK_HORIZONTAL_AXIS = 3 # FREE RIGHT_JOYSTICK_VERTICAL_AXIS = 4 # drive -LEFT_TRIGGER_AXIS = 2 # extend auger -RIGHT_TRIGGER_AXIS = 5 # retract auger -DPAD_HORIZONTAL_AXIS = 6 # FREE +LEFT_TRIGGER_AXIS = 2 # retract linear actuator +RIGHT_TRIGGER_AXIS = 5 # extend linear actuator +DPAD_HORIZONTAL_AXIS = 6 # DUMP DPAD_VERTICAL_AXIS = 7 # FREE