示例#1
0
class ControlSystemServer(object):
    """

    here we are using rospy.Timer functionality to measure co2 every second
    https://roboticsbackend.com/how-to-use-a-ros-timer-in-python-to-publish-data-at-a-fixed-rate/
    https://roboticsbackend.com/ros-rate-roscpy-roscpp/
    but every global reconfiguration cycle works with time.localtime()
    why
    """
    def __init__(self):
        # hardcoded constants
        self._success_response = "success: "
        self._error_response = "error: "
        self._logsep = ";"

        # start node
        rospy.init_node('control_server', log_level=rospy.DEBUG)

        # get roslaunch params

        # names for self topics
        self._logname = rospy.get_param('~control_log_name', 'control_system')
        self._log_node_name = rospy.get_param('~control_log_node_name', 'control_log_node')
        self._service_name = rospy.get_param('~control_service_name', 'control_system')
        self._raw_co2_pub_name = rospy.get_param('~control_raw_co2_pub_name', 'raw_co2_pub')
        self._operator_pub_name = rospy.get_param('~control_operator_pub_name', 'operator_pub')

        # devices services names
        # self._relay_service_name = rospy.get_param('~control_relay_service_name', 'relay_device')
        self._led_service_name = rospy.get_param('~control_led_service_name', 'led_device')
        self._sba5_service_name = rospy.get_param('~control_sba5_service_name', 'sba5_device')  # we are using sba5 by default co2 sensor
        self._exp_service_name = rospy.get_param('~control_exp_service_name', 'exp_system')

        # experiment params
        self._LSM_exp_red = int(rospy.get_param('~control_LSM_exp_red', 250))  # mA old = 142
        # here we need 306 mA, but our devices can work only with Ir < 250 mA
        self._LSM_exp_white = int(rospy.get_param('~control_LSM_exp_white', 114))  # mA  old = 76
        self._LSM_control_red = int(rospy.get_param('~control_LSM_control_red', 237))  # mA
        self._LSM_control_white = int(rospy.get_param('~control_LSM_control_white', 122))  # mA
        self._full_experiment_loop_time = int(rospy.get_param('~control_full_experiment_loop_time', 900.0)) # sec
        self._isolated_measure_time = int(rospy.get_param('~control_isolated_measure_time', 480.0))  # sec
        self._n2_calibration_time = int(rospy.get_param('~control_n2_calibration_time', 90.0))  # depends on
        self._air_valves_open_time = int(rospy.get_param('~control_air_valves_open_time', 15.0))  # sec
        self._co2_measure_time = int(rospy.get_param('~control_co2_measure_time', 1.0))  # sec
        self._ventilation_time = self._full_experiment_loop_time - self._isolated_measure_time - \
            self._n2_calibration_time - self._air_valves_open_time

        # flags of current control regime
        #self._mode = rospy.get_param('~control_start_mode', 'life_support')
        self._mode = rospy.get_param('~control_start_mode', 'experiment')
        self._mode_flag = 'co2'
        # mode can be :
        # experiment
        # life_support
        # test
        # search_experiment
        # ...
        #self._sba5_measure_allowed = False  # by default
        # self._at_work = rospy.get_param('~control_start_at_work', True)
        # start node and loop immediately after launch or

        # rpi_gpio device stub
        # TODO add this params to .launch file
        self._gpio_handler = RelayHandler(
            n2_valve=5,
            air_pump_1=6,
            air_pump_2=12,
            cooler_1=13,
            air_valve_1=19,
            air_valve_2=26,
            ndir_pump=16,
            empty=20
        )


        # params of search experiment
        self._co2_search_time_start = 0
        self._co2_search_time_stop = 0
        self._current_search_point_id = 0
        self._led_delay_time = 0  # for setting up leds in selected time in vent period
        # by default:
        self._current_red = self._LSM_exp_red
        self._current_white = self._LSM_exp_white
        self._current_search_is_finished = 1  # by default it is not finished



        # create log topic publisher
        self._log_pub = rospy.Publisher(self._log_node_name, String, queue_size=10)
        # create data topic publisher
        self._co2_pub = rospy.Publisher(self._raw_co2_pub_name, Temperature, queue_size=10)
        # create operator-topic to send important msgs to operator
        # self._operator_pub = rospy.Publisher(self._operator_pub_name, String, queue_size=10)

        # logger
        self._logger = CustomLogger(name=self._logname, logpub=self._log_pub)
        self._logger.info("control_server init")

        # create Locks
        self._co2_measure_allowed_event = Event()

        # serial errors handling
        # self._last_serial_error_time = 0
        # self._delay_after_serial_error = 300



        self._logger.info("control_server service creation")

        # service
        self._service = rospy.Service(self._service_name, ControlSystem, self._handle_request)

        self._logger.info("check if we are in experiment mode")


        # self._serial_error_counter = 0
        # self._serial_error_max = 5
        # subscribe to rosout_agg to know if there is fatal error and we need to do smth
        # self._system_log_sub = rospy.Subscriber(
        #     name='/rosout_agg', data_class=Log,
        #     callback=self._log_callback,
        #     queue_size=5)



        # reinit sba5
        if self._mode == 'full_experiment' or self._mode == 'full_experiment':
            self._logger.info("update sba5 and allow sba5 measures")
            self._update_sba5_params()

            # allow measures of sba5
            self._co2_measure_allowed_event.set()

            # self._default_red = 142
            # self._default_white = 76

            self._current_red = self._LSM_exp_red
            self._current_white = self._LSM_exp_white

            # create timers for async periodic tasks using internal ros mechanics
            # Create a ROS Timer for reading data
            rospy.Timer(rospy.Duration(1), self._get_sba5_measure)  # 1 Hz
            # create ros timer for main loop

        if self._mode == 'life_support_dual':
            self._dual_led_service_name = rospy.get_param('~control_dual_led_service_name')
            self._current_red = self._LSM_control_red
            self._current_white = self._LSM_control_white

        if self._mode == 'k30_experiment':
            self._k30_service_name = rospy.get_param('~k30_service_name', 'k30_device')
            self._k30_calibration_time = int(rospy.get_param('~k30_calibration_time', '25'))
            self._current_red = self._LSM_control_red
            self._current_white = self._LSM_control_white

            rospy.Timer(rospy.Duration(2), self._get_k30_data)  # 0.5 Hz


        self._logger.info("go to loop")
        self._loop()

    # ===================== loops for different control modes ======================

    def _loop(self):
        while not rospy.is_shutdown():
            # check if mode was changed

            if self._mode == 'life_support':
                self._life_support_loop()
            elif self._mode == 'life_support_dual':
                self._life_support_dual_loop()
            elif self._mode == 'test':
                self._test_loop()
            elif self._mode == 'full_experiment':
                self._full_experiment_loop()
            elif self._mode == 'k30_experiment':
                self._k30_experiment_loop()
            else:
                self._logger.error("current mode is not real mode: {}".format(self._mode))
                rospy.sleep(1)

    def _test_loop(self):
        t = time.localtime()
        # every 15 minutes by default
        if t.tm_min % (self._full_experiment_loop_time / 60.0) == 0:
            self._logger.debug("start test loop again")
            rospy.sleep(self._air_valves_open_time)
            self._logger.debug("we have done nothing, really")

    def _life_support_loop(self):
        # one loop

        t = time.localtime()
        # every 15 minutes by default
        if t.tm_min % (self._full_experiment_loop_time / 60.0) == 0:
            self._logger.debug("start life support loop again")
            # start it again
            # reset light parameters to default
            self._logger.info('red = {} white = {} type = {}'.format(
                self._current_red, self._current_white, type(self._current_red)))
            self._set_new_light_mode(self._current_red, self._current_white)
            # set inside ventilation coolers on
            self._set_new_relay_state('set_vent_coolers', 0)
            # start ventilation
            self._start_ventilation()
            # self._set_new_relay_state('set_ndir_pump', 0)  # for test only
            # wait for self._ventilation_time
            rospy.sleep(self._ventilation_time)  # its not bad because service calls works in parallel threads
            # then stop it
            self._stop_ventilation()
            # self._set_new_relay_state('set_ndir_pump', 1)  # for test only
            # then wait and do nothing

    def _life_support_dual_loop(self):
        # one loop

        t = time.localtime()
        # every 15 minutes by default
        if t.tm_min % (self._full_experiment_loop_time / 60.0) == 0:
            self._logger.debug("start life support dual loop again")
            # start it again
            # reset light parameters to default
            self._logger.info('red = {} white = {} type = {}'.format(
                self._current_red, self._current_white, type(self._current_red)))
            self._set_new_light_mode(self._current_red, self._current_white)
            self._set_new_light_mode(self._current_red, self._current_white, led_service_name=
                                     self._dual_led_service_name)
            # set inside ventilation coolers on
            self._set_new_relay_state('set_vent_coolers', 0)
            # start ventilation
            self._start_ventilation()
            # self._set_new_relay_state('set_ndir_pump', 0)  # for test only
            # wait for self._ventilation_time
            rospy.sleep(self._ventilation_time)  # its not bad because service calls works in parallel threads
            # then stop it
            self._stop_ventilation()
            # self._set_new_relay_state('set_ndir_pump', 1)  # for test only
            # then wait and do nothing

    def _full_experiment_loop(self):
        # one loop
        # all experiment
        t = time.localtime()
        # every 15 minutes by default
        if t.tm_min % (self._full_experiment_loop_time/60.0) == 0:
            # start it again
            self._logger.info("start full experiment loop again")
            self._logger.info("start full experiment loop again")
            # self._get_current_point()

            # set inside ventilation coolers on
            self._set_new_relay_state('set_vent_coolers', 0)
            # start ventilation and calibration
            self._start_ventilation()
            self._logger.info("ventilation started")
            # get new led light params from exp_node
            self._get_current_point()

            self._logger.info("we got new exp point: mode={} id={} red={} white={}".format(
                self._mode_flag, self._current_search_point_id, self._current_red, self._current_white
            ))
            self._logger.info("we got new exp point: mode={}  id={} red={} white={}".format(
                self._mode_flag, self._current_search_point_id, self._current_red, self._current_white
            ))

            # check new mode flag
            if self._mode_flag == 'co2':  # TODO FIX
                # stop measuring co2 using threading event
                self._co2_measure_allowed_event.clear()
                self._logger.info("We have set measure flag to {}".format(self._co2_measure_allowed_event.is_set()))
                # do calibration of sba-5
                self._logger.info("sba5 calibration started")
                self._perform_sba5_calibration()
                self._logger.info("sba5 calibration ended")
                # start measuring co2 again
                self._co2_measure_allowed_event.set()
                self._logger.info("We have set measure flag to {}".format(self._co2_measure_allowed_event.is_set()))
            #
            # self._co2_search_time_start = rospy.Time.now()
            # # send sign to operator
            #  self._logger.info("co2_search_time started {}".format(self._co2_search_time_start))

            # wait for self._ventilation_time
            rospy.sleep(self._led_delay_time)

            self._set_new_light_mode(self._current_red, self._current_white)

            rospy.sleep(self._ventilation_time - self._led_delay_time)


            # stop ventilation
            self._stop_ventilation()
            self._logger.info("stop ventilation")

            self._co2_search_time_start = rospy.Time.now()
            # send sign to operator

            ts = datetime.datetime.fromtimestamp(
            self._co2_search_time_start.to_sec()).strftime('%Y_%m_%d %H:%M:%S')
            self._logger.info("co2_search_time started {}".format(ts))
            self._logger.info("co2_search_time started {}".format(ts))
            # wait self._isolated_measure_time
            rospy.sleep(self._isolated_measure_time)

            self._co2_search_time_stop = rospy.Time.now()

            te = datetime.datetime.fromtimestamp(
            self._co2_search_time_start.to_sec()).strftime('%Y_%m_%d %H:%M:%S')

            self._logger.info("co2_search_time stopped {}".format(te))
            self._logger.info("co2_search_time stopped {}".format(te))
            # send start and stop times of this search point to exp_node
            self._send_point_data()
            self._logger.info("data sent to exp_system")

    def _k30_experiment_loop(self):
        # one loop
        # all experiment
        t = time.localtime()
        # every 15 minutes by default
        if t.tm_min % (self._full_experiment_loop_time/60.0) == 0:
            # start it again
            self._logger.info("start k30 experiment loop again")
            self._logger.info("start k30 experiment loop again")
            # self._get_current_point()

            # set inside ventilation coolers on
            self._set_new_relay_state('set_vent_coolers', 0)
            # start ventilation and calibration
            self._start_ventilation()
            self._logger.info("ventilation started")
            # get new led light params from exp_node
            self._get_current_point()

            self._logger.info("we got new exp point: mode={} id={} red={} white={}".format(
                self._mode_flag, self._current_search_point_id, self._current_red, self._current_white
            ))
            self._logger.info("we got new exp point: mode={}  id={} red={} white={}".format(
                self._mode_flag, self._current_search_point_id, self._current_red, self._current_white
            ))
            self._set_new_light_mode(self._current_red, self._current_white)
            # check new mode flag
            if self._mode_flag == 'co2':  # TODO FIX
                # stop measuring co2 using threading event
                self._co2_measure_allowed_event.clear()
                self._logger.info("We have set measure flag to {}".format(self._co2_measure_allowed_event.is_set()))
                # do calibration of sba-5
                self._logger.info("k30 calibration started")
                self._perform_k30_calibration()
                self._logger.info("k30 calibration ended")
                # start measuring co2 again
                self._co2_measure_allowed_event.set()
                self._logger.info("We have set measure flag to {}".format(self._co2_measure_allowed_event.is_set()))

            # wait for self._ventilation_time
            rospy.sleep(self._ventilation_time)
            # stop ventilation
            self._stop_ventilation()
            self._logger.info("stop ventilation")

            self._co2_search_time_start = rospy.Time.now()
            # send sign to operator

            ts = datetime.datetime.fromtimestamp(
                self._co2_search_time_start.to_sec()).strftime('%Y_%m_%d %H:%M:%S')
            self._logger.info("co2_search_time started {}".format(ts))
            self._logger.info("co2_search_time started {}".format(ts))
            # wait self._isolated_measure_time
            rospy.sleep(self._isolated_measure_time)

            self._co2_search_time_stop = rospy.Time.now()

            te = datetime.datetime.fromtimestamp(
                self._co2_search_time_start.to_sec()).strftime('%Y_%m_%d %H:%M:%S')

            self._logger.info("co2_search_time stopped {}".format(te))
            self._logger.info("co2_search_time stopped {}".format(te))
            # send start and stop times of this search point to exp_node
            self._send_point_data()
            self._logger.info("data sent to exp_system")

    # =============================== support methods ==============================

    def _operator_call(self, msg):
        # just sends msg to operator topic
        _time = datetime.datetime.now()
        msg = str(_time) + self._logsep + str(self._logname) + self._logsep + msg
        self._operator_pub.publish(msg)

    # def _log_callback(self, log_msg):
    #
    #     # hardcoded thing just to handle serial errors
    #     if not time.time() - self._last_serial_error_time >= self._delay_after_serial_error:
    #         if log_msg.name == '/serial_node' and log_msg.level == 8:
    #             self._logger.warning("we got serial error {}, but we will ignore it for a time".format(log_msg))
    #     else:
    #         if log_msg.name == '/serial_node' and log_msg.level == 8:
    #             self._logger.error("we got serial error {}".format(log_msg))
    #             self._serial_error_counter += 1
    #             if self._serial_error_counter >= self._serial_error_max:
    #                 self._logger.error("max number of serial error counted: {}".format(self._serial_error_counter))
    #                 self._restart_serial_node()
    #
    #                 self._serial_error_counter = 0
    #             self._last_serial_error_time = time.time()

    def _get_k30_data(self, event=None):
        # if self._sba5_measure_allowed:
        # event is rospy.TimerEvent
        if self._mode_flag == 'co2':  # TODO FIX
            self._co2_measure_allowed_event.wait()
            rospy.wait_for_service(self._sba5_service_name)
            try:
                # sba_device = rospy.ServiceProxy(self._sba5_service_name, SBA5Device)
                # raw_resp = sba_device("measure_co2")
                k30_device = rospy.ServiceProxy(self._k30_service_name, K30Device)
                raw_resp = k30_device("get_data")
                # self._logger.debug("We got raw response from sba5 : {}".format(raw_resp.response))
                # self._logger.debug("Type of raw response : {}".format(type(raw_resp.response)))
                # self._logger.debug("Size of raw response : {}".format(len(raw_resp.response)))
                # lets get co2 measure from that string
                # pattern = re.compile(r'\w+: (\d+.\d+)')  # for answers like "success: 55.21"
                # co2_data = float(pattern.findall(raw_resp.response)[0])

                co2_data = float(raw_resp)


                self._logger.debug("k30 measure_co2: We find co2 : {}".format(co2_data))
                # add to self array
                # self._add_new_data_to_array(co2_data)

                # then publish it
                self._publish_sba5_measure(co2_data)
                # self._logger.debug("We published it")

                return float(co2_data)

            except Exception as e:
                exc_info = sys.exc_info()
                err_list = traceback.format_exception(*exc_info)
                self._logger.warning("Service call failed: {}".format(err_list))

    def _perform_k30_calibration(self):
        self._logger.debug("start k30 recalibration")
        # stop NDIRGA external air pump
        self._set_new_relay_state('set_ndir_pump', 1)
        # open n2 valve
        self._set_new_relay_state('set_n2_valve', 0)
        # send Z to sba-5
        # rospy.wait_for_service(self._sba5_service_name)
        try:
            #  wait until nitrogen fills the volume
            rospy.sleep(self._k30_calibration_time-5)  # TODO fix it
            k30_device = rospy.ServiceProxy(self._k30_service_name, K30Device)
            raw_resp = k30_device('recalibrate')
            self._logger.debug("We got raw response from k30 {}".format(raw_resp))

        except Exception as e:
            exc_info = sys.exc_info()
            err_list = traceback.format_exception(*exc_info)
            self._logger.error("Service call failed: {}".format(err_list))
            # raise ControlSystemException(e)

        # close n2 valve
        self._set_new_relay_state('set_n2_valve', 1)
        # start NDIRGA external air pump
        self._set_new_relay_state('set_ndir_pump', 0)

    # def _restart_serial_node(self):
    #     # we just need to kill serial node
    #     # master must rebirth it
    #
    #     # self._logger.warning("trying to kill relay node")
    #
    #     # os.system("rosnode kill /relay_device")
    #     # time.sleep(1.5)
    #
    #     self._logger.warning("trying to kill serial node")
    #
    #     os.system("rosnode kill /serial_node")
    #
    #     self._logger.warning("waiting for serial node respawn")
    #
    #     time.sleep(1.5)
    #
    #     # then check if it was created again
    #     serial_node_found = False
    #     # relay_device_found = False
    #     # while not serial_node_found and relay_device_found:
    #     while not serial_node_found:
    #         nodes = os.popen("rosnode list").readlines()
    #         for i in range(len(nodes)):
    #             nodes[i] = nodes[i].replace("\n", "")
    #             if nodes[i] == "/serial_node":
    #                 serial_node_found = True
    #                 self._logger.warning("found serial node in rosnode list")
    #
    #             # if nodes[i] == "/relay_device":
    #             #     relay_device_found = True
    #             #     self._logger.warning("found relay_device in rosnode list")
    #         time.sleep(0.5)

        # time.sleep(5)

        # # then send respawn signal to relay device to restore relay state on mcu
        # self._logger.warning("send respawn signal to relay")
        # rospy.wait_for_service(self._relay_service_name)
        # try:
        #     relay_wrapper = rospy.ServiceProxy(self._relay_service_name, RelayDevice)
        #     resp = relay_wrapper("respawn", 1)
        #     self._logger.debug(resp)
        #     return resp
        #
        # except rospy.ServiceException, e:
        #     exc_info = sys.exc_info()
        #     err_list = traceback.format_exception(*exc_info)
        #     self._logger.error("Service call failed: {}".format(err_list))


    def _send_point_data(self):
        self._logger.info("try to send data about current search point")
        rospy.wait_for_service(self._exp_service_name)
        try:
            exp_device = rospy.ServiceProxy(self._exp_service_name, ExpSystem)
            resp = exp_device(command="set_point_data",
                              point_id=self._current_search_point_id,
                              start_time=self._co2_search_time_start,
                              end_time=self._co2_search_time_stop
                              )
            self._logger.debug(resp)
            return resp

        except Exception as e:
            exc_info = sys.exc_info()
            err_list = traceback.format_exception(*exc_info)
            self._logger.error("Service call failed: {}".format(err_list))

    def _get_current_point(self):
        self._logger.info("try to get new search point")
        rospy.wait_for_service(self._exp_service_name, 1)  # NOTE check this place
        try:
            exp_device = rospy.ServiceProxy(self._exp_service_name, ExpSystem)
            resp = exp_device(command="get_current_point")
            self._mode_flag = resp.response
            self._current_search_point_id = resp.point_id
            self._current_red = resp.red
            self._current_white = resp.white
            self._led_delay_time = resp.led_delay
            self._logger.info(resp)
            return resp

        except Exception as e:
            exc_info = sys.exc_info()
            err_list = traceback.format_exception(*exc_info)
            self._logger.error("Service call failed: {}".format(err_list))

    # def _update_control_params(self):
    #     # get new params and set them to corresponding self.xxxx values
    #     # NOTE: method is DEPRECATED
    #     self._set_new_light_mode(self._current_red, self._current_white)  # for a time

    def _start_ventilation(self):
        # open drain valves  'set_air_valves'
        self._set_new_relay_state('set_air_valves', 0)
        # wait time to open them ~15 sec
        rospy.sleep(self._air_valves_open_time)
        # start drain pumps 'set_air_pumps'
        self._set_new_relay_state('set_air_pumps', 0)
        pass

    def _stop_ventilation(self):
        # stop drain pumps
        self._set_new_relay_state('set_air_pumps', 1)
        # wait 1 sec
        rospy.sleep(1.0)
        # close drain valves
        self._set_new_relay_state('set_air_valves', 1)
        pass

    def _perform_sba5_calibration(self):

        self._logger.debug("start sba recalibration")
        # stop NDIRGA external air pump
        self._set_new_relay_state('set_ndir_pump', 1)
        # open n2 valve
        self._set_new_relay_state('set_n2_valve', 0)
        # send Z to sba-5
        rospy.wait_for_service(self._sba5_service_name)
        try:
            sba_device = rospy.ServiceProxy(self._sba5_service_name, SBA5Device)
            raw_resp = sba_device('recalibrate')
            self._logger.debug("We got raw response from sba5 {}".format(raw_resp))
            # wait for calibration time 21/40/90 sec
            rospy.sleep(self._n2_calibration_time)

        except Exception as e:
            exc_info = sys.exc_info()
            err_list = traceback.format_exception(*exc_info)
            self._logger.error("Service call failed: {}".format(err_list))
            # raise ControlSystemException(e)

        # close n2 valve
        self._set_new_relay_state('set_n2_valve', 1)
        # start NDIRGA external air pump
        self._set_new_relay_state('set_ndir_pump', 0)
        pass

    def _publish_sba5_measure(self, data):
        self._logger.debug("publish_sba5_measure: We got data: {}".format(data))
        co2_msg = Temperature()
        co2_msg.header.stamp = rospy.Time.now()
        co2_msg.temperature = data
        self._logger.debug("Message: {}".format(co2_msg))
        self._co2_pub.publish(co2_msg)

    def _set_new_relay_state(self, command, state):
        # command - str, state - 0 or 1
        self._logger.info("start setting new relay mode '{}' '{}' ".format(command, state))
        # rospy.wait_for_service(self._relay_service_name)
        try:
        #     relay_wrapper = rospy.ServiceProxy(self._relay_service_name, RelayDevice)
        #     resp = relay_wrapper(command, state)
        #     self._logger.debug(resp)
        #     return resp

            self._gpio_handler.parse_old_command(command=command, arg=state)

        except Exception, e:
            exc_info = sys.exc_info()
            err_list = traceback.format_exception(*exc_info)
            self._logger.error("Service call failed: {}".format(err_list))
