def __init__(self):
'''The layout should be a dictionary of the form used in thruster_mapper.launch
an excerpt is quoted here for posterity
thrusters:
FLH: {
motor_id: 0,
position: [0.2678, 0.2795, 0.0],
direction: [-0.866, 0.5, 0.0],
bounds: [-90.0, 90.0]
}
FLV: {
...
}
...
'''
self.num_thrusters = 0
rospy.init_node('thruster_mapper')
self.last_command_time = rospy.Time.now()
self.thruster_layout = wait_for_param('thruster_layout')
self.thruster_name_map = []
self.dropped_thrusters = []
self.B = self.generate_B(self.thruster_layout)
self.Binv = np.linalg.pinv(self.B)
self.min_thrusts, self.max_thrusts = self.get_ranges()
self.default_min_thrusts, self.default_max_thrusts = np.copy(self.min_thrusts), np.copy(self.max_thrusts)
self.update_layout_server = rospy.Service('update_thruster_layout', UpdateThrusterLayout, self.update_layout)
# Expose B matrix through a srv
self.b_matrix_server = rospy.Service('b_matrix', BMatrix, self.get_b_matrix)
self.wrench_sub = rospy.Subscriber('wrench', WrenchStamped, self.request_wrench_cb, queue_size=1)
self.actual_wrench_pub = rospy.Publisher('wrench_actual', WrenchStamped, queue_size=1)
self.thruster_pub = rospy.Publisher('thrusters/thrust', Thrust, queue_size=1)
def get_parameter_range(parameter_root):
'''
ex: parameter_root='/vision/buoy/red'
this will then fetch /vision/buoy/red/hsv_low and hsv_high
'''
low_param, high_param = parameter_root + '/hsv_low', parameter_root + '/hsv_high'
rospy.logwarn("Blocking -- waiting for parameters {} and {}".format(
low_param, high_param))
wait_for_param(low_param)
wait_for_param(high_param)
low = rospy.get_param(low_param)
high = rospy.get_param(high_param)
rospy.loginfo("Got {} and {}".format(low_param, high_param))
return np.array([low, high]).transpose()
def __init__(self):
'''The layout should be a dictionary of the form used in thruster_mapper.launch
an excerpt is quoted here for posterity
thrusters:
FLH: {
motor_id: 0,
position: [0.2678, 0.2795, 0.0],
direction: [-0.866, 0.5, 0.0],
bounds: [-90.0, 90.0]
}
FLV: {
...
}
...
'''
self.num_thrusters = 0
rospy.init_node('thruster_mapper')
self.last_command_time = rospy.Time.now()
self.thruster_layout = wait_for_param('thruster_layout')
self.thruster_name_map = []
self.dropped_thrusters = []
self.B = self.generate_B(self.thruster_layout)
self.Binv = np.linalg.pinv(self.B)
self.min_thrusts, self.max_thrusts = self.get_ranges()
self.default_min_thrusts, self.default_max_thrusts = np.copy(
self.min_thrusts), np.copy(self.max_thrusts)
self.update_layout_server = rospy.Service('update_thruster_layout',
UpdateThrusterLayout,
self.update_layout)
# Expose B matrix through a srv
self.b_matrix_server = rospy.Service('b_matrix', BMatrix,
self.get_b_matrix)
self.wrench_sub = rospy.Subscriber('wrench',
WrenchStamped,
self.request_wrench_cb,
queue_size=1)
self.actual_wrench_pub = rospy.Publisher('wrench_actual',
WrenchStamped,
queue_size=1)
self.wrench_error_pub = rospy.Publisher('wrench_error',
WrenchStamped,
queue_size=1)
self.thruster_pub = rospy.Publisher('thrusters/thrust',
Thrust,
queue_size=1)
def __init__(self):
'''The layout should be a dictionary of the form used in thruster_mapper.launch
an excerpt is quoted here for posterity
busses:
- port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A403IMC9-if00-port0
thrusters:
FLV: {
node_id: 10,
frame_id: /base_link,
position: [0.1583, 0.169, 0.0142],
direction: [0, 0, -1]
}
FLL: {
node_id: 11,
frame_id: /base_link,
position: [0.2678, 0.2795, 0],
direction: [-0.866, 0.5, 0]
}
- port: ....
thrusters: ....
'''
self.num_thrusters = 0
rospy.init_node('thruster_mapper')
self.last_command_time = rospy.Time.now()
self.thruster_layout = wait_for_param('busses')
self.thruster_name_map = []
self.dropped_thrusters = []
self.B = self.generate_B(self.thruster_layout)
self.min_thrusts, self.max_thrusts = self.get_ranges()
self.default_min_thrusts, self.default_max_thrusts = np.copy(self.min_thrusts), np.copy(self.max_thrusts)
self.update_layout_server = rospy.Service('update_thruster_layout', UpdateThrusterLayout, self.update_layout)
# Expose B matrix through a srv
self.b_matrix_server = rospy.Service('b_matrix', BMatrix, self.get_b_matrix)
self.wrench_sub = rospy.Subscriber('wrench', WrenchStamped, self.request_wrench_cb, queue_size=1)
self.actual_wrench_pub = rospy.Publisher('wrench_actual', WrenchStamped, queue_size=1)
self.thruster_pub = rospy.Publisher('thrusters/thrust', Thrust, queue_size=1)
def fail_thruster(self, srv):
self.alert_thruster_loss(srv.thruster_name, None)
return FailThrusterResponse()
if __name__ == '__main__':
usage_msg = "Interface to Sub8's VideoRay M5 thrusters"
desc_msg = "Specify a path to the configuration.json file containing the thrust calibration data"
parser = argparse.ArgumentParser(usage=usage_msg, description=desc_msg)
parser.add_argument(
'--configuration_path',
dest='config_path',
help=
'Designate the absolute path of the calibration/configuration json file'
)
args = parser.parse_args(rospy.myargv()[1:])
config_path = args.config_path
rospy.init_node('videoray_m5_thruster_driver')
layout_parameter = '/busses'
rospy.loginfo(
"Thruster Driver waiting for parameter, {}".format(layout_parameter))
busses = wait_for_param(layout_parameter)
if busses is None:
raise (rospy.exceptions.ROSException(
"Failed to find parameter '{}'".format(layout_parameter)))
thruster_driver = ThrusterDriver(config_path, busses)
rospy.spin()
if __name__ == '__main__':
PKG = 'sub8_videoray_m5_thruster'
usage_msg = "Interface to Sub8's VideoRay M5 thrusters"
desc_msg = "Specify a path to the configuration.json file containing the thrust calibration data"
parser = argparse.ArgumentParser(usage=usage_msg, description=desc_msg)
parser.add_argument(
'--configuration_path',
dest='config_path',
default=rospkg.RosPack().get_path(PKG) + '/config/calibration.json',
help=
'Designate the absolute path of the calibration/configuration json file'
)
args = parser.parse_args(rospy.myargv()[1:])
config_path = args.config_path
rospy.init_node('videoray_m5_thruster_driver')
layout_parameter = '/thruster_layout'
rospy.loginfo(
"Thruster Driver waiting for parameter, {}".format(layout_parameter))
thruster_layout = wait_for_param(layout_parameter)
if thruster_layout is None:
raise (rospy.exceptions.ROSException(
"Failed to find parameter '{}'".format(layout_parameter)))
thruster_driver = ThrusterDriver(config_path,
thruster_layout['thruster_ports'],
thruster_layout['thrusters'])
rospy.spin()
# So that thruster_mapper updates the B-matrix
self.update_thruster_out_alarm()
return {}
def unfail_thruster(self, srv):
''' Undoes effect of self.fail_thruster '''
self.failed_thrusters.remove(srv.thruster_name)
self.deactivated_thrusters.remove(srv.thruster_name)
self.update_thruster_out_alarm()
return {}
if __name__ == '__main__':
PKG = 'sub8_videoray_m5_thruster'
usage_msg = "Interface to Sub8's VideoRay M5 thrusters"
desc_msg = "Specify a path to the configuration.json file containing the thrust calibration data"
parser = argparse.ArgumentParser(usage=usage_msg, description=desc_msg)
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('videoray_m5_thruster_driver')
layout_parameter = '/thruster_layout'
rospy.loginfo("Thruster Driver waiting for parameter, {}".format(layout_parameter))
thruster_layout = wait_for_param(layout_parameter)
if thruster_layout is None:
raise IOError('/thruster_layout rosparam needs to be set before launching the thruster driver')
thruster_driver = ThrusterDriver(thruster_layout['thruster_ports'], thruster_layout['thrusters'])
rospy.spin()