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

src/digger/digger/digger_node.py

@@ -1,14 +1,12 @@
 # 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
 import rclpy
 from rclpy.node import Node
 
-# Import ROS 2 formatted message types
-from std_msgs.msg import Bool
 
 # Import custom ROS 2 interfaces
 from rovr_interfaces.srv import MotorCommandSet, MotorCommandGet
@@ -33,16 +31,13 @@ def __init__(self):
         self.srv_lift_stop = self.create_service(Stop, "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(Stop, "lift/zero", self.zero_lift_callback)
+        self.srv_lower_lift = self.create_service(Stop, "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)
@@ -59,14 +54,8 @@ 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
 
@@ -101,17 +90,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),
+                value=float(position + self.lift_encoder_offset),
             )
-        )
+        ).result()
 
     def stop_lift(self) -> None:
         """This method stops the lift."""
@@ -139,6 +126,9 @@ 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)
 
+    def lower_lift(self) -> None:
+        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."""
@@ -158,12 +148,10 @@ def toggle_callback(self, request, response):
         response.success = 0  # indicates success
         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)
+        # ^ this should already wait due to vesc set position not returning until done
         response.success = 0  # indicates success
         return response
 
@@ -182,23 +170,20 @@ def lift_set_power_callback(self, request, response):
     def zero_lift_callback(self, request, response):
         """This service request zeros the lift system."""
         self.zero_lift()
+        while not self.top_limit_pressed:
+            pass
         response.success = 0  # indicates success
         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)
+    def lower_lift_callback(self, request, response):
+        """This service request reverse-zeros the lift system, putting it at the lowest point"""
+        self.lower_lift()
+        while not self.bottom_limit_pressed:
+            pass
+        response.success = 0  # indicates success
+        return response
+
+    # No more timer callback because setPos works
 
     # Define subscriber callback methods here
     def limit_switch_callback(self, limit_switches_msg):

src/dumper/dumper/dumper_node.py

@@ -95,8 +95,9 @@ def toggle_callback(self, request, response):
         return response
 
     def extend_dumper(self) -> None:
+        self.set_power(self.DUMPER_POWER)
         while not self.top_limit_pressed:
-            self.set_power(self.DUMPER_POWER)
+            pass
         self.stop()
 
     def extend_callback(self, request, response):
@@ -106,8 +107,9 @@ def extend_callback(self, request, response):
         return response
 
     def retract_dumper(self) -> None:
+        self.set_power(-self.DUMPER_POWER)
         while not self.bottom_limit_pressed:
-            self.set_power(-self.DUMPER_POWER)
+            pass
         self.stop()
 
     def retract_callback(self, request, response):

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 {
@@ -185,9 +189,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
   }
 
@@ -196,6 +204,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
   }
 
@@ -267,6 +279,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/rovr_control/auto_dig_server.py

@@ -1,31 +1,26 @@
 import rclpy
+from rclpy.node import Node
 from rclpy.action import ActionServer
 
-from std_msgs.msg import Bool
-
 from rovr_interfaces.action import AutoDig
 from rovr_interfaces.srv import Drive, Stop, SetPosition, SetPower
 from rclpy.action.server import ServerGoalHandle, CancelResponse
 
-from rovr_control.node_util import AsyncNode
+from asyncio import sleep
 
 
-class AutoDigServer(AsyncNode):
+class AutoDigServer(Node):
     def __init__(self):
         super().__init__("auto_dig_server")
         self._action_server = ActionServer(
             self, AutoDig, "auto_dig", self.execute_callback, cancel_callback=self.cancel_callback
         )
 
-        # TODO: This should not be needed anymore after ticket #257 is implemented!
-        self.digger_goal_subscription = self.create_subscription(
-            Bool, "/digger/goal_reached", self.digger_goal_callback, 10
-        )
-
         self.cli_drivetrain_drive = self.create_client(Drive, "drivetrain/drive")
         self.cli_drivetrain_stop = self.create_client(Stop, "drivetrain/stop")
 
         self.cli_lift_zero = self.create_client(Stop, "lift/zero")
+        self.cli_lift_lower = self.create_client(Stop, "lift/lower")
         self.cli_lift_setPosition = self.create_client(SetPosition, "lift/setPosition")
         self.cli_lift_set_power = self.create_client(SetPower, "lift/setPower")
         self.cli_lift_stop = self.create_client(Stop, "lift/stop")
@@ -72,43 +67,26 @@ async def execute_callback(self, goal_handle: ServerGoalHandle):
             return result
 
         # Zero the digger
-        self.cli_lift_zero.call_async(Stop.Request())
-        # Wait for the lift goal to be reached
-        await self.digger_sleep()
-
-        # Lower the digger onto the ground
-        self.cli_lift_setPosition.call_async(
-            SetPosition.Request(position=goal_handle.request.lift_digging_start_position)
-        )
-        # Wait for the lift goal to be reached
-        await self.digger_sleep()
+        await self.cli_lift_zero.call_async(Stop.Reqest())
 
         # Start the digger belt
-        self.cli_digger_setPower.call_async(SetPower.Request(power=goal_handle.request.digger_belt_power))
-
-        # Drive forward while digging
-        start_time = self.get_clock().now().nanoseconds
-        elapsed = self.get_clock().now().nanoseconds - start_time
-        # accelerate for 2 seconds
-        # TODO: completing ticket #298 can replace this while loop with a motor ramp up service
-        while elapsed < 2e9:
-            self.cli_lift_set_power.call_async(SetPower.Request(power=-0.05e-9 * (elapsed)))
-            self.cli_drivetrain_drive.call_async(Drive.Request(forward_power=0.25e-9 * (elapsed), turning_power=0.0))
-            self.get_logger().info("Accelerating lift and drive train")
-            elapsed = self.get_clock().now().nanoseconds - start_time
-            await self.async_sleep(0.1)  # Allows for task to be canceled
-
-        self.get_logger().info("Auto Driving")
-        await self.async_sleep(12)  # Allows for task to be canceled
-        self.get_logger().info("Done Driving")
-
-        # Stop driving and skimming
-        await self.cli_drivetrain_stop.call_async(Stop.Request())
+        await self.cli_digger_setPower.call_async(SetPower.Request(power=goal_handle.request.digger_belt_power))
+
+        # Lower the digger towards the ground slowly
+        await self.cli_lift_lower.call_async(Stop.Reqest())
+
+        self.get_logger().info("Start of Auto Digging in Place")
+        await sleep(3)  # Stay at lowest pos for 3 seconds while digging
+        self.get_logger().info("Done Digging in Place")
+
+        # Stop skimming
         await self.cli_digger_stop.call_async(Stop.Request())
 
-        self.cli_lift_setPosition.call_async(SetPosition.Request(position=goal_handle.request.lift_dumping_position))
+        await self.cli_lift_setPosition.call_async(
+            SetPosition.Request(position=goal_handle.request.lift_dumping_position)
+        )
+        # raise lift to dumping position
         # Wait for the lift goal to be reached
-        await self.digger_sleep()
 
         self.get_logger().info("Autonomous Digging Procedure Complete!")
         goal_handle.succeed()
@@ -117,11 +95,9 @@ async def execute_callback(self, goal_handle: ServerGoalHandle):
     def cancel_callback(self, cancel_request: ServerGoalHandle):
         """This method is called when the action is canceled."""
         self.get_logger().info("Goal is cancelling")
-        if not self.digger_goal_reached.done():
-            self.cli_drivetrain_stop.call_async(Stop.Request())
-        if super().cancel_callback(cancel_request) == CancelResponse.ACCEPT:
-            self.cli_drivetrain_stop.call_async(Stop.Request())
-            self.cli_digger_stop.call_async(Stop.Request())
+        self.cli_drivetrain_stop.call_async(Stop.Request())
+        self.cli_digger_stop.call_async(Stop.Request())
+        self.cli_lift_stop.call_async(Stop.Request())
         return CancelResponse.ACCEPT