(WIP) Implemented proper ROS service behavior and modified Auto Dig and Auto Offload logic

config/rovr_control.yaml

@@ -9,3 +9,5 @@
     lift_dumping_position: -1000 # Measured in encoder counts!
     lift_digging_start_position: -3050 # Measured in encoder counts!
     lift_digging_end_position: -3150 # Measured in encoder counts!
+    dig_time: 5
+    dump_time: 5

src/digger/digger/digger_node.py

@@ -1,21 +1,20 @@
 # This ROS 2 node contains code for the digger subsystem of the robot.
 # Original Author: Anthony Brogni <[email protected]> in Fall 2023
-# Maintainer: Anthony Brogni <brogn002@umn.edu>
-# Last Updated: November 2023
+# Maintainer: Charlie Parece <parec020@umn.edu>
+# Last Updated: October 2024
 
 # Import the ROS 2 Python module
+from warnings import deprecated
 import rclpy
 from rclpy.node import Node
-
-# Import ROS 2 formatted message types
-from std_msgs.msg import Bool
+from rclpy.executors import MultiThreadedExecutor
 
 # Import custom ROS 2 interfaces
 from rovr_interfaces.srv import MotorCommandSet, MotorCommandGet
 from rovr_interfaces.srv import SetPower, SetPosition
 from rovr_interfaces.msg import LimitSwitches
 from std_srvs.srv import Trigger
-
+from rclpy.task import Future
 
 class DiggerNode(Node):
     def __init__(self):
@@ -34,16 +33,13 @@ def __init__(self):
         self.srv_lift_stop = self.create_service(Trigger, "lift/stop", self.stop_lift_callback)
         self.srv_lift_set_power = self.create_service(SetPower, "lift/setPower", self.lift_set_power_callback)
         self.srv_zero_lift = self.create_service(Trigger, "lift/zero", self.zero_lift_callback)
+        self.srv_lower_lift = self.create_service(Trigger, "lift/lower", self.lower_lift_callback)
 
         # Define publishers here
-        self.publisher_goal_reached = self.create_publisher(Bool, "digger/goal_reached", 10)
 
         # Define subscribers here
         self.limit_switch_sub = self.create_subscription(LimitSwitches, "limitSwitches", self.limit_switch_callback, 10)
 
-        # Define timers here
-        self.timer = self.create_timer(0.1, self.timer_callback)
-
         # Define default values for our ROS parameters below #
         self.declare_parameter("DIGGER_BELT_MOTOR", 2)
         self.declare_parameter("DIGGER_LIFT_MOTOR", 1)
@@ -60,20 +56,17 @@ def __init__(self):
 
         # Current state of the digger belt
         self.running = False
-        # Current goal position (in degrees)
-        self.current_goal_position = 0
         # Current position of the lift motor in degrees
         self.current_position_degrees = 0  # Relative encoders always initialize to 0
-        # Goal Threshold
-        # if abs(self.current_goal_position - ACTUAL VALUE) <= self.goal_threshold,
-        # then we should publish True to /digger/goal_reached
-        self.goal_threshold = 320  # in degrees of the motor # TODO: Tune this threshold if needed
         # Current state of the lift system
         self.lift_running = False
 
         # Limit Switch States
         self.top_limit_pressed = False
         self.bottom_limit_pressed = False