class SI7021DeviceServer(object):
    """

    """
    def __init__(self):

        # start node
        rospy.init_node('si7021_device_server', log_level=rospy.DEBUG)
        # get roslaunch params and reinit part of params
        self._logname = rospy.get_param('~si7021_log_name', 'si7021')
        self._lost_data_marker = rospy.get_param('~si7021_lost_data_marker',
                                                 -65536)
        self._log_node_name = rospy.get_param('~si7021_log_node_name',
                                              'si7021_log_node')
        self._hum_pub_name = rospy.get_param('~si7021_hum_pub_name',
                                             'si7021_1_hum_pub')
        self._temp_pub_name = rospy.get_param('~si7021_temp_pub_name',
                                              'si7021_1_temp_pub')

        self._timeout = rospy.get_param('~si7021_timeout', 1)
        self._measure_interval = rospy.get_param('~si7021_measure_interval')

        # create log topic publisher
        self._log_pub = rospy.Publisher(self._log_node_name,
                                        String,
                                        queue_size=10)

        self._hum_pub = rospy.Publisher(self._hum_pub_name,
                                        Temperature,
                                        queue_size=10)
        self._temp_pub = rospy.Publisher(self._temp_pub_name,
                                         Temperature,
                                         queue_size=10)

        # logger
        self._logger = CustomLogger(name=self._logname, logpub=self._log_pub)
        self._logger.debug("si7021_device_server init")
        # print("we here init")

        # and go to infinite loop
        # self._service = rospy.Service(self._service_name, SBA5Device, self.handle_request)
        self._loop()

    def _loop(self):
        while not rospy.is_shutdown():
            rospy.sleep(self._measure_interval)
            try:
                temp, hum = get_si7021_data()

                msg = Temperature()
                msg.temperature = temp
                msg.header.stamp.secs = rospy.get_time()
                self._temp_pub.publish(msg)

                msg = Temperature()
                msg.temperature = hum
                msg.header.stamp.secs = rospy.get_time()
                self._hum_pub.publish(msg)
            except Exception as e:
                msg = Temperature()
                msg.temperature = self._lost_data_marker
                msg.header.stamp.secs = rospy.get_time()
                self._temp_pub.publish(msg)

                self._hum_pub.publish(msg)
