def __init__(self): time.sleep(1) self.robot_state_msgs = RobotState() self.battery_state_msgs = BatteryState() self.twist_msgs = Twist() # self.diagnostics_speak_thread = {} self.auto_undocking = False self.diagnostics_list = [] if rospy.get_param("~speak_warn", True): self.diagnostics_list.append(DiagnosticStatus.WARN) if rospy.get_param("~speak_error", True): self.diagnostics_list.append(DiagnosticStatus.ERROR) if rospy.get_param("~speak_stale", True): self.diagnostics_list.append(DiagnosticStatus.STALE) # self.base_breaker = rospy.ServiceProxy('base_breaker', BreakerCommand) # self.battery_sub = rospy.Subscriber("battery_state", BatteryState, self.battery_callback, queue_size = 1) self.cmd_vel_sub = rospy.Subscriber("base_controller/command_unchecked", Twist, self.cmd_vel_callback, queue_size = 1) self.robot_state_sub = rospy.Subscriber("robot_state", RobotState, self.robot_state_callback, queue_size = 1) self.diagnostics_status_sub = rospy.Subscriber("diagnostics", DiagnosticArray, self.diagnostics_status_callback, queue_size = 1) self.undock_sub = rospy.Subscriber("/undock/status", GoalStatusArray, self.undock_status_callback) # self.cmd_vel_pub = rospy.Publisher("base_controller/command", Twist, queue_size=1)
def __init__(self): time.sleep(1) self.battery_sub = rospy.Subscriber("battery_state", BatteryState, self.battery_callback, queue_size = 1) self.cmd_vel_sub = rospy.Subscriber("base_controller/command", Twist, self.cmd_vel_callback, queue_size = 1) self.robot_state_sub = rospy.Subscriber("robot_state", RobotState, self.robot_state_callback, queue_size = 1) self.diagnostics_status_sub = rospy.Subscriber("diagnostics", DiagnosticArray, self.diagnostics_status_callback, queue_size = 1) self.base_breaker = rospy.ServiceProxy('base_breaker', BreakerCommand) # self.robot_state_msgs = RobotState() self.battery_state_msgs = BatteryState() self.twist_msgs = Twist() # self.diagnostics_speak_thread = {}
def __init__(self): # Internal parameters for the functions of this driver self._publish_rate = 50 # Hz rate. # The state of the arm, gripper, and base breakers self._arm_breaker_state = BreakerState( name="arm_breaker", state=BreakerState.STATE_ENABLED) self._base_breaker_state = BreakerState( name="base_breaker", state=BreakerState.STATE_ENABLED) self._gripper_breaker_state = BreakerState( name="gripper_breaker", state=BreakerState.STATE_ENABLED) # The cached state of the robot self._robot_state = RobotState( ready=True, breakers=[ self._arm_breaker_state, self._base_breaker_state, self._gripper_breaker_state ], charger=ChargerState( state=0, # Unknown what this actually is charging_mode=2, # "Not Charging" according to the comments battery_voltage=SimulatedRobotDriver.BATTERY_FULL_VOLTAGE, battery_capacity=SimulatedRobotDriver.BATTERY_FULL_CAPACITY)) self._robot_state_lock = Lock() # The cached state of the gripper self._gripper_state = GripperState(ready=True) self._gripper_state.joints.append( FetchJointState( name="gripper_joint", control_mode=3, # Based on values on the robot position=0.05, # Default start position of open )) self._gripper_state_lock = Lock() # Create the diagnostic updater self._updater = diagnostic_updater.Updater() self._updater.setHardwareID("none") self._updater.add( "arm_breaker", produce_breaker_diagnostic_func(self._arm_breaker_state)) self._updater.add( "base_breaker", produce_breaker_diagnostic_func(self._base_breaker_state)) self._updater.add( "gripper_breaker", produce_breaker_diagnostic_func(self._gripper_breaker_state)) # Publishers self._robot_state_publisher = rospy.Publisher('/robot_state', RobotState, queue_size=1) self._gripper_state_publisher = rospy.Publisher('/gripper_state', GripperState, queue_size=1) # Subscribers self._joint_state_sub = rospy.Subscriber('/joint_states', JointState, self._on_joint_state) # The services to set and reset the breakers self._arm_breaker_service = rospy.Service("/arm_breaker", BreakerCommand, self.set_arm_breaker) self._base_breaker_service = rospy.Service("/base_breaker", BreakerCommand, self.set_base_breaker) self._gripper_breaker_service = rospy.Service("/gripper_breaker", BreakerCommand, self.set_gripper_breaker) # Simulation service to put the battery into low mode or not self._battery_low_service = rospy.Service( "~battery_to_low", Trigger, self._on_battery_to_level( SimulatedRobotDriver.BATTERY_LOW_VOLTAGE, SimulatedRobotDriver.BATTERY_LOW_CAPACITY)) self._battery_nominal_service = rospy.Service( "~battery_to_nominal", Trigger, self._on_battery_to_level( SimulatedRobotDriver.BATTERY_FULL_VOLTAGE, SimulatedRobotDriver.BATTERY_FULL_CAPACITY))