+        self.top_limit_event = Future()
+        self.bottom_limit_event = Future()
+
 
         # Maximum value of the lift motor encoder (bottom of the lift system) IN DEGREES
         self.MAX_ENCODER_DEGREES = (
@@ -102,44 +95,15 @@ def toggle(self, digger_belt_power: float) -> None:
         else:
             self.set_power(digger_belt_power)
 
-    # TODO: This method can probably be deleted during the implementation of ticket #257
-    def set_position(self, position: int) -> None:
-        """This method sets the position (in degrees) of the digger."""
-        self.current_goal_position = position  # goal position should be in degrees
-        self.cli_motor_set.call_async(
-            MotorCommandSet.Request(
-                type="position",
-                can_id=self.DIGGER_LIFT_MOTOR,
-                value=float(self.current_goal_position + self.lift_encoder_offset),
-            )
-        )
-
     def stop_lift(self) -> None:
         """This method stops the lift."""
         self.lift_running = False
+        self.top_limit_event.set_result(False)
+        self.bottom_limit_event.set_result(False)
         self.cli_motor_set.call_async(
             MotorCommandSet.Request(type="duty_cycle", can_id=self.DIGGER_LIFT_MOTOR, value=0.0)
         )
 
-    def lift_set_power(self, power: float) -> None:
-        """This method sets power to the lift system."""
-        self.lift_running = True
-        if power > 0 and self.top_limit_pressed:
-            self.get_logger().warn("WARNING: Top limit switch pressed!")
-            self.stop_lift()  # Stop the lift system
-            return
-        if power < 0 and self.bottom_limit_pressed:
-            self.get_logger().warn("WARNING: Bottom limit switch pressed!")
-            self.stop_lift()  # Stop the lift system
-            return
-        self.cli_motor_set.call_async(
-            MotorCommandSet.Request(type="duty_cycle", can_id=self.DIGGER_LIFT_MOTOR, value=power)
-        )
-
-    def zero_lift(self) -> None:
-        """This method zeros the lift system by slowly raising it until the top limit switch is pressed."""
-        self.lift_set_power(0.05)
-
     # Define service callback methods here
     def set_power_callback(self, request, response):
         """This service request sets power to the digger belt."""
@@ -159,12 +123,17 @@ def toggle_callback(self, request, response):
         response.success = True
         return response
 
-    # TODO: This method needs to be modified during the implementation of ticket #257
-    # to return a proper future indicating when the goal position has been reached
-    # so that rclpy.spin_until_future_complete() can be used to wait for the goal.
-    def set_position_callback(self, request, response):
-        """This service request sets the position of the lift."""
-        self.set_position(request.position)
+    async def set_position_callback(self, request, response):
+        """This service request sets the position of the lift.
+        TODO: Make sure MotorCommandSet.Request(type="position", 
+        is cancellable otherwise this will fail"""
+        await self.cli_motor_set.call_async(
+            MotorCommandSet.Request(
+                type="position",
+                can_id=self.DIGGER_LIFT_MOTOR,
+                value=float(request.position + self.lift_encoder_offset),
+            )
+        )
         response.success = True
         return response
 
@@ -173,51 +142,39 @@ def stop_lift_callback(self, request, response):
         self.stop_lift()
         response.success = True
         return response
-
+
+    @deprecated
     def lift_set_power_callback(self, request, response):
         """This service request sets power to the digger belt."""
-        self.lift_set_power(request.power)
-        response.success = True
         return response
 
-    def zero_lift_callback(self, request, response):
+    async def zero_lift_callback(self, request, response):
         """This service request zeros the lift system."""
-        self.zero_lift()
-        response.success = True
+        self.lift_set_power(0.05)
+        self.top_limit_event = Future()
+        self.top_limit_event.add_done_callback(self.stop_lift)
+        await self.top_limit_event
         return response
 
-    # Define timer callback methods here
-    def timer_callback(self):
-        """Publishes whether or not the current goal position has been reached."""
-        # This service call will return a future object, that will eventually contain the position in degrees
-        future = self.cli_motor_get.call_async(MotorCommandGet.Request(type="position", can_id=self.DIGGER_LIFT_MOTOR))
-        future.add_done_callback(self.done_callback)
-
-    def done_callback(self, future):
-        self.current_position_degrees = future.result().data
-        goal_reached_msg = Bool(
-            data=abs(self.current_goal_position + self.lift_encoder_offset - self.current_position_degrees)
-            <= self.goal_threshold
-        )
-        self.publisher_goal_reached.publish(goal_reached_msg)
+    async def lower_lift_callback(self, request, response):
+        """This service request reverse-zeros the lift system, putting it at the lowest point"""
+        if self.bottom_limit_pressed:
+            return response
+        self.lift_set_power(-0.05)
+        self.bottom_limit_event = Future()
+        self.bottom_limit_event.add_done_callback(self.stop_lift)
+        await self.bottom_limit_event
+        return self.bottom_limit_event.result()
 
     # Define subscriber callback methods here
     def limit_switch_callback(self, limit_switches_msg):
         """This subscriber callback method is called whenever a message is received on the limitSwitches topic."""
-        if not self.top_limit_pressed and limit_switches_msg.top_limit_switch:
-            self.stop_lift()  # Stop the lift system
-        if not self.bottom_limit_pressed and limit_switches_msg.bottom_limit_switch:
-            self.stop_lift()  # Stop the lift system
-        self.top_limit_pressed = limit_switches_msg.top_limit_switch
-        self.bottom_limit_pressed = limit_switches_msg.bottom_limit_switch
-        if self.top_limit_pressed:  # If the top limit switch is pressed
-            self.lift_encoder_offset = self.current_position_degrees
-            self.get_logger().debug("Current position in degrees: " + str(self.current_position_degrees))
-            self.get_logger().debug("New lift encoder offset: " + str(self.lift_encoder_offset))
-        elif self.bottom_limit_pressed:  # If the bottom limit switch is pressed
-            self.lift_encoder_offset = self.current_position_degrees - self.MAX_ENCODER_DEGREES
-            self.get_logger().debug("Current position in degrees: " + str(self.current_position_degrees))
-            self.get_logger().debug("New lift encoder offset: " + str(self.lift_encoder_offset))
+        if self.top_limit_pressed and not self.top_limit_event.done():
+            self.top_limit_pressed = limit_switches_msg.top_limit_switch
+            self.top_limit_event.set_result(True)
+        if self.bottom_limit_pressed and not self.top_limit_event.done():
+            self.bottom_limit_pressed = limit_switches_msg.bottom_limit_switch
+            self.bottom_limit_event.set_result(True)
 
 
 def main(args=None):
@@ -226,11 +183,15 @@ def main(args=None):
 
     node = DiggerNode()
     node.get_logger().info("Initializing the Digger subsystem!")
-    rclpy.spin(node)
-
+    executor = MultiThreadedExecutor()
+    executor.add_node(node)
+    #rclpy.spin(node)
+    executor.spin()
+
+    executor.shutdown()
     node.destroy_node()
     rclpy.shutdown()
-
+    
 
 # This code does NOT run if this file is imported as a module
 if __name__ == "__main__":

src/dumper/dumper/dumper_node.py

@@ -12,7 +12,7 @@
 from rovr_interfaces.srv import MotorCommandSet, MotorCommandGet, SetPower
 from rovr_interfaces.msg import LimitSwitches
 from std_srvs.srv import Trigger
-
+from threading import Event, Thread
 
 class DumperNode(Node):
     def __init__(self):
@@ -35,16 +35,19 @@ def __init__(self):
         # Define default values for our ROS parameters below #
         self.declare_parameter("DUMPER_MOTOR", 11)
         self.declare_parameter("DUMPER_POWER", 0.5)
+        self.declare_parameter("dump_time", 5)
         # 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.dumpTime = self.get_parameter("dump_time").value
 
         # Print the ROS Parameters to the terminal below #
         self.get_logger().info("DUMPER_MOTOR has been set to: " + str(self.DUMPER_MOTOR))
 
         # Current state of the dumper
         self.running = False
-
+        self.top_limit_event = Event()
+        self.bottom_limit_event = Event()
         self.limit_switch_sub = self.create_subscription(LimitSwitches, "limitSwitches", self.limit_switch_callback, 10)
 
     # Define subsystem methods here
@@ -66,6 +69,8 @@ def set_power(self, dumper_power: float) -> None:
     def stop(self) -> None:
         """This method stops the dumper."""
         self.running = False
+        self.top_limit_event.set()
+        self.bottom_limit_event.set()
         self.cli_motor_set.call_async(MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=0.0))
 
     def toggle(self, dumper_power: float) -> None:
