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