Beispiel #1
0
def ros_status():
    st = os.statvfs('/')
    free = st.f_bavail * st.f_frsize
    total = st.f_blocks * st.f_frsize
    used = (st.f_blocks - st.f_bfree) * st.f_frsize

    data = {
        'is_ros_connected_bool':
        ros.is_connected,
        'distro_str':
        roslibpy.Param(ros, 'rosdistro').get(callback=None,
                                             timeout=30).rstrip().capitalize(),
        'version_str':
        roslibpy.Param(ros, 'rosversion').get(callback=None,
                                              timeout=30).rstrip(),
        'free':
        round(free, 2),
        'total':
        round(total, 2),
        'used':
        round(used, 2),
        'used_percent':
        round(100 * used / total, 2)
    }

    return jsonify(data)
Beispiel #2
0
def get_params():
    print("ROS connected:", ros.is_connected)
    robot_id_param = roslibpy.Param(ros, "/QTCStatePublisherNode/robot_id")

    global robot_id
    global qtcTopic
    global sitTopic
    global rejection_KL_thresh

    robot_id = robot_id_param.get()
    print(str(robot_id))

    rejection_thresh_param = roslibpy.Param(
        ros, "/robot" + str(robot_id) + "/qsr/situation_rejection_threshold")
    rejection_KL_thresh = rejection_thresh_param.get()

    qtcTopic = roslibpy.Topic(ros, "/robot" + str(robot_id) + "/qsr/qtc_state",
                              "std_msgs/String")
    sitTopic = roslibpy.Topic(
        ros, "/robot" + str(robot_id) + "/qsr/situation_predictions",
        "std_msgs/String")

    print(qtcTopic.name)
    print(sitTopic.name)

    # Current and recent human and then robot positions and angles:
    # [xh0, yh0, hh0, xh1, yh1, hh1, xr0, yr0, hr0, xr1, yr1, hr1]
    qtcTopic.subscribe(lambda message: qtc_update_callback(message))
Beispiel #3
0
def ros_set_param():
    data = request.get_json()
    param = data['param']
    value = data['value']
    roslibpy.Param(ros, param).set(value, callback=None, timeout=30)

    # X=1(desligado) ou X=3(ligado)
    if param == 'exposure_auto':
        os.system('v4l2-ctl --set-ctrl=exposure_auto=' + str(value))

    # Range 0 < X < 2500
    elif param == 'exposure_absolute':
        is_exposure_auto = value = roslibpy.Param(ros, 'exposure_auto').get(
            callback=None, timeout=30)

        if 0 <= value <= 2500 and is_exposure_auto == 1:
            os.system('v4l2-ctl --set-ctrl=exposure_absolute=' + str(value))

    # Range 0 < X < 200
    elif param == 'brightness':
        if 0 <= value <= 200:
            os.system('v4l2-ctl --set-ctrl=brightness=' + str(value))

    # Range 50 < X < 200
    elif param == 'saturation':
        if 50 <= value <= 200:
            os.system('v4l2-ctl --set-ctrl=saturation=' + str(value))

    # Undefined
    elif param == 'contrast':
        os.system('v4l2-ctl --set-ctrl=contrast=' + str(value))

    # X=0(desligado) ou X=1(ligado)
    elif param == 'white_balance_temperature_auto':
        os.system('v4l2-ctl --set-ctrl=white_balance_temperature_auto=' +
                  str(value))

    # Range 2800 < X < 6000
    elif param == 'white_balance_temperature':
        roslibpy.Param(ros,
                       'white_balance_temperature_auto').get(callback=None,
                                                             timeout=30)

        if 50 <= value <= 200:
            os.system('v4l2-ctl --set-ctrl=white_balance_temperature=' +
                      str(value))

    return jsonify(value)
    def load_srdf(self, parameter_name='/robot_description_semantic'):
        """Loads an SRDF model from the specified ROS parameter.

        Parameters
        ----------
        parameter_name : str, optional
            Name of the ROS parameter containing the robot semantics.
            Defaults to ``/robot_description_semantic``.

        Returns
        -------
        str
            SRDF model of the robot currently loaded in ROS.
        """
        if self.local_cache_enabled and self.robot_name:
            filename = self._srdf_filename

            if _cache_file_exists(filename):
                return _read_file(filename)

        param = roslibpy.Param(self.ros, parameter_name)
        srdf = param.get(timeout=TIMEOUT)

        if self.local_cache_enabled:
            _write_file(self._srdf_filename, srdf)

        return srdf
    def load_urdf(self, parameter_name='/robot_description'):
        """Loads a URDF model from the specified ROS parameter.

        Parameters
        ----------
        parameter_name : str, optional
            Name of the ROS parameter containing the robot description.
            Defaults to ``/robot_description``.

        Returns
        -------
        str
            URDF model of the robot currently loaded in ROS.
        """
        if self.local_cache_enabled and self.robot_name:
            filename = self._urdf_filename

            if _cache_file_exists(filename):
                return _read_file(filename)

        param = roslibpy.Param(self.ros, parameter_name)
        urdf = param.get(timeout=TIMEOUT)

        self.robot_name = self._read_robot_name(urdf)

        if self.local_cache_enabled:
            # Retrieve filename again, now that robot_name
            # has been loaded from URDF
            _write_file(self._urdf_filename, urdf)

        return urdf
