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'}