def __init__(self, rate=20): '''I can't swim on account of my ass-mar''' self.rate = rate self.servo_max_rotation = 0.3 self.controller_max_rotation = self.servo_max_rotation / self.rate rospy.init_node('azi_drive', log_level=rospy.DEBUG) # rospy.init_node('azi_drive', log_level=rospy.WARN) rospy.logwarn("Setting maximum rotation speed to {} rad/s".format(self.controller_max_rotation)) Azi_Drive.set_delta_alpha_max(self.controller_max_rotation) # These should not be queued! Old commands are garbage. # Unfortunately, we have to queue these, because the subscriber cannot process two sequential # thrust messages as quickly as they are sent self.thrust_pub = rospy.Publisher('thruster_config', thrusterNewtons, queue_size=4) self.servo_pub = rospy.Publisher('dynamixel/dynamixel_full_config', DynamixelFullConfig, queue_size=4) rospy.Subscriber('wrench', WrenchStamped, self._wrench_cb, queue_size=1) # Thrust topic id's for each thruster self.left_id = 3 self.right_id = 2 # Time between messages before azi_drive shuts off # self.control_timeout = 2 # secs self.control_timeout = np.inf self.default_angles = np.array([0.0, 0.0], dtype=np.float32) self.default_forces = np.array([0.0, 0.0], dtype=np.float32) # Left, Right self.cur_angles = self.default_angles[:] self.cur_forces = self.default_forces[:] self.prev_angles = None self.set_servo_angles(self.cur_angles) self.set_forces(self.cur_forces) # Initializations self.last_msg_time = time() self.des_fx, self.des_fy, self.des_torque = 0.0, 0.0, 0.0 # Prepare a shutdown service rospy.loginfo("----------Waiting for control_manager service-------------") rospy.wait_for_service('azi_drive/stop') self.stop_boat_proxy = rospy.ServiceProxy('azi_drive/stop', AziStop)
class Controller(object): def __init__(self, rate=100): '''I can't swim on account of my ass-mar''' # Register azi_drive as a controller rospy.wait_for_service('/controller/register_controller', 30.0) rospy.ServiceProxy('/controller/register_controller', register_controller)('azi_drive') self.rate = rate self.servo_max_rotation = 6.0 self.controller_max_rotation = self.servo_max_rotation / self.rate # rospy.init_node('azi_drive', log_level=rospy.WARN) # Set up kill handling self.kill_listener = KillListener(self.on_kill, self.on_unkill) self.kill_broadcaster = KillBroadcaster( id='Azi_drive', description='Azi drive node shutdown') try: self.kill_broadcaster.clear() except rospy.service.ServiceException, e: rospy.logwarn(str(e)) self.killed = False rospy.logwarn("Setting maximum rotation speed to {} rad/s".format( self.controller_max_rotation)) Azi_Drive.set_delta_alpha_max(self.controller_max_rotation) #Azi_Drive.set_thrust_bound((-50,50)) # These should not be queued! Old commands are garbage. # Unfortunately, we have to queue these, because the subscriber cannot process two sequential # thrust messages as quickly as they are sent self.thrust_pub = rospy.Publisher('thruster_config', thrusterNewtons, queue_size=4) self.servo_pub = rospy.Publisher('controller/dynamixel_full_config', ControlDynamixelFullConfig, queue_size=4) self.next_wrench = WrenchStamped().wrench rospy.Subscriber('wrench', WrenchStamped, self._wrench_cb, queue_size=1) # Thrust topic id's for each thruster self.left_id = 3 self.right_id = 2 # Time between messages before azi_drive shuts off # self.control_timeout = 2 # secs self.control_timeout = np.inf self.default_angles = np.array([0.0, 0.0], dtype=np.float32) self.default_forces = np.array([0.0, 0.0], dtype=np.float32) self.pwm_forces = np.array([0.0, 0.0], dtype=np.float64) # Left, Right self.cur_angles = self.default_angles[:] self.cur_forces = self.default_forces[:] self.prev_angles = None self.set_servo_angles(self.cur_angles) self.set_forces(self.cur_forces) # Initializations self.last_msg_time = time() self.des_fx, self.des_fy, self.des_torque = 0.0, 0.0, 0.0 self.MAX_NEWTONS = 100.0 #Full forward Jacksons motors self.MIN_NEWTONS = -100.0 #Full reverse Jacksons self.ABS_MAX_PW = .002 self.ABS_MIN_PW = .001 self.ZERO_NEWTONS = 0 self.REV_CONV = (self.ZERO_NEWTONS - self.MIN_NEWTONS) / (0 - self.ABS_MIN_PW) self.FWD_CONV = (self.MAX_NEWTONS - self.ZERO_NEWTONS) / self.ABS_MAX_PW
def __init__(self, rate=20): '''I can't swim on account of my ass-mar''' self.rate = rate self.servo_max_rotation = 0.3 self.controller_max_rotation = self.servo_max_rotation / self.rate rospy.Subscriber("control_arbiter", Bool, self.control_callback) rospy.Subscriber("pwm1_alias", Float64, self.pwm1_cb) rospy.Subscriber("pwm2_alias", Float64, self.pwm2_cb) rospy.init_node('azi_drive', log_level=rospy.DEBUG) # rospy.init_node('azi_drive', log_level=rospy.WARN) rospy.logwarn("Setting maximum rotation speed to {} rad/s".format( self.controller_max_rotation)) Azi_Drive.set_delta_alpha_max(self.controller_max_rotation) # These should not be queued! Old commands are garbage. # Unfortunately, we have to queue these, because the subscriber cannot process two sequential # thrust messages as quickly as they are sent self.thrust_pub = rospy.Publisher('thruster_config', thrusterNewtons, queue_size=4) self.servo_pub = rospy.Publisher('dynamixel/dynamixel_full_config', DynamixelFullConfig, queue_size=4) rospy.Subscriber('wrench', WrenchStamped, self._wrench_cb, queue_size=1) # Thrust topic id's for each thruster self.left_id = 3 self.right_id = 2 self.control_kill = False # Time between messages before azi_drive shuts off # self.control_timeout = 2 # secs self.control_timeout = np.inf self.default_angles = np.array([0.0, 0.0], dtype=np.float32) self.default_forces = np.array([0.0, 0.0], dtype=np.float32) self.pwm_forces = np.array([0.0, 0.0], dtype=np.float64) # Left, Right self.cur_angles = self.default_angles[:] self.cur_forces = self.default_forces[:] self.prev_angles = None self.set_servo_angles(self.cur_angles) self.set_forces(self.cur_forces) # Initializations self.last_msg_time = time() self.des_fx, self.des_fy, self.des_torque = 0.0, 0.0, 0.0 # Prepare a shutdown service rospy.loginfo( "----------Waiting for control_manager service-------------") rospy.wait_for_service('azi_drive/stop') self.stop_boat_proxy = rospy.ServiceProxy('azi_drive/stop', AziStop) self.MAX_NEWTONS = 100.0 #Full forward Jacksons motors self.MIN_NEWTONS = -100.0 #Full reverse Jacksons self.ABS_MAX_PW = .002 self.ABS_MIN_PW = .001 self.ZERO_NEWTONS = 0 self.REV_CONV = (self.ZERO_NEWTONS - self.MIN_NEWTONS) / (0 - self.ABS_MIN_PW) self.FWD_CONV = (self.MAX_NEWTONS - self.ZERO_NEWTONS) / self.ABS_MAX_PW