def soft_estop(self):
# Stop Moving
utils.publish_data_constant(self.pub_pos_enable, utils.get_axes(), False)
utils.publish_data_constant(self.pub_power, utils.get_axes(), 0)
self.powers = None
self.last_powers = None
self.pose = None
def run(self):
rate = rospy.Rate(self.REFRESH_HZ)
warned = False
event_id = 0
while not rospy.is_shutdown():
rate.sleep()
if (self.pose and self.twist) or (self.pose and self.power) or (
self.twist and self.power):
# More than one seen in one update cycle, so warn and continue
rospy.logerr(
"===> Controls received conflicting desired states! Halting robot. <==="
)
self.soft_estop()
continue
elif not self.pose and not self.twist and not self.power:
self.soft_estop()
if not warned:
rospy.logwarn(bcolors.WARN + (
"===> Controls received no desired state! Halting robot. "
"(Event %d) <===" % event_id) + bcolors.RESET)
warned = True
continue
# Now we have either pose XOR powers
if warned:
rospy.loginfo(bcolors.OKGREEN + (
"===> Controls now receiving desired state (End event %d) <==="
% (event_id)) + bcolors.RESET)
event_id += 1
warned = False
if self.pose:
self.enable_loops()
utils.publish_data_dictionary(self.pub_pos, utils.get_axes(),
self.pose)
self.pose = None
elif self.twist:
self.enable_loops()
self.disable_pos_loop()
utils.publish_data_dictionary(self.pub_vel, utils.get_axes(),
self.twist)
self.twist = None
elif self.power:
self.disable_loops()
# Enable stabilization on all axes with 0 power input
for p in self.power.keys():
if self.power[p] == 0:
utils.publish_data_constant(self.pub_vel_enable, [p],
True)
# Publish velocity setpoints to all Velocity loops, even though some are not enabled
utils.publish_data_dictionary(self.pub_vel, utils.get_axes(),
self.power)
utils.publish_data_dictionary(self.pub_power, utils.get_axes(),
self.power)
self.power = None
def soft_estop(self):
# Stop Moving
self.disable_loops()
utils.publish_data_constant(self.pub_control_effort, utils.get_axes(),
0)
self.twist = None
self.pose = None
self.power = None
def run(self):
rospy.init_node('desired_state')
rate = rospy.Rate(self.REFRESH_HZ)
warned = False
event_id = 0
while not rospy.is_shutdown():
rate.sleep()
if self.pose and self.powers:
# More than one seen in one update cycle, so warn and continue
rospy.logerr("===> Controls received both position and power! Halting robot. <===")
self.soft_estop()
continue
elif not self.pose and not self.powers:
self.soft_estop()
if not warned:
rospy.logwarn(bcolors.WARN + ("===> Controls received neither position nor power! Halting robot. (Event %d) <===" % event_id) + bcolors.RESET)
warned = True
continue
# Now we have either pose XOR powers
if warned:
rospy.loginfo(bcolors.OKGREEN + ("===> Controls now receiving %s (End event %d) <===" % ("position" if self.pose else "powers", event_id)) + bcolors.RESET)
event_id += 1
warned = False
if self.pose:
self.enable_loops()
utils.publish_data_dictionary(self.pub_pos, utils.get_axes(), self.pose)
self.pose = None
elif self.powers:
if self.last_powers is None or self.powers != self.last_powers:
self.enable_loops()
# Hold Position on the current state
self.hold = dict(self.state)
self.last_powers = dict(self.powers)
# Nonzero entries bypass PID
# If any nonzero xy power, disable those position pid loops
if self.powers['x'] != 0 or self.powers['y'] != 0:
utils.publish_data_constant(self.pub_pos_enable, ['x', 'y'], False)
# If any nonzero rpy power, disable those position pid loops
elif self.powers['roll'] != 0 or self.powers['pitch'] != 0 or self.powers['yaw'] != 0:
utils.publish_data_constant(self.pub_pos_enable, ['roll', 'pitch', 'yaw'], False)
# If any nonzero z power, disable those position pid loops
if self.powers['z'] != 0:
utils.publish_data_constant(self.pub_pos_enable, ['z'], False)
# TODO: BOTH cases
utils.publish_data_dictionary(self.pub_power, utils.get_axes(), self.powers)
# Publish current state to the desired state for PID
utils.publish_data_dictionary(self.pub_pos, utils.get_axes(), self.hold)
self.powers = None
def enable_loops(self):
# Enable all PID Loops
utils.publish_data_constant(self.pub_pos_enable, utils.get_axes(), True)
def disable_pos_loop(self):
# Disable position loop
utils.publish_data_constant(self.pub_pos_enable, utils.get_axes(),
False)
def disable_loops(self):
utils.publish_data_constant(self.pub_pos_enable, utils.get_axes(),
False)
utils.publish_data_constant(self.pub_vel_enable, utils.get_axes(),
False)