Beispiel #6
0
def run_me():
    ros_client = roslibpy.Ros(host='161.253.72.254', port=9090)
    print("Connecting...")
    param_sender = roslibpy.Param(ros_client, '/pub_joint_param')

    def set_param():
        while True:
            if not ros_client.is_connected:
                print('Wait a minute, ROS connection is down.')
                break
            updating_angles = read_data(input_file_name)
            param_sender.set(updating_angles)
            time.sleep(0.01)

    def read_data(file_name):
        # use with to close file after reading automatically
        # read only
        with open(file_name, "r") as text_file:
            # read strings that are seperated with space
            # a dummy tail is needed in this txt file
            # because by default, txt file will have a \n in the end
            string_line0 = text_file.read().split(' ')
        # convert strings into float numbers
        float_line0 = [
            float(string_line0[0]),
            float(string_line0[1]),
            float(string_line0[2]),
            float(string_line0[3]),
            float(string_line0[4]),
            float(string_line0[5])
        ]
        return float_line0

    ros_client.on_ready(set_param, run_in_thread=True)
    ros_client.run_forever()
Beispiel #7
0
    def __init__(self, ros, namespace='/rob1'):
        """Initialize a new robot client instance.

        Parameters
        ----------
        ros : :class:`RosClient`
            Instance of a ROS connection.
        namespace : :obj:`str`
            Namespace to allow multiple robots to be controlled through the same ROS instance.
            Optional. If not specified, it will use namespace ``/rob1``.
        """
        self.ros = ros
        self.counter = SequenceCounter()
        if not namespace.endswith('/'):
            namespace += '/'
        self._version_checked = False
        self._server_protocol_check = dict(event=threading.Event(),
                                           param=roslibpy.Param(
                                               ros,
                                               namespace + 'protocol_version'),
                                           version=None)
        self.ros.on_ready(self.version_check)
        self.topic = roslibpy.Topic(ros,
                                    namespace + 'robot_command',
                                    'compas_rrc_driver/RobotMessage',
                                    queue_size=None)
        self.feedback = roslibpy.Topic(ros,
                                       namespace + 'robot_response',
                                       'compas_rrc_driver/RobotMessage',
                                       queue_size=0)
        self.feedback.subscribe(self.feedback_callback)
        self.topic.advertise()
        self.futures = {}

        self.ros.on('closing', self._disconnect_topics)
Beispiel #8
0
    def __init__(self,robot_id):
        # init HMM model
        working_dir = os.path.dirname(os.path.realpath(__file__))

        # Load per-situation HMMs
        self.pblHMM = hmms.DtHMM.from_file(working_dir+"/../Models/pblHMM_k_3_hf.npz")
        self.pbrHMM = hmms.DtHMM.from_file(working_dir+"/../Models/pbrHMM_k_3_hf.npz")
        self.rotlHMM = hmms.DtHMM.from_file(working_dir+"/../Models/rotlHMM_k_3_hf.npz")
        self.rotrHMM = hmms.DtHMM.from_file(working_dir+"/../Models/rotrHMM_k_3_hf.npz")
        self.pclHMM = hmms.DtHMM.from_file(working_dir+"/../Models/pclHMM_k_3_hf.npz")
        self.pcrHMM = hmms.DtHMM.from_file(working_dir+"/../Models/pcrHMM_k_3_hf.npz")

        # Configuration parameters
        self.rejection_KL_thresh = 0.025
        N_nodes = 81

        self.classes = ["PBL", "PBR", "ROTL", "ROTR", "PCL", "PCR"]

        # Functions for mapping QTC_C states to integers
        # Create list of QTC_C states so that indices can be used as integer state IDs compatible with HMM library
        QTC_symbols = []
        for i in range(0, 4):
            QTC_symbols.append("-")
            QTC_symbols.append("0")
            QTC_symbols.append("+")
        # print("QTC symbols:", QTC_symbols[:3])
        self.QTC_C_states = list(combinations(QTC_symbols, 4))
        self.QTC_C_states = [state[0] + state[1] + state[2] + state[3] for state in self.QTC_C_states]
        self.QTC_C_states = list(np.unique(self.QTC_C_states))

        # init param
        self.prev_time = time.time()
        self.qtc_seq = []

        # init ros
        ros_client = roslibpy.Ros(host='localhost', port=9090)
        ros_client.run()
        print("ROS connected:", ros_client.is_connected)
        
        # read parameters
        print("Robot Id :", robot_id)

        rejection_thresh_param = roslibpy.Param(ros_client, "/robot"+str(robot_id)+"/qsr/situation_rejection_threshold")
        self.rejection_KL_thresh = rejection_thresh_param.get()

        print("rejection_KL_thresh:",self.rejection_KL_thresh )
        
        # define topics
        self.qtcTopic = roslibpy.Topic(ros_client, "/robot"+str(robot_id)+"/qsr/qtc_state", "std_msgs/String")
        self.sitTopic = roslibpy.Topic(ros_client, "/robot"+str(robot_id)+"/qsr/situation_predictions", "std_msgs/String")

        # define subscribers
        self.qtcTopic.subscribe(lambda message: self.qtc_update_callback(message))

        while ros_client.is_connected:
            time.sleep(1)

        ros_client.terminate()
 def OnListBoxSelectparam(self, event):
     index = self.m_listBoxparam.GetSelection()
     self.paramselectname = self.m_listBoxparam.GetItems()[index]
     self.selectparam = roslibpy.Param(self.client, self.paramselectname)
     # self.selectparam.get(self.getparamdetailcallback)
     # str_show = 'getparam  \n'+ self.paramselectname
     str_line = '-----------------------\n'
     self.m_textparam.AppendText(str_line + self.paramselectname + '\n' +
                                 str_line)
Beispiel #10
0
 def receive_robot_description(self, robot_description):
     robot_name, uris = self.read_robot_name_and_uris_from_urdf(robot_description)
     self.robot_name = robot_name
     self.status.update({"robot_name_received": True})
     # Import resource files
     self.import_resource_files(uris)
     # Save robot_description.urdf
     self.save_robot_description(robot_description)
     # Save robot_description_semantic.urdf
     param = roslibpy.Param(self.ros, '/robot_description_semantic')
     param.get(self.save_robot_description_semantic)
     # Update status
     self.check_status()
Beispiel #11
0
def run_me():
    ros_client = roslibpy.Ros(host='161.253.72.254', port=9090)
    print("[INFO] Connecting...")
    pub_joint_param_mount = roslibpy.Param(ros_client, '/pub_joint_param')

    def param_rw():
        print("[INFO] ROS is connected. Hold on tight.")
        time.sleep(0.5)
        while True:
            if not ros_client.is_connected:
                print('[INFO] Wait a minute, ROS connection is down.')
                break
            updating_angles = read_data(input_file_name)
            pub_joint_param_mount.set(updating_angles)
            time.sleep(0.01)
            pub_joint_param_mount.get(
                lambda the_list: print("[INFO] Joint Angles: ", the_list),
                lambda error_msg: print("[Error] ", error_msg))
            time.sleep(0.01)

    def read_data(file_name):
        # use with to close file after reading automatically
        # read only
        with open(file_name, "r") as text_file:
            # read strings that are seperated with space
            # a dummy tail is needed in this txt file
            # because by default, txt file will have a \n in the end
            string_line0 = text_file.read().split(' ')
        # convert strings into float numbers
        float_line0 = [
            float(string_line0[0]),
            float(string_line0[1]),
            float(string_line0[2]),
            float(string_line0[3]),
            float(string_line0[4]),
            float(string_line0[5])
        ]
        return float_line0

    ros_client.on_ready(param_rw, run_in_thread=True)
    ros_client.run_forever()
    if not ros_client.is_connected:
        print("[WARNING] ROS is not connected.",
              "Please rerun this script after ROS started.")
        sys.exit()
Beispiel #12
0
def run_me():
    ros_client = roslibpy.Ros(host='161.253.72.254', port=9090)
    print("Connecting...")
    param_test = roslibpy.Param(ros_client, '/wtf')

    def get_param():
        while True:
            if not ros_client.is_connected:
                print('Wait a minute, ROS connection is down.')
                break
            # pass whatever is passing into here to somewhere else
            param_test.get(
                lambda test_value: print("[Callback Value] ", test_value),
                lambda error_msg: print("[Error] ", error_msg))
            time.sleep(0.01)

    ros_client.on_ready(get_param, run_in_thread=True)
    ros_client.run_forever()
Beispiel #13
0
 def load(self): # cannot use 'import' as method name...
     param = roslibpy.Param(self.ros, '/robot_description')
     param.get(self.receive_robot_description)
Beispiel #14
0
def ros_get_param(param_name):
    value = roslibpy.Param(ros, param_name).get(callback=None, timeout=30)
    return jsonify(value)