def vs_on(self):
'''
Function for victim recognition
'''
rate = rospy.Rate(self.__sensor_update_rate)
while (not rospy.is_shutdown()) and (self.__state == 'RUNNING'):
sensor = self.__find_victim()
for v in sensor:
if sensor[v]['status'] == True:
# Send the signal that a victim have been found and her location
pos = self.__models_service(
v, ''
) # Call the service to receive the global victim position
# Create pose as pose2D
p = Twist()
p.linear.x = pos.pose.position.x
p.linear.y = pos.pose.position.y
p.linear.z = pos.pose.position.z
# Create message
msg = events_message()
msg.event = 'victim_recognized'
msg.position.append(p)
self.__pub.publish(
msg) # Publish the found victim location
self.__victims.remove(
v) # Remove the found victim from the list
rate.sleep()
def __init__(self):
Thread.__init__(self)
motion_topic = rospy.get_param("move_topic",default="move_base/status") # Get name of topic to know that the robot is moving
motion_w = rospy.get_param("move_consuption_weight",default="0.0035") # Get the weight of motion on battery consuption
victim_w = rospy.get_param("v_sensor_consuption_weight",default="0.0009") # Get the weight of victim sensor on battery consuption
gas_w = rospy.get_param("g_sensor_consuption_weight",default="0.0005") # Get the weight of gas sensor on battery consuption
# Get parameters
self.upd_rate = 10.0 # Rate in which the battery level is updated
self.pub_rate = rospy.get_param('publish_rate', default = 1.0) # Rate in which the battery level is published
#Variables for battery consumption simulation
self.battery_level = 100 # Battery level percent
# Weights for move and sensors on battery consumption
self.__weights = {'move': motion_w,
'victim_sensor': victim_w,
'gas_sensor': gas_w}
self.__sensors_status = {'move_base': False, 'victim_sensor': False, 'gas_sensor': False} # Variable to know sensor status
# Message object
self.msg = events_message()
self.msg.event = 'battery_level'
self.msg.param.append(self.battery_level)
# Publisher object for periodically sending battery level
self.pub = rospy.Publisher("battery_monitor/out", events_message, queue_size=10)
rospy.Subscriber("battery_monitor/in", events_message, self.event_receiver) # Topic to receive occured events
rospy.Subscriber("victim_sensor/in", events_message, self.victim_sensor_status) # Topic to monitor victim sensor status
rospy.Subscriber("gas_sensor/in", events_message, self.gas_sensor_status) # Topic to monitor gas sensor status
rospy.Subscriber(motion_topic, GoalStatusArray, self.move_base_status) # Topic to monitor move_base status
def __init__(self, name):
self.robot_name = name
self.suspending = False
self.state = 'IDLE' # Set IDLE state
self.region = [] # Region to be explored
# Exploration service
# self._frontier_client = SimpleActionClient("/{}/exploration_server_node".format(self.robot_name), ExploreAction) # Create a client to the exploration node
# self._frontier_client.wait_for_server() # Wait server to be ready
# self._goal = ExploreGoal() # Message to send the goal region
# self._goal.strategy_plugin = rospy.get_param("plugin_name", default="exploration_server::ExamplePlugin") # Select the exploration plugin
# self._goal.boundary.header.frame_id = "earth" # Set frame wich the points are related to
# self._goal.start_point.header.frame_id = "earth"
# Exploration service
self._exp_client = SimpleActionClient(
"exploration_server", ExecuteExplorationAction
) # Create a client to the exploration server
self._exp_client.wait_for_server() # Wait server to be ready
# Maneuvers out
self.pub = rospy.Publisher("/{}/maneuvers/out".format(name),
events_message,
queue_size=10) # Publisher object
self.msg = events_message() # Message object
def gs_on(self):
'''
Function for gas recognition
'''
rate = rospy.Rate(self.__sensor_update_rate)
while (not rospy.is_shutdown()) and (self.__state == 'GS_ON'):
sensor = self.__find_gas()
for g in sensor:
if sensor[g]['status'] == True:
# Verify if is a new point to publish
if self.__points_with_gas:
count = 0
for point in self.__points_with_gas:
dist = ((self.__current_pose.x - point.x)**2 +
(self.__current_pose.y - point.y)**2)**(
0.5)
if dist < self.__update_radius:
break
else:
count += 1
if count == len(self.__points_with_gas):
# Send the signal that a gas leak have been found and its location
self.__points_with_gas.append(
deepcopy(self.__current_pose))
msg = events_message()
msg.event = 'gas_leak'
msg.position.append(self.__current_pose)
self.__pub.publish(
msg) # Publish the found gas location
else:
# Send the signal that a gas leak have been found and its location
self.__points_with_gas.append(
deepcopy(self.__current_pose))
msg = events_message()
msg.event = 'gas_leak'
msg.position.append(self.__current_pose)
self.__pub.publish(
msg) # Publish the found gas location
# self.__victims.remove(g) # Remove the found gas from the list
rate.sleep()
def __init__(self, name):
self.robot_name = name
self.state = 'IDLE' # Maneuver current state
self.suspending = False
self._move_client = SimpleActionClient(
"/{}/move_base".format(self.robot_name),
MoveBaseAction) # Server name = robot_name/move_base
self._move_client.wait_for_server()
self.pub = rospy.Publisher("/{}/maneuvers/out".format(name),
events_message,
queue_size=10) # Publisher object
self.msg = events_message() # Message object
def __init__(self, name):
self.robot_name = name
self.state = 'IDLE'
# Landing service
self._land_client = SimpleActionClient(
"safe_land_server",
ExecuteLandAction) # Create a client to the v_search server
self._land_client.wait_for_server() # Wait server to be ready
self._land_goal = ExecuteLandGoal() # Message to send the goal region
self.pub = rospy.Publisher("/{}/maneuvers/out".format(name),
events_message,
queue_size=10) # Publisher object
self.msg = events_message() # Message object
def __init__(self, name):
self.robot_name = name
self.state = 'IDLE'
#Variables to control the speed
self.max_vel = rospy.get_param("move_base/DWAPlannerROS/max_vel_x")
self.max_ang_vel = rospy.get_param(
"move_base/DWAPlannerROS/max_rot_vel")
self.current_x_speed = self.max_vel / 2
self.current_ang_speed = self.max_ang_vel / 2
self.__pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)
self.pub = rospy.Publisher("/{}/maneuvers/out".format(name),
events_message,
queue_size=10) # Publisher object
self.msg = events_message() # Message object
def __init__(self, name):
self.robot_name = name
self.suspending = False
self.state = 'IDLE' # Set IDLE state
self.region = [] # Region to be explored
# Exploration service
self._assess_client = SimpleActionClient(
"assessment_server", ExecuteDroneExplorationAction
) # Create a client to the v_search server
self._assess_client.wait_for_server() # Wait server to be ready
# Maneuvers out
self.pub = rospy.Publisher("/{}/maneuvers/out".format(name),
events_message,
queue_size=10) # Publisher object
self.msg = events_message() # Message object
def __init__(self, name):
self.robot_name = name
self.state = 'IDLE'
self.odometry = None
self.odometry_me = Condition()
#Variables to control the speed
self.max_vel = 0.8 #rospy.get_param()
self.max_ang_vel = 1.0 #rospy.get_param()
self.current_speed = self.max_vel / 2
self.current_ang_speed = self.max_ang_vel / 2
#Publish msg to execute the motion
self.__tele_pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)
self.__tele_msg = Twist()
# self.__last_time = rospy.get_time()
self.pub = rospy.Publisher("/{}/maneuvers/out".format(name),
events_message,
queue_size=10) # Publisher object
self.msg = events_message() # Message object
def __init__(self, name):
self.state = 'IDLE'
self.suspending = False
self.trajectory_client = SimpleActionClient(
"approach_server", ExecuteDroneApproachAction)
self.trajectory_client.wait_for_server()
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
rospy.Subscriber("ground_truth/state", Odometry, self.poseCallback)
self.sonar_me = Condition()
self.current_height = None
self.odometry_me = Condition()
self.odometry = None
self.pub = rospy.Publisher("/{}/maneuvers/out".format(name),
events_message,
queue_size=10) # Publisher object
self.msg = events_message() # Message object
def run(self):
'''
Main loop of the interface for generating events
'''
self.events_sub = rospy.Subscriber("/events_trigger_ihm_in", trace_events, self.events_callback)
pub = None
rate = rospy.Rate(10)
self.param = [] # Parameters of the event
while not rospy.is_shutdown():
#Get data from the Window
event, values = self.window.Read(timeout=10)
if event in (None, 'Cancel'): # If user closes window or clicks cancel
print('\nCLOSING EVENT INTERFACE ...\n')
break
if self.models_window:
event, m_values = self.models_window.Read(timeout=10)
if event in (None, 'Cancel'):
self.models_window.Close()
self.models_window = None
else:
values = {**values, **m_values}
## Second Window events ####################
if (event == 'models') and (not self.models_window):
initial_model = [e for e in self.models if (self.robots[1] + '_PLANT') in e][0]
initial_path = self.sm_path + initial_model
initial_values = [e for e in self.models if (self.robots[1] + '_PLANT') in e]
self.models_layout = [
# MODELS
[sg.Text("Model type: "), sg.Radio("PLANT", "type", key= 'plant_model', default = True, enable_events= True), sg.Radio("SUPERVISOR", "type", key = 'sup_model', enable_events= True)],
[sg.Text("Robot: "), sg.InputCombo(values = self.robots[1:], default_value= self.robots[1], key = 'models_robot_selected', size = (30,10), enable_events = True)],
[sg.Text("Model: "), sg.InputCombo(values = initial_values, default_value = initial_model, key = 'selected_model', size = (30,10), enable_events = True)],
[sg.Image(filename = initial_path, key="_IMAGE_", background_color="white")],
]
self.models_window = sg.Window("MODELS VISUALIZER", size=(650,500),layout = self.models_layout, finalize = True, resizable = True)
elif event == 'plant_model':
self.models_window.Element('selected_model').update(values = [e for e in self.models if (values['models_robot_selected'] + '_PLANT') in e])
elif event == 'sup_model':
self.models_window.Element('selected_model').update(values = [e for e in self.models if (values['models_robot_selected'] + '_SUP') in e])
elif event == 'models_robot_selected':
if values['plant_model']:
self.models_window.Element('selected_model').update(values = [e for e in self.models if (values['models_robot_selected'] + '_PLANT') in e])
else:
self.models_window.Element('selected_model').update(values = [e for e in self.models if (values['models_robot_selected'] + '_SUP') in e])
elif event == 'selected_model':
#Update the Automaton Image
try:
self.models_window.Element("_IMAGE_").update(filename = self.sm_path + values['selected_model'])
except:
pass
##############################################
# Change on trace option
if event in ['trace_option', 'refresh']:
self.window['tracer'].update('')
#Refresh tracer
if not self.trace.empty:
for i in self.trace.index:
if (values['trace_option'] == self.trace.at[i,'robot']) or (values['trace_option'] == 'all'):
if self.trace.at[i,'event_type'] == 'controllable':
color = 'blue'
else:
color = 'red'
if self.__events[self.trace.at[i,'event']].is_controllable():
color = 'blue'
else:
color = 'red'
text = self.trace.loc[[i]].drop(columns=['event_type','enabled_events']).to_string(header=False, justify='left')
self.window['tracer'].print(text, text_color=color)
elif event == 'save':
# Save content of tracer into a csv file
filename = values['save']
if filename:
if '.' not in filename:
filename += ".csv"
if '.csv' in filename:
self.trace.to_csv(filename)
else:
sg.Popup('Wrong file extension!', title='Saving failure!')
self.window['save'].update(value='')
## TRIGGER EVENTS
elif event == 'selected_robot':
# Update events to trigger
self.window.Element('selected_event').update('')
self.window.Element('selected_event').update(values = self.robots_events[values['selected_robot']])
if event == 'trigger':
# An event is triggered
try:
robot_namespace = '/' + values['selected_robot'] + '/'
event = values['selected_event']
# Translate non-controllable events to low-level call
ll_event = self.translation_table[(self.translation_table['high-level'] == event)]['low-level'].array[0]
topic = robot_namespace + self.translation_table[(self.translation_table['high-level'] == event)]['topic'].array[0]
if 'bat_' in event:
# Fake battery state change
topic = topic.replace('/out','/in') # Get battery_monitor/in topic
pub = rospy.Publisher("{}".format(topic), events_message, queue_size=10)
msg = events_message()
msg.event = ll_event
if (event == 'bat_OK') or (event == 'uav_bat_OK'):
msg.param.append(70.0) # At level = 60 the system consider bat_OK
elif (event == 'bat_L') or (event == 'uav_bat_L'):
msg.param.append(24.5) # At level = 25 the system consider bat_L
elif (event == 'bat_LL') or (event == 'uav_bat_LL'):
msg.param.append(9.0) # At level = 9 the system consider bat_LL
elif 'failure' in ll_event or 'rst_f' in event:
# Fake failures
topic = topic.replace('/out','/in') # Get failures_monitor/in topic
pub = rospy.Publisher("{}".format(topic), events_message, queue_size=10)
msg = events_message()
msg.event = ll_event
elif ('gas_sensor/out' in topic) or ('victim_sensor/out' in topic):
# Fake sensor error
if ll_event in ['erro','reset']:
topic = topic.replace('/out','/in') # Get sensor/in topic to fake a failure
pub = rospy.Publisher("{}".format(topic), events_message, queue_size=10)
msg = events_message()
msg.event = ll_event
elif ll_event in ['gas_leak', 'victim_recognized']:
if len(self.param) > 2:
pub = rospy.Publisher("{}".format(topic), events_message, queue_size=10)
msg = events_message()
msg.event = ll_event
p = Twist()
p.linear.x = self.param[0]
p.linear.y = self.param[1]
p.linear.z = self.param[2]
msg.param.append(p)
else:
sg.popup_error('Param error!')
elif 'error' in ll_event:
# Fake errors
topic = topic.replace('/out','/in') # Get failures_monitor/in topic
pub = rospy.Publisher("{}".format(topic), events_message, queue_size=10)
msg = events_message()
msg.event = ll_event
else:
# Another events
pub = rospy.Publisher("{}".format(topic), events_message, queue_size=10)
msg = events_message()
msg.event = ll_event
if pub:
while pub.get_num_connections() < 1:
rate.sleep()
pub.publish(msg)
self.param = [] # Clear param
self.window['param_list'].Update(values=self.param) # Clear param screen
except:
sg.popup_error("No robot or event selected!")
elif event == 'add_param':
# Add a new item as parameter for the event
if values['new_param']:
self.param.append(eval(values['new_param']))
self.window['new_param'].update('')
self.window['param_list'].Update(values=self.param)
elif event == 'remove_param':
# Remove an item from the list of parameters
if self.window['param_list'].GetIndexes():
item = self.window['param_list'].GetIndexes()[0]
self.param.pop(item)
self.window['param_list'].Update(values=self.param)
# New event occured
if self.new_trace:
self.new_trace = False
# Update tracer screen
# if self.trace.tail(1).index[0] > 0:
if (values['trace_option'] == 'all') or (values['trace_option'] == self.trace.tail(1).iloc[0]['robot']):
if self.trace.tail(1).iloc[0]['event_type'] == 'controllable':
color = 'blue'
else:
color = 'red'
text = self.trace.tail(1).drop(columns=['event_type','enabled_events']).to_string(header=False, justify='left')
self.window['tracer'].print(text, text_color=color)
self.trace.to_csv(trace_filename)
if self.update_allowed_events:
self.update_allowed_events = False
# Update events to trigger
self.window.Element('selected_event').update('')
self.window.Element('selected_event').update(values = self.robots_events[values['selected_robot']])
if self.update_model:
if self.models_window:
self.models_window.Element("_IMAGE_").update(filename= self.sm_path + values['selected_model'])
self.update_model = False
self.window.Close()
def run(self):
'''
Main loop of the interface for generating events
'''
self.update_missions = False # Flag the need of update of the table
edit_mission_window = None # Singal if the Edit window is ON
selected_mission = None # Current selected mission ID
while not rospy.is_shutdown():
#Get data from the Window
event, values = self.window.Read(timeout=100)
if event in (None,
'Cancel'): # If user closes window or clicks cancel
print('\nCLOSING COMMANDER INTERFACE ...\n')
break
if edit_mission_window:
event, m_values = edit_mission_window.Read(timeout=10)
if event in (None, 'Cancel'):
edit_mission_window.Close()
edit_mission_window = None
else:
values = {**values, **m_values}
# Get selected mission id
if values['missions'] and not edit_mission_window:
table = self.window.Element('missions').Get()
selected_mission = table[values['missions'][0]][0]
msg = events_message()
if (event == 'robot_to_call') or self.update_robot_info:
for b in self.robots_buttons[values['robot_to_call']]:
self.window[b].update(disabled=not self.robots_buttons[
values['robot_to_call']][b])
elif event == 'tele':
if values['robot_to_call'] in self.requisitions:
self.requisitions.pop(
self.requisitions == values['robot_to_call'])
self.window['requisitions'].update('')
for r in self.requisitions:
self.odometry_me.acquire()
text = r + " at --> " + self.robots_info[r][2]
self.odometry_me.release()
self.window['requisitions'].print(text,
text_color='red')
msg.event = 'teleoperation_requisition'
self.publishers.loc[values['robot_to_call'],
'communication'].publish(msg)
elif event == 'rst_f':
msg.event = 'reset'
self.publishers.loc[values['robot_to_call'],
'fail_monitor'].publish(msg)
elif event == 'rst_vs':
msg.event = 'reset'
self.publishers.loc[values['robot_to_call'], 'vs'].publish(msg)
elif event == 'rst_gs':
msg.event = 'reset'
self.publishers.loc[values['robot_to_call'], 'gs'].publish(msg)
elif event == 'Add':
add_layout = [[
sg.Text('Mission ID:', size=(17, 1)),
sg.Input(key='m_id', size=(13, 1))
],
[
sg.Text('Mission PRIORITY:', size=(17, 1)),
sg.Input(key='m_p', size=(13, 1))
], [sg.OK(), sg.Cancel()]]
add_window = sg.Window('NEW MISSION',
layout=add_layout,
size=(300, 100))
add_event, add_values = add_window.Read()
add_window.Close()
if add_event == 'OK':
if add_values['m_id'] not in self.missions.index:
if (int(float(add_values['m_p'])) < 10) and (int(
float(add_values['m_p'])) >= 0):
self.missions.loc[add_values['m_id']] = [
int(float(add_values['m_p'])), 0, 0, 0,
'empty',
Mission(add_values['m_id'],
int(float(add_values['m_p'])))
]
self.update_missions = True
self.missions_details[add_values['m_id']] = []
else:
sg.popup_error(
'WRONG MISSION PRIORITY',
'Mission PRIORITY must be between 0 and 10!')
else:
sg.popup_error('WRONG MISSION ID',
'Mission ID already exist!')
elif event == 'Load':
load_path = sg.popup_get_file('Mission to load')
if load_path:
loaded_mission = Mission()
loaded_mission.load(load_path)
mission_id = loaded_mission.id
mission_priority = loaded_mission.priority
n_tasks = len(loaded_mission.tasks.index)
self.missions.loc[mission_id] = [
int(float(mission_priority)), n_tasks, 0, 0,
'not sended',
Mission(mission_id, int(float(mission_priority)))
]
self.missions_details[mission_id] = []
seq = 0
for t in loaded_mission.tasks.index:
if not loaded_mission.tasks.loc[t, 'agent']:
agent = ''
else:
agent = loaded_mission.tasks.loc[t, 'agent']
maneuver = loaded_mission.tasks.loc[t, 'maneuver']
vs = loaded_mission.tasks.loc[t, 'vs']
gs = loaded_mission.tasks.loc[t, 'gs']
if maneuver == 'approach':
p = loaded_mission.tasks.loc[t, 'position']
pos = [p['x'], p['y'], p['z'], p['theta']]
reg = []
elif maneuver in ['assessment', 'search']:
r = loaded_mission.tasks.loc[t, 'region']
pos = []
reg = [r['x0'], r['y0'], r['x1'], r['y1']]
else:
pos = []
reg = []
task = [seq, maneuver, agent, pos, reg, vs, gs]
seq += 1
self.missions_details[mission_id].append(task)
self.update_missions = True
elif (event == 'View/Edit') and (
not edit_mission_window) and selected_mission:
edit_m_layout = [[
sg.Table(values=[['', '', '', '', '']],
size=(70, 18),
background_color='white',
text_color='black',
col_widths=[15, 15, 15, 15, 15, 20, 15],
auto_size_columns=False,
justification='left',
key='tasks',
headings=self.tasks_menu)
],
[
sg.Button('ADD TASK',
key='add_task',
size=(6, 1),
button_color=('white',
'green')),
sg.Button('REMOVE TASK',
key='remove_task',
size=(10, 1),
button_color=('white', 'red')),
sg.Button('/\\',
key='task_up',
size=(3, 1),
button_color=('white',
'purple')),
sg.Button('\\/',
key='task_down',
size=(3, 1),
button_color=('white',
'purple'))
]]
edit_mission_window = sg.Window('MISSION DETAILS',
layout=edit_m_layout,
size=(1050, 370),
finalize=True,
resizable=True)
if self.missions_details[selected_mission]:
edit_mission_window.Element('tasks').update(
values=self.missions_details[selected_mission])
event, m_values = edit_mission_window.Read(timeout=10)
values = {**values, **m_values}
# {'agent': None, 'position': None, 'region': None, 'vs': False, 'gs': False, 'maneuver': None}
# ['sequence', 'maneuver', 'agent', 'position', 'region', 'victim sensor', 'gas sensor']
elif (event == 'Send') and selected_mission and (
selected_mission not in self.sended_missions):
if self.missions_details[selected_mission]:
for t in self.missions_details[selected_mission]:
task = self.missions.loc[selected_mission,
'object'].get_std_task()
task['maneuver'] = t[1]
task['agent'] = t[2]
if t[3]:
task['position'] = {
'x': float(t[3][0]),
'y': float(t[3][1]),
'z': float(t[3][2]),
'theta': float(t[3][3])
}
if t[4]:
task['region'] = {
'x0': float(t[4][0]),
'y0': float(t[4][1]),
'x1': float(t[4][2]),
'y1': float(t[4][3])
}
task['vs'] = t[5]
task['gs'] = t[6]
task = self.missions.loc[selected_mission,
'object'].add_task(task)
self.sended_missions.append(selected_mission)
#Send mission to task allocator
path = os.path.dirname(os.path.abspath(__file__))
mission_path = path + '/missions/{}.xml'.format(
selected_mission)
self.missions.loc[selected_mission,
'object'].save(mission_path)
self.missions.at[selected_mission, 'status'] = 'idle'
msg = mission()
msg.id = selected_mission
msg.filename = mission_path
msg.priority = self.missions.at[selected_mission,
'priority']
self.mission_pub.publish(msg)
self.update_missions = True
else:
sg.popup_error('EMPTY MISSION',
'Can not send empty mission!')
elif (event == 'Remove') and selected_mission:
# Send abort mission to task allocator
if selected_mission in self.sended_missions:
msg = mission()
msg.id = selected_mission
msg.priority = -1
self.mission_pub.publish(msg)
self.missions.drop(selected_mission, inplace=True)
self.missions_details.pop(selected_mission)
self.update_missions = True
## EDIT MISSION Window events ####################
# Get selected task
if edit_mission_window and values['tasks']:
task_table = edit_mission_window.Element('tasks').Get()
selected_task = task_table[values['tasks'][0]][0]
else:
selected_task = None
if selected_mission not in self.sended_missions:
if event == 'add_task':
robots = ['']
robots += self.robots
maneuvers = [
'approach', 'assessment', 'search', 'return_to_base'
]
add_t_layout = [
[
sg.Text('Agent:', size=(9, 1)),
sg.InputCombo(robots,
default_value='',
key='agent',
size=(17, 1),
enable_events=True)
],
[
sg.Text('Maneuver:', size=(9, 1)),
sg.InputCombo(maneuvers,
default_value=maneuvers[0],
key='maneuver',
size=(17, 1),
enable_events=True)
],
[
sg.Frame('Position',
[[
sg.Text('x:', size=(3, 1)),
sg.Input(key='pos_x',
size=(6, 1),
disabled=False),
sg.Text('y:', size=(3, 1)),
sg.Input(key='pos_y',
size=(6, 1),
disabled=False)
],
[
sg.Text('z:', size=(3, 1)),
sg.Input(key='pos_z',
size=(6, 1),
disabled=False),
sg.Text('\u03F4:', size=(3, 1)),
sg.Input(key='theta',
size=(6, 1),
disabled=False)
]])
],
[
sg.Frame('Region', [
[
sg.Text('x0:', size=(3, 1)),
sg.Input(key='reg_x0',
size=(6, 1),
disabled=True),
sg.Text('y0:', size=(3, 1)),
sg.Input(key='reg_y0',
size=(6, 1),
disabled=True)
],
[
sg.Text('\u0394x:', size=(3, 1)),
sg.Input(key='reg_x',
size=(6, 1),
disabled=True),
sg.Text('\u0394y:', size=(3, 1)),
sg.Input(key='reg_y',
size=(6, 1),
disabled=True)
],
])
],
[
sg.Text('Victim Sensor Status:', size=(21, 1)),
sg.InputCombo([False, True],
default_value='False',
key='vs_status')
],
[
sg.Text('Gas Sensor Status:', size=(21, 1)),
sg.InputCombo([False, True],
default_value='False',
key='gs_status')
], [sg.OK(), sg.Cancel()]
]
add_window = sg.Window('NEW TASK',
layout=add_t_layout,
size=(250, 330),
finalize=True)
# ['sequence', 'maneuver', 'agent', 'position', 'region', 'victim sensor', 'gas sensor']
while True:
e, v = add_window.Read()
if e in (None, 'Cancel'):
add_window.Close()
break
elif e == 'OK':
## Tasks is the next on the mission
if not self.missions_details[selected_mission]:
seq = 0
else:
seq = len(
self.missions_details[selected_mission])
ok = True
for x in [
'pos_x', 'pos_y', 'pos_z', 'theta',
'reg_x0', 'reg_y', 'reg_x', 'reg_y'
]:
if v[x] and not v[x].replace('.', '',
1).isdigit():
ok = False
if ok:
if v['maneuver'] == 'approach':
pos = [
v['pos_x'], v['pos_y'], v['pos_z'],
v['theta']
]
reg = []
elif v['maneuver'] in ['assessment', 'search']:
pos = []
reg = [
v['reg_x0'], v['reg_y0'], v['reg_x'],
v['reg_y']
]
else:
pos = []
reg = []
task = [
seq, v['maneuver'], v['agent'], pos, reg,
v['vs_status'], v['gs_status']
]
self.missions_details[selected_mission].append(
task)
self.missions.loc[selected_mission,
'n_tasks'] += 1
self.missions.loc[selected_mission,
'status'] = 'not sended'
add_window.Close()
edit_mission_window.Element('tasks').update(
values=self.
missions_details[selected_mission])
self.update_missions = True
else:
sg.popup_error(
'WRONG VALUES ATTRIBUTION',
'Position and Region must be a number!')
elif e == 'agent':
if 'UAV' in v['agent']:
add_window['maneuver'].update(values=[
'approach', 'assessment', 'search',
'return_to_base'
])
add_window['gs_status'].update(values=[False])
elif 'pioneer3at' in v['agent']:
add_window['maneuver'].update(values=[
'approach', 'search', 'return_to_base'
])
add_window['gs_status'].update(
values=[False, True])
else:
add_window['maneuver'].update(values=[
'approach', 'assessment', 'search',
'return_to_base'
])
add_window['gs_status'].update(
values=[False, True])
elif e == 'maneuver':
if v['maneuver'] in ['assessment', 'search']:
add_window['pos_x'].update(disabled=True)
add_window['pos_y'].update(disabled=True)
add_window['pos_z'].update(disabled=True)
add_window['reg_x0'].update(disabled=False)
add_window['reg_y0'].update(disabled=False)
add_window['reg_x'].update(disabled=False)
add_window['reg_y'].update(disabled=False)
elif v['maneuver'] == 'approach':
add_window['pos_x'].update(disabled=False)
add_window['pos_y'].update(disabled=False)
add_window['pos_z'].update(disabled=False)
add_window['reg_x0'].update(disabled=True)
add_window['reg_y0'].update(disabled=True)
add_window['reg_x'].update(disabled=True)
add_window['reg_y'].update(disabled=True)
else:
add_window['pos_x'].update(disabled=True)
add_window['pos_y'].update(disabled=True)
add_window['pos_z'].update(disabled=True)
add_window['reg_x0'].update(disabled=True)
add_window['reg_y0'].update(disabled=True)
add_window['reg_x'].update(disabled=True)
add_window['reg_y'].update(disabled=True)
elif (event == 'remove_task') and selected_task != None:
self.missions_details[selected_mission]
self.missions_details[selected_mission].pop(selected_task)
for i in self.missions_details[selected_mission][
selected_task:]:
self.missions_details[selected_mission][
i[0] - 1][0] = i[0] - 1
self.missions.loc[selected_mission, 'n_tasks'] -= 1
edit_mission_window.Element('tasks').update(
values=self.missions_details[selected_mission])
if not self.missions_details[selected_mission]:
self.missions.loc[selected_mission, 'status'] = 'empty'
self.update_missions = True
elif event == 'task_up' and selected_task != None:
if selected_task > 0:
temp = self.missions_details[selected_mission][
selected_task - 1]
self.missions_details[selected_mission][selected_task][
0] -= 1
self.missions_details[selected_mission][
selected_task - 1] = self.missions_details[
selected_mission][selected_task]
self.missions_details[selected_mission][
selected_task] = temp
self.missions_details[selected_mission][selected_task][
0] += 1
edit_mission_window.Element('tasks').update(
values=self.missions_details[selected_mission])
elif event == 'task_down' and selected_task != None:
if selected_task < len(
self.missions_details[selected_mission]):
temp = self.missions_details[selected_mission][
selected_task + 1]
self.missions_details[selected_mission][selected_task][
0] += 1
self.missions_details[selected_mission][
selected_task + 1] = self.missions_details[
selected_mission][selected_task]
self.missions_details[selected_mission][
selected_task] = temp
self.missions_details[selected_mission][selected_task][
0] -= 1
edit_mission_window.Element('tasks').update(
values=self.missions_details[selected_mission])
else:
if event in [
'add_task', 'remove_task', 'task_up', 'task_down'
]:
sg.popup_error(
'NOT EDITABLE',
'Already sended missions are not editable!')
##################################################
if self.update_missions:
self.update_missions = False
table = []
self.window.Element('missions').update(values=[])
for m in self.missions.index:
table.append([
m, self.missions.at[m, 'priority'],
self.missions.at[m, 'n_tasks'],
self.missions.at[m, 'current_task'],
self.missions.at[m, 'progress'],
self.missions.at[m, 'status']
])
self.window.Element('missions').update(values=table)
if self.update_robot_info:
self.update_robot_info = False
v = []
for r in self.robots_info:
v.append(self.robots_info[r])
self.window.Element('robots_info').update(values=v)
self.window.Close()
def run(self):
self.event_type = 'uncontrollable' # Event type selected on Radio
self.enabled_e = [] # List of enabled events
self.param = [] # Parameters of the event
trace = Thread(
target=self.events_trace) # Thread for the tracer monitor
trace.start() # Start event tracer as a thread
while True:
#Get data from the Window
event, values = self.window.Read(timeout=10)
if event in (None,
'Cancel'): # If user closes window or clicks cancel
print('\nCLOSING EVENT INTERFACE ...\n')
break
# New event occured on the Prodct System
if self.new_trace == True:
self.new_trace = False
#Update tracer screen
if self.trace.tail(1).index[0] > 0:
if self.__events[self.trace.tail(1)
['event'].values[0]].is_controllable():
color = 'blue'
else:
color = 'red'
text = self.trace.tail(1).drop(
columns=['enabled_events', 'states']).to_string(
header=False, justify='left')
self.window['tracer'].print(text, text_color=color)
#Update the Automaton Image
try:
self.window.Element("_IMAGE_").update(
filename="output/" + values['option'] + ".png")
except:
pass
if event == 'option':
#Update the Automaton Image
try:
self.window.Element("_IMAGE_").update(
filename="output/" + values['option'] + ".png")
except:
pass
elif event == 'controllable':
#Show list of enabled controllable events
self.window.Element('selected_event').update(
values=self.enabled_e)
self.event_type = 'controllable'
elif event == 'uncontrollable':
#Show list of uncontrollable events
self.window.Element('selected_event').update(
values=self.__not_cont_e)
self.event_type = 'uncontrollable'
if event == 'trigger':
# An event is triggered
if values['controllable'] == True:
trigger_event(
values['selected_event'], self.param
) # Call the execution of the controllable event
else:
# Trigger uncontrollable events
event = values['selected_event']
# Translate non-controllable events to low-level call
ll_event = self.translation_table[(
self.translation_table['high-level'] == event
)]['low-level'].array[0]
topic = self.translation_table[(
self.translation_table['high-level'] == event
)]['topic'].array[0]
# Fake uncontrollable events
if 'erro' in ll_event:
if 'maneuvers/out' in topic:
# Fake maneuver error
move_base_client = SimpleActionClient(
"{}move_base".format(rospy.get_namespace()),
MoveBaseAction) # Get move_base service
move_base_client.wait_for_server()
move_base_client.cancel_all_goals(
) # Cancel the current motion
elif ('gas_sensor/out'
in topic) or ('victim_sensor/out' in topic):
# Fake sensor error
topic = topic.replace(
'/out',
'/in') # Get sensor/in topic to fake a failure
pub = rospy.Publisher("/{}".format(topic),
events_message,
queue_size=10)
msg = events_message()
msg.event = ll_event
pub.publish(msg) # Publish to the sensor topic
elif 'bat_' in event:
# Fake battery state change
topic = topic.replace(
'/out', '/in') # Get battery_monitor/in topic
pub = rospy.Publisher("/{}".format(topic),
events_message,
queue_size=10)
msg = events_message()
msg.event = ll_event
if event == 'bat_OK':
msg.param.append(
60.0
) # At level = 60 the system consider bat_OK
elif event == 'bat_L':
msg.param.append(
30.0
) # At level = 30 the system consider bat_L
elif event == 'bat_LL':
msg.param.append(
9.0) # At level = 9 the system consider bat_LL
pub.publish(
msg) # Publish to the battery_monitor topic
elif 'failure' in ll_event:
# Fake failures
topic = topic.replace(
'/out', '/in') # Get failures_monitor/in topic
pub = rospy.Publisher("/{}".format(topic),
events_message,
queue_size=10)
msg = events_message()
msg.event = ll_event
pub.publish(
msg) # Publish to the failures_monitor topic
elif 'ihm' in topic:
# Fake commander comands
pub = rospy.Publisher("/{}".format(topic),
events_message,
queue_size=10)
msg = events_message()
msg.event = ll_event
pub.publish(msg) # Publish to the IHM/out topic
self.param = [] # Clear param
self.window['param_list'].Update(
values=self.param) # Clear param screen
elif event == 'refresh':
self.window['tracer'].update('')
#Refresh tracer
if not self.trace.empty:
for i in self.trace.iloc[1:].index:
if self.__events[self.trace.at[
i, 'event']].is_controllable():
color = 'blue'
else:
color = 'red'
text = self.trace.loc[[i]].drop(
columns=['enabled_events', 'states']).to_string(
header=False, justify='left')
self.window['tracer'].print(text, text_color=color)
elif event == 'save':
# Save content of tracer into a csv file
filename = values['save']
if filename:
if '.' not in filename:
filename += ".csv"
if '.csv' in filename:
self.trace.drop(columns=['enabled_events', 'states'
]).to_csv(filename)
else:
sg.Popup('Wrong file extension!',
title='Saving failure!')
self.window['save'].update(value='')
elif event == 'add_param':
# Add a new item as parameter for the event
if values['new_param']:
self.param.append(eval(values['new_param']))
self.window['new_param'].update('')
self.window['param_list'].Update(values=self.param)
elif event == 'remove_param':
# Remove an item from the list of parameters
if self.window['param_list'].GetIndexes():
item = self.window['param_list'].GetIndexes()[0]
self.param.pop(item)
self.window['param_list'].Update(values=self.param)
self.window.Close()
import rospy
from random import random as rd
from time import sleep
from actionlib import SimpleActionClient, GoalStatus
from trajectory_action_pkg.msg import ExecuteDroneApproachAction, ExecuteDroneApproachGoal
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from gazebo_msgs.srv import GetModelState
DIST = 1.5
trajectory_clients = {}
end_publishers = {}
st_sub = {}
msg2pub = events_message()
models_service = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState) # Get model service from Gazebo
def maneuver_event(msg, robot):
trials = 0
if msg.event == "start_teleoperation":
robot_state = models_service(robot, '')
while trials < 4:
# Try to move
if 'UAV' in robot:
dest = ExecuteDroneApproachGoal()
dest.goal.position.x = robot_state.pose.position.x + 2 * DIST * rd(