From e75931a87705d79a892f1bca2e742837def9d2c0 Mon Sep 17 00:00:00 2001 From: anshmenghani Date: Tue, 8 Apr 2025 18:41:03 -0400 Subject: [PATCH 1/2] add no motion calibration led --- .../candle_node/src/2025_candle_prematch.py | 30 +++++++++++++++++-- .../launch/2025_compbot_jetson.launch | 1 + 2 files changed, 29 insertions(+), 2 deletions(-) diff --git a/zebROS_ws/src/candle_node/src/2025_candle_prematch.py b/zebROS_ws/src/candle_node/src/2025_candle_prematch.py index eac49568f..f796768ec 100755 --- a/zebROS_ws/src/candle_node/src/2025_candle_prematch.py +++ b/zebROS_ws/src/candle_node/src/2025_candle_prematch.py @@ -12,6 +12,7 @@ from candle_controller_msgs.msg import CANdleColour from apriltag_msgs.msg import ApriltagArrayStamped from sensor_msgs.msg import JointState +from pigeon2_state_msgs.msg import Pigeon2State from angles import shortest_angular_distance from math import hypot from geometry_msgs.msg import TwistStamped @@ -51,6 +52,8 @@ HEARTBEAT = 20 # last in row BLINK_STATE = True +NO_MOTION_CALIBRATION = 15 + # ------------------ TAGSLAM_TIMEOUT = 10.0 # seconds IMU_ZERO_ANGLE_THRESHOLD = 0.1 # radians, approx. 5 degrees @@ -68,6 +71,12 @@ imu_orientation: Quaternion = Quaternion(0,0,0,1) +zeroed_while_calibrating = False + +time_last = rospy.Time().to_sec() + +pigeon_zeroing_count = 0 # pigeon2 no motion count of last message + def twist_callback(msg: TwistStamped): if abs(hypot(msg.twist.linear.x, msg.twist.linear.y)) < 0.05 and abs(msg.twist.angular.z) < 0.05: status_array[CMD_VEL_ZERO] = GREEN @@ -171,6 +180,21 @@ def button_box_callback(msg: ButtonBoxState2025): if has_been_zeroed: status_array[HAS_BEEN_ZEROED] = GREEN +def imu_zero_callback(pigeon2state: Pigeon2State): + global zeroed_while_calibrating + global time_last + global pigeon_zeroing_count + + pigeon_no_motion_count = pigeon2state.no_motion_count[pigeon2state.name.index("pigeon2")] + time_delta = rospy.Time().to_sec() - time_last + if int(pigeon_no_motion_count) != pigeon_zeroing_count or time_delta < 0.75: + zeroed_while_calibrating = True + time_last = rospy.Time().to_sec() + pigeon_zeroing_count = pigeon_no_motion_count + status_array[NO_MOTION_CALIBRATION] = GREEN + else: + zeroed_while_calibrating = False + wanted_x = None imu_orientation = None is_enabled = False @@ -195,6 +219,8 @@ def button_box_callback(msg: ButtonBoxState2025): cmd_vel_sub = rospy.Subscriber("/frcrobot_jetson/swerve_drive_controller/cmd_vel_out", TwistStamped, twist_callback, tcp_nodelay=True) auto_mode_sub = rospy.Subscriber("/auto/auto_mode", AutoMode, auto_mode_callback, tcp_nodelay=True) button_box_sub = rospy.Subscriber("/frcrobot_rio/button_box_states", ButtonBoxState2025, button_box_callback, tcp_nodelay=True) + imu_zero_sub = rospy.Subscriber("/frcrobot_jetson/pigeon2_states", Pigeon2State, imu_zero_callback, tcp_nodelay=True) + r = rospy.Rate(10) led_arr_msg = ColourArrayRequest() for idx, i in enumerate(status_array): @@ -276,9 +302,9 @@ def button_box_callback(msg: ButtonBoxState2025): colour_client(led_arr_msg) except: rospy.logerr_throttle(1.0, "2025_candle_prematch: unable to call LED service?") - for idx in [DOT9V0, DOT9V1, DOT10V0, DOT10V1, COLOR_RAW_MATCH_DATA, COLOR_MATCH_DATA, COLOR_MATCH_DATA_2, TAGSLAM_ALIVE, IMU_CORRECT, AUTO_POSITION_CLOSE, AUTO_ROTATION_CLOSE, ELEVATOR_AVOID_TRIGGERED, HAVE_CORAL, CMD_VEL_ZERO, CORRECT_AUTO, HAS_BEEN_ZEROED]: + for idx in [DOT9V0, DOT9V1, DOT10V0, DOT10V1, COLOR_RAW_MATCH_DATA, COLOR_MATCH_DATA, COLOR_MATCH_DATA_2, TAGSLAM_ALIVE, IMU_CORRECT, AUTO_POSITION_CLOSE, AUTO_ROTATION_CLOSE, ELEVATOR_AVOID_TRIGGERED, HAVE_CORAL, CMD_VEL_ZERO, CORRECT_AUTO, HAS_BEEN_ZEROED, NO_MOTION_CALIBRATION]: blink_idx(idx, ORANGE) blink_idx(HEARTBEAT, (255, 255, 255)) if is_enabled: break - r.sleep() \ No newline at end of file + r.sleep() diff --git a/zebROS_ws/src/controller_node/launch/2025_compbot_jetson.launch b/zebROS_ws/src/controller_node/launch/2025_compbot_jetson.launch index 3a5fbc41f..8368be1a2 100644 --- a/zebROS_ws/src/controller_node/launch/2025_compbot_jetson.launch +++ b/zebROS_ws/src/controller_node/launch/2025_compbot_jetson.launch @@ -58,6 +58,7 @@ elevator_controller intake_controller roller_controller + pigeon2_state_controller robot_code_ready_controller" />