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)