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)