Portions of autonomous mode

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))
 

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)

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,21 +61,23 @@ 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()
 
             # 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()