def draw(self, context):
props = bpy.context.scene.robot_props
self.layout.operator(robot.AddRobotOperator.bl_idname,
icon="SYSTEM",
text="Add robot")
if len(robot.RobotSet()) > 0:
box = self.layout.box()
scene = context.scene
box.label(text="Available robots")
for item in scene.delete_robot_collection:
box.prop(item, "selected", text=item.name)
box.operator(robot.DeleteRobotOperator.bl_idname,
icon="TRASH",
text="Delete robot")
box = self.layout.box()
idx = context.scene.selected_robot_props.prop_robot_id
txt = "Robot selected : " + str(robot.RobotSet().getRobot(
idx).name) if idx >= 0 else "No robot selected"
txt = txt if len(robot.RobotSet()) > 0 else "No robot available"
box.label(text=txt)
box.operator(robot.SelectRobotOperator.bl_idname,
icon="CURVE_PATH",
text="Select robot")
row = self.layout.row()
row.operator(robot.SaveRobotOperator.bl_idname,
icon="EXPORT",
text="Save robot")
row.operator(robot.LoadRobotOperator.bl_idname,
icon="IMPORT",
text="Load robot")
def append_pose(self, context):
global pd
# validate new action
pos0 = pd.current_action.p0
pos1 = pd.current_action.p1
idx = bpy.context.scene.selected_robot_props.prop_robot_id
robot = robot_tools.RobotSet().getRobot(idx)
robot_obj = bpy.data.objects[robot.name]
area_robot_obj = bpy.data.objects[robot.area_name]
res = is_colliding(idx, robot_obj, area_robot_obj, pos0, pos1)
if res:
self.report({'ERROR'}, "Collision detected: Robot will collide if takes this path")
return {'FINISHED'}
else:
self.report({'INFO'}, "Undetected collision")
# Guardamos nueva action y dibujamos la informacion para la action previa
pd.current_action.draw_annotation(context)
pc.TempPathContainer().appendAction(pd.current_action)
# Siguiente action
p0 = pd.current_action.p1
p1 = pd.current_action.p1
pd.current_action = path.Action(p0, p1)
cl.CursorListener.select_cursor()
return {'FINISHED'}
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 poll(cls, context):
# begin local import
import robot
# end local import
exists_robot = len(robot.RobotSet()) > 0
robot_selected = context.scene.selected_robot_props.prop_robot_id >= 0
running_plan = context.scene.com_props.prop_running_nav
paused_plan = context.scene.com_props.prop_paused_nav
return not isListenerActive() and exists_robot and robot_selected and ((running_plan and paused_plan) or (not running_plan))
def toggle_deactivate_options(mode):
sel_rob_id = bpy.context.scene.selected_robot_props.prop_robot_id
if sel_rob_id < 0:
return
r = robot.RobotSet().getRobot(sel_rob_id)
if mode == robot_modes_summary.index("ROBOT_MODE"):
r.lock()
if mode == robot_modes_summary.index("EDITOR_MODE"):
r.unlock()
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()
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 execute(self, context):
global pd
for obj in context.selected_objects:
for action_index, action in enumerate(pc.TempPathContainer()):
if obj.name_full == action._arrow:
next_action_index = -1
if len(pc.TempPathContainer()) > action_index + 1:
next_action_index = action_index + 1
# Comprobar colision
pos0 = action.p0
pos1 = pd.current_action.p1 if next_action_index < 0 else pc.TempPathContainer()[next_action_index].p1
idx = bpy.context.scene.selected_robot_props.prop_robot_id
robot = robot_tools.RobotSet().getRobot(idx)
robot_obj = bpy.data.objects[robot.name]
area_robot_obj = bpy.data.objects[robot.area_name]
res = is_colliding(idx, robot_obj, area_robot_obj, pos0, pos1)
if res:
self.report({'ERROR'}, "Collision detected: Resulting path after deleting this point is not valid")
return {'FINISHED'}
else:
self.report({'INFO'}, "Undetected collision")
p0 = action.p0
if next_action_index < 0:
# mover punto fijo del current_action a action.p1
pd.current_action.move_fixed(p0)
else:
# mover punto fijo del next_action a action.p1
pc.TempPathContainer()[next_action_index].move_fixed(p0)
action.del_annotation()
pc.TempPathContainer().remove(action_index)
return {'FINISHED'}
return {'FINISHED'}
def apply_modification(self, context):
global pd
# validate current_action and next_action
pos0 = pd.current_action.p0
pos1 = pd.current_action.p1
pos2 = getattr(pd.next_action, 'p1', None)
idx = bpy.context.scene.selected_robot_props.prop_robot_id
robot = robot_tools.RobotSet().getRobot(idx)
robot_obj = bpy.data.objects[robot.name]
area_robot_obj = bpy.data.objects[robot.area_name]
res_first_action = is_colliding(idx, robot_obj, area_robot_obj, pos0, pos1)
res_second_action = is_colliding(idx, robot_obj, area_robot_obj, pos1, pos2) if pos2 is not None else False
if res_first_action or res_second_action:
self.report({'ERROR'}, "Collision detected: Robot will collide if takes this path")
return {'FINISHED'}
else:
self.report({'INFO'}, "Undetected collision")
# Guardamos nueva action y dibujamos la informacion para la action previa
pd.current_action.draw_annotation(context)
#pc.TempPathContainer().appendAction(pd.current_action)
last_action = pc.TempPathContainer().getLastAction()
pd.next_action = None
# Siguiente action
p1 = last_action.p1
pd.current_action = path.Action(p1, p1)
cl.CursorListener.select_cursor()
context.scene.isModifying = False
cl.CursorListener.set_pose(p1)
return {'FINISHED'}
def is_colliding(idx, robot_obj, area_robot_obj, pos0, pos1):
rs = robot_tools.RobotSet()
obstacles = []
for obj in bpy.data.objects:
if obj.object_type == "WALL":
bpy.ops.mesh.primitive_cube_add()
cube = bpy.context.active_object
cube.dimensions = obj.dimensions.xyz
cube.location = obj.dimensions.xyz/2.0
save_cursor_loc = bpy.context.scene.cursor.location.xyz
bpy.context.scene.cursor.location = Vector((0,0,0))
bpy.ops.object.origin_set(type='ORIGIN_CURSOR')
bpy.context.scene.cursor.location = save_cursor_loc
cube.location = obj.location.xyz
cube.rotation_euler.z = obj.rotation_euler.z
bpy.ops.object.origin_set(type='ORIGIN_GEOMETRY', center='MEDIAN')
bpy.context.scene.collection.objects.link(cube)
obstacles.append(cube)
cube.object_type = "TEMPORAL"
if obj.object_type == "OBSTACLE_MARGIN":
o = obj.copy()
o.parent = None
o.object_type = "TEMPORAL"
o.location = obj.parent.location.xyz
o.dimensions = obj.dimensions.xyz
o.rotation_euler = obj.parent.rotation_euler
bpy.context.scene.collection.objects.link(o)
obstacles.append(o)
if obj.object_type == "ROBOT": # Search other robots
for c in obj.children:
if c.object_type == "ROBOT_MARGIN": # Get robot margin
robot = rs.getRobotByName(obj.name_full)
if robot.idn != idx:
margin = c
o = margin.copy()
o.parent = None
o.object_type = "TEMPORAL"
o.location = margin.parent.location.xyz
o.dimensions = margin.dimensions.xyz
o.rotation_euler = margin.parent.rotation_euler
bpy.context.scene.collection.objects.link(o)
obstacles.append(o)
# Copy robot
bpy.ops.mesh.primitive_cube_add()
area_robot_obj_tmp = bpy.context.active_object
area_robot_obj_tmp.dimensions = Vector((area_robot_obj.dimensions.x, area_robot_obj.dimensions.y, area_robot_obj.dimensions.z))
area_robot_obj_tmp.location = Vector((robot.loc.x, robot.loc.y, (area_robot_obj.dimensions.z/2.0)))
area_robot_obj_tmp.rotation_euler.z = pos0.rotation.z
bpy.ops.object.select_all(action='DESELECT')
area_robot_obj_tmp.select_set(True)
save_cursor_loc = bpy.context.scene.cursor.location.xyz
bpy.context.scene.cursor.location = Vector((robot.loc.x, robot.loc.y, 0))
bpy.ops.object.origin_set(type='ORIGIN_CURSOR')
bpy.context.scene.cursor.location = save_cursor_loc
# Check
res = collision_detection.check_collision(area_robot_obj_tmp, pos0, pos1, obstacles)
# Remove all objects
for obj in obstacles:
if obj.object_type == "TEMPORAL":
bpy.data.objects.remove(obj, do_unlink=True)
bpy.data.objects.remove(area_robot_obj_tmp, do_unlink=True)
return res
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'}