Skip to content

Commit

Permalink
Commands rework (#28)
Browse files Browse the repository at this point in the history 
* Added command classes

* created basic framework

* created auto

* Fixed issues from testing

* added factory functions

* Added documentation

* Fixes from testing
  • Loading branch information
@brandongasser
brandongasser authored Feb 13, 2024
1 parent 59c06af commit f710a93
Show file tree
Hide file tree
Showing 7 changed files with 486 additions and 609 deletions.
349 changes: 153 additions & 196 deletions src/control_pkg/control_pkg/auto.py
View file
Original file line number Diff line number Diff line change
@@ -1,218 +1,175 @@
import rclpy

from rclpy.node import Node
from std_msgs.msg import Int8, Float32
from std_msgs.msg import Float32, Int8
from geometry_msgs.msg import Twist
from utilities.tools import Tools
import time

from control_pkg.commands import Runner, Command
from control_pkg.drive_commands import DriveDistanceCommand, DriveToWaypointCommand
from control_pkg.wait_commands import WaitCommand, WaitUntilCommand
from control_pkg.turn_command import TurnToDegreesCommand

class Auto(Node):

def __init__(self):
super().__init__('auto')

self.running = False
self.state = 0
self.pivot_updated = False
self.pivot_position = 0
self.degree_deadband = 0.2
# Set up ROS stuff
self.pivot_position = None
self.position = None

self.pivot_position_updated = False
self.position_updated = False

self.speed_publisher = self.create_publisher(Twist, '/cmd_vel', 10)
self.pivot_publisher = self.create_publisher(Int8, '/vehicle/pivot', 10)
self.speed_publisher = self.create_publisher(Twist, '/cmd_vel', 10)

self.create_subscription(Twist, '/apriltag', self.update_position, 10)
self.create_subscription(Float32, '/sensor/pivot', self.update_pivot_position, 10)

# Start runner
self.runner = Runner()
self.create_timer(0.01, self.runner.run)

# Start auto command
self.runner.start_command(self.get_auto_command())

def get_auto_command(self) -> Command:
"""
Generates and returns the command to start when auto is started
self.pivot_sub = self.create_subscription(Float32, '/sensor/pivot', self.update_pivot, 10)

self.create_timer(0.01, self.determine_control)
Returns:
Command: Command to start when auto is started
"""
# Waypoint after left turn out
left_waypoint = Twist()
left_waypoint.linear.x = -2.5
left_waypoint.linear.z = 2
left_waypoint.angular.y = 180

def determine_control(self):
if not self.pivot_updated:
return
match self.state:
case 0:
self.get_logger().info('turning to 0 degrees')
self.turn_to_degrees(0)
return
case 1:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 2:
self.get_logger().info('driving at 100% speed for 1 meter')
self.drive_distance(1.0, 2.16)
return
case 3:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 4:
self.get_logger().info('turning to -20.75 degrees')
self.turn_to_degrees(-20.75)
return
case 5:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 6:
self.get_logger().info('driving at 100% speed for 3.14 meters')
self.drive_distance(1.0, 3.14)
return
case 7:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 8:
self.get_logger().info('turning to 0 degrees')
self.turn_to_degrees(0)
return
case 9:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 10:
self.get_logger().info('driving at 100% speed for 1.5 meters')
self.drive_distance(1.0, 2.5)
return
case 11:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 12:
self.get_logger().info('driving at -100% speed for 1.5 meters')
self.drive_distance(-1.0, 2.25)
return
case 13:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 14:
self.get_logger().info('turning to -20.75 degrees')
self.turn_to_degrees(-20.75)
return
case 15:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 16:
self.get_logger().info('driving at -100% speed for 3.14 meters')
self.drive_distance(-1.0, 3.3)
return
case 17:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 18:
self.get_logger().info('turning to 20.75 degrees')
self.turn_to_degrees(20.75)
return
case 19:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 20:
self.get_logger().info('driving at 100% speed for 3.14 meters')
self.drive_distance(1.0, 3.4)
return
case 21:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 22:
self.get_logger().info('turing to 0 degrees')
self.turn_to_degrees(0)
return
case 23:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 24:
self.get_logger().info('driving at 100% speed for 1.5 meters')
self.drive_distance(1.0, 2.5)
return
case 25:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 26:
self.get_logger().info('driving at -100% speed for 1.5 meters')
self.drive_distance(-1.0, 2.5)
return
case 27:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 28:
self.get_logger().info('turning to 20.75 degrees')
self.turn_to_degrees(20.75)
return
case 29:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 30:
self.get_logger().info('driving at -100% speed for 3.14 meters')
self.drive_distance(-1.0, 3.4)
return
case 31:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 32:
self.get_logger().info('turning to 0 degrees')
self.turn_to_degrees(0)
return
case 33:
self.get_logger().info('waiting for 2 seconds')
self.wait_time(2)
return
case 34:
self.get_logger().info('driving at -100% speed for 1 meter')
self.drive_distance(-1.0, 2.16)
return
case _:
self.get_logger().info('done')
return
# Waypoint after right turn out
right_waypoint = Twist()
right_waypoint.linear.x = 2.5
right_waypoint.linear.z = 2
right_waypoint.angular.y = 0

# Factory functions for removing redundancy
wait = lambda time_seconds : WaitCommand(time_seconds)
turn_to_degrees = lambda degrees : TurnToDegreesCommand(degrees, self.get_pivot_position, self.drive_pivot)
drive_to_waypoint = lambda waypoint : DriveToWaypointCommand(waypoint, self.get_position, self.get_pivot_position, self.drive_pivot, self.drive)
drive_distance = lambda speed, distance : DriveDistanceCommand(speed, distance, self.drive)
wait_until = lambda condition : WaitUntilCommand(condition)

# Creating and returning command
return wait_until(lambda : self.position_updated and self.pivot_position_updated)\
.and_then(turn_to_degrees(0))\
.and_then(wait(2))\
.and_then(drive_to_waypoint(left_waypoint))\
.and_then(wait(2))\
.and_then(turn_to_degrees(0))\
.and_then(wait(2))\
.and_then(drive_distance(1, 2))\
.and_then(wait(2))\
.and_then(turn_to_degrees(-22))\
.and_then(wait(2))\
.and_then(drive_distance(1, 3.14 * 1.90475 / 4))\
.and_then(wait(2))\
.and_then(drive_distance(-1, 3.14 * 1.90471 / 4))\
.and_then(wait(2))\
.and_then(turn_to_degrees(0))\
.and_then(wait(2))\
.and_then(drive_distance(-1, 1.5))\
.and_then(wait(2))\
.and_then(turn_to_degrees(-18.624))\
.and_then(wait(2))\
.and_then(drive_distance(-1, 2.25 * 3.14 / 2))\
.and_then(wait(2))\
.and_then(turn_to_degrees(0))\
.and_then(wait(2))\
.and_then(drive_distance(-1, 1.5))\
.and_then(wait(2))\
.and_then(drive_to_waypoint(right_waypoint))\
.and_then(wait(2))\
.and_then(turn_to_degrees(0))\
.and_then(wait(2))\
.and_then(drive_distance(1, 2))\
.and_then(wait(2))\
.and_then(turn_to_degrees(-22))\
.and_then(wait(2))\
.and_then(drive_distance(1, 3.14 * 1.90475 / 4))\
.and_then(wait(2))\
.and_then(drive_distance(-1, 3.14 * 1.90475 / 4))\
.and_then(wait(2))\
.and_then(turn_to_degrees(0))\
.and_then(wait(2))\
.and_then(drive_distance(-1, 1.5))\
.and_then(wait(2))\
.and_then(turn_to_degrees(18.624))\
.and_then(wait(2))\
.and_then(drive_distance(-1, 2.25 * 3.14 / 2))\
.and_then(wait(2))\
.and_then(turn_to_degrees(0))\
.and_then(wait(2))\
.and_then(drive_distance(-1, 1.5))

def drive_pivot(self, speed) -> None:
"""
Drives the pivot left or right or stops the pivot
def turn_to_degrees(self, degrees):
Args:
speed (int): -1 for left, 1 for right, 0 for stop
"""
msg = Int8()
if abs(self.pivot_position - degrees) <= self.degree_deadband:
self.state += 1
msg.data = 0
else:
msg.data = 1 if self.pivot_position < degrees else -1
msg.data = speed
self.pivot_publisher.publish(msg)

def drive_distance(self, speed, distance):
run_time = 0
if distance >= 2/3:
run_time = (1/3) + distance - (2/3)
else:
run_time = ((distance / (2/3)) ** 2) / 3
self.drive_speed_time(speed, run_time)
def update_pivot_position(self, msg: Float32) -> None:
"""
Updates the pivot position when a new message is heard
def drive_speed_time(self, speed, run_time):
if not self.running:
self.end_time = time.time() + run_time
self.running = True
msg = Twist()
if time.time() >= self.end_time:
msg.linear.x = 0.0
self.state += 1
self.running = False
else:
msg.linear.x = speed
self.speed_publisher.publish(msg)
Args:
msg (Float32): Message containing the new pivot position
"""
self.pivot_position_updated = True
self.pivot_position = msg.data

def get_pivot_position(self) -> float:
"""
Gets the current pivot position
Returns:
float: Current pivot position
"""
return self.pivot_position

def update_position(self, msg: Twist) -> None:
"""
Updates the position of the vehicle when a new message is heard
Args:
msg (Twist): Message containing the new position
"""
self.position_updated = True
self.position = msg

def get_position(self) -> float:
"""
Gets the current position of the vehicle
def wait_time(self, run_time):
if not self.running:
self.end_time = time.time() + run_time
self.running = True
if time.time() >= self.end_time:
self.state += 1
self.running = False
Returns:
float: Current position of the vehicle
"""
return self.position

def drive(self, speed: float) -> None:
"""
Drives the vehicle at the given speed
def update_pivot(self, msg: Float32):
self.pivot_updated = True
self.pivot_position = Tools.potentiometer_to_degrees(msg.data)
Args:
speed (float): Speed to drive at in range [-1..1]
"""
msg = Twist()
msg.linear.x = speed
self.speed_publisher.publish(msg)

def main():
rclpy.init()
Expand All @@ -222,6 +179,6 @@ def main():

node.destroy_node()
rclpy.shutdown()

if __name__ == '__main__':
main()
Loading

0 comments on commit f710a93

Please sign in to comment.