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
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):
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 __init__(self):
rospy.init_node('thruster_controls')
self.sim = rospy.get_param('~/thruster_controls/sim')
if not self.sim:
self.pub = rospy.Publisher(self.ROBOT_PUB_TOPIC,
ThrusterSpeeds,
queue_size=3)
else:
self.pub = rospy.Publisher(self.SIM_PUB_TOPIC,
Float32MultiArray,
queue_size=3)
self.enable_service = rospy.Service('enable_controls', SetBool,
self.handle_enable_controls)
self.tm = ThrusterManager(
os.path.join(sys.path[0], '../config/cthulhu.config'))
self.listener = TransformListener()
for d in utils.get_axes():
rospy.Subscriber(utils.get_controls_move_topic(d), Float64,
self._on_pid_received, d)
rospy.Subscriber(utils.get_power_topic(d), Float64,
self._on_power_received, d)
self.pid_outputs = np.zeros(6)
self.pid_outputs_local = np.zeros(6)
self.powers = np.zeros(6)
self.t_allocs = np.zeros(8)
def __init__(self):
rospy.init_node('desired_state')
for d in utils.get_axes():
self.pub_pos[d] = rospy.Publisher(utils.get_pid_topic(d),
Float64,
queue_size=3)
self.pub_pos_enable[d] = rospy.Publisher(
utils.get_pos_pid_enable(d), Bool, queue_size=3)
self.pub_vel_enable[d] = rospy.Publisher(
utils.get_vel_pid_enable(d), Bool, queue_size=3)
self.pub_vel[d] = rospy.Publisher(utils.get_vel_topic(d),
Float64,
queue_size=3)
self.pub_control_effort[d] = rospy.Publisher(
utils.get_controls_move_topic(d), Float64, queue_size=3)
self.pub_power[d] = rospy.Publisher(utils.get_power_topic(d),
Float64,
queue_size=3)
self.listener = TransformListener()
rospy.Subscriber(self.DESIRED_POSE_TOPIC, Pose, self._on_pose_received)
rospy.Subscriber(self.DESIRED_TWIST_TOPIC, Twist,
self._on_twist_received)
rospy.Subscriber(self.DESIRED_POWER_TOPIC, Twist,
self._on_power_received)
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 receive_odometry(self, odometry):
if 'base_link' in self.listener.getFrameStrings():
local_pose = utils.transform_pose(self.listener, 'odom',
'base_link', odometry.pose.pose)
utils.publish_data_dictionary(self._pub_pose, utils.get_axes(),
utils.parse_pose(local_pose))
local_twist = utils.transform_twist(self.listener, 'odom',
'base_link',
odometry.twist.twist)
utils.publish_data_dictionary(self._pub_twist, utils.get_axes(),
utils.parse_twist(local_twist))
local_state = Odometry()
local_state.header.frame_id = 'base_link'
local_state.pose.pose = local_pose
local_state.twist.twist = local_twist
self._pub_local_state.publish(local_state)
def __init__(self):
for d in utils.get_axes():
self.state[d] = 0
self.hold[d] = 0
rospy.Subscriber(utils.get_pose_topic(d), Float64, self._on_state_received, d)
self.pub_pos[d] = rospy.Publisher(utils.get_pid_topic(d), Float64, queue_size=3)
self.pub_pos_enable[d] = rospy.Publisher(utils.get_pid_enable(d), Bool, queue_size=3)
self.pub_power[d] = rospy.Publisher(utils.get_power_topic(d), Float64, queue_size=3)
rospy.Subscriber(self.DESIRED_POSE_TOPIC, Pose, self._on_pose_received)
rospy.Subscriber(self.DESIRED_TWIST_POWER, Twist, self.on_powers_received)
def __init__(self):
self._pub_pose = {}
self._pub_twist = {}
for d in utils.get_axes():
self._pub_pose[d] = rospy.Publisher(utils.get_pose_topic(d),
Float64,
queue_size=3)
self._pub_twist[d] = rospy.Publisher(utils.get_twist_topic(d),
Float64,
queue_size=3)
rospy.init_node('state_republisher')
rospy.Subscriber(self.STATE_TOPIC, Odometry, self.receive_odometry)
rospy.spin()
def __init__(self):
rospy.init_node('thruster_controls')
self.pub = rospy.Publisher(self.ROBOT_PUB_TOPIC,
ThrusterSpeeds,
queue_size=3)
self.enable_service = rospy.Service('enable_controls', SetBool,
self.handle_enable_controls)
self.tm = ThrusterManager(
rr.get_filename('package://controls/config/cthulhu.config',
use_protocol=False))
self.pid_outputs = np.zeros(6)
self.pid_outputs_local = np.zeros(6)
self.powers = np.zeros(6)
self.t_allocs = np.zeros(8)
for d in utils.get_axes():
rospy.Subscriber(utils.get_controls_move_topic(d), Float64,
self._on_pid_received, d)
rospy.Subscriber(utils.get_power_topic(d), Float64,
self._on_power_received, d)
def _on_power_received(self, val, direction):
if val.data != 0:
self.pid_outputs[utils.get_axes().index(direction)] = val.data
self.t_allocs = self.tm.calc_t_allocs(self.pid_outputs)
def receive_odometry(self, odometry):
pose = utils.parse_pose(odometry.pose.pose)
utils.publish_data_dictionary(self._pub_pose, utils.get_axes(), pose)
twist = utils.parse_twist(odometry.twist.twist)
utils.publish_data_dictionary(self._pub_twist, utils.get_axes(), twist)
def _on_power_received(self, val, direction):
self.powers[utils.get_axes().index(direction)] = val.data
self.update_thruster_allocs()
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)
