Beispiel #1
0
    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)
Beispiel #2
0
 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()
Beispiel #3
0
 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()
Beispiel #4
0
    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)
Beispiel #5
0
    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)
Beispiel #6
0
    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)
Beispiel #7
0
    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)
Beispiel #8
0
            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()
Beispiel #9
0
 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)
Beispiel #10
0
    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()
Beispiel #11
0
 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)