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)
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))
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
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()
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)
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)
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()
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()
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()
def load(self): # cannot use 'import' as method name... param = roslibpy.Param(self.ros, '/robot_description') param.get(self.receive_robot_description)
def ros_get_param(param_name): value = roslibpy.Param(ros, param_name).get(callback=None, timeout=30) return jsonify(value)