def send_plan():
cnh.Buffer().clear_reached_poses()
# send plan
if len(pc.PathContainer()) > 0:
sel_robot_id = bpy.context.scene.selected_robot_props.prop_robot_id
r = robot.RobotSet().getRobot(sel_robot_id)
pose_robot = r.pose
poses_list = pc.PathContainer().poses
robot_obj = bpy.data.objects[r.name]
area_robot_obj = bpy.data.objects[r.area_name]
if pose_robot != poses_list[0]:
collide = pathEditor.is_colliding(sel_robot_id, robot_obj,
area_robot_obj,
pose_robot,
poses_list[0])
if collide:
self.report({
"ERROR"
}, "Collision : robot cannot be moved to the indicated start position"
)
return
# Comprobar colision para el resto de poses de la ruta
for pose_index in range(len(poses_list) - 1):
collide = pathEditor.is_colliding(
sel_robot_id, robot_obj, area_robot_obj,
poses_list[pose_index], poses_list[pose_index + 1])
if collide:
self.report(
{"ERROR"},
"Collision: robot cannot execute current path")
return
bpy.context.scene.com_props.prop_last_sent_packet += 1
pid = bpy.context.scene.com_props.prop_last_sent_packet
status = cnh.ConnectionHandler().send_plan(pid, poses_list)
self.report({"INFO"},
"Plan was sent" if status == len(poses_list) else
"Plan fail {0}".format(status))
if status == len(poses_list):
com_props.prop_last_sent_packet += 1
pid = com_props.prop_last_sent_packet
cnh.ConnectionHandler().send_start_plan(pid)
com_props.prop_running_nav = True
com_props.prop_paused_nav = False
else:
self.report({
"ERROR"
}, "There is not plan created : Please, design a plan to execute"
)
# update path status
context.scene.com_props.prop_last_path_update = pc.PathContainer(
).getLastUpdate()
def execute(self, context):
wm = context.window_manager
self._timer = wm.event_timer_add(0.1, window=context.window)
wm.modal_handler_add(self)
SocketModalOperator.switching = True
sel_rob_id = context.scene.selected_robot_props.prop_robot_id
if sel_rob_id < 0:
SocketModalOperator.error = "Robot not selected"
return {'RUNNING_MODAL'}
r = robot.RobotSet().getRobot(sel_rob_id)
client_ip = r.client_ip
client_port = r.client_port
try:
cnh.ConnectionHandler().create_socket((client_ip, client_port),
(r.ip, r.port))
except:
SocketModalOperator.error = "error in socket creation"
return {'RUNNING_MODAL'}
import threading
self.thread = threading.Thread(target=SocketModalOperator.run,
args=(self, ))
self.thread.start()
context.scene.com_props.prop_last_sent_packet += 1
new_pid = context.scene.com_props.prop_last_sent_packet
new_mode = robot_modes_summary.index("ROBOT_MODE")
initial_speed = context.scene.com_props.prop_speed
if not cnh.ConnectionHandler().send_change_mode(
new_pid, new_mode, initial_speed):
SocketModalOperator.error = "server not available : could not be switched to robot mode / no ack received"
SocketModalOperator.switching = False
self.closed = True
return {'RUNNING_MODAL'}
SocketModalOperator.switching = False
toggle_deactivate_options(robot_modes_summary.index("ROBOT_MODE"))
SocketModalOperator.error = ""
SocketModalOperator.closed = False
bpy.context.scene.com_props.prop_mode = robot_modes_summary.index(
"ROBOT_MODE")
return {'RUNNING_MODAL'}
def cancel(self, context):
"""
Se ejecuta al cerrar la aplicación sin salir de modo robot
"""
if context.scene.com_props.prop_running_nav:
context.scene.com_props.prop_last_sent_packet += 1
pid = context.scene.com_props.prop_last_sent_packet
if not cnh.ConnectionHandler().send_stop_plan(pid):
self.report({'ERROR'}, "Can not stop plan : no ack received")
else:
context.scene.com_props.prop_running_nav = False
context.scene.com_props.prop_paused_nav = False
if context.scene.com_props.prop_mode == robot_modes_summary.index(
"ROBOT_MODE"):
context.scene.com_props.prop_last_sent_packet += 1
new_pid = context.scene.com_props.prop_last_sent_packet
new_mode = robot_modes_summary.index("EDITOR_MODE")
initial_speed = context.scene.com_props.prop_speed
context.scene.com_props.prop_mode = robot_modes_summary.index(
"EDITOR_MODE")
if not cnh.ConnectionHandler().send_change_mode(
new_pid, new_mode, initial_speed):
self.report({
'ERROR'
}, "Changed to editor mode but not in server : ack not received"
)
SocketModalOperator.running = False
cnh.ConnectionHandler().remove_socket()
if context.scene.is_cursor_active:
bpy.ops.scene.stop_cursor_listener()
if RUNNING_STATUS in hudWriter.HUDWriterOperator._textos:
del hudWriter.HUDWriterOperator._textos[RUNNING_STATUS]
if CAPTURE_STATUS in hudWriter.HUDWriterOperator._textos:
del hudWriter.HUDWriterOperator._textos[CAPTURE_STATUS]
context.scene.com_props.prop_capture_running = False
def execute(self, context):
older_speed = context.scene.com_props.prop_speed
curre_speed = self.update_speed
context.scene.com_props.prop_last_sent_packet += 1
pid = context.scene.com_props.prop_last_sent_packet
if not cnh.ConnectionHandler().send_change_speed(pid, curre_speed):
self.report({"ERROR", "Speed can not be changed"})
else:
context.scene.com_props.prop_speed = curre_speed
return {'FINISHED'}
def run(operator):
hudWriter.HUDWriterOperator._textos[
APP_STATUS] = SocketModalOperator.ROBOT_MODE
limit = 10
n_rcv = limit
SocketModalOperator.running = True
while SocketModalOperator.running:
if cnh.ConnectionHandler().hasSocket():
rcv = cnh.ConnectionHandler().receive_packet(operator)
if rcv:
n_rcv = limit
else:
operator.report({'INFO'}, 'No communication')
n_rcv -= 1
if n_rcv == 0:
n_rcv = limit
operator.report({'ERROR'}, 'Unavailable server: changing mode')
bpy.ops.wm.change_mode()
hudWriter.HUDWriterOperator._textos[
APP_STATUS] = SocketModalOperator.EDITOR_MODE # hudWriter.Texto(text="EDITOR mode")
def execute(self, context):
context.scene.com_props.prop_last_sent_packet += 1
pid = context.scene.com_props.prop_last_sent_packet
cnh.ConnectionHandler().send_stop_plan(pid)
context.scene.com_props.prop_running_nav = False
context.scene.com_props.prop_paused_nav = False
context.scene.com_props.prop_last_path_update = -1
cnh.Buffer().clear_reached_poses()
if RUNNING_STATUS in hudWriter.HUDWriterOperator._textos:
del hudWriter.HUDWriterOperator._textos[RUNNING_STATUS]
return {'FINISHED'}
def _apply_move(self, context, value, type):
speed = context.scene.com_props.prop_speed
displacement = 1
pid = context.scene.com_props.prop_last_sent_packet + 1
action = {
('PRESS', 'W'):
lambda: ch.ConnectionHandler().send_manual_translation_packet(
pid, 0, +displacement, speed),
('PRESS', 'S'):
lambda: ch.ConnectionHandler().send_manual_translation_packet(
pid, 0, -displacement, speed),
('PRESS', 'D'):
lambda: ch.ConnectionHandler().send_manual_translation_packet(
pid, +displacement, 0, speed),
('PRESS', 'A'):
lambda: ch.ConnectionHandler().send_manual_translation_packet(
pid, -displacement, 0, speed),
('PRESS', 'RIGHT_ARROW'):
lambda: ch.ConnectionHandler().send_manual_rotation_packet(
pid, +displacement, speed),
('PRESS', 'LEFT_ARROW'):
lambda: ch.ConnectionHandler().send_manual_rotation_packet(
pid, -displacement, speed),
('RELEASE', 'W'):
lambda: ch.ConnectionHandler().send_manual_stop_packet(pid),
('RELEASE', 'S'):
lambda: ch.ConnectionHandler().send_manual_stop_packet(pid),
('RELEASE', 'D'):
lambda: ch.ConnectionHandler().send_manual_stop_packet(pid),
('RELEASE', 'A'):
lambda: ch.ConnectionHandler().send_manual_stop_packet(pid),
('RELEASE', 'RIGHT_ARROW'):
lambda: ch.ConnectionHandler().send_manual_stop_packet(pid),
('RELEASE', 'LEFT_ARROW'):
lambda: ch.ConnectionHandler().send_manual_stop_packet(pid)
}
last_sent = ManualControlEventsOperator._last_sent
if last_sent is not None and last_sent[0] == value and last_sent[
1] == type:
return {'RUNNING_MODAL'}
if (value, type) not in action: return {'PASS_THROUGH'}
# Send message
rescode = action[(value, type)]()
print("ACK rescode from send translation ", rescode)
context.scene.com_props.prop_last_sent_packet = pid
ManualControlEventsOperator._last_sent = (value, type)
if rescode is not None and rescode:
return {'RUNNING_MODAL'} # is correct
else:
self.report({'ERROR'}, f"WARNING: Robot could not be stopped")
return {'RUNNING_MODAL'}
def execute(self, context):
com_props = context.scene.com_props
path_changed = com_props.prop_last_path_update < pc.PathContainer(
).getLastUpdate()
def send_plan():
cnh.Buffer().clear_reached_poses()
# send plan
if len(pc.PathContainer()) > 0:
sel_robot_id = bpy.context.scene.selected_robot_props.prop_robot_id
r = robot.RobotSet().getRobot(sel_robot_id)
pose_robot = r.pose
poses_list = pc.PathContainer().poses
robot_obj = bpy.data.objects[r.name]
area_robot_obj = bpy.data.objects[r.area_name]
if pose_robot != poses_list[0]:
collide = pathEditor.is_colliding(sel_robot_id, robot_obj,
area_robot_obj,
pose_robot,
poses_list[0])
if collide:
self.report({
"ERROR"
}, "Collision : robot cannot be moved to the indicated start position"
)
return
# Comprobar colision para el resto de poses de la ruta
for pose_index in range(len(poses_list) - 1):
collide = pathEditor.is_colliding(
sel_robot_id, robot_obj, area_robot_obj,
poses_list[pose_index], poses_list[pose_index + 1])
if collide:
self.report(
{"ERROR"},
"Collision: robot cannot execute current path")
return
bpy.context.scene.com_props.prop_last_sent_packet += 1
pid = bpy.context.scene.com_props.prop_last_sent_packet
status = cnh.ConnectionHandler().send_plan(pid, poses_list)
self.report({"INFO"},
"Plan was sent" if status == len(poses_list) else
"Plan fail {0}".format(status))
if status == len(poses_list):
com_props.prop_last_sent_packet += 1
pid = com_props.prop_last_sent_packet
cnh.ConnectionHandler().send_start_plan(pid)
com_props.prop_running_nav = True
com_props.prop_paused_nav = False
else:
self.report({
"ERROR"
}, "There is not plan created : Please, design a plan to execute"
)
# update path status
context.scene.com_props.prop_last_path_update = pc.PathContainer(
).getLastUpdate()
if com_props.prop_running_nav:
if com_props.prop_paused_nav:
if path_changed:
send_plan()
self.report({
'INFO'
}, "Paused plan: cancel current plan and start a new plan")
hudWriter.HUDWriterOperator._textos[
RUNNING_STATUS] = SocketModalOperator.RUNNING_PLAN
else:
com_props.prop_last_sent_packet += 1
pid = com_props.prop_last_sent_packet
cnh.ConnectionHandler().send_resume_plan(pid)
com_props.prop_running_nav = True
com_props.prop_paused_nav = False
self.report({'INFO'}, "Paused plan: resume current plan")
hudWriter.HUDWriterOperator._textos[
RUNNING_STATUS] = SocketModalOperator.RUNNING_PLAN
else:
com_props.prop_last_sent_packet += 1
pid = com_props.prop_last_sent_packet
cnh.ConnectionHandler().send_pause_plan(pid)
com_props.prop_running_nav = True
com_props.prop_paused_nav = True
self.report({'INFO'}, "Running plan: pause current plan")
hudWriter.HUDWriterOperator._textos[
RUNNING_STATUS] = SocketModalOperator.PAUSED_PLAN
else:
if com_props.prop_paused_nav:
self.report({'ERROR'}, "not running and paused")
com_props.prop_running_nav = False
com_props.prop_paused_nav = False
else:
send_plan()
hudWriter.HUDWriterOperator._textos[
RUNNING_STATUS] = SocketModalOperator.RUNNING_PLAN
return {'FINISHED'}
def modal(self, context, event):
if event.type == "TIMER":
if context.scene.com_props.prop_running_nav and not context.scene.com_props.prop_paused_nav:
last_pose = pc.PathContainer().poses[-1]
reached_poses = cnh.Buffer().get_reached_poses()
num_reached_poses = len(reached_poses)
num_path_poses = len(pc.PathContainer().poses)
end_reached = cnh.Buffer().end_reached()
if num_reached_poses == num_path_poses and end_reached:
context.scene.com_props.prop_running_nav = False
context.scene.com_props.prop_paused_nav = False
if RUNNING_STATUS in hudWriter.HUDWriterOperator._textos:
del hudWriter.HUDWriterOperator._textos[RUNNING_STATUS]
if num_reached_poses != num_path_poses and end_reached:
self.report({'ERROR'}, "End reached not expected")
context.scene.com_props.prop_running_nav = False
context.scene.com_props.prop_paused_nav = False
capture_started = cnh.Buffer().capture_started()
capture_ended = cnh.Buffer().capture_ended()
if capture_started:
prop_capture_running = context.scene.com_props.prop_capture_running
if prop_capture_running:
self.report({'ERROR'}, "Capture is already started")
else:
context.scene.com_props.prop_capture_running = True
self.report({'INFO'}, "Capture started")
hudWriter.HUDWriterOperator._textos[
CAPTURE_STATUS] = SocketModalOperator.CAPTURE_ON
if capture_ended:
prop_capture_running = context.scene.com_props.prop_capture_running
if not prop_capture_running:
self.report({'ERROR'}, "Capture was not started")
else:
context.scene.com_props.prop_capture_running = False
self.report({'INFO'}, "Capture ended")
if CAPTURE_STATUS in hudWriter.HUDWriterOperator._textos:
del hudWriter.HUDWriterOperator._textos[CAPTURE_STATUS]
if not SocketModalOperator.closed and context.scene.com_props.prop_rendering:
# Render robot position
sel_robot_id = bpy.context.scene.selected_robot_props.prop_robot_id
if sel_robot_id < 0:
self.report({'ERROR'}, "No robot selected")
else:
r = robot.RobotSet().getRobot(sel_robot_id)
trace = cnh.Buffer().get_last_trace_packet()
if trace is not None:
pose = trace.pose
r.loc = Vector((pose.x, pose.y, 0))
r.rotation = Euler((0, 0, pose.gamma))
if SocketModalOperator.closed:
SocketModalOperator.switching = True
if SocketModalOperator.error != "":
self.report({'ERROR'}, self.error)
SocketModalOperator.running = False
cnh.ConnectionHandler().remove_socket()
else:
bpy.context.scene.com_props.prop_last_sent_packet += 1
new_pid = bpy.context.scene.com_props.prop_last_sent_packet
new_mode = robot_modes_summary.index("EDITOR_MODE")
initial_speed = context.scene.com_props.prop_speed
bpy.context.scene.com_props.prop_mode = robot_modes_summary.index(
"EDITOR_MODE")
if not cnh.ConnectionHandler().send_change_mode(
new_pid, new_mode, initial_speed):
self.report({
'ERROR'
}, "Changed to editor mode but not in server : ack not received"
)
SocketModalOperator.switching = False
SocketModalOperator.running = False
self.thread.join() # se espera a que acabe el hilo
cnh.ConnectionHandler().remove_socket()
self.clear_hud(context)
return {'FINISHED'}
#update_gui()
toggle_deactivate_options(
robot_modes_summary.index("EDITOR_MODE"))
SocketModalOperator.running = False
self.thread.join() # se espera a que acabe el hilo
cnh.ConnectionHandler().remove_socket()
if context.scene.is_cursor_active:
bpy.ops.scene.stop_cursor_listener()
cnh.Buffer().clear_reached_poses()
SocketModalOperator.switching = False
self.report({'INFO'}, "Socket closed")
context.scene.com_props.prop_running_nav = False
context.scene.com_props.prop_paused_nav = False
self.clear_hud(context)
return {'FINISHED'}
return {'PASS_THROUGH'}