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