示例#3
0
class K30DeviceServer:
    """

    """
    def __init__(self):
        # hardcoded constants
        self._success_response = "success: "
        self._error_response = "error: "

        # start node
        rospy.init_node('k30_device_server', log_level=rospy.DEBUG)
        self._logname = rospy.get_param('~k30_log_name', 'k30')
        self._log_node_name = rospy.get_param('~k30_log_node_name',
                                              'k30_log_node')
        self._port = rospy.get_param(
            '~k30_port', '/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_'
            'UART_Bridge_Controller_0003-if00-port0')
        self._baudrate = rospy.get_param('~k30_baudrate', 9600)
        self._timeout = rospy.get_param('~k30_timeout', 1)
        self._service_name = rospy.get_param('~k30_service_name', 'k30_device')
        self._calibration_time = rospy.get_param('~k30_calibration_time', 25)
        self._lost_data_marker = rospy.get_param('~k30_lost_data_marker',
                                                 -65536)

        # create log topic publisher
        self._log_pub = rospy.Publisher(self._log_node_name,
                                        String,
                                        queue_size=10)

        # logger
        self._logger = CustomLogger(name=self._logname, logpub=self._log_pub)
        self._logger.debug("k30_device_server init")

        # device
        self._device = K30(devname=self._port,
                           baudrate=self._baudrate,
                           timeout=self._timeout)

        # service
        self._service = rospy.Service(self._service_name, K30Device,
                                      self.handle_request)
        self._loop()

    def _loop(self):
        rospy.spin()

    def handle_request(self, req):
        self._logger.debug("we got request: {}".format(req))

        if not req.command:
            # if we got empty string
            resp = self._error_response + 'empty_command'
            return resp

        elif req.command == 'get_info':
            try:
                ans = self._device.get_info()
                resp = self._success_response + ans
                return resp
            except Exception as e:
                resp = self._error_response + e.args[0]
                return resp

        elif req.command == 'get_data':
            try:
                co2, ans = self._device.get_co2()
                # resp = self._success_response + ans
                return str(co2)
            except Exception as e:
                # resp = self._error_response + e.args[0]
                co2 = self._lost_data_marker
                return str(co2)

        elif req.command == 'recalibrate':
            try:
                ans = self._device.recalibrate(ctime=5)
                resp = self._success_response + ans
                return resp
            except Exception as e:
                resp = self._error_response + e.args[0]
                return resp
示例#4
0
class MYSQLHandler(object):
    def __init__(self):
        print("========== start init")

        # start node
        rospy.init_node('mysql_data_plotter_server',
                        log_level=rospy.DEBUG,
                        anonymous=True)

        # self logging
        self._logname = rospy.get_param('~mysql_data_plotter_log_name',
                                        'mysql_data_plotter')
        self._log_node_name = rospy.get_param(
            '~mysql_data_plotter_log_node_name', 'mysql_data_plotter_log_node')

        # create log topic publisher
        self._log_pub = rospy.Publisher(self._log_node_name,
                                        String,
                                        queue_size=10)

        # logger
        self._logger = CustomLogger(name=self._logname, logpub=self._log_pub)
        self._logger.debug("mysql_data_plotter_server init")

        # for default we will use current ip in tun0 protocol
        # if there is no any tun0 - it must be a critical error

        self.ipv4 = os.popen('ip addr show tun0').read().split(
            "inet ")[1].split("/")[0]

        self.web_addr = self.ipv4
        self.web_port = 8090  # on every rpi
        self.list_of_hdf = list()
        self.metastring = "experiment metadata: \n"

        # TODO load description from experiments table and add to metastring
        # self.list_of_datasets =

        print("========== start get list of datasets")

        # TODO; fix -all below only for test  ||||
        #                            vvvv
        self.list_of_tables = rospy.get_param('~mysql_data_plotter_raw_topics')
        # self.list_of_tables = [  # TODO must be loaded from .launch
        #     {'name': '/bmp180_1_temp_pub', 'id': 1, 'type': 'temperature', 'dtype': 'float64', 'units': 'C', 'status': 'raw_data'},
        #     {'name': '/bmp180_1_pressure_pub', 'id': 2, 'type': 'pressure', 'dtype': 'float64', 'units': 'kPa', 'status': 'raw_data'},
        #     {'name': '/raw_co2_pub', 'type': 'co2', 'id': 3, 'dtype': 'float64', 'units': 'ppmv', 'status': 'raw_data'},
        #     {'name': '/si7021_1_hum_pub', 'id': 4, 'type': 'humidity', 'dtype': 'float64', 'units': 'percents', 'status': 'raw_data'},
        #     {'name': '/si7021_1_temp_pub', 'id': 5, 'type': 'temperature', 'dtype': 'float64', 'units': 'C', 'status': 'raw_data'}
        # ]

        print("========== end of init")

    def get_dataset(self, name):
        print("========== start get dataset {}".format(name))

        info_dict = None

        # lets find dict with same name as user wants to see
        for i in self.list_of_tables:
            if i['name'] == name:
                info_dict = i

        # dirty
        meta = info_dict

        # table name, where data located must be placed in field "status"
        if info_dict['status'] == 'raw_data':
            # if raw, it must be in raw_data table and must be defined by sensor_id
            sensor_id = info_dict['id']

            con = pymysql.connect(
                host='localhost',
                user='******',
                password='******',
                # db='experiment',
                charset='utf8mb4',
                cursorclass=pymysql.cursors.DictCursor)

            cur = con.cursor()

            cur.execute("use experiment")

            comm_str = 'select data, time from raw_data where sensor_id = {}'.format(
                sensor_id)

            self._logger.debug("comm_str: {}".format(comm_str))

            cur.execute(comm_str)

            res_list = cur.fetchall()  # it must be array of dicts
            # where each dict is one line from db

            con.close()

            print("========== end of get dataset")
            return res_list, meta
