def __init__(self):
self.last_image = None
self.last_image_timestamp = None
self.last_draw_image = None
self.image_sub = sub8_ros_tools.Image_Subscriber(
'/down/left/image_raw', self.image_cb)
self.image_pub = sub8_ros_tools.Image_Publisher(
"down/left/target_info")
self.occ_grid = MarkerOccGrid(self.image_sub,
grid_res=.05,
grid_width=500,
grid_height=500,
grid_starting_pose=Pose2D(x=250,
y=250,
theta=0))
self.range = sub8_ros_tools.get_parameter_range('/color/channel_guide')
self.channel_width = sub8_ros_tools.wait_for_param(
'/vision/channel_width')
self.pose_service = rospy.Service("vision/channel_marker/2D",
VisionRequest2D, self.request_pipe)
# Occasional status publisher
self.timer = rospy.Timer(rospy.Duration(.5), self.publish_target_info)
def update_layout(self, srv):
'''Update the physical thruster layout.
This should only be done in a thruster-out event
'''
self.thruster_layout = wait_for_param('busses')
self.B = self.generate_B(self.thruster_layout)
self.min_thrust, self.max_thrust = self.get_ranges()
return UpdateThrusterLayoutResponse()
def update_layout(self, srv):
'''Update the physical thruster layout.
This should only be done in a thruster-out event
'''
self.thruster_layout = wait_for_param('busses')
self.B = self.generate_B(self.thruster_layout)
self.min_thrust, self.max_thrust = self.get_ranges()
return UpdateThrusterLayoutResponse()
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.min_thrusts = np.array(
[-1.82, -1.82, -1.82, -1.82, -1.82, -1.82, -1.82,
-1.82]) #blueRobotics T100
self.max_thrusts = np.array(
[2.36, 2.36, 2.36, 2.36, 2.36, 2.36, 2.36, 2.36])
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 __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.kill_listener = AlarmListener('kill', self.kill_cb)
# Not the proper way to do this at all, but the kill listener class is being wonky right now
self.killed = True
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.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.kill_listener = AlarmListener('kill', self.kill_cb)
# Not the proper way to do this at all, but the kill listener class is being wonky right now
self.killed = True
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.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.min_thrusts = np.array([-1.82, -1.82, -1.82, -1.82, -1.82, -1.82, -1.82, -1.82]) #blueRobotics T100
self.max_thrusts = np.array([2.36, 2.36, 2.36, 2.36, 2.36, 2.36, 2.36, 2.36])
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)
parameters={
'thruster_name': thruster_name,
'fault_info': fault_info
}
)
self.failed_thrusters.append(thruster_name)
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()
def __init__(self):
# Keep some knowledge of which thrusters we have working
self.thruster_layout = wait_for_param('busses')
self.update_layout = rospy.ServiceProxy('update_thruster_layout',
UpdateThrusterLayout)
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()
def __init__(self):
# Keep some knowledge of which thrusters we have working
self.thruster_layout = wait_for_param('busses')
self.update_layout = rospy.ServiceProxy('update_thruster_layout', UpdateThrusterLayout)