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):
import datetime as dt
date = str(dt.datetime.now())
global update_time
update = update_time < pc.PathContainer().getLastUpdate()
update_time = pc.PathContainer().getLastUpdate()
log = "PathEditor | " + date + " | update = " + str(update) + " : " + str(update_time) + "\n"
log += "\tPathContainer : " + str(len(pc.PathContainer())) + "\n"
for p in pc.PathContainer().poses:
log += "\t\t" + str(p) + "\n"
log += "\tTempPathContainer : " + str(len(pc.TempPathContainer())) + "\n"
self.report({'INFO'}, log)
return {'FINISHED'}
def execute(self, context):
# poses
poses = pc.PathContainer().poses
json_poses = {'num_poses': len(poses), 'poses': []}
for p in poses:
json_poses['poses'].append({
'x': p.x,
'y': p.y,
'z': p.z,
'rx': p.alpha,
'ry': p.beta,
'rz': p.gamma,
})
# filepath
filepath_str = self.filepath
filepath = Path(filepath_str)
path = filepath.parent
name = Path(filepath.name)
if not export_poses(path, name, json_poses):
self.report({'ERROR'}, f"Cannot write in {str(path / name)}")
else:
self.report({'INFO'}, f"Exported path in {str(path / name)}")
return {'FINISHED'}
def notifyStop(self, operator):
pathContainer = pc.PathContainer()
temp = pc.TempPathContainer()
temp.pushActions()
del self.current_action
self.current_action = None
showCeil()
def notifyStart(self, operator):
bpy.context.scene.cursor.location = Vector((0.0, 0.0, 0.0))
bpy.context.scene.cursor.rotation_euler = Euler((0.0, 0.0, 0.0))
if len(robot_tools.RobotSet()) <= 0:
operator.report({'ERROR'}, 'No robot available : add a robot to create a path')
bpy.ops.scene.stop_cursor_listener('INVOKE_DEFAULT')
return
idx = bpy.context.scene.selected_robot_props.prop_robot_id
assert idx >= 0 and idx in robot_tools.RobotSet(), "Error : no robot selected or not in list"
robot = robot_tools.RobotSet().getRobot(idx)
last_action = pc.PathContainer().getLastAction()
if len(pc.PathContainer()) > 0 and last_action is not None:
# Obtenemos ultima pose como inicio para crear nuevas poses
loc = last_action.p1.loc
angle = last_action.p1.rotation
else:
loc = robot.loc + Vector((bpy.context.scene.TOL, bpy.context.scene.TOL, bpy.context.scene.TOL))
angle = robot.rotation
# Movemos el cursor a la posicion de comienzo
new_pose = path.Pose.fromVector(loc, angle)
cl.CursorListener.set_pose(new_pose)
self.current_action = path.Action(new_pose, new_pose)
if len(pc.PathContainer()) == 0:
self.current_action.set_first_action()
cl.CursorListener.select_cursor()
hideCeil()
pc.TempPathContainer().loadActions()
filepath = Path(filepath_str)
path = filepath.parent
name = Path(filepath.name)
if (json_poses := read_poses(path, name)) is None:
self.report({'ERROR'}, f"Cannot read {str(path / name)}")
else:
self.report({'INFO'}, f"Path loaded from {str(path / name)}")
poses = parse_data(json_poses)
print(poses)
if poses is None:
self.report({'ERROR'},
"Error while parsing poses. Check file content.")
else:
pc.PathContainer().loadPoses(poses, context=context)
context.window_manager.load_file = str(path / name)
return {'FINISHED'}
class ExportPosesOperator(bpy.types.Operator, ExportHelper):
bl_idname = "export.export_poses_operator"
bl_label = "Save path"
filename_ext = ".json"
filter_glob: bpy.props.StringProperty(default="*.json", options={'HIDDEN'})
@classmethod
def poll(cls, context):
def execute(self, context):
pc.PathContainer().clear()
return {'FINISHED'}
def poll(cls, context):
running_plan = context.scene.com_props.prop_running_nav
paused_plan = context.scene.com_props.prop_paused_nav
return len(pc.PathContainer()) > 0 and (not running_plan or (running_plan and paused_plan)) and not context.scene.is_cursor_active
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'}