@@ -95,8 +100,12 @@ def toggle_callback(self, request, response):
         return response
 
     def extend_dumper(self) -> None:
-        while not self.top_limit_pressed:
-            self.set_power(self.DUMPER_POWER)
+        self.top_limit_event.clear()
+        self.set_power(self.DUMPER_POWER)
+        self.top_limit_event.wait()
+        #while not self.top_limit_pressed and self.running:
+            #pass
+        self.top_limit_event.clear()
         self.stop()
 
     def extend_callback(self, request, response):
@@ -106,21 +115,25 @@ def extend_callback(self, request, response):
         return response
 
     def retract_dumper(self) -> None:
-        while not self.bottom_limit_pressed:
-            self.set_power(-self.DUMPER_POWER)
+        self.bottom_limit_event.clear()
+        self.set_power(-self.DUMPER_POWER)
+        self.bottom_limit_event.wait()
+        #while not self.bottom_limit_pressed and self.running:
+            #pass
+        self.bottom_limit_event.clear()
         self.stop()
 
     def retract_callback(self, request, response):
         """This service request retracts the dumper"""
         self.retract_dumper()
         response.success = True
         return response
-
+    # NOTE: Dump is no longer used because time.sleep is not cancellable! see Auto_offload_server for dump implementation workaround 
     def dump(self) -> None:
         # extend the dumper
         self.extend_dumper()
         # wait for 5 seconds before retracting the dumper
-        time.sleep(5)
+        time.sleep(self.dumpTime)
         # retract the dumper
         self.retract_dumper()
 
@@ -136,7 +149,13 @@ def limit_switch_callback(self, msg: LimitSwitches):
         if not self.bottom_limit_pressed and msg.bottom_limit_switch:
             self.stop()  # Stop the lift system
         self.top_limit_pressed = msg.top_limit_switch
+        if(self.top_limit_pressed):
+            self.top_limit_event.set()
+            self.bottom_limit_event.clear()
         self.bottom_limit_pressed = msg.bottom_limit_switch
+        if(self.bottom_limit_pressed):
+            self.bottom_limit_event.set()
+            self.top_limit_event.clear()
 
 
 def main(args=None):

src/motor_control/src/motor_control_node.cpp

@@ -24,6 +24,7 @@
 #include <string>
 #include <tuple> // for tuples
 #include <vector>
+#include <cmath>
 
 using namespace std::chrono_literals;
 using std::placeholders::_1;
@@ -141,6 +142,9 @@ class PIDController {
   bool isContinuousInputEnabled() {
     return this->continuous;
   }
+  int32_t getTargTach() {
+    return this->targTach;
+  }
 };
 
 class MotorControlNode : public rclcpp::Node {
@@ -221,9 +225,13 @@ class MotorControlNode : public rclcpp::Node {
       this->pid_controllers[id]->isActive = false;
     }
     int32_t data = rpm;
-
+    
     send_can(id + 0x00000300, data); // ID must be modified to signify this is a RPM command
     this->current_msg[id] = std::make_tuple(id + 0x00000300, data); // update the hashmap
+    while(abs(rpm-this->can_data[id].velocity - rpm) > this->velocityThreshold)
+    {
+      continue;
+    }
     RCLCPP_DEBUG(this->get_logger(), "Setting the velocity of CAN ID: %u to %d RPM", id, rpm); // Print Statement
   }
 
@@ -232,6 +240,10 @@ class MotorControlNode : public rclcpp::Node {
     if (this->pid_controllers[id]) {
       this->pid_controllers[id]->setRotation(position);
     }
+    while(abs(this->can_data[id].tachometer - this->pid_controllers[id]->getTargTach()) > this->tachThreshold)
+    {
+      continue;
+    }
     RCLCPP_DEBUG(this->get_logger(), "Setting the position of CAN ID: %u to %d degrees", id, position); // Print Statement
   }
 
@@ -303,6 +315,9 @@ class MotorControlNode : public rclcpp::Node {
     }
   }
 
+  int velocityThreshold = 50; // TODO: Tune this value
+  int tachThreshold = 100; // TODO: Tune this value
+
   // Listen for CAN status frames sent by our VESC motor controllers
   void CAN_callback(const can_msgs::msg::Frame::SharedPtr can_msg) {
     uint32_t motorId = can_msg->id & 0xFF;

src/rovr_control/launch/main_launch.py

@@ -50,7 +50,7 @@ def generate_launch_description():
         package="dumper",
         executable="dumper_node",
         name="dumper_node",
-        parameters=["config/dumper_config.yaml", "config/motor_control.yaml"],
+        parameters=["config/dumper_config.yaml", "config/motor_control.yaml","config/rovr_control.yaml"],
         output="screen",
     )