class RegressionTestScript(object):
    _CREDENTIAL = './conf/credential.json'
    _REGRESSION_SHEET_NAME = 'GRP__RegressionTesting'
    _CFS_SHEET_NAME = 'GRP__Component-Feature-Service'
    _SINGLE_REGRESSION = "\n"
    _TEST_COUNT = 0
    _FAILED_COUNT = 0

    def __init__(self, product, service, zuul_url, zuul_route):
        self._PRODUCT = product
        self._SERVICE = service
        self._BASE_PATH = os.path.expanduser('~') + "/Documents/GRP"
        now = datetime.now()
        log_dir = "/var/log/grp/regression-test-automation/%s/%s/%s" % \
                  (self._PRODUCT, self._SERVICE, '{0:%Y-%m-%d__%H.%M.%S}'.format(now))
        self.log = CustomLogger(log_dir).get_logger()
        self.log.info("Script starts executes at : {0:%Y-%m-%d %H:%M:%S}\n".format(now))
        pcfs = service.split('/')
        if len(pcfs) != 4:
            sys.exit("Invalid service : %s\nPlease enter a valid service." % service)
        self._COMPONENT = pcfs[0]
        self._FEATURE = pcfs[1]
        self._VERSION = pcfs[2]
        self._SERVICE_NAME = pcfs[3]
        self._ZUUL_URL = zuul_url
        self._ZUUL_ROUTE = zuul_route

    def set_up(self):
        self.log.info("Setup environment variables.")
        self.g = pygsheets.authorize(service_file=self._CREDENTIAL)

        self.log.debug("\n_PRODUCT = %s\n_SERVICE = %s\n_CREDENTIAL = %s\n_REGRESSION_SHEET_NAME = %s\n"
                       "_CFS_SHEET_NAME = %s\ng = %s\n" %
                       (self._PRODUCT, self._SERVICE, self._CREDENTIAL, self._REGRESSION_SHEET_NAME,
                        self._CFS_SHEET_NAME, str(self.g)))

    def load_regression_sheet(self):
        self.log.debug("Start loading data from regression sheet.")
        worksheet = self.g.open(self._REGRESSION_SHEET_NAME)
        worksheet = worksheet.worksheet('title', self._PRODUCT)
        worksheet = worksheet.get_all_values()
        self.log.debug("Worksheet data : " + str(worksheet))

        data = list(filter(lambda x: self._SERVICE.strip() == x[2].strip(), worksheet))
        if len(data) == 0:
            self.log.error("Service not found. Product : %s, Service : %s" % (self._PRODUCT, self._SERVICE))
            sys.exit("Service not found")

        self.regression_data = data
        print(data)
        self.log.debug("Regression test data : " + str(self.regression_data))
        self.log.info("Loading complete.\n")

    def create_base_directory(self):
        self.log.debug("Start creating base directory for test script.")
        product_dir = self._BASE_PATH + "/" + self._PRODUCT + "-service" + "/tests/regression"
        if not os.path.exists(product_dir):
            self.log.error("directory not found : %s", product_dir)
            sys.exit(-1)
        self.product_dir = product_dir
        service = self._SERVICE.split("/")
        self.directory = self._BASE_PATH + "/" + self._PRODUCT + "-service/tests/regression/" + service[0] + \
                         "-service/python/" + service[0] + "-" + service[1] + "-" + service[2] + "/" + service[3] + "/"
        self.log.info("Test script directory : %s" % self.directory)
        try:
            if not os.path.exists(self.directory):
                os.makedirs(self.directory)
            if not os.path.exists(self.directory + "data/"):
                os.makedirs(self.directory + "data/")
            if not os.path.exists(self.directory + "script/"):
                os.makedirs(self.directory + "script/")
        except Exception as e:
            self.log.error("Error when creating directory %s : %s", self.directory, str(e))
            sys.exit(-1)
        self.log.debug("Directory created : \n1 : %s\n2 : %sdata\n3 : %sscript\n" % (
            self.directory, self.directory, self.directory))

    def get_service_request_response(self):
        self.log.info("Start loading request and response json.")
        rows = self.g.open(self._CFS_SHEET_NAME).worksheet('title', self._PRODUCT).get_all_values()
        component_col_no = rows[3].index("component")
        feature_col_no = rows[3].index("feature")
        version_col_no = rows[3].index("version")
        service_col_no = rows[3].index("service")
        data = list(filter(lambda x: self._COMPONENT == x[component_col_no] and self._FEATURE == x[feature_col_no]
                        and self._VERSION == x[version_col_no] and self._SERVICE_NAME == x[service_col_no], rows))
        if len(data) == 0:
            print("Service not defined in CFS sheet : %s" % self._SERVICE)
            sys.exit(-1)

        data = data[0]
        try:
            request_json = json.loads(data[rows[3].index("test-request-json")])
            response_json = json.loads(data[rows[3].index("test-response-json")])
            request_json['header']['requestId'] = None
            request_json['header']['requestTime'] = None
            request_json['header']['requestType'] = None
            request_json['header']['requestSource'] = "python-regression"
            request_json['header']['requestSourceService'] = "regression-script"
            self.log.debug("Request json : \n%s", json.dumps(request_json, indent=4))
            self.log.debug("Response json : \n%s", json.dumps(response_json, indent=4))
            with open(self.directory + "data/request.json", "w") as f:
                f.write(json.dumps(request_json, sort_keys=False, indent=4))
            with open(self.directory + "data/response.json", "w") as f:
                f.write(json.dumps(response_json, sort_keys=False, indent=4))
        except Exception as e:
            self.log.error("Error when loading request and response json : %s", str(e))
            sys.exit(-1)
        self.log.info("Loading complete.\n")

    def generate_basetest(self):
        with open("./templates/basetest.txt", "r") as r:
            with open(self.directory + "script/basetest.py", "w") as w:
                w.write(r.read())
        self.log.info("File created : basetest.py ")

    def generate_run_regression(self):
        with open("./templates/run_regression.txt", "r") as r:
            with open(self.directory + "run_regression.sh", "w") as w:
                w.write(r.read().replace("<ZUUL_URL>", self._ZUUL_URL).replace("<ZUUL_ROUTE>", self._ZUUL_ROUTE)
                        .replace("<SERVICE>", self._SERVICE).replace("<PRODUCT>", self._PRODUCT).replace("<COMPONENT>", self._COMPONENT))
        self.log.info("File created : run_regression.sh ")

    def generate_run_regression_single(self):
        with open("./templates/run_regression_single.txt", "r") as r:
            with open(self.directory + "run_regression_single.sh", "w") as w:
                w.write(r.read().replace("<ZUUL_URL>", self._ZUUL_URL).replace("<ZUUL_ROUTE>", self._ZUUL_ROUTE)
                        .replace("<SERVICE>", self._SERVICE))
        self.log.info("File created : run_regression_single.sh\n")

    def create_environment(self):
        self.log.debug("Start creating environment.")
        # with open("./conf/port.json", "r") as r:
        #     ports = json.loads(r.read())
        #     self.port = ports[self._PRODUCT]
        #     if not self.port:
        #         self.log.error("Port not found for service : %s", self._SERVICE)
        #         sys.exit(-1)
        # self.log.info("Service Port : " + self.port)
        self.create_base_directory()
        self.get_service_request_response()
        self.generate_basetest()
        self.generate_run_regression()
        self.generate_run_regression_single()
        self.log.debug("Environment created.\n")

    def register_script(self, file, attribute, type):
        self._SINGLE_REGRESSION += "#./%s ${URL} ${SERVICE} ${TOKEN}\n" % file

    def create_request_null_test_script(self, attribute):
        self._TEST_COUNT += 1
        file = "service_test_request_null_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        with open("./templates/request_null.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                w.write(r.read().replace("<attribute>", attribute))
        self.register_script(file, attribute, "null")
        self.log.info("File created : %s", file)

    def create_request_empty_test_script(self, attribute):
        self._TEST_COUNT += 1
        file = "service_test_request_empty_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        with open("./templates/request_empty.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                w.write(r.read().replace("<attribute>", attribute))
        self.register_script(file, attribute, "empty")
        self.log.info("File created : %s", file)

    def create_request_wrongtype_test_script(self, attribute, type):
        if type == 'str':
            type = "\"\""
        elif type == 'double':
            type = "1.00"
        elif type == '[]' or type == 'array':
            type = "[]"
        elif type == '{}' or type == 'object':
            type = "{}"
        elif type == 'boolean':
            type = "True"
        elif type == 'int':
            type = "1"
        else:
            print("Type not found :", type)

        self._TEST_COUNT += 1
        file = "service_test_request_wrongtype_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        with open("./templates/request_wrongtype.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                w.write(r.read().replace("<attribute>", attribute).replace("<type>", type))
        self.register_script(file, attribute + " data type", "wrong")
        self.log.info("File created : %s", file)

    def request_wronglength_formater(self, attribute, length):
        len_dic = {"condition": "<length_condition>", "min": "1", "max": "20"}
        if not length:
            self.log.error("Pattern not define of wronglength for request attribute : %s", attribute)
            self._FAILED_COUNT += 1
            return len_dic
        length = length.strip()

        if re.match(r'^[0-9]+$', length):
            min_max = [int(length) - 8, int(length) + 8]
            len_dic["condition"] = "len(suffix) != " + length
        elif re.match(r'^:[0-9]+$', length):
            length = length.replace(":", "")
            min_max = [1, int(length) + 16]
            len_dic["condition"] = "len(suffix) > " + length
        elif re.match(r'^[0-9]+:$', length):
            length = length.replace(":", "")
            min_max = [int(length) - 8, int(length) + 8]
            len_dic["condition"] = "len(suffix) < " + length
        elif re.match(r'^[0-9]+:[0-9]+$', length):
            v_range = length.split(":")
            min_max = [int(v_range[0]) - 8, int(v_range[1]) + 8]
            len_dic["condition"] = "len(suffix) < " + v_range[0] + " or len(suffix) > " + v_range[1]
        elif re.match(r'^[0-9,]+$', length):
            v_nums = list(map(lambda n: int(n), length.split(",")))
            min_max = [min(v_nums) - 8, max(v_nums) + 8]
            len_dic["condition"] = "len(suffix) not in " + str(v_nums)
        else:
            self.log.error("Pattern not match of wronglength for request attribute : %s", attribute)
            self._FAILED_COUNT += 1
            return len_dic

        min_max[0] = min_max[0] if min_max[0] > 1 else 1
        len_dic["min"] = str(min_max[0])
        len_dic["max"] = str(min_max[1])
        return len_dic

    def create_request_wronglength_test_script(self, attribute, length):
        len_dic = self.request_wronglength_formater(attribute, length)
        self._TEST_COUNT += 1
        file = "service_test_request_wronglength_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        with open("./templates/request_wronglength.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                w.write(r.read().replace("<attribute>", attribute).replace("<min>", len_dic["min"])
                        .replace("<max>", len_dic["max"]).replace("<length_condition>", len_dic["condition"]))

        self.register_script(file, attribute + " length", "wrong")
        self.log.info("File created : %s", file)

    def create_request_malformed_test_script(self, attribute, pattern):
        length_value = "range(1, 30)"
        length = pattern.get("wronglength")
        if length:
            length = length.strip()
            if re.match(r'^[0-9]+$', length):
                length_value = "[%s]" % length
            elif re.match(r'^:[0-9]+$', length):
                length_value = "range(1, %d)" % (int(length.replace(":", "")) + 1)
            elif re.match(r'^[0-9]+:$', length):
                l = int(length.replace(":", ""))
                length_value = "range(%d, %d)" % (l, l + 16)
            elif re.match(r'^[0-9]+:[0-9]+$', length):
                l = length.split(":")
                length_value = "range(%s, %d)" % (l[0], int(l[1]) + 1)
            elif re.match(r'^[0-9,]+$', length):
                length_value = "[ %s ]" % length

        self._TEST_COUNT += 1
        file = "service_test_request_malformed_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        with open("./templates/request_malformed.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                if pattern.get("malformed") is None:
                    self._FAILED_COUNT += 1
                    pattern["malformed"] = "<malformed_pattern>"
                    self.log.error("Pattern not define of malformed for request attribute : %s", attribute)
                w.write(r.read().replace("<attribute>", attribute)
                        .replace("<malformed_pattern>", pattern.get("malformed"))
                        .replace("<length_value>", length_value))
        self.register_script(file, attribute, "malformed")
        self.log.info("File created : %s", file)

    def create_request_invalid_test_script(self, attribute, pattern, data_type):
        self._TEST_COUNT += 1
        file = "service_test_request_invalid_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        if isinstance(pattern, type([])):
            with open("./templates/request_invalid_str.txt", "r") as r:
                with open(self.directory + "script/" + file, "w") as w:
                    if pattern is None:
                        self._FAILED_COUNT += 1
                        pattern = "<values>"
                    w.write(r.read().replace("<attribute>", attribute).replace("<values>", str(pattern)))
        elif pattern is not None and re.match(r'^[0-9:]+$', pattern):
            value = 0
            if re.match(r'^[0-9]+$', pattern):
                value = float(pattern) - (random.random() * 10 + 1)
            elif re.match(r'^:[0-9]+$', pattern):
                value = float(pattern.replace(":", "")) + (random.random() * 10 + 1)
            elif re.match(r'^[0-9]+:$', pattern):
                value = float(pattern.replace(":", "")) - (random.random() * 10 + 1)
            elif re.match(r'^[0-9]+:[0-9]+$', pattern):
                v = pattern.split(":")
                value = float(v[0]) - (random.random() * 10 + 1)

            if data_type == 'int':
                value = str(round(value))
            elif data_type == 'double':
                value = str(round(value, 2))
            else:
                value = '"' + str(round(value)) + '"'

            with open("./templates/request_invalid_number.txt", "r") as r:
                with open(self.directory + "script/" + file, "w") as w:
                    w.write(r.read().replace("<attribute>", attribute).replace("<value>", value))
        else:
            self._FAILED_COUNT += 1
            self.log.error("Pattern not define of invalid for request attribute : %s", attribute)
            with open("./templates/request_invalid.txt", "r") as r:
                with open(self.directory + "script/" + file, "w") as w:
                    w.write(r.read().replace("<attribute>", attribute))
        self.register_script(file, attribute, "invalid")
        self.log.info("File created : %s", file)

    def create_request_unknown_test_script(self):
        self._TEST_COUNT += 1
        file = "service_test_request_unknown_attribute.py"
        with open("./templates/request_unknown.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                w.write(r.read())
        self.register_script(file, "attribute", "unknown")
        self.log.info("File created : %s", file)
        self._SINGLE_REGRESSION += "\n"

    def create_response_null_test_script(self, attribute):
        self._TEST_COUNT += 1
        file = "service_test_response_null_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        with open("./templates/response_null.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                w.write(r.read().replace("<attribute>", attribute))
        self.register_script(file, attribute, "null")
        self.log.info("File created : %s", file)

    def create_response_empty_test_script(self, attribute, is_mandatory):
        self._TEST_COUNT += 1
        file = "service_test_response_empty_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        null_conditon = ""
        if not is_mandatory:
            null_conditon = "self._ATTR not in self.response_data['body'] or "
        with open("./templates/response_empty.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                w.write(r.read().replace("<attribute>", attribute).replace("<null_checker>", null_conditon))
        self.register_script(file, attribute, "empty")
        self.log.info("File created : %s", file)

    def create_response_mismatched_test_script(self, attribute):
        self._TEST_COUNT += 1
        file = "service_test_response_mismatched_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        with open("./templates/response_mismatched.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                w.write(r.read().replace("<attribute>", attribute))
        self.register_script(file, attribute, "mismatched")
        self.log.info("File created : %s", file)

    def create_response_wrongtype_test_script(self, attribute, type, is_mandatory):
        if type == 'str':
            type = "[type(\"\")]"
        elif type == 'double':
            type = "[type(1.00), type(1)]"
        elif type == '[]' or type == 'array':
            type = "[type([])]"
        elif type == '{}' or type == 'object':
            type = "[type({})]"
        elif type == 'boolean':
            type = "[type(True)]"
        elif type == 'int':
            type = "[type(1)]"
        else:
            print("Type not found :", type)
            type = "<type>"

        self._TEST_COUNT += 1
        file = "service_test_response_wrongtype_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        null_conditon = ""
        if not is_mandatory:
            null_conditon = "self._ATTR not in self.response_data['body'] or "
        with open("./templates/response_wrongtype.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                w.write(r.read().replace("<attribute>", attribute).replace("<type>", type)
                        .replace("<null_checker>", null_conditon))
        self.register_script(file, attribute + " data type", "wrong")
        self.log.info("File created : %s", file)

    def response_wronglength_formater(self, attribute, length):
        value = "len(self.response_data['body'][self._ATTR])"
        if not length:
            self.log.error("Pattern not define of wronglength for response attribute : %s", attribute)
            return "<length_condition>"
        length = length.strip()

        if re.match(r'^[0-9]+$', length):
            return value + " == " + length
        elif re.match(r'^:[0-9]+$', length):
            length = length.replace(":", "")
            return value + " <= " + length
        elif re.match(r'^[0-9]+:$', length):
            length = length.replace(":", "")
            return value + " >= " + length
        elif re.match(r'^[0-9]+:[0-9]+$', length):
            v_range = length.split(":")
            return value + " >= " + v_range[0] + " and " + value + " <= " + v_range[1]
        elif re.match(r'^[0-9,]+$', length):
            v_nums = list(map(lambda n: int(n), length.split(",")))
            return value + " in " + str(v_nums)
        else:
            self.log.error("Pattern not match of wronglength for response attribute : %s", attribute)
            return "<length_condition>"

    def create_response_wronglength_test_script(self, attribute, length, is_mandatory):
        length_condition = self.response_wronglength_formater(attribute, length)
        self._TEST_COUNT += 1
        file = "service_test_response_wronglength_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        if not is_mandatory:
            length_condition = "self._ATTR not in self.response_data['body'] or %s" % length_condition
        with open("./templates/response_wronglength.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                w.write(r.read().replace("<attribute>", attribute)
                        .replace("<length_condition>", length_condition))
        self.register_script(file, attribute + " length", "wrong")
        self.log.info("File created : %s", file)

    def create_response_malformed_test_script(self, attribute, pattern, is_mandatory):
        self._TEST_COUNT += 1
        file = "service_test_response_malformed_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        null_conditon = ""
        with open("./templates/response_malformed.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                if pattern is None:
                    self._FAILED_COUNT += 1
                    pattern = "<malformed_pattern>"
                    self.log.error("Pattern not define of malformed for response attribute : %s", attribute)
                if not is_mandatory:
                    null_conditon = "self._ATTR not in self.response_data['body'] or "
                w.write(r.read().replace("<attribute>", attribute).replace("<malformed_pattern>", pattern)
                        .replace("<null_checker>", null_conditon))
        self.register_script(file, attribute, "malformed")
        self.log.info("File created : %s", file)

    def create_response_invalid_test_script(self, attribute, pattern, is_mandatory):
        self._TEST_COUNT += 1
        file = "service_test_response_invalid_%s.py" % re.sub('([A-Z]+)', r'_\1', attribute).lower()
        null_conditon = ""
        if not is_mandatory:
            null_conditon = "self._ATTR not in self.response_data['body'] or "
        if isinstance(pattern, type([])):
            with open("./templates/response_invalid_str.txt", "r") as r:
                with open(self.directory + "script/" + file, "w") as w:
                    w.write(r.read().replace("<attribute>", attribute).replace("<values>", str(pattern))
                            .replace("<null_condition>", null_conditon))
        elif pattern is not None and re.match(r'^[0-9:]+$', pattern):
            value = 0
            condition = "float(self.response_data[\"body\"][\"%s\"])" % attribute
            error_message = ""
            if re.match(r'^[0-9]+$', pattern):
                condition = condition + " != " + pattern
                error_message = pattern
            elif re.match(r'^:[0-9]+$', pattern):
                value = pattern.replace(":", "")
                condition = condition + " > " + value
                error_message = "greater than " + value
            elif re.match(r'^[0-9]+:$', pattern):
                value = pattern.replace(":", "")
                condition = condition + " < " + value
                error_message = "less than " + value
            elif re.match(r'^[0-9]+:[0-9]+$', pattern):
                value = pattern.split(":")
                condition = condition + " < " + value[0] + " or " + condition + " > " + value[1]
                error_message = "less than " + value[0] + " or greater than " + value[1]

            with open("./templates/response_invalid_number.txt", "r") as r:
                with open(self.directory + "script/" + file, "w") as w:
                    w.write(r.read().replace("<attribute>", attribute)
                            .replace("<condition>", condition)
                            .replace("<error_message>", error_message)
                            .replace("<null_condition>", null_conditon))
        else:
            self.log.error("Pattern not define of invalid for response attribute : %s", attribute)
            self._FAILED_COUNT += 1
            with open("./templates/response_invalid.txt", "r") as r:
                with open(self.directory + "script/" + file, "w") as w:
                    w.write(r.read().replace("<attribute>", attribute))

        self.register_script(file, attribute, "invalid")
        self.log.info("File created : %s", file)

    def create_response_unknown_test_script(self, mandatory_attrs, optional_attrs):
        self._TEST_COUNT += 1
        file = "service_test_response_unknown_attribute.py"
        with open("./templates/response_unknown.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                w.write(r.read().replace("<mandatory_attribute_list>", mandatory_attrs)
                        .replace("<optional_attribute_list>", optional_attrs))
        self.register_script(file, "attribute", "unknown")
        self.log.info("File created : %s", file)

    def create_response_missing_test_script(self, mandatory_attrs):
        self._TEST_COUNT += 1
        file = "service_test_response_missing_attribute.py"
        with open("./templates/response_missing.txt", "r") as r:
            with open(self.directory + "script/" + file, "w") as w:
                w.write(r.read().replace("<mandatory_attribute_list>", mandatory_attrs))
        self.register_script(file, "attribute", "missing")
        self.log.info("File created : %s", file)

    def generate_request_test_script(self, data):
        self.log.debug("Attribute name : %s", data[5])
        attr_dic = {"attribute": data[5], "mandatory": False}
        pattern = {}
        if len(data) >= 17 and re.match(r"^\{.*\}$", data[16]):
            pattern = json.loads(data[16])
            self.log.debug("Pattern : \n%s", json.dumps(pattern))

        if "null" in data[9]:
            attr_dic['mandatory'] = True
            self.create_request_null_test_script(data[5])
        if "empty" in data[8]:
            self.create_request_empty_test_script(data[5])
        if "wrongtype" in data[7]:
            self.create_request_wrongtype_test_script(data[5], data[6])
        if "wronglength" in data[10]:
            self.create_request_wronglength_test_script(data[5], pattern.get('wronglength'))
        if "malformed" in data[11]:
            self.create_request_malformed_test_script(data[5], pattern)
        if "invalid" in data[13]:
            self.create_request_invalid_test_script(data[5], pattern.get('invalid'), data[6])

        self._SINGLE_REGRESSION += "\n"
        return attr_dic

    def generate_response_test_script(self, data):
        self.log.debug("Attribute name : %s", data[5])
        attr_dic = {"attribute": data[5], "mandatory": False}
        pattern = {}
        if len(data) >= 17 and re.match(r"^\{.*\}$", data[16]):
            pattern = json.loads(data[16])

        if "null" in data[9]:
            attr_dic['mandatory'] = True
            self.create_response_null_test_script(data[5])
        if "empty" in data[8]:
            self.create_response_empty_test_script(data[5], attr_dic['mandatory'])
        if "wrongtype" in data[7]:
            self.create_response_wrongtype_test_script(data[5], data[6], attr_dic['mandatory'])
        if "wronglength" in data[10]:
            self.create_response_wronglength_test_script(data[5], pattern.get('wronglength'), attr_dic['mandatory'])
        if "malformed" in data[11]:
            self.create_response_malformed_test_script(data[5], pattern.get('malformed'), attr_dic['mandatory'])
        if "mismatch" in data[12]:
            self.create_response_mismatched_test_script(data[5])
        if "invalid" in data[13]:
            self.create_response_invalid_test_script(data[5], pattern.get('invalid'), attr_dic['mandatory'])

        self._SINGLE_REGRESSION += "\n"
        return attr_dic

    def create_test_script(self):
        self.log.info("Start creating request test script")
        (seq(self.regression_data).filter(lambda x: "request" in x[3])
         .map(lambda d: self.generate_request_test_script(d)).to_list())
        self.create_request_unknown_test_script()

        self.log.info("\nStart creating response test script")

        response_attr_list = seq(self.regression_data).filter(lambda x: "response" in x[3]) \
            .map(lambda d: self.generate_response_test_script(d)).to_list()

        mandatory_res_attrs = str(seq(response_attr_list).filter(lambda d: d['mandatory'])
                                  .map(lambda d: d['attribute']).to_list())
        optional_res_attrs = str(seq(response_attr_list).filter(lambda d: not d['mandatory'])
                                 .map(lambda d: d['attribute']).to_list())

        self.create_response_missing_test_script(mandatory_res_attrs)
        self.create_response_unknown_test_script(mandatory_res_attrs, optional_res_attrs)

        with open(self.directory + "run_regression_single.sh", "a") as a:
            a.write(self._SINGLE_REGRESSION)

        self.log.debug("Update run_regression_single.sh file")

        self.log.info("Successfully created : (%d/%d)", self._TEST_COUNT - self._FAILED_COUNT, self._TEST_COUNT)
        self.log.info("Only Template create : (%d/%d)", self._FAILED_COUNT, self._TEST_COUNT)

    def change_script_permission(self):
        self.log.debug("Change file mode to 775")
        os.system('chmod 775 ' + self.directory + 'run_regression*')
        os.system('chmod 775 ' + self.directory + 'script/*')

    def run(self):
        start_time = int(round(time.time() * 1000))
        self.set_up()
        self.load_regression_sheet()
        self.create_environment()
        self.create_test_script()
        self.change_script_permission()
        self.log.info("")
        self.log.info("Script execution end at : {0:%Y-%m-%d %H:%M:%S}".format(datetime.now()))
        self.log.info('Execution Time : %d ms', int(round(time.time() * 1000)) - start_time)
class ExpSystemServer(object):
    """
    ros wrapper for handling exp points
    """
    def __init__(self):
        # hardcoded constants
        self._success_response = "success: "
        self._error_response = "error: "

        # start node
        rospy.init_node('exp_server', log_level=rospy.DEBUG)

        # get roslaunch params

        # names for self topics
        self._logname = rospy.get_param('~exp_log_name', 'exp_system')
        self._log_node_name = rospy.get_param('~exp_log_node_name', 'exp_log_node')
        self._service_name = rospy.get_param('~exp_service_name', 'exp_system')
        self._exp_pub_name = rospy.get_param('~exp_pub_name', 'exp_pub')

        # mode of work
        # no matter which method is chosen we will parse params for all of them from .launch file
        self._mode = rospy.get_param('~exp_mode_name', 'table')
        # self._stand_type = rospy.get_param('~exp_lamp_type', 'exp')
        # self._height_of_lamps = rospy.get_param('~exp_lamp_h', 25)

        self._search_table = rospy.get_param('exp_search_table')
        self._exp_description = rospy.get_param('mysql_data_saver_experiment_description')
        self._exp_id = self._exp_description["experiment_number"]
        self._db_params=rospy.get_param('mysql_db_params')
        self._db_name = "experiment" + str(self._exp_id)
        # self._exp_config_path = rospy.get_param('~exp_config_path', 'test.xml')
        # todo add here parsing params of gradient search and other smart methods

        # create log topic publisher
        self._log_pub = rospy.Publisher(self._log_node_name, String, queue_size=10)

        # logger
        self._logger = CustomLogger(name=self._logname, logpub=self._log_pub)
        self._logger.info("exp_server init")

        # create exp_db if it is not exists now
        self._create_exp_db()

        # create search method handler object
        # it must not contain any ros api, because i hate ros
        if self._mode == 'table':
            self._search_handler = TableSearchHandler(
                self._search_table, self._db_params, self._exp_description, self._logger)
        elif self._mode == 'ordered_table':
            self._search_handler = OrderedTableSearchHandler(
                self._search_table, self._db_params, self._exp_description, self._logger)

        # todo add other search modes
        # now we have to calculate current search point
        # self._search_handler will calculate and store it
        # until we get reqv from control system

        self._search_handler.calculate_next_point()

        # service
        self._service = rospy.Service(self._service_name, ExpSystem, self._handle_request)

        self._loop()

    def _loop(self):
        rospy.spin()

    def _handle_request(self, req):

        # self._logger.debug("+++++++++++++++++++++++++++++++++++++++++++++++")
        # self._logger.debug("+++++++++++++++++++++++++++++++++++++++++++++++")
        self._logger.debug("we got request: {}".format(req))

        if not req.command:
            # if we got empty string
            resp = self._error_response + 'empty_command'
            return resp

        elif req.command == 'set_point_data':
            # TODO fix

            try:
                self._set_point_data(req.point_id, req.start_time, req.end_time)
                resp = ExpSystemResponse()
                resp.response = self._success_response
                self._logger.info("we got set_point_data reqv. data from control:")
                start_time_ = datetime.datetime.fromtimestamp(
                    req.start_time.to_sec()).strftime('%Y_%m_%d %H:%M:%S')
                end_time_ = datetime.datetime.fromtimestamp(
                    req.end_time.to_sec()).strftime('%Y_%m_%d %H:%M:%S')
                self._logger.info("t_start={} t_stop={}".format(
                    start_time_, end_time_
                ))
                return resp
            except Exception as e:
                exc_info = sys.exc_info()
                err_list = traceback.format_exception(*exc_info)
                self._logger.error("Service call failed: {}".format(err_list))
                resp = ExpSystemResponse()
                resp.response = self._error_response + e.args[0]
                return resp

        elif req.command == 'get_current_point':
            # request from control_node to get current point_id
            try:
                mode, p_id, red, white, led_delay = self._get_current_point()
                resp = ExpSystemResponse()
                resp.response = mode
                resp.point_id = p_id
                resp.red = red
                resp.white = white
                resp.led_delay = led_delay
                self._logger.info("we got get_current_point reqv from control:mode={} p_id={} red={} white={} delay={}".format(
                    mode, p_id, red, white, led_delay
                ))

                return resp
            except Exception as e:
                exc_info = sys.exc_info()
                err_list = traceback.format_exception(*exc_info)
                self._logger.error("Service call failed: {}".format(err_list))
                resp = ExpSystemResponse()
                resp.response = self._error_response + e.args[0]
                # resp = self._error_response + e.args[0]
                return resp

        else:
            resp = ExpSystemResponse()
            resp.response = self._error_response + 'unknown command'
            return resp

    # def _get_last_search_point_from_db(self):
    #     """
    #     sends request to mariadb to get last row in exp_data table , corresponds to exp_id
    # WE NEED ALL SEARCH POINTS FROM THIS EXP because they were used randomly
    # and we dont know how exactly
    #     """
    #     pass

    # def _load_done_points_from_db(self):
    #     # search method must do it by itself, because it depends very much
    #     # anyway use : to find points calculated today
    #     # SELECT * FROM raw_data where dayofmonth(time)=dayofmonth(now());
    #
    #
    #
    #     pass

    # def _get_point_from_db_by_id(self, point_id):
    #     pass

    def _set_point_data(self, p_id, start_time, stop_time):
        # this command handles incoming msg with data about experiment
        # we want to get from this msg
        # uint32 point_id
        # time start_time
        # time end_time
        # and put that to correspond row in mysql db
        #

        # NOTE we have to use update statement
        # https://oracleplsql.ru/update-mariadb.html
        # update raw_data set data_id=5754714, exp_id=200, time='2020-10-19 23:23:06',
        # data=500 where data_id=5754714;
        # self._logger.debug("type(start_time): {}".format(type(start_time)))

        self._search_handler.update_point_data(p_id, start_time, stop_time)

    def _get_current_point(self):

        return self._search_handler.calculate_next_point()

        # return (random.randint(0,100), 120, 55)

    def _create_exp_db(self):

        con = pymysql.connect(host=self._db_params["host"],
                              user=self._db_params["user"],
                              password=self._db_params["password"],
                              # db='experiment',
                              charset='utf8mb4',
                              cursorclass=pymysql.cursors.DictCursor)

        cur = con.cursor()

        cur.execute("use {}".format(self._db_name))


        cur.execute('create table if not exists exp_data'
                    '(point_id bigint unsigned primary key not null auto_increment,'
                    'step_id int unsigned,'  # id of search step whom this point belongs
                    'exp_id  SMALLINT unsigned,'
                    'red int,'
                    'white int,'
                    'start_time timestamp,'  # start time of dataset
                    'end_time timestamp,'  # end time of dataset
                    'number_of_data_points int unsigned,'  # num of points in set
                    'point_of_calc double,'  # point X where we calculate value of derivative
                    'f_val double,'  # value of derivative dCO2/dt(X) on this dataset
                    'q_val double,'
                    'is_finished SMALLINT unsigned '  # flag to mark if point was finished correctly and how
                    ')'
                    )
        # finished SMALLINT unsigned'
        # 0 - finished correctly
        # 1 - not finished
        #

        cur.execute('describe exp_data')
        self._logger.debug(cur.fetchall())

        cur.execute('commit')
        self._logger.debug(cur.fetchall())
示例#7
0
class WatchdogServer(object):
    def __init__(self):
        # hardcoded constants
        self._success_response = "success: "
        self._error_response = "error: "

        # start node
        rospy.init_node('watchdog_server',
                        anonymous=True,
                        log_level=rospy.DEBUG)

        # get roslaunch params

        # names for self topics
        self._logname = rospy.get_param('~watchdog_log_name', 'data_saver')
        self._log_node_name = rospy.get_param('~watchdog_log_node_name',
                                              'watchdog')
        self._service_name = rospy.get_param('~watchdog_service_name',
                                             'watchdog')

        # create log topic publisher
        self._log_pub = rospy.Publisher(self._log_node_name,
                                        String,
                                        queue_size=10)

        # start logger
        self._logger = CustomLogger(name=self._logname, logpub=self._log_pub)
        self._logger.debug("data_saver server init")

        self.dead_flag = False

        # service
        self._service = rospy.Service(self._service_name, Watchdog,
                                      self._handle_request)

        self._logger.debug("end of init, go to loop")

        self._loop()

    def _loop(self):
        try:
            while not rospy.core.is_shutdown() and not self.dead_flag:
                rospy.sleep(1)
        except KeyboardInterrupt:
            self._logger.warning("keyboard interrupt, shutting down")
            rospy.core.signal_shutdown('keyboard interrupt')

    def _handle_request(self, req):
        self._logger.debug("+++++++++++++++++++++++++++++++++++++++++++++++")
        self._logger.debug("+++++++++++++++++++++++++++++++++++++++++++++++")
        self._logger.debug("we got request: {}".format(req))

        if not req.command:
            # if we got empty string
            resp = self._error_response + 'empty_command'
            return resp

        elif req.command == 'shutdown':
            self.dead_flag = True
            resp = self._success_response + "self.dead_flag is {}".format(
                self.dead_flag)
            return resp

        # elif req.command == 'free_lock':
        #     self._data_write_lock.release()
        #     resp = self._success_response + "the lock has been released"
        #     # lock will be acqured forever if we dont call free_lock command
        #     return resp

        else:
            resp = self._error_response + 'unknown command'
            return resp
class SBA5DeviceServer(object):
    """
    class to receive messages from other ros nodes, handle it
    send messages to PP-systems SBA-5 IRGA and parse its output back as response
    """
    def __init__(self):
        # hardcoded constants
        self._success_response = "success: "
        self._error_response = "error: "


        # start node
        rospy.init_node('sba5_device_server', log_level=rospy.DEBUG)

        # get roslaunch params and reinit part of params
        self._calibration_time = int(rospy.get_param('~sba5_calibration_time'))
        self._logname = rospy.get_param('~sba5_log_name', 'sba5')
        self._log_node_name = rospy.get_param('~sba5_log_node_name', 'sba5_log_node')
        self._port = rospy.get_param('~sba5_port', '/dev/serial/by-id/usb-FTDI_FT230X_Basic_UART_DN03WQZS-if00-port0')
        self._baudrate = int(rospy.get_param('~sba5_baudrate', 19200))
        self._timeout = float(rospy.get_param('~sba5_timeout', 0.2))
        self._service_name = rospy.get_param('~sba5_service_name', 'sba5_device')


        # create log topic publisher
        self._log_pub = rospy.Publisher(self._log_node_name, String, queue_size=10)

        # logger
        self._logger = CustomLogger(name=self._logname, logpub=self._log_pub)
        self._logger.debug("sba5_device_server init")

        # self._logger.info("sba5_device_server cal time is {}, type is {}".format(
        #     self._calibration_time, type(self._calibration_time)
        # ))

        # service
        self._service = rospy.Service(self._service_name, SBA5Device, self.handle_request)
        self._loop()


    def _loop(self):
        #self._logger.debug("led_device_server ready to serve")
        rospy.spin()

    def handle_request(self, req):

        self._logger.debug("+++++++++++++++++++++++++++++++++++++++++++++++")
        self._logger.debug("+++++++++++++++++++++++++++++++++++++++++++++++")
        self._logger.debug("we got request: {}".format(req))

        if not req.command:
            # if we got empty string
            resp = self._error_response + 'empty_command'
            return resp

        elif req.command == 'get_info':
            try:
                ans = self.get_info()
                resp = self._success_response + ans
                return resp
            except SBA5DeviceException as e:
                resp = self._error_response + e.args[0]
                return resp

        elif req.command == 'measure_full':
            try:
                ans = self.do_measurement()
                resp = self._success_response + ans
                return resp
            except SBA5DeviceException as e:
                resp = self._error_response + e.args[0]
                return resp

        elif req.command == 'measure_co2':
            try:
                ans = self.do_measurement()
                self._logger.debug("raw result from measure {}".format(ans))
                pattern = re.compile(r'M \d+ \d+ (\d+.\d+) \d+.\d+ \d+.\d+ \d+.\d+ \d+ \d+\r\n')
                res = pattern.findall(ans)

                self._logger.debug("we have found this {}".format(res[0]))
                resp = self._success_response + str(res[0])
                return resp

            except Exception as e:
                # todo fix, find root of problem
                # [ERROR][1598038134.061680]: Error processing request: list index out of range
                # ['Traceback (most recent call last):\n',
                #  '  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 632, in _handle_request\n    response = convert_return_to_response(self.handler(request), self.response_class)\n',
                #  '  File "/opt/ros/melodic/lib/ros_farmer_pc/sba5_device.py", line 307, in handle_request\n    self._logger.debug("we have found this {}".format(res[0]))\n',
                #  'IndexError: list index out of range\n']
                # [ERROR][1598038134.067210]: 1598038134.07; control_system; error
                #Service call failed: service[ / sba5_device] responded
                # with an error: error processing request: list index out of range
                resp = self._error_response + e.args[0]
                return resp

        elif req.command == 'recalibrate':
            try:
                ans = self.do_calibration()
                resp = self._success_response + ans
                return resp
            except SBA5DeviceException as e:
                resp = self._error_response + e.args[0]
                return resp

        elif req.command == 'init':
            try:
                ans = self.set_initial_params()
                resp = self._success_response + ans
                return resp
            except SBA5DeviceException as e:
                resp = self._error_response + e.args[0]
                return resp

        else:
            resp = self._error_response + 'unknown command'
            return resp
        pass

    # =========================== low level commands ===================================

    def send_command(self, command):
        """
        Command must ends with \r\n !
        Its important
        :param command:
        :return:
        """
        """
        To initiate a command, the USB port or RS232 port sends an ASCII character or string.
        A single character command is acted on immediately when the character is received.
        A string command is acted on after the command string terminator <CR> is received. 
        The command can be sent with or without a checksum. If a checksum is sent, a “C” follows 
        the checksum value.
        For example,
        Device sends command without checksum: S,11,1<CR>
        Device sends command with checksum: S,11,1,043C<CR>
        On successfully receiving a command string, the SBA5+ sends an acknowledgement by 
        echoing back to all the ports the Command String and “OK”, each terminated with a <CR> 
        and<linefeed>.
        """
        # \r\n
        try:
            ser = serial.Serial(
                port=self._port,
                baudrate=self._baudrate,
                timeout=self._timeout
            )
            bcom = command.encode('utf-8')
            ser.write(bcom)

        except Exception as e:
            raise SBA5DeviceException("SBAWrapper error while send command: {}".format(e))
            # self._logger.error("SBAWrapper error while send command: {}".format(e))
        # then try to read answer
        # it must be two messages, ended with \r\n
        try:
            ser = serial.Serial(
                port=self._port,
                baudrate=self._baudrate,
                timeout=self._timeout
            )
            echo = (ser.readline()).decode('utf-8')
            status = (ser.readline()).decode('utf-8')
            return echo+status
            #return status

        except Exception as e:
            raise SBA5DeviceException("SBAWrapper error while read answer from command: {}".format(e))
            # self._logger.error("SBAWrapper error while read answer from command: {}".format(e))

    def set_initial_params(self):
        # TODO: mb we need protection from calling that method twice?
        # async part of init
        # we have to send some commands before start regular work of unit
        self._logger.debug("Second part of CO2Sensor init")
        # ans = await self.sensor.send_command("?\r\n")
        # self.logger.info("Command ?, answer: {}".format(ans))
        # if not "OK" in ans:
        #     self.logger.info("CO2SensorError: {}".format(ans))
        #     return "CO2SensorError: {}".format(ans)
        ans_ = ""
        # we need to shut down auto measurements
        ans = self.send_command("!\r\n")
        ans_ += ans
        self._logger.debug("Command !, answer: {}".format(ans)[:-1])
        # we need to shut down auto zero operations
        ans += self.send_command("A0\r\n")
        ans_ += ans
        self._logger.debug("Command A0, answer: {}".format(ans)[:-1])
        # we need to set format of output
        ans += self.send_command("F252\r\n")
        ans_ += ans
        self._logger.debug("Command F252, answer: {}".format(ans)[:-1])
        # we need to start pump
        ans += self.send_command("P1\r\n")
        ans_ += ans
        self._logger.debug("Command P1, answer: {}".format(ans)[:-1])
        # set time of calibration
        if self._calibration_time == 90:
            command = "EL\r\n"
        elif self._calibration_time == 40:
            command = "EM\r\n"
        elif self._calibration_time == 21:
            command = "ES\r\n"
        else:
            self._logger.error("wrong initial calibration time, {}".format(self._calibration_time))
            self._logger.error("default time will be 90 sec")
            command = "EL\r\n"

        ans += self.send_command(command)
        ans_ += ans
        self._logger.debug("Command calibraton, answer: {}".format(ans)[:-1])
        return ans_

    def get_info(self):
        # only first line of answer
        ans = self.send_command("?\r\n")
        self._logger.debug("Getting info from SBA5")
        return ans[:-1]

    def do_calibration(self):
        ans = self.send_command("Z\r\n")
        self._logger.debug("Starting calibration of SBA5")
        return ans

    def do_measurement(self):
        ans = self.send_command("M\r\n")
        self._logger.debug("Do measure SBA5")
        self._logger.debug(("SBA5 result is {}".format(ans))[:-1])  #its try to remove last \n from here
        return ans

    def do_command(self, com):
        ans = self.send_command(com)
        self._logger.info("send {} command to SBA5".format(com)[:-1])
        return ans
示例#9
0
class ScalesDeviceServer(object):
    """

    """
    def __init__(self):

        # start node
        rospy.init_node('scales_device_server', log_level=rospy.DEBUG)
        # get roslaunch params and reinit part of params
        self._logname = rospy.get_param('scales_log_name', 'scales')
        self._lost_data_marker = rospy.get_param('scales_lost_data_marker',
                                                 -65536)
        self._log_node_name = rospy.get_param('scales_log_node_name',
                                              'scales_log_node')
        self._data_pub_name = rospy.get_param('scales_data_pub_name',
                                              'scales_data_pub')
        self._port = rospy.get_param(
            'scales_port',
            '/dev/serial/by-id/usb-USB_Vir_USB_Virtual_COM-if00')
        self._baudrate = rospy.get_param('scales_baudrate', 9600)
        self._timeout = rospy.get_param('scales_timeout', 1)
        self._measure_interval = rospy.get_param('scales_measure_interval', 5)

        # create log topic publisher
        self._log_pub = rospy.Publisher(self._log_node_name,
                                        String,
                                        queue_size=10)

        self._data_pub = rospy.Publisher(self._data_pub_name,
                                         Temperature,
                                         queue_size=10)

        # logger
        self._logger = CustomLogger(name=self._logname, logpub=self._log_pub)
        self._logger.debug("scales_device_server init")
        # print("we here init")

        # and go to infinite loop
        # self._service = rospy.Service(self._service_name, SBA5Device, self.handle_request)
        self._loop()

    def _loop(self):
        # rospy.sleep(1)
        while not rospy.is_shutdown():
            res = self.get_mean_data(self._measure_interval)
            msg = Temperature()
            msg.temperature = res
            msg.header.stamp.secs = rospy.get_time()
            self._data_pub.publish(msg)
            # print("we alive")

    def get_mean_data(self, dtime):
        w_datas = []
        t_start = time.time()
        with serial.Serial(self._port, self._baudrate,
                           timeout=self._timeout) as scales:
            while time.time() - t_start < dtime:
                try:
                    raw_data = scales.readline()
                    pattern = re.compile(
                        r'\w\w,\w\w,\s*(\d+.\d+)\s*\w'
                    )  # for answers like "ST, GS, 55.210 g"
                    w_data = float(pattern.findall(raw_data)[0])
                    w_datas.append(w_data)
                    res = numpy.mean(w_datas)
                except Exception as e:
                    exc_info = sys.exc_info()
                    err_list = traceback.format_exception(*exc_info)
                    self._logger.error(
                        "scales serial error: {}".format(err_list))
                    res = self._lost_data_marker
        return res

    def get_data(self):
        with serial.Serial(self._port, self._baudrate,
                           timeout=self._timeout) as scales:
            try:
                raw_data = scales.readline()
                pattern = re.compile(r'\w\w, \w\w, (\d+.\d+) \w'
                                     )  # for answers like "ST, GS, 55.210 g"
                w_data = float(pattern.findall(raw_data)[0])
                return w_data
            except Exception as e:
                exc_info = sys.exc_info()
                err_list = traceback.format_exception(*exc_info)
                self._logger.error("scales serial error: {}".format(err_list))
                return self._lost_data_marker
示例#10
0
class LedDeviceServer(object):
    """
    class to receive messages from other ros nodes, handle it
    send messages to led driver and parse its output back as response
    """
    def __init__(self):
        # hardcoded constants
        self._success_response = "success"
        self._error_response = "error: "


        # start node
        rospy.init_node('led_device_server', anonymous=True, log_level=rospy.DEBUG)

        # get roslaunch params and reinit part of params
        self._logname = rospy.get_param('~led_log_name', 'LED')
        self._log_node_name = rospy.get_param('~led_log_node_name', 'led_log_node')
        self._port = rospy.get_param('~led_port', '/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0')
        self._baudrate = rospy.get_param('~led_baudrate', 19200)
        self._timeout = rospy.get_param('~led_timeout', 10)
        self._service_name = rospy.get_param('~led_service_name')
        self._max_red_current = rospy.get_param('~led_max_red_current', 250)
        self._min_red_current = rospy.get_param('~led_min_red_current', 10)
        self._max_white_current = rospy.get_param('~led_max_white_current', 250)
        self._min_white_current = rospy.get_param('~led_min_white_current', 10)

        # create log topic publisher
        self._log_pub = rospy.Publisher(self._log_node_name, String, queue_size=10)

        # logger
        self._logger = CustomLogger(name=self._logname, logpub=self._log_pub)
        self._logger.debug("led_device_server init")

        # service
        self._service = rospy.Service(self._service_name, LedDevice, self.handle_request)
        self._loop()


    def _loop(self):
        #self._logger.debug("led_device_server ready to serve")
        rospy.spin()

    def handle_request(self, req):
        # check params from request
        # at first find what user wants from us:
        self._logger.info("+++++++++++++++++++++++++++++++++++++++++++++++")
        self._logger.debug("+++++++++++++++++++++++++++++++++++++++++++++++")
        self._logger.info("we got request: {}".format(req))

        if not req.command:
            # if we got empty string
            resp = self._error_response + 'empty_command'
            return resp

        elif req.command == 'start':
            try:
                self._start()
                resp = self._success_response
                return resp
            except LedDeviceException as e:
                resp = self._error_response + e.args[0]
                return resp

        elif req.command == 'stop':
            try:
                self._stop()
                resp = self._success_response
                return resp
            except LedDeviceException as e:
                resp = self._error_response + e.args[0]
                return resp

        elif req.command == 'start_configure':
            try:
                self._start_configure()
                resp = self._success_response
                return resp
            except LedDeviceException as e:
                resp = self._error_response + e.args[0]
                return resp

        elif req.command == 'finish_configure_and_save':
            try:
                self._finish_configure_with_saving()
                resp = self._success_response
                return resp
            except LedDeviceException as e:
                resp = self._error_response + e.args[0]
                return resp

        elif req.command == 'set_current':
            if req.red < self._min_red_current or req.red > self._max_red_current:
                resp = self._error_response + 'too big red current'
                return resp
            elif req.white < self._min_white_current or req.white > self._max_white_current:
                resp = self._error_response + 'too big white current'
                return resp
            else:
                try:
                    # red
                    self._set_current(0, int(req.red))
                    # white
                    self._set_current(1, int(req.white))
                    resp = self._success_response
                    return resp

                except LedDeviceException as e:
                    resp = self._error_response + e.args[0]
                    return resp

        elif req.command == 'full_reconfigure':
            if req.red < self._min_red_current or req.red > self._max_red_current:
                resp = self._error_response + 'too big red current'
                return resp
            elif req.white < self._min_white_current or req.white > self._max_white_current:
                resp = self._error_response + 'too big white current'
                return resp
            else:
                try:
                    self._stop()
                    self._start_configure()
                    # red
                    self._set_current(0, int(req.red))
                    # white
                    self._set_current(1, int(req.white))
                    self._finish_configure_with_saving()
                    self._start()
                    resp = self._success_response
                    return resp

                except LedDeviceException as e:
                    resp = self._error_response + e.args[0]
                    return resp

        else:
            resp = self._error_response + 'unknown command'
            return resp

    # ==================== low level command wrappers ==================

    def send_command(self,
                     com,
                     log_comment=None
                     ):
        ans = None
        self._logger.debug("-------------------------------")
        if (log_comment):
            self._logger.info("Sending {}".format(log_comment))

        else:
            self._logger.debug("We want to send this:")
        self._logger.debug(self.parse_command(com))
        try:
            ser = serial.Serial(port=self._port, baudrate=self._baudrate, timeout=self._timeout)
            ser.write(com)
        except Exception as e:
            self._logger.error("Error happened while write: {}".format(e))
            self._logger.debug("-------------------------------")
            raise LedDeviceException("Error happened while write: {}".format(e))

        try:
            ans = ser.read(len(com))  # returns str in python2, oooooff
            self._logger.debug("We  have read {} bytes".format(len(ans)))

        except Exception as e:
            self._logger.error("Error happened while read: {}".format(e))
            self._logger.debug("-------------------------------")
            raise LedDeviceException("Error happened while read: {}".format(e))

        if (not ans or (len(ans) != len(com))):
            self._logger.error("Broken answer from GIC: {}".format(ans))
            self._logger.debug("-------------------------------")
            raise LedDeviceException("Broken answer from GIC: {}".format(ans))
        else:
            self._logger.info("Succesfully got answer from GIC:")

            # lets try to decode to int
            byte_ans = bytearray()
            for b_ in ans:
                b_decoded = ord(b_)  # important when encode ser.read() output back to int
                self._logger.debug("Decoded answer byte: {}".format(hex(b_decoded)))
                byte_ans.extend([b_decoded])

            self._logger.debug(self.parse_command(byte_ans))
            return byte_ans

    def crc16_ccitt(self, data_, crc=0xffff):
        """Calculate the crc16 ccitt checksum of some data
        A starting crc value may be specified if desired.  The input data
        is expected to be a sequence of bytes (string) and the output
        is an integer in the range (0, 0xFFFF).  No packing is done to the
        resultant crc value.  To check the value a checksum, just pass in
        the data byes and checksum value.  If the data matches the checksum,
        then the resultant checksum from this function should be 0.
        """
        tab = CRC16_CCITT_TAB  # minor optimization (now in locals)
        # for byte in six.iterbytes(data_):
        for byte in data_:
            self._logger.debug("current byte is {}".format(hex(byte)))
            crc = (((crc << 8) & 0xff00) ^ tab[((crc >> 8) & 0xff) ^ byte])
            self._logger.debug("current crc is {}".format(hex(byte)))

        self._logger.debug("final crc is {}".format(hex(crc & 0xffff)))
        return crc & 0xffff

    def parse_command(self, com):
        # parse content of command
        data_length = com[2]
        length = len(com)
        parsed_output = ""
        self._logger.debug("-------------------------------")
        self._logger.debug("Parsed command ")
        for b_ in com:
            pass
            # self._logger.debug("{} - type of raw byte".format(type(b_)))
            # self._logger.debug("{} - type of raw byte, and hex value of it {} ".format(type(b_), hex(b_)))
            # self._logger.debug("{} - raw byte ".format(hex(b_)))
        self._logger.debug("------------------")
        self._logger.debug("{} - header byte ".format(hex(com[0])))
        self._logger.debug("{} - destination byte".format(hex(com[1])))
        self._logger.debug("{} - length of command".format(hex(com[2])))
        self._logger.debug("{} - type of command".format(hex(com[3])))
        if data_length > 1:
            # parse content of command
            for i in range(4, 4 + data_length - 1):
                self._logger.debug("{} - data byte".format(hex(com[i])))
        else:
            pass
        self._logger.debug("{} - last byte of CRC16 ccitt control sum".format(hex(com[length - 2])))
        self._logger.debug("{} - first byte of CRC16 ccitt control sum".format(hex(com[length - 1])))
        self._logger.debug("-------------------------------")
        return parsed_output

    def simple_command(self,
                       ACK=0x00,
                       NACK=0x80,
                       ctype=0x00,
                       data=None,
                       name=None
                       ):

        # data is list of ints or None

        # there is a simple command template
        command = self.create_command(ctype=ctype, data=data)
        ans = self.send_command(command, log_comment=name)
        if ans:
            answer = bytearray(ans)
            if ACK in answer:
                self._logger.debug("There is ACK flag {} in answer ".format(hex(ACK)))
                self._logger.debug("-------------------------------")
                return answer
            if NACK in answer:
                self._logger.debug("There is NACK flag {} in answer ".format(hex(NACK)))
                self._logger.debug("Something went wrong in GIC")
                self._logger.debug("-------------------------------")
                raise LedDeviceException("There is NACK flag {} in answer ".format(hex(NACK)))
        else:
            self._logger.debug("Something went wrong, we got no answer")
            self._logger.debug("-------------------------------")
            raise LedDeviceException("Something went wrong")

    def create_command(self,
                       preamble=0x55,
                       direction=0xCC,
                       length=None,
                       ctype=0x01,
                       data=None
                       ):
        command = bytearray()
        # print("preamble ", preamble)
        command.extend([preamble])
        # print("direction ", direction)
        command.extend([direction])
        if (not length):
            # length of command is length of data + 1 byte of command_type
            if (data):
                length = len(bytearray(data)) + 1
                # split and add to length

                # length = int.to_bytes(lenn, 1, byteorder = 'big')
                command.extend([length])
            else:
                length = 0x01
                command.extend([length])
        else:
            command.extend([length])
        # print("length ", length)
        # print("ctype ", ctype)
        command.extend([ctype])
        if (data):
            # data must be list or none
            command.extend(data)
            # print("data ", data)
        # crc should be calculated only for LENGTH | TYPE | DATA fields
        payload = bytearray()
        payload.extend([length])
        payload.extend([ctype])
        if (data):
            payload.extend(data)
        crc_raw = self.crc16_ccitt(payload)  # returns int
        self._logger.debug("{} - crc raw ".format(hex(crc_raw)))
        # crc_bytes = crc_raw.to_bytes(2, byteorder='little')  # byteorder='little'

        first_byte = ((crc_raw & 0xff00) >> 8)
        last_byte = (crc_raw & 0x00ff)

        self._logger.debug("{} - first crc byte".format(hex(first_byte)))

        self._logger.debug("{} - last crc byte".format(hex(last_byte)))
        # then reorder
        crc_bytes = bytearray([last_byte, first_byte])

        # its important
        # print("crc_bytes ", crc_bytes)
        command.extend(crc_bytes)
        return command

    # ======================= real commands ===========================

    def _start(self):
        # START = bytearray(b'\x55\xCC\x01\x02\x7C\x0E')
        return self.simple_command(
            ACK=0x02,
            NACK=0x82,
            ctype=0x02,
            data=None,
            name="START"
        )

    def _stop(self):
        # STOP = bytearray(b'\x55\xCC\x01\x03\x5D\x1E')
        return self.simple_command(
            ACK=0x03,
            NACK=0x83,
            ctype=0x03,
            data=None,
            name="STOP"
        )

    def _start_configure(self):
        # START_CONFIGURE = bytearray(b'\x55\xCC\x01\x05\x9B\x7E')
        return self.simple_command(
            ACK=0x05,
            NACK=0x85,
            ctype=0x05,
            data=None,
            name="START_CONFIGURE"
        )

    def _set_current(self, channel=0, value=100):
        # SET_CURRENT = bytearray(b'\x55\xCC\x04\x0B\x01\xE8\x03\x00\x00')
        # SET_CURRENT_200_1 = bytearray(b'\x55\xCC\x04\x0B\x01\xC8\x00\xD8\x2E')
        # SET_CURRENT_200_0 = bytearray(b'\x55\xCC\x04\x0B\x00\xC8\x00\xE8\x19')
        # SET_CURRENT_50_1 = bytearray(b'\x55\xCC\x04\x0B\x01\x32\x00\xD2\xD2')
        data = bytearray()
        data.extend([channel])
        # split value bytes and add in little-endian to data
        #
        #       first_byte = ((crc_raw & 0xff00) >> 8)
        #       last_byte = (crc_raw & 0x00ff)
        #       self._logger.debug("current byte is{}".format(hex(byte)))

        self._logger.debug("value raw {}".format(hex(value)))
        # crc_bytes = crc_raw.to_bytes(2, byteorder='little')  # byteorder='little'
        value_first_byte = ((value & 0xff00) >> 8)
        self._logger.debug("value first byte {}".format(hex(value_first_byte)))  # , value_first_byte)
        value_last_byte = (value & 0x00ff)
        self._logger.debug("value last byte {}".format(hex(value_last_byte)))  # , value_last_byte)
        # data.extend(int.to_bytes(channel, 1, byteorder='big'))
        # data.extend(int.to_bytes(value, 2, byteorder='little'))
        data.extend([value_last_byte, value_first_byte])
        self._logger.debug("final data array {}".format(data))
        return self.simple_command(
            ACK=0x0B,
            NACK=0x8B,
            ctype=0x0B,
            data=data,
            name="SET_CURRENT_{}_{}".format(channel, value)
        )

    def _finish_configure_with_saving(self):
        # FINISH_CONFIGURE_WITH_SAVING = bytearray(b'\x55\xCC\x01\x07\xD9\x5E')
        return self.simple_command(
            ACK=0x07,
            NACK=0x87,
            ctype=0x07,
            data=None,
            name="FINISH_CONFIGURE_WITH_SAVING"
        )

    def _exit_without_saving(self):
        # EXIT_WITHOUT_SAVING = bytearray(b'"\x55\xCC\x01\x06\xF8\x4E')
        return self.simple_command(
            ACK=0x06,
            NACK=0x86,
            ctype=0x06,
            data=None,
            name="EXIT_WITHOUT_SAVING"
        )
示例#11
0
class MYSQLDataSaver(object):

    def generate_experiment_id(self, date_=None, type_=None, number_=None):
        """
        date - str
        type - str
        number - str
        """
        if not date_:
            date_ = datetime.datetime.now().strftime('%Y_%m_%d')
        if not type_:
            type_ = "TEST"
        if not number_:
            number_ = "0000"
        # experiment id is string like that YYYY_MM_DD_TYPE_NUMB
        # where YYYY_MM_DD is data of start
        # TYPE is short type of experiment (like TEST, TABLE, SEARCH etc)
        # NUMB is uniq number of experiment

        id_str = date_ + type_ + number_

        return id_str

    def __init__(self):
        # hardcoded constants
        self._success_response = "success: "
        self._error_response = "error: "

        # start node
        rospy.init_node('mysql_data_saver_server', log_level=rospy.INFO)

        # get roslaunch params

        self._description = rospy.get_param('mysql_data_saver_experiment_description')  # do not add ~ !!
        self._db_name = "experiment" + str(self._description["experiment_number"])

        # names for self topics
        self._logname = rospy.get_param('~mysql_data_saver_log_name', 'mysql_data_saver')
        self._lost_data_marker = rospy.get_param('~mysql_data_saver_lost_data_marker',
                                        -65536)
        self._log_node_name = rospy.get_param('~mysql_data_saver_log_node_name', 'mysql_data_saver_log')
        # self._service_name = rospy.get_param('~mysql_data_saver_service_name', 'mysql_data_saver')
        self._db_params = rospy.get_param('mysql_db_params')
        self._system_log_sub = rospy.Subscriber(
            name='/rosout_agg', data_class=Log,
            callback=self._log_callback,
            queue_size=50)
        # create log topic publisher
        self._log_pub = rospy.Publisher(self._log_node_name, String, queue_size=10)

        # start logger
        self._logger = CustomLogger(name=self._logname, logpub=self._log_pub)
        self._logger.info("mysql_data_saver_server init")

        default_id = self.generate_experiment_id()
        # self._experiment_id = rospy.get_param('~mysql_data_saver_experiment_id', default_id)
        # experiment id is string like that YYYY_MM_DD_TYPE_NUMB
        # where YYYY_MM_DD is data of start
        # TYPE is short type of experiment (like TEST, TABLE, SEARCH etc)
        # NUMB is uniq number of experiment

        # TODO get params of sql database

        # TODO really get

        self.con = pymysql.connect(host=self._db_params["host"],
                              user=self._db_params["user"],
                              password=self._db_params["password"],
                              # db='experiment',
                              charset='utf8mb4',
                              cursorclass=pymysql.cursors.DictCursor)

        self.con.close()  # check if db available



        self._raw_topics = rospy.get_param('mysql_data_saver_raw_topics')
        # self._exp_topics = rospy.get_param('~mysql_data_saver_exp_topics')



        # list to keep subscribers (for what?)
        self._subscribers_list = list()

        # check if tables in db exists, or update it
        self._create_database()

        # TODO insert into sensors table all sensors from .launch if not exists

        #  TODO and insert into experiment table current experiment if not exists
        # sub to all topics from parsed string
        self._logger.debug("creating raw subs")
        for topic in self._raw_topics:
            # for now we think that all raw_data_topics have type "sensor_msgs/Temperature"
            # it must be in architecture of all system !
            # so here we think that it is "sensor_msgs/Temperature" as default
            s = rospy.Subscriber(name=topic['name'], data_class=Temperature,
                                 callback=self._raw_data_callback, callback_args=topic,
                                 queue_size=2)
            print(s)
            self._subscribers_list.append(s)

        self._logger.debug("end of init, go to loop")

        self._loop()

    def _loop(self):
        rospy.spin()

    def _log_callback(self, log_msg):


        exp_id_ = self._description["experiment_number"]

        time_ = datetime.datetime.fromtimestamp(
            log_msg.header.stamp.to_sec()).strftime('%Y_%m_%d %H:%M:%S')

        level_ = log_msg.level
        # # Pseudo-constants
        # DEBUG = 1
        # INFO = 2
        # WARN = 4
        # ERROR = 8
        # FATAL = 16
        node_ = log_msg.name

        msg_ = log_msg.msg

        if level_ == 1:
            log_table_name = "debug_logs"

        if level_ == 2:
            log_table_name = "info_logs"

        if level_ == 4:
            log_table_name = "warn_logs"

        if level_ == 8:
            log_table_name = "error_logs"

        if level_ == 16:
            log_table_name = "fatal_logs"

        con = pymysql.connect(host=self._db_params["host"],
                              user=self._db_params["user"],
                              password=self._db_params["password"],
                              # db='experiment',
                              charset='utf8mb4',
                              cursorclass=pymysql.cursors.DictCursor)

        cur = con.cursor()

        cur.execute("use {}".format(self._db_name))

        comm_str = 'insert into {}' \
                   '(exp_id, time, level, node, msg)' \
                   'values("{}", "{}","{}", "{}", "{}")'.format(log_table_name,
            exp_id_, time_, level_, node_, msg_)

        # print("comm_str: {}".format(comm_str))  # TODO: remove after debug
        try:
            cur.execute(comm_str)
        except Exception as e:
            print("Error while saving logs from :")
            print(e)
            print(log_msg.msg)

        cur.execute('commit')
        con.close()

    def _raw_data_callback(self, data_message, topic_info):

        # topic_info must be dict and contain 'name', 'id', 'type', 'dtype', 'units', 'status':
        # example:
        # { name: '/bmp180_1_temp_pub', id: '2', type: 'temperature', dtype: 'float64', units: 'C', status:'raw },

        # example how to convert rospy time to datetime
        # (datetime.datetime.fromtimestamp(rospy.Time.now().to_sec())).strftime('%Y_%m_%d,%H:%M:%S')

        data_ = data_message.temperature
        self._logger.debug("data: {}".format(data_))

        if data_ != self._lost_data_marker:

            time_ = datetime.datetime.fromtimestamp(
                data_message.header.stamp.to_sec()).strftime('%Y_%m_%d %H:%M:%S')
            self._logger.debug("time: {}".format(time_))

            sensor_ = topic_info["id"]
            self._logger.debug("sensor: {}".format(sensor_))

            exp_id_ = self._description["experiment_number"]
            self._logger.debug("exp_id: {}".format(exp_id_))

            con = pymysql.connect(host=self._db_params["host"],
                                  user=self._db_params["user"],
                                  password=self._db_params["password"],
                                  # db='experiment',
                                  charset='utf8mb4',
                                  cursorclass=pymysql.cursors.DictCursor)

            cur = con.cursor()

            cur.execute("use {}".format(self._db_name))

            comm_str = 'insert into raw_data'\
                       '(exp_id, time, sensor_id, data)'\
                       'values("{}", "{}","{}", "{}" )'.format(
                exp_id_, time_, sensor_, data_)

            self._logger.debug("comm_str: {}".format(comm_str))

            cur.execute(comm_str)

            cur.execute('commit')
            con.close()
        else:
            self._logger.debug("we got lost data marker from "
                                 "node and we wont save it to db")


    def _create_database(self):
        self._logger.debug("create database")

        con = pymysql.connect(host=self._db_params["host"],
                              user=self._db_params["user"],
                              password=self._db_params["password"],
                              # db='experiment',
                              charset='utf8mb4',
                              cursorclass=pymysql.cursors.DictCursor)

        cur = con.cursor()

        cur.execute("CREATE DATABASE IF NOT EXISTS {}".format(self._db_name))
        cur.execute("use {}".format(self._db_name))

        # TODO load params from .launch and put to the database


        # create all log tables for different types of error status
        for log_table_name in ["fatal_logs", "error_logs", "warn_logs", "info_logs", "debug_logs"]:
            self._logger.info("create tables for all logs")
            cur.execute('create table if not exists {}'
                        ' ( log_id bigint unsigned primary key not null auto_increment,'
                        ' exp_id SMALLINT unsigned,'
                        ' time timestamp,'
                        ' level TINYINT,'
                        ' node varchar(100),'
                        ' msg varchar(2000) )'.format(log_table_name)
                        )

            cur.execute('describe {}'.format(log_table_name))
            print(cur.fetchall())

        self._logger.info("create table raw_data")

        cur.execute('create table if not exists raw_data'
                    ' (data_id bigint unsigned primary key not null auto_increment,'
                    ' exp_id  SMALLINT unsigned,'
                    ' time timestamp,'
                    ' sensor_id SMALLINT unsigned,'
                    ' data double)'
                    )

        cur.execute('describe raw_data')
        print(cur.fetchall())

        self._logger.info("create table sensors")

        cur.execute('create table if not exists sensors'
                    '(sensor_id SMALLINT unsigned primary key not null,'
                    'name varchar(100),'
                    'type varchar(100),'
                    'units varchar(100),'
                    'prec double,'
                    'description varchar(1000),'
                    'status varchar(100)'
                    ')')

        # put default data from launch file to sensors table
        self._logger.info("creating sensors table")
        for topic in self._raw_topics:
            # TODO add data from dict to table
            pass

        cur.execute('describe sensors')
        print(cur.fetchall())

        self._logger.info("create table experiments")

        cur.execute('create table if not exists experiments'
                    '( exp_id SMALLINT unsigned primary key not null,'
                    'start_date timestamp,'
                    'end_date timestamp,'
                    'params varchar(1000)'
                    ')'
                    )

        cur.execute('describe experiments')
        print(cur.fetchall())

        cur.execute('commit')
        con.close()