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
