diff --git a/zebROS_ws/src/behaviors/config/2024_align_speaker_config.yaml b/zebROS_ws/src/behaviors/config/2024_align_speaker_config.yaml index 3cd5849d4..fd0b45796 100644 --- a/zebROS_ws/src/behaviors/config/2024_align_speaker_config.yaml +++ b/zebROS_ws/src/behaviors/config/2024_align_speaker_config.yaml @@ -1,3 +1,3 @@ tolerance: 0.05 velocity_tolerance: 0.5 -min_samples: 5 \ No newline at end of file +min_samples: 50 # 0.2 seconds \ No newline at end of file diff --git a/zebROS_ws/src/behaviors/config/2024_align_to_trap.yaml b/zebROS_ws/src/behaviors/config/2024_align_to_trap.yaml index 404cb9e39..8dc3c8698 100644 --- a/zebROS_ws/src/behaviors/config/2024_align_to_trap.yaml +++ b/zebROS_ws/src/behaviors/config/2024_align_to_trap.yaml @@ -2,9 +2,15 @@ x_tolerance: 0.03 y_tolerance: 0.03 angle_tolerance: 0.03 -x_offset: 0.65 +amp_x_tolerance: 0.2 +amp_y_tolerance: 0.05 +amp_angle_tolerance: 0.05 + + +x_offset: 0.85 y_offset: 0.0 +# CHECK THESE OFFSETS x_offset_amp: 0.0 y_offset_amp: 0.0 diff --git a/zebROS_ws/src/behaviors/config/2024_clawster_config.yaml b/zebROS_ws/src/behaviors/config/2024_clawster_config.yaml index 5e9f97fe7..c7fb786b6 100644 --- a/zebROS_ws/src/behaviors/config/2024_clawster_config.yaml +++ b/zebROS_ws/src/behaviors/config/2024_clawster_config.yaml @@ -6,7 +6,7 @@ claw: switch_name: "claw_limit_switch" preshooter: - intake_speed: 0.90 + intake_speed: 0.85 outtake_speed: 0.9 switch_name: "preshooter_limit_switch" diff --git a/zebROS_ws/src/behaviors/config/2024_climbing_config.yaml b/zebROS_ws/src/behaviors/config/2024_climbing_config.yaml index 5bc2b68b4..2d575bd56 100644 --- a/zebROS_ws/src/behaviors/config/2024_climbing_config.yaml +++ b/zebROS_ws/src/behaviors/config/2024_climbing_config.yaml @@ -5,10 +5,10 @@ trap_retract_speed: 20.0 fast_retract_acceleration: 40.0 trap_retract_acceleration: 40.0 extend_height: 55.0 -trap_height: 10.0 -fast_height: 10.0 +trap_height: 15.0 +fast_height: 15.0 joint_name: climb_leader loop_rate: 50.0 tolerance: 10.0 -pivot_angle: 1.9 -min_pivot_angle: 1.7 +pivot_angle: 1.7 +min_pivot_angle: 1.65 \ No newline at end of file diff --git a/zebROS_ws/src/behaviors/config/2024_rumble_server_config.yaml b/zebROS_ws/src/behaviors/config/2024_rumble_server_config.yaml index 745332f84..ccdac359d 100644 --- a/zebROS_ws/src/behaviors/config/2024_rumble_server_config.yaml +++ b/zebROS_ws/src/behaviors/config/2024_rumble_server_config.yaml @@ -4,4 +4,4 @@ preshooter_limit_switch_name: preshooter_limit_switch intake_limit_switch_name: diverter_limit_switch claw_limit_switch_name: claw_limit_switch -rumble_on_note: 30000 # out of 65535 \ No newline at end of file +rumble_on_note: 65534 # out of 65535 \ No newline at end of file diff --git a/zebROS_ws/src/behaviors/config/2024_shooting_config.yaml b/zebROS_ws/src/behaviors/config/2024_shooting_config.yaml index 0d92a6a3b..e43055045 100644 --- a/zebROS_ws/src/behaviors/config/2024_shooting_config.yaml +++ b/zebROS_ws/src/behaviors/config/2024_shooting_config.yaml @@ -22,6 +22,8 @@ amp_top_right_speed: 60.0 # rad/s amp_bottom_left_speed: 100.0 # rad/s amp_bottom_right_speed: 100.0 # rad/s amp_pivot_position: 1.0 # rad +amp_drive_back_time: 0.5 # sec +amp_drive_back_speed: 2.0 # m/s # trap trap_top_left_speed: 325.0 # rad/s diff --git a/zebROS_ws/src/behaviors/config/auto_mode_config.yaml b/zebROS_ws/src/behaviors/config/auto_mode_config.yaml index e6e80c34f..230513412 100644 --- a/zebROS_ws/src/behaviors/config/auto_mode_config.yaml +++ b/zebROS_ws/src/behaviors/config/auto_mode_config.yaml @@ -49,7 +49,7 @@ auto_mode_6: ["shoot_center_subwoofer", "start_intake", "drive_1_5_m", "shoot_fr auto_mode_7: ["shoot_center_subwoofer", "pause_two", "pause_two", "spin_down_shooter"] # 8: shoot preloaded note, drive back TESTED AND WORKS -auto_mode_8: ["shoot_center_subwoofer", "drive_2_5_m"] +auto_mode_8: ["shoot_center_subwoofer", "drive_2_5_m", "spin_down_shooter"] # 9: no-op auto_mode_9: ["pause_quarter"] diff --git a/zebROS_ws/src/behaviors/launch/2024_align_to_trap.launch b/zebROS_ws/src/behaviors/launch/2024_align_to_trap.launch index a30c6e069..065e83e82 100644 --- a/zebROS_ws/src/behaviors/launch/2024_align_to_trap.launch +++ b/zebROS_ws/src/behaviors/launch/2024_align_to_trap.launch @@ -73,6 +73,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/zebROS_ws/src/behaviors/launch/2024_drive_and_score.launch b/zebROS_ws/src/behaviors/launch/2024_drive_and_score.launch index f4e7c5c4c..260c0c185 100644 --- a/zebROS_ws/src/behaviors/launch/2024_drive_and_score.launch +++ b/zebROS_ws/src/behaviors/launch/2024_drive_and_score.launch @@ -1,6 +1,6 @@ - + diff --git a/zebROS_ws/src/behaviors/src/2024_align_to_speaker_server.py b/zebROS_ws/src/behaviors/src/2024_align_to_speaker_server.py index 5bc139a5a..400035f3e 100755 --- a/zebROS_ws/src/behaviors/src/2024_align_to_speaker_server.py +++ b/zebROS_ws/src/behaviors/src/2024_align_to_speaker_server.py @@ -22,6 +22,7 @@ class Aligner: _result = behavior_actions.msg.AlignToSpeaker2024Result() def __init__(self, name): + self.msg = None self.valid_samples = 0 self._action_name = name self.tolerance = rospy.get_param("tolerance") @@ -45,7 +46,7 @@ def __init__(self, name): self.pub_dist_and_ang_vel = rospy.Publisher("/speaker_align/dist_and_ang", behavior_actions.msg.AutoAlignSpeaker, queue_size=1) #distance and angle self._feedback.aligned = False - + self.feed_forward = True def imu_callback(self, imu_msg): @@ -63,18 +64,22 @@ def imu_callback(self, imu_msg): trans = self.tfBuffer.lookup_transform('base_link', offset_point.header.frame_id, rospy.Time()) destination = tf2_geometry_msgs.do_transform_point(offset_point, trans) - msg = std_msgs.msg.Float64() + self.msg = std_msgs.msg.Float64() dist_ang_msg = behavior_actions.msg.AutoAlignSpeaker() dist_ang_msg.distance = math.sqrt(destination.point.x ** 2 + destination.point.y ** 2) - msg.data = math.pi + self.current_yaw + math.atan2(destination.point.y, destination.point.x) + self.msg.data = math.pi + self.current_yaw + math.atan2(destination.point.y, destination.point.x) dist_ang_msg.angle = math.atan2(destination.point.y, destination.point.x) self.pub_dist_and_ang_vel.publish(dist_ang_msg) except Exception as e: rospy.logwarn_throttle(1, f"align_to_speaker: can't publish distance {e}") - if self._feedback.error < self.tolerance and abs(self.current_orient_effort) < self.velocity_tolerance: + if self.msg is not None: + self._feedback.error = abs(angles.shortest_angular_distance(self.msg.data, self.current_yaw)) + + #rospy.loginfo(f"errr {self._feedback.error} tolerance {self.tolerance}") + if self._feedback.error < self.tolerance: # and abs(self.current_orient_effort) < self.velocity_tolerance self.valid_samples += 1 else: self.valid_samples = 0 @@ -117,29 +122,30 @@ def aligner_callback(self, goal: behavior_actions.msg.AlignToSpeaker2024Goal): rate.sleep() continue - msg = std_msgs.msg.Float64() + self.msg = std_msgs.msg.Float64() dist_ang_msg = behavior_actions.msg.AutoAlignSpeaker() dist_ang_msg.distance = math.sqrt(destination.point.x ** 2 + destination.point.y ** 2) - msg.data = math.pi + self.current_yaw + math.atan2(destination.point.y, destination.point.x) + self.msg.data = math.pi + self.current_yaw + math.atan2(destination.point.y, destination.point.x) dist_ang_msg.angle = math.atan2(destination.point.y, destination.point.x) - self._feedback.error = abs(angles.shortest_angular_distance(msg.data, self.current_yaw)) self._feedback.aligned = False - self.object_publish.publish(msg) + self.object_publish.publish(self.msg) self.pub_dist_and_ang_vel.publish(dist_ang_msg) cmd_vel_msg = geometry_msgs.msg.Twist() cmd_vel_msg.angular.x = 0 cmd_vel_msg.angular.y = 0 - cmd_vel_msg.angular.z = self.current_orient_effort + 1.0 * numpy.sign(self.current_orient_effort) * int(self.feed_forward) + cmd_vel_msg.angular.z = self.current_orient_effort + if abs(self._feedback.error) > 0.1: + cmd_vel_msg.angular.z += 1.0 * numpy.sign(self.current_orient_effort) * int(self.feed_forward) cmd_vel_msg.linear.x = 0 cmd_vel_msg.linear.y = 0 cmd_vel_msg.linear.z = 0 self.pub_cmd_vel.publish(cmd_vel_msg) - rospy.loginfo_throttle(0.5, f"Align to speaker {abs(angles.shortest_angular_distance(msg.data, self.current_yaw))}") + rospy.loginfo_throttle(0.5, f"Align to speaker {abs(angles.shortest_angular_distance(self.msg.data, self.current_yaw))}") if self.valid_samples >= self.min_samples: self._feedback.aligned = True diff --git a/zebROS_ws/src/behaviors/src/2024_align_to_trap.py b/zebROS_ws/src/behaviors/src/2024_align_to_trap.py index ec697c3a9..71eeb8d40 100755 --- a/zebROS_ws/src/behaviors/src/2024_align_to_trap.py +++ b/zebROS_ws/src/behaviors/src/2024_align_to_trap.py @@ -36,9 +36,13 @@ class Aligner: def __init__(self, name): self._action_name = name - self.x_tolerance = rospy.get_param("x_tolerance") - self.y_tolerance = rospy.get_param("y_tolerance") - self.angle_tolerance = rospy.get_param("angle_tolerance") + self.trap_x_tolerance = rospy.get_param("x_tolerance") + self.trap_y_tolerance = rospy.get_param("y_tolerance") + self.trap_angle_tolerance = rospy.get_param("angle_tolerance") + + self.amp_x_tolerance = rospy.get_param("amp_x_tolerance") + self.amp_y_tolerance = rospy.get_param("amp_y_tolerance") + self.amp_angle_tolerance = rospy.get_param("amp_angle_tolerance") self.x_offset = rospy.get_param("x_offset") self.y_offset = rospy.get_param("y_offset") @@ -59,21 +63,36 @@ def __init__(self, name): self.orientation_command_pub = rospy.Publisher("/teleop/orientation_command", std_msgs.msg.Float64, queue_size=1) self.object_subscribe = rospy.Subscriber("/imu/zeroed_imu", sensor_msgs.msg.Imu, self.imu_callback) - self.x_state_pub = rospy.Publisher("x_position_pid/x_state_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True) - self.x_cmd_pub = rospy.Publisher("x_position_pid/x_cmd_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True) - self.x_enable_pub = rospy.Publisher("x_position_pid/x_enable_pub", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True) - self.x_command = 0 + self.trap_x_state_pub = rospy.Publisher("x_position_pid/x_state_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True) + self.trap_x_cmd_pub = rospy.Publisher("x_position_pid/x_cmd_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True) + self.trap_x_enable_pub = rospy.Publisher("x_position_pid/x_enable_pub", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True) + self.trap_x_command = 0 + # self.x_command_sub = rospy.Publisher("x_position_pid/x_command", std_msgs.msg.Float64, self.x_command_callback, tcp_nodelay=True) + + self.trap_y_state_pub = rospy.Publisher("y_position_pid/y_state_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True) + self.trap_y_cmd_pub = rospy.Publisher("y_position_pid/y_cmd_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True) + self.trap_y_enable_pub = rospy.Publisher("y_position_pid/y_enable_pub", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True) + self.trap_y_command = 0 + + + + self.amp_x_state_pub = rospy.Publisher("x_position_pid_amp/x_state_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True) + self.amp_x_cmd_pub = rospy.Publisher("x_position_pid_amp/x_cmd_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True) + self.amp_x_enable_pub = rospy.Publisher("x_position_pid_amp/x_enable_pub", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True) + self.amp_x_command = 0 # self.x_command_sub = rospy.Publisher("x_position_pid/x_command", std_msgs.msg.Float64, self.x_command_callback, tcp_nodelay=True) - self.y_state_pub = rospy.Publisher("y_position_pid/y_state_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True) - self.y_cmd_pub = rospy.Publisher("y_position_pid/y_cmd_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True) - self.y_enable_pub = rospy.Publisher("y_position_pid/y_enable_pub", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True) - self.y_command = 0 + self.amp_y_state_pub = rospy.Publisher("y_position_pid_amp/y_state_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True) + self.amp_y_cmd_pub = rospy.Publisher("y_position_pid_amp/y_cmd_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True) + self.amp_y_enable_pub = rospy.Publisher("y_position_pid_amp/y_enable_pub", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True) + self.amp_y_command = 0 # self.y_command_sub = rospy.Publisher("y_position_pid/y_command", std_msgs.msg.Float64, self.y_command_callback, tcp_nodelay=True) self.team_subscribe = rospy.Subscriber("/frcrobot_rio/match_data", MatchSpecificData, self.match_data_callback) - self.enable_pub = rospy.Publisher("align_to_trap_pid/pid_enable", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True) + self.trap_enable_pub = rospy.Publisher("align_to_trap_pid/pid_enable", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True) + self.amp_enable_pub = rospy.Publisher("align_to_amp_pid/pid_enable", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True) + self.sub_effort = rospy.Subscriber("/teleop/orient_strafing/control_effort", std_msgs.msg.Float64, self.robot_orientation_effort_callback) @@ -105,9 +124,39 @@ def aligner_callback(self, goal: behavior_actions.msg.AlignToTrap2024Goal): if goal.destination == goal.AMP: rospy.loginfo("2024_align_to_trap: Aligning to amp") closest_frame = self.RED_AMP if self.color == MatchSpecificData.ALLIANCE_COLOR_RED else self.BLUE_AMP + self.x_enable_pub = self.amp_x_enable_pub + self.y_enable_pub = self.amp_y_enable_pub + self.enable_pub = self.amp_enable_pub + self.x_state_pub = self.amp_x_state_pub + self.x_cmd_pub = self.amp_x_cmd_pub + self.x_enable_pub = self.amp_x_enable_pub + self.x_command = self.amp_x_command + self.y_state_pub = self.amp_y_state_pub + self.y_cmd_pub = self.amp_y_cmd_pub + self.y_enable_pub = self.amp_y_enable_pub + self.y_command = self.amp_y_command + self.x_tolerance = self.amp_x_tolerance + self.y_tolerance = self.amp_y_tolerance + self.angle_tolerance = self.amp_angle_tolerance + elif goal.destination == goal.SUBWOOFER: rospy.loginfo("2024_align_to_trap: Aligning to subwoofer") closest_frame = self.RED_SUBWOOFER if self.color == MatchSpecificData.ALLIANCE_COLOR_RED else self.BLUE_SUBWOOFER + self.x_enable_pub = self.amp_x_enable_pub + self.y_enable_pub = self.amp_y_enable_pub + self.enable_pub = self.amp_enable_pub + self.x_state_pub = self.amp_x_state_pub + self.x_cmd_pub = self.amp_x_cmd_pub + self.x_enable_pub = self.amp_x_enable_pub + self.x_command = self.amp_x_command + self.y_state_pub = self.amp_y_state_pub + self.y_cmd_pub = self.amp_y_cmd_pub + self.y_enable_pub = self.amp_y_enable_pub + self.y_command = self.amp_y_command + self.x_tolerance = self.amp_x_tolerance + self.y_tolerance = self.amp_y_tolerance + self.angle_tolerance = self.amp_angle_tolerance + else: rospy.loginfo("2024_align_to_trap: Aligning to trap") for frame in (self.RED_TAGS if self.color == MatchSpecificData.ALLIANCE_COLOR_RED else self.BLUE_TAGS): @@ -116,6 +165,21 @@ def aligner_callback(self, goal: behavior_actions.msg.AlignToTrap2024Goal): if distance < closest_distance: closest_frame = frame closest_distance = distance + self.x_enable_pub = self.trap_x_enable_pub + self.y_enable_pub = self.trap_y_enable_pub + self.enable_pub = self.trap_enable_pub + self.x_state_pub = self.trap_x_state_pub + self.x_cmd_pub = self.trap_x_cmd_pub + self.x_enable_pub = self.trap_x_enable_pub + self.x_command = self.trap_x_command + self.y_state_pub = self.trap_y_state_pub + self.y_cmd_pub = self.trap_y_cmd_pub + self.y_enable_pub = self.trap_y_enable_pub + self.y_command = self.trap_y_command + self.x_tolerance = self.trap_x_tolerance + self.y_tolerance = self.trap_y_tolerance + self.angle_tolerance = self.trap_angle_tolerance + trans = self.tfBuffer.lookup_transform(closest_frame, "map", rospy.Time()) if goal.destination == goal.TRAP: diff --git a/zebROS_ws/src/behaviors/src/2024_drive_and_score.py b/zebROS_ws/src/behaviors/src/2024_drive_and_score.py index 50635cd17..dcd5efb5e 100755 --- a/zebROS_ws/src/behaviors/src/2024_drive_and_score.py +++ b/zebROS_ws/src/behaviors/src/2024_drive_and_score.py @@ -11,14 +11,16 @@ import math from behavior_actions.msg import DriveAndScore2024Action, DriveAndScore2024Goal, DriveAndScore2024Feedback, DriveAndScore2024Result -from behavior_actions.msg import Clawster2024Action, Clawster2024Feedback, Clawster2024Result, Clawster2024Goal -from behavior_actions.msg import Arm2024Action, Arm2024Feedback, Arm2024Result, Arm2024Goal import behavior_actions.msg from tf.transformations import euler_from_quaternion # may look like tf1 but is actually tf2 from frc_msgs.msg import MatchSpecificData import std_srvs.srv import angles import numpy +import geometry_msgs.msg + +# While the snap to angle button is held, spin up the shooter for amp and override driver +# Once amp shot button is pressed, call shooting server with goal=AMP. make shooting server drive backwards for amp. # Used for anytime we need to drive to a position and then score, (amp/trap) as opposed to shooting where we just need to align (2024_align_to_speaker_server.py and other) @@ -38,16 +40,9 @@ def __init__(self, name): self.shooting_client = actionlib.SimpleActionClient('/shooting/shooting_server_2024', behavior_actions.msg.Shooting2024Action) rospy.loginfo("2024_intaking_server: waiting for shooting server") self.shooting_client.wait_for_server() - - self.arm_client = actionlib.SimpleActionClient('/arm/move_arm_server_2024', Arm2024Action) - rospy.loginfo("2024_intaking_server: waiting for arm server") - self.arm_client.wait_for_server() - - self.clawster_client = actionlib.SimpleActionClient('/clawster/clawster_server_2024', Clawster2024Action) - rospy.loginfo("2024_intaking_server: waiting for clawster server") - self.clawster_client.wait_for_server() + self.pub_cmd_vel = rospy.Publisher("/align/cmd_vel", geometry_msgs.msg.Twist, queue_size=1) self.align_done = False - self.arm_done = False + self.shooting_done = False self._as = actionlib.SimpleActionServer(self._action_name, DriveAndScore2024Action, execute_cb=self.score_cb, auto_start = False) self._as.start() @@ -55,22 +50,16 @@ def __init__(self, name): def align_done_cb(self, status, result): rospy.loginfo("2024 drive and score, align server done") self.align_done = True - - def arm_done_cb(self, status, result): - rospy.loginfo("2024 drive and score, arm server done") - self.arm_done = True def shooting_done_cb(self, status, result): - rospy.loginfo("2024 drive and score, shooting server done") self.shooting_done = True - + def score_cb(self, goal: DriveAndScore2024Goal): self.feed_forward = True success = True self.align_done = False - self.arm_done = False self.shooting_done = False - + have_shot = False rospy.loginfo(f"Drive and score 2024 - called with goal {goal}") r = rospy.Rate(60.0) align_goal = behavior_actions.msg.AlignToTrap2024Goal() @@ -79,57 +68,84 @@ def score_cb(self, goal: DriveAndScore2024Goal): rospy.loginfo(f"Drive and score 2024 - going amp") align_goal.destination = align_goal.AMP self.align_client.send_goal(align_goal, done_cb=self.align_done_cb) - # also start moving arm - arm_goal = Arm2024Goal() - arm_goal.path = arm_goal.AMP - self.arm_client.send_goal(arm_goal, done_cb=self.arm_done_cb) + shooting_goal = behavior_actions.msg.Shooting2024Goal() + shooting_goal.mode = shooting_goal.AMP + shooting_goal.leave_spinning = True + shooting_goal.setup_only = True + self.shooting_client.send_goal(shooting_goal) + elif goal.destination == goal.TRAP: rospy.loginfo(f"Drive and score 2024 - going trap") align_goal.destination = align_goal.TRAP self.align_client.send_goal(align_goal, done_cb=self.align_done_cb) + shooting_goal = behavior_actions.msg.Shooting2024Goal() + shooting_goal.mode = shooting_goal.TRAP + shooting_goal.leave_spinning = True + shooting_goal.setup_only = True + self.shooting_client.send_goal(shooting_goal) # for this case we just wait until we are done and then send shooting # for telling if we hvae - shooting = False while not rospy.is_shutdown(): # check that preempt has not been requested by the client if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() - self.arm_client.cancel_goals_at_and_before_time(rospy.Time.now()) self.align_client.cancel_goals_at_and_before_time(rospy.Time.now()) self.shooting_client.cancel_goals_at_and_before_time(rospy.Time.now()) - self.clawster_client.cancel_goals_at_and_before_time(rospy.Time.now()) success = False break - rospy.loginfo_throttle(0.1, f"2024_align_and_SCORE: waiting on {'aligning' if not self.align_done else ''} {'arm' if not self.arm_done and goal.destination == goal.AMP else ''} {'shooting' if not self.shooting_done and goal.destination == goal.TRAP else ''}") + rospy.loginfo_throttle(0.1, f"2024_align_and_SCORE: waiting on {'aligning' if not self.align_done else ''} {'shooting' if not self.shooting_done and goal.destination == goal.TRAP else ''}") # should be good to outake - if goal.destination == goal.AMP and self.align_done and self.arm_done: - rospy.loginfo("Drive and score 2024 - align and arm done") - # want to outtake from claw - clawster_goal = Clawster2024Goal() - clawster_goal.mode = clawster_goal.OUTTAKE - clawster_goal.destination = clawster_goal.CLAW - self.clawster_client.send_goal(clawster_goal) - # we have fired the note at this point so we are done + if goal.destination == goal.AMP and self.align_done and not have_shot: + rospy.loginfo("Drive and score 2024 - align and amp done") + cmd_vel_msg = geometry_msgs.msg.Twist() + start = rospy.Time.now() + + while (rospy.Time.now() - start < rospy.Duration(0.5)): + cmd_vel_msg.angular.x = 0 + cmd_vel_msg.angular.y = 0 + cmd_vel_msg.angular.z = 0 + cmd_vel_msg.linear.x = -1.0 + cmd_vel_msg.linear.y = 0 + cmd_vel_msg.linear.z = 0 + self.pub_cmd_vel.publish(cmd_vel_msg) + r.sleep() + shooting_goal = behavior_actions.msg.Shooting2024Goal() + shooting_goal.mode = shooting_goal.AMP + shooting_goal.leave_spinning = False + self.shooting_client.send_goal(shooting_goal, done_cb=self.shooting_done_cb) + have_shot = True + + if goal.destination == goal.AMP and self.align_done and self.shooting_done: + rospy.loginfo("DRIVE AND SCORE, shooting and align done") + # drive back + cmd_vel_msg = geometry_msgs.msg.Twist() + start = rospy.Time.now() + + while (rospy.Time.now() - start < rospy.Duration(0.25)): + cmd_vel_msg.angular.x = 0 + cmd_vel_msg.angular.y = 0 + cmd_vel_msg.angular.z = 0 + cmd_vel_msg.linear.x = 2.0 + cmd_vel_msg.linear.y = 0 + cmd_vel_msg.linear.z = 0 + self.pub_cmd_vel.publish(cmd_vel_msg) + r.sleep() + self._result.success = True self._as.set_succeeded(self._result) - return + return - if goal.destination == goal.TRAP and self.align_done and not shooting: + if goal.destination == goal.TRAP and self.align_done: rospy.loginfo("2024 drive and score - trap aligned, shooting") # need to shoot - shooting = True shooting_goal = behavior_actions.msg.Shooting2024Goal() shooting_goal.mode = shooting_goal.TRAP - self.shooting_client.send_goal(shooting_goal, done_cb=self.shooting_done_cb) - - if goal.destination == goal.TRAP and self.shooting_done: - # have sent note so we are done + self.shooting_client.send_goal(shooting_goal) self._result.success = True self._as.set_succeeded(self._result) - rospy.loginfo('%s: Succeeded Trap (hopefully)' % self._action_name) return self._as.publish_feedback(self._feedback) diff --git a/zebROS_ws/src/behaviors/src/2024_rumble_server.py b/zebROS_ws/src/behaviors/src/2024_rumble_server.py index 3797b8533..185879e1e 100755 --- a/zebROS_ws/src/behaviors/src/2024_rumble_server.py +++ b/zebROS_ws/src/behaviors/src/2024_rumble_server.py @@ -14,6 +14,8 @@ from norfair_ros.msg import Detections, Detection from frc_msgs.srv import RumbleCommand, RumbleCommandRequest, RumbleCommandResponse from frc_msgs.msg import MatchSpecificData +from candle_controller_msgs.srv import AnimationRequest, AnimationResponse, Animation + import math import time @@ -44,6 +46,13 @@ def __init__(self, name): self.intake_limit_switch_name = rospy.get_param("intake_limit_switch_name") self.rumble_value = rospy.get_param("rumble_on_note") + self.candle_srv = rospy.ServiceProxy('/frcrobot_jetson/candle_controller/animation', Animation) + + ''' +rosservice call /frcrobot_jetson/candle_controller/animation "{speed: -0.5, start: 8, count: 18, animation_type: 2, red: 255, green: 0, blue: 0, white: 0, + direction: 0, brightness: 0.0, reversed: false, param4: 0.0, param5: 0.0}" + ''' + self.match_data_sub = rospy.Subscriber("/frcrobot_rio/match_data", MatchSpecificData, self.match_data_cb) self.rumble = rospy.Timer(rospy.Duration(1.0/20.0), self.rumble_loop) self.closest_note = 900 @@ -81,6 +90,15 @@ def notes_callback(self, msg: Detections): def match_data_cb(self, data: MatchSpecificData): + if not data.Enabled: + rospy.loginfo_throttle(5, "TERMINATING") + candle_req = AnimationRequest() + candle_req.animation_type = candle_req.ANIMATION_TYPE_LARSON + candle_req.red = 255 + candle_req.start = 8 + candle_req.count = 18 + self.candle_srv.call(candle_req) + if data.Autonomous and data.Enabled: self.should_run_loop = False else: diff --git a/zebROS_ws/src/behaviors/src/2024_shooting_server.py b/zebROS_ws/src/behaviors/src/2024_shooting_server.py index 223e1733b..be755759a 100755 --- a/zebROS_ws/src/behaviors/src/2024_shooting_server.py +++ b/zebROS_ws/src/behaviors/src/2024_shooting_server.py @@ -11,6 +11,7 @@ from behavior_actions.msg import Clawster2024Goal, Clawster2024Feedback, Clawster2024Result, Clawster2024Action from std_msgs.msg import Float64 from interpolating_map import InterpolatingMap +from geometry_msgs.msg import Twist class ShootingServer(object): @@ -30,6 +31,9 @@ def __init__(self, name): rospy.loginfo("2024_shooting_server: waiting for clawster") self.preshooter_client.wait_for_server() + # used for driving back after amp shot + self.cmd_vel_pub = rospy.Publisher("/auto_note_align/cmd_vel", Twist, queue_size=1, tcp_nodelay=True) + # speeds_map: [[distance: [top_left_speed, top_right_speed, bottom_left_speed, bottom_right_speed]], ...] speeds_map_param = rospy.get_param("speeds_map") @@ -104,6 +108,8 @@ def __init__(self, name): self.amp_bottom_left_speed = rospy.get_param("amp_bottom_left_speed") self.amp_bottom_right_speed = rospy.get_param("amp_bottom_right_speed") self.amp_pivot_position = rospy.get_param("amp_pivot_position") + self.amp_drive_back_time = rospy.get_param("amp_drive_back_time") + self.amp_drive_back_speed = rospy.get_param("amp_drive_back_speed") # Trap (constant speeds and angle) self.trap_top_left_speed = rospy.get_param("trap_top_left_speed") @@ -320,7 +326,7 @@ def preshooter_done_cb(state, result): while not preshooter_done and not rospy.is_shutdown(): rospy.loginfo_throttle(0.5, "2024_shooting_server: waiting for preshooter") if self.server.is_preempt_requested(): - rospy.loginfo("2024_shooting_server: preempted") + rospy.loginfo("2024_shooting_server: preempted preshooter") # ensure shooter turned off self.shooter_client.cancel_goals_at_and_before_time(rospy.Time.now()) @@ -334,10 +340,56 @@ def preshooter_done_cb(state, result): self.preshooter_client.cancel_goals_at_and_before_time(rospy.Time.now()) self.server.set_preempted() - + if goal.AMP: + rospy.loginfo("DRIVING BACK") + cmd_vel_msg = Twist() + start = rospy.Time.now() + + while (rospy.Time.now() - start) < rospy.Duration(self.amp_drive_back_time): + cmd_vel_msg.angular.x = 0 + cmd_vel_msg.angular.y = 0 + cmd_vel_msg.angular.z = 0 + cmd_vel_msg.linear.x = self.amp_drive_back_speed + cmd_vel_msg.linear.y = 0 + cmd_vel_msg.linear.z = 0 + self.cmd_vel_pub.publish(cmd_vel_msg) + rospy.loginfo("Publishing cmd _vel ") + r.sleep() + + cmd_vel_msg.angular.x = 0 + cmd_vel_msg.angular.y = 0 + cmd_vel_msg.angular.z = 0 + cmd_vel_msg.linear.x = 0 + cmd_vel_msg.linear.y = 0 + cmd_vel_msg.linear.z = 0 + self.cmd_vel_pub.publish(cmd_vel_msg) return r.sleep() + if goal.mode == goal.AMP: + rospy.loginfo("AMP DRIVE BACK") + cmd_vel_msg = Twist() + start = rospy.Time.now() + + while (rospy.Time.now() - start) < rospy.Duration(self.amp_drive_back_time): + cmd_vel_msg.angular.x = 0 + cmd_vel_msg.angular.y = 0 + cmd_vel_msg.angular.z = 0 + cmd_vel_msg.linear.x = self.amp_drive_back_speed + cmd_vel_msg.linear.y = 0 + cmd_vel_msg.linear.z = 0 + self.cmd_vel_pub.publish(cmd_vel_msg) + rospy.loginfo("Publishing cmd _vel ") + r.sleep() + + cmd_vel_msg.angular.x = 0 + cmd_vel_msg.angular.y = 0 + cmd_vel_msg.angular.z = 0 + cmd_vel_msg.linear.x = 0 + cmd_vel_msg.linear.y = 0 + cmd_vel_msg.linear.z = 0 + self.cmd_vel_pub.publish(cmd_vel_msg) + pivot_goal.pivot_position = 0.5 self.pivot_client.send_goal(pivot_goal) diff --git a/zebROS_ws/src/controller_node/config/zedx/zedx.yaml b/zebROS_ws/src/controller_node/config/zedx/zedx.yaml index 56d9f4654..546404326 100644 --- a/zebROS_ws/src/controller_node/config/zedx/zedx.yaml +++ b/zebROS_ws/src/controller_node/config/zedx/zedx.yaml @@ -4,8 +4,8 @@ general: camera_model: 'zedx' - grab_resolution: 'HD1200' # HD1080, HD1200, SVGA, AUTO - grab_frame_rate: 15 # 120, 60, 30, 15 + grab_resolution: 'SVGA' # HD1080, HD1200, SVGA, AUTO + grab_frame_rate: 30 # 120, 60, 30, 15 depth: min_depth: 0.3 # Min: 0.3, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory diff --git a/zebROS_ws/src/controller_node/launch/2024_compbot_jetson.launch b/zebROS_ws/src/controller_node/launch/2024_compbot_jetson.launch index 7c563a749..52976cf22 100644 --- a/zebROS_ws/src/controller_node/launch/2024_compbot_jetson.launch +++ b/zebROS_ws/src/controller_node/launch/2024_compbot_jetson.launch @@ -61,13 +61,14 @@ note_diverter_controller climb_controller pigeon2_imu_state_controller + cancoder_state_controller robot_code_ready_controller" />