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)
