From a846b5658fe905e79c5e0c3844e3a0018a6f4620 Mon Sep 17 00:00:00 2001 From: Joe Date: Sun, 8 Mar 2020 15:20:37 -0500 Subject: [PATCH] add portions of autonomous mode --- .../unnamed_toaster/scripts/match_logic.py | 101 ++++++++++++++---- .../unnamed_toaster/scripts/shooter_math.py | 38 ++++--- .../scripts/ros_table_interface.py | 9 +- 3 files changed, 110 insertions(+), 38 deletions(-) diff --git a/unnamed_toaster/unnamed_toaster/scripts/match_logic.py b/unnamed_toaster/unnamed_toaster/scripts/match_logic.py index 5562352..261b946 100755 --- a/unnamed_toaster/unnamed_toaster/scripts/match_logic.py +++ b/unnamed_toaster/unnamed_toaster/scripts/match_logic.py @@ -32,31 +32,51 @@ import tf2_ros import tf2_geometry_msgs -from geometry_msgs.msg import PointStamped +from geometry_msgs.msg import PointStamped, Twist from std_msgs.msg import Float64, String from shooter_math import ShooterMath + class MatchLogic: def __init__(self): - # Publishers and Subscribers + # Publisher to command turret angle self.turret_command_pub = rospy.Publisher("turret_command", Float64, queue_size=1) - self.target_sub = rospy.Subscriber("target_point", PointStamped, ShooterMath.calculate_azimuth) # Setup to calculate the target - self.mode_sub = rospy.Subscriber("robot_mode", String, self.update_mode) # Setup to run mode logic on mode update + + # Publisher to command robot base + self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1) + + # Get the robot mode from network tables + self.robot_mode = "No Mode" + self.mode_sub = rospy.Subscriber("robot_mode", String, self.robot_mode_cb) + + # Get our starting position + self.init_pose = -1.0 + self.init_pose_sub = rospy.Subscriber("init_pose", Float64, self.init_pose_cb) # Setup TF2 # http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28Python%29 self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) - # Other - self.robot_mode = "No Mode" - - def update_mode(self, msg): # Update the mode. + self.shooter_math = ShooterMath(self.tf_buffer) + + # + # ROS Callbacks + # + + def robot_mode_cb(self, msg): self.robot_mode = msg.data + def init_pose_cb(self, msg): + self.init_pose = msg.data + + # + # Match Logic + # + def run(self): # Switch modes = { @@ -72,23 +92,68 @@ def run(self): def Teleop(self): rospy.loginfo("RobotMode: Teleoperated") - + def Autonomous(self): rospy.loginfo("RobotMode: Autonomous") - # Do at same time - # - Backup to cross the line - # - Move turret to close enough - # Enable Shooter - # Fire when ready - # Fire all balls - # Track and intake balls on the field - + rate = rospy.Rate(25.0) + + # Wait until init pose is valid + while self.init_pose < 0.0: + rate.sleep() + + # Our starting position is our distance (in meters) from the + # right side of the field. Field width is 8.21055 meters. + # The high target is located 2.404364 meters from right side. + field_y = self.init_pose - 2.404364 + + # The starting line is 3.05m from the wall, but we are backwards + field_x = -3.05 + + # Set turret angle + angle = self.shooter_math.get_turret_angle(field_x, field_y) + rospy.loginfo("Turret command {}".format(angle)) + turret_command = self.shooter_math.get_turret_msg(angle) + self.turret_command_pub.publish(turret_command) + + # Back up to cross line + twist_msg = Twist() + twist_msg.linear.x = 0.5 + + rate = rospy.Rate(25.0) + start_time = rospy.get_time() + while not rospy.is_shutdown(): + + rospy.loginfo("Move") + + # TODO: add a monitor on laser distance to wall + + # Limit the amount of time we can drive + if rospy.get_time() - start_time > 1.0: + self.cmd_vel_pub.publish(Twist()) + break + + self.cmd_vel_pub.publish(twist_msg) + rate.sleep() + + # Track with shooter + while self.robot_mode == "Autonomous": + + rospy.loginfo("SHOOTING AND WAITING UNTIL END OF AUTO") + rospy.sleep(1) + + # TODO: Enable Shooter when aligned + + #self.turret_command_pub.publish(self.shooter_math.desired_turret_command) + rate.sleep() + + # TODO: Track and intake balls on the field + def Disabled(self): rospy.loginfo("RobotMode: Disabled") def Test(self): rospy.loginfo("RobotMode: Test") - + def NoMode(self): rospy.loginfo("RobotMode: NoMode, transmitted mode was: " + str(self.robot_mode)) diff --git a/unnamed_toaster/unnamed_toaster/scripts/shooter_math.py b/unnamed_toaster/unnamed_toaster/scripts/shooter_math.py index bc7b9b5..d40db21 100644 --- a/unnamed_toaster/unnamed_toaster/scripts/shooter_math.py +++ b/unnamed_toaster/unnamed_toaster/scripts/shooter_math.py @@ -40,19 +40,29 @@ class ShooterMath: - def __init__(self): - #self.pub = rospy.Publisher("turret_command", Float64, queue_size=1) + def __init__(self, tf_buffer): + self.tf_buffer = tf_buffer + self.desired_turret_command = None - # Setup TF2 - # http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28Python%29 - self.tf_buffer = tf2_ros.Buffer() - self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + self.target_sub = rospy.Subscriber("target_point", PointStamped, self.callback) - #self.sub = rospy.Subscriber("target_point", PointStamped, self.target_update) + # Get a command message for our shooter, based on distance to target + def get_shooter_msg(self, distance): + command = None + return command - self.last_target_command = None + # Get a command message for our turret, based on an angle + def get_turret_msg(self, angle): + command = Float64() + command.data = angle + return command - def calculate_azimuth(self, msg): + # Get a command angle for our turret based on a target point + def get_turret_angle(self, x, y): + return atan2(y, x) + + # ROS Callback for data from target_localization node + def callback(self, msg): # Get transformation between target point frame (laser?) and base_link try: @@ -66,12 +76,6 @@ def calculate_azimuth(self, msg): target_point = tf2_geometry_msgs.do_transform_point(msg, transform) # Find yaw angle for turret - angle = atan2(target_point.point.y, target_point.point.x) - - # Command stuff - command = Float64() - command.data = angle - - self.last_target_command = command - #self.pub.publish(command) + angle = self.get_turret_angle(target_point.point.x, target_point.point.y) + self.desired_turret_command = self.get_turret_msg(angle) diff --git a/unnamed_toaster/unnamed_toaster_hw_interface/scripts/ros_table_interface.py b/unnamed_toaster/unnamed_toaster_hw_interface/scripts/ros_table_interface.py index 34ebaf1..34800b9 100755 --- a/unnamed_toaster/unnamed_toaster_hw_interface/scripts/ros_table_interface.py +++ b/unnamed_toaster/unnamed_toaster_hw_interface/scripts/ros_table_interface.py @@ -36,13 +36,14 @@ from unnamed_toaster_hw_interface.robot_odom import RobotOdom from unnamed_toaster_hw_interface.turret import Turret -from std_msgs.msg import String +from std_msgs.msg import String, Float64 class ROSTableInterface: def __init__(self): # Publisher self.robot_mode_pub = rospy.Publisher("robot_mode", String, queue_size=1) + self.init_pose_pub = rospy.Publisher("init_pose", Float64, queue_size=1) # Tables ip = rospy.get_param("~ip", "roboRIO-1721-FRC") @@ -60,14 +61,13 @@ def run(self): # Odom Get info left = self.table.getFloat('Port', 0) right = self.table.getFloat('Starboard', 0) - index = self.table.getInt('rosIndex', 0) + index = self.table.getInt('rosIndex', 0) # Logic for odom sawtooth control if index != previous_odom_index: self.odom.update(left, right) self.odom.publish() previous_index = index - # Update turret self.turret.update() self.turret.publish() @@ -75,6 +75,9 @@ def run(self): # Publish mode self.robot_mode_pub.publish(self.table.getString("RobotMode", "Waiting for Rio")) + # Publish init pose + self.init_pose_pub.publish(self.table.getFloat("distance_along_init_line", -1.0)) + # Sleep rate.sleep()