Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Draft
wants to merge 62 commits into
base: main
Choose a base branch
from
Draft
Show file tree
Hide file tree
Changes from 57 commits
Commits
Show all changes
62 commits
Select commit Hold shift + click to select a range
5c98c98
Initial Commit of the Drivetrain Refactor
Isopod00 Sep 21, 2024
9382a9c
Merge branch 'main' into Refactor-swerve-into-tank
Isopod00 Sep 22, 2024
a976131
refactored read_serial.py to not use absolute encoders per task #275…
kznhq Sep 24, 2024
3ef8cc7
Removed instantiated PID controllers
andrewtomfohrde Sep 25, 2024
2f2f232
Merge branch 'Refactor-swerve-into-tank' of https://github.com/GOFIRS…
andrewtomfohrde Sep 25, 2024
9e64914
deleted all absolute encoders from the arduino code
umnrobotics Sep 25, 2024
846b2fe
Merge branch 'Refactor-swerve-into-tank' of https://github.com/GOFIRS…
umnrobotics Sep 25, 2024
0c1b0b8
refactored drivetrain_node to arcade drive from swerve
ashtondemmer Sep 25, 2024
d81054b
Merge branch 'Refactor-swerve-into-tank' of https://github.com/GOFIRS…
ashtondemmer Sep 25, 2024
d502c20
Merge branch 'main' into Refactor-swerve-into-tank
Isopod00 Sep 25, 2024
b4c1b05
Autoformat the drivetrain_node
Isopod00 Sep 25, 2024
d0ea543
Fixed number of bytes to read in read_serial
Isopod00 Sep 25, 2024
1cf4c28
Minor Whitespace Fixes
Isopod00 Sep 25, 2024
ad4e84c
Fixed the TODO for the Gazebo publishers
Isopod00 Sep 25, 2024
6609ef2
Refactor all usages of the drive ROS service
Isopod00 Sep 26, 2024
c027945
Merge branch 'main' into Refactor-swerve-into-tank
Isopod00 Sep 27, 2024
466a705
Remove remaining mentions of absolute encoders
Isopod00 Sep 29, 2024
e984b9a
Merge branch 'main' into Refactor-swerve-into-tank
Isopod00 Sep 29, 2024
912db1a
changed swerve joints "type" from continuous to fixed
kznhq Oct 1, 2024
c2a1bc4
Fixed Nav2 for tank drive
cparece1 Oct 1, 2024
f1e6d55
Updated autodig to do in-place digging
cparece1 Oct 2, 2024
62f99ef
Merge branch 'main' into AutoDig-Improvements
Isopod00 Oct 2, 2024
3a2056d
small update to autodig to align with new robot design and prepare fo…
cparece1 Oct 3, 2024
7a2fb6c
finally commiting this garbage. Scuffed fix for Skimmer node, decent …
cparece1 Oct 4, 2024
dfc0719
Fixed the motor_control_node (it got messed up in the previous commit)
Isopod00 Oct 4, 2024
bd70e1b
Merge branch 'main' into AutoDig-Improvements
Isopod00 Oct 4, 2024
2e93187
Made set_position calls wait to return until position has been reached
cparece1 Oct 9, 2024
cb6d18e
Merge branch 'main' into AutoDig-Improvements
Isopod00 Oct 9, 2024
a058008
Fixed up set position and set velocity so they dont return til they'r…
cparece1 Oct 11, 2024
413be1e
Merge branch 'main' into AutoDig-Improvements
Isopod00 Oct 11, 2024
3656b70
Merge branch 'main' into AutoDig-Improvements
Isopod00 Oct 12, 2024
1b4fb4a
Removed the async node class, and adapted the digger node, and the A…
cparece1 Oct 16, 2024
8b3fa30
Changed logic for extend/retract dumper, reread AutoDig, AutoOff, Dum…
cparece1 Oct 18, 2024
dfff57b
TargTach was private so not accessable, added a getter to work around…
cparece1 Oct 18, 2024
74f651c
Motor_control_node now builds
cparece1 Oct 18, 2024
b0d67cc
Merge branch 'AutoDig-Improvements' of https://github.com/GOFIRST-Rob…
cparece1 Oct 18, 2024
297e80e
Merge remote-tracking branch 'origin/main' into AutoDig-Improvements
Isopod00 Oct 22, 2024
cc8ee93
Digger_node should be good, motor control has thresholds, but still r…
cparece1 Oct 23, 2024
f5e5957
Merge branch 'main' into AutoDig-Improvements
Isopod00 Oct 23, 2024
f7c39f7
Auto Formatting
Isopod00 Oct 23, 2024
48fe448
velocityThreshold and tachThreshold cleanup
Isopod00 Oct 23, 2024
847ba94
Merge branch 'main' into AutoDig-Improvements
Isopod00 Oct 25, 2024
c71be3b
Merge branch 'main' into AutoDig-Improvements
Isopod00 Oct 29, 2024
b17b67e
Add async await syntax for set_position
jmblixt3 Oct 30, 2024
6cf106c
Merge branch 'main' into AutoDig-Improvements
Isopod00 Oct 31, 2024
9de17c8
Merge branch 'main' into AutoDig-Improvements
Isopod00 Nov 5, 2024
fa0d28e
Merge branch 'main' into AutoDig-Improvements
Isopod00 Nov 8, 2024
b52316c
Re-Added Async_sleep via asyncnode to control time spent digging and …
cparece1 Nov 13, 2024
3623d28
Added workaround to make dumping berm cancellable (dump is now a seri…
cparece1 Nov 15, 2024
b6fd434
Switched method used for raising lift
cparece1 Nov 15, 2024
3741a42
dumper and digger now use events for limit switches
cparece1 Nov 15, 2024
82c01be
fixed event import and instantiation
cparece1 Nov 15, 2024
e05dd76
made stop also set events
cparece1 Nov 15, 2024
ed4f5e8
Merge branch 'main' into AutoDig-Improvements
Isopod00 Nov 19, 2024
e95ff0f
switched to future from event
cparece1 Nov 22, 2024
67c4146
trying smthn new with MultiThreadedExecutor
cparece1 Nov 22, 2024
413388a
trying to fix import
cparece1 Nov 22, 2024
d1d9f34
fixed running to be lift running
umnrobotics Nov 22, 2024
570e9ce
Digger node with futures to control limit switch behavior
jmblixt3 Nov 22, 2024
114bb19
fixed zero callback
cparece1 Nov 27, 2024
610e507
Cleaned up
cparece1 Nov 27, 2024
d9512aa
Merge branch 'main' into AutoDig-Improvements
Isopod00 Dec 3, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions config/rovr_control.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
84 changes: 35 additions & 49 deletions src/digger/digger/digger_node.py
Original file line number Diff line number Diff line change
@@ -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
from rclpy.executors import MultiThreadedExecutor

# Import custom ROS 2 interfaces
from rovr_interfaces.srv import MotorCommandSet, MotorCommandGet
Expand All @@ -34,16 +32,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)
Expand All @@ -60,21 +55,16 @@ 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


# Maximum value of the lift motor encoder (bottom of the lift system) IN DEGREES
self.MAX_ENCODER_DEGREES = (
-3600 * 360 / 42
Expand Down Expand Up @@ -102,18 +92,6 @@ 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
Expand All @@ -140,6 +118,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."""
Expand All @@ -159,12 +140,16 @@ 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):
async def set_position_callback(self, request, response):
"""This service request sets the position of the lift."""
self.set_position(request.position)
"""This method sets the position (in degrees) of the digger."""
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

Expand All @@ -183,23 +168,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()
cparece1 marked this conversation as resolved.
Show resolved Hide resolved
while not self.top_limit_pressed and self.running:
rclpy.spin_once(self)
response.success = True
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 and self.running:
rclpy.spin_once(self)
response.success = True
return response

# No more timer callback because setPos works

# Define subscriber callback methods here
def limit_switch_callback(self, limit_switches_msg):
Expand All @@ -226,11 +208,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__":
Expand Down
35 changes: 27 additions & 8 deletions src/dumper/dumper/dumper_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand All @@ -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
Expand All @@ -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:
Expand Down Expand Up @@ -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):
Expand All @@ -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()

Expand All @@ -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):
Expand Down
17 changes: 16 additions & 1 deletion src/motor_control/src/motor_control_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <string>
#include <tuple> // for tuples
#include <vector>
#include <cmath>

using namespace std::chrono_literals;
using std::placeholders::_1;
Expand Down Expand Up @@ -141,6 +142,9 @@ class PIDController {
bool isContinuousInputEnabled() {
return this->continuous;
}
int32_t getTargTach() {
return this->targTach;
}
};

class MotorControlNode : public rclcpp::Node {
Expand Down Expand Up @@ -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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Once again waiting in while loops like this is quite cpu intensive, try something event driven

}
RCLCPP_DEBUG(this->get_logger(), "Setting the velocity of CAN ID: %u to %d RPM", id, rpm); // Print Statement
}

Expand All @@ -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;
}
Comment on lines +243 to +246
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What do you do if you want to stop the motor before it reaches its final position

RCLCPP_DEBUG(this->get_logger(), "Setting the position of CAN ID: %u to %d degrees", id, position); // Print Statement
}

Expand Down Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion src/rovr_control/launch/main_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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",
)

Expand Down
2 changes: 1 addition & 1 deletion src/rovr_control/launch/main_no_joysticks_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ def generate_launch_description():
package="dumper",
executable="dumper_node",
name="dumper_node",
parameters=["config/motor_control.yaml"],
parameters=["config/motor_control.yaml","config/rovr_control.yaml"],
output="screen",
)

Expand Down
Loading