def command_callback(self, msg): try: if msg.isValid(): if msg.getReceiver() == "tf_filter_supervisor": command = msg.getCommand() Logger.log(command) if command == 'start': sls = msg.getData('slot_size', float) if sls is None: sls = self.default_sls self.startFiltering(msg.getData('tf_name'), slot_size=sls) elif command == 'stop': self.stopFiltering() self._send_command_result(True) elif command == 'clear': self.clearFiltering(msg.getData('tf_name')) self._send_command_result(True) elif command == 'clearstart': self.clearFiltering(msg.getData('tf_name')) sls = msg.getData('slot_size', float) if sls is None: sls = self.default_sls self.startFiltering(msg.getData('tf_name'), slot_size=sls) elif command == 'reset': self.resetFiltering() self._send_command_result(True) else: Logger.error("INVALID input") self._send_command_result(False) except Exception as e: print(e) self._send_command_result(False)
def on_enter_steady(self): self._sendFeedback(state="steady", status=self.feedback.STEADY) Logger.log("State: STEADY") self._sendResult(success=True, command=self.current_command) self.trajectory_correct = False self.current_command = None self.idle()
def on_enter_ready(self): Logger.log("State: READY") self._sendFeedback(state="ready") self.start_moving_timer = self.init_start_moving_timer # if self.robot.getController().isMoving(epsilon=self.vel_noise_mean * 1.05): # TODO start-moving timer disabled!!!!!!!!!!!!! if len(self.trajectory_generator.current_trajectory) > 0: self.moving()
def start(self, robot_list, sensor_list, instruction_list, subtask_list): Logger.log("\n\n ************* SFM start ***********") self.managerSFM.loadInstructions(instruction_list) for robot_name in robot_list: self.managerSFM.addRobot(robot_name) for sensor_name in sensor_list: self.managerSFM.addSensor(sensor_name) for subtask_name in subtask_list: self.managerSFM.addSubTask(subtask_name) self.sfm.getModel().next()
def on_enter_generatetrajectory(self): Logger.log("State: GENERATETRAJECTORY") self._sendFeedback(state="generatetrajectory", command=self.current_command) self.generate_timer = self.init_generate_timer self.t_gen = -1 self.trajectoryThread = threading.Thread( target=self._generateTrajectoryFromCommand) self.trajectoryThread.start()
def joystick_key_callback(self, down, up): # if self.joystick.KEY_BACK in down: # Logger.log(" ALARM: CLEAR ") # self.target_tf = self.current_tf # self.alarm_proxy.resetAlarm() # self.target_follower.resetAlarm() if self.joystick.KEY_START in down: Logger.log(" USER ALARM: ON ") self.alarm_proxy.setAlarm() self.target_follower.setAlarm() if self.joystick.KEY_LOG in down: self.target_tf = self.current_tf
def __init__(self, namespace): self.namespace = namespace self.action_client = actionlib.SimpleActionClient( CommandProxyConfiguration.getProxyName(self.namespace), TextCommandAction) Logger.log( "Command Proxy '{}' is waiting for server...".format(namespace)) self.action_client.wait_for_server() Logger.log("Command Proxy '{}' ready!".format(namespace)) ##### self.feedback_callbacks = [] self.done_callbacks = [] self.active_callbacks = []
def __init__(self, robot_name): self.robot_name = robot_name # CREATE STATE MACHINE self.supervisorSFM = SFMachineRobotSupervisorDefinition( self.robot_name) self.sfm = machines.SFMachine(name="sfm_" + self.robot_name + "_supervisor", model=self.supervisorSFM) # DEFINE SM STATES self.sfm.addState("start") self.sfm.addState("idle") self.sfm.addState("controlling") self.sfm.addState("trajectory") self.sfm.addState("alarm") self.sfm.addState("direct") # DEFINE SM TRANSITIONS # start ... self.sfm.addTransition("idle", "start", "idle") # idle ... self.sfm.addTransition("idle", "idle", "idle") self.sfm.addTransition("alarm", "idle", "alarm") self.sfm.addTransition("direct", "idle", "direct") self.sfm.addTransition("trajectory", "idle", "trajectory") # controlling ... self.sfm.addTransition("idle", "controlling", "idle") self.sfm.addTransition("controlling", "controlling", "controlling") self.sfm.addTransition("trajectory", "controlling", "trajectory") self.sfm.addTransition("alarm", "controlling", "alarm") # alarm ... self.sfm.addTransition("idle", "alarm", "idle") self.sfm.addTransition("alarm", "alarm", "alarm") # direct ... self.sfm.addTransition("idle", "direct", "idle") self.sfm.addTransition("direct", "direct", "direct") self.sfm.addTransition("controlling", "direct", "controlling") self.sfm.addTransition("trajectory", "direct", "trajectory") self.sfm.addTransition("alarm", "direct", "alarm") # trajectory ... self.sfm.addTransition("trajectory", "trajectory", "trajectory") self.sfm.addTransition("idle", "trajectory", "idle") self.sfm.addTransition("alarm", "trajectory", "alarm") self.sfm.create() self.sfm.set_state("start") Logger.log("\n\n ************* SFM ready to start ***********")
def on_enter_checktrajectory(self): Logger.log("State: CHECKTRAJECTORY") self._sendFeedback(state="checktrajectory") # print("{}".format(self.trajectory_generator.current_trajectory)) if not self.trajectory_generator.computationSuccess(): self.error("NO_TRAJECTORY") else: if self._isOnTarget(): Logger.log("Already on Target") self._sendFeedback(state="checktrajectory", text="Already on Target") self.steady() else: self.trajectory_correct = True self.ready()
def __init__(self, service_name, service_manifest=IKService, timeout=5.0): self.service_name = service_name self.service_manifest = service_manifest self.active = False Logger.log("GeneralIKService: waiting for service " + service_name + "...") try: rospy.wait_for_service(self.service_name, timeout=timeout) except rospy.ROSException as e: Logger.error("Service timeout:" + str(e)) return self.service = rospy.ServiceProxy( self.service_name, service_manifest) self.active = True Logger.log("Service Connected!")
def start(self, robot_command): self.robotSFM.uploadCurrentCommand(robot_command) if self.sfm.getModel().state != "error": try: self.sfm.getModel().sendcommand() Logger.log("\n\n ************* SFM start ***********") except: self.sfm.getModel().error("transition_error") else: error_code = self.sfm.getModel().err self.sfm.getModel()._sendResult( success=False, error=error_code, command=self.sfm.getModel().current_command) Logger.error( "\n\n ************* persist error {} *********** \n\n".format( error_code))
def on_loop_generatetrajectory(self): self._sendFeedback(text=str(self.init_generate_timer), state="generatetrajectory", command=self.current_command, status=self.feedback.WAITING_FOR_MOVING) t_new = self.generate_timer / self.node_frequency if self.t_gen != t_new: self.t_gen = t_new Logger.log("waiting start (time: {}s)".format(self.t_gen)) if self.trajectory_generator.isTrajectoryAvailable(): self.trajectoryThread.join() self.checktrajectory() elif self.generate_timer <= 0 or self.generate_trajectory_failure: self.trajectoryThread.join() self.error("NO_TRAJECTORY") self.generate_timer -= 1
def __init__(self, task_name): self.task_name = task_name # CREATE STATE MACHINE self.managerSFM = SFMachineTaskManagerDefinition(self.task_name) self.sfm = machines.SFMachine(name="sfm_" + self.task_name + "_manager", model=self.managerSFM) # DEFINE SM STATES self.sfm.addState("start") self.sfm.addState("idle") self.sfm.addState("next") self.sfm.addState("action") self.sfm.addState("alarm") # DEFINE SM TRANSITIONS # start ... self.sfm.addTransition("idle", "start", "idle") self.sfm.addTransition("next", "start", "next") # idle ... self.sfm.addTransition("idle", "idle", "idle") self.sfm.addTransition("alarm", "idle", "alarm") self.sfm.addTransition("next", "idle", "next") self.sfm.addTransition("action", "idle", "action") # next ... self.sfm.addTransition("next", "next", "next") self.sfm.addTransition("alarm", "next", "alarm") self.sfm.addTransition("action", "next", "action") self.sfm.addTransition("idle", "next", "idle") # alarm ... self.sfm.addTransition("idle", "alarm", "idle") self.sfm.addTransition("action", "alarm", "action") self.sfm.addTransition("next", "alarm", "next") self.sfm.addTransition("alarm", "alarm", "alarm") # action ... self.sfm.addTransition("idle", "action", "idle") self.sfm.addTransition("next", "action", "next") self.sfm.addTransition("action", "action", "action") self.sfm.create() self.sfm.set_state("start") Logger.log("\n\n ************* SFM ready to start ***********")
def command_callback(self, msg): try: if msg.isValid(): if msg.getReceiver() == self.command_proxy_server_name: command = msg.getCommand() try: data = msg.getData("data") except: data = None Trigger(data) Logger.log(self.command_proxy_server_name.replace("_supervisor", "") + ": " + command) else: self.command_server_last_message = None else: self.command_server_last_message = None Logger.error(self.command_proxy_server_name.replace("_supervisor", "") + ": invalid message!!") self.sendResponse(False) except Exception as e: self.sendResponse(False)
def on_enter_error(self, error_type=""): Logger.log("State: ERROR") self._sendFeedback(state="error") self.trajectory_generator.clearTrajectory() if error_type == "NO_COMMAND": self.err = self.result.ERROR_NO_COMMAND Logger.warning( "Command string NOT recognized. Waiting for a new command") elif error_type == "NO_TRAJECTORY": self.err = self.result.ERROR_NO_TRAJECTORY Logger.warning( "Trajectory NOT computed. Waiting for a new command") elif error_type == "START_MOVING": self.err = self.result.ERROR_START_MOVING Logger.warning( "Moving Time Out. Target NOT Reached. Waiting for a new command" ) elif error_type == "END_TRAJECTORY": self.err = self.result.ERROR_END_TRAJECTORY Logger.error( "Trajectory Time Out. Target NOT Reached. Waiting for a new command" ) elif error_type == "ROBOT_ALARM": self.err = self.result.ERROR_ROBOT_ALARM self.in_allarm = True Logger.error( "\n\n\n ROBOT ALARM!!! \n Restore the machine and Reset the Alarm \n\n\n" ) elif error_type == "COMMAND_CONFLICT": self.err = self.result.ERROR_COMMAND_CONFLICT Logger.error( "\n\n\n COMMAND CONFLICT!!! \n Waiting for a new command \n\n\n" ) else: self.err = self.result.ERROR_UNKNOWN Logger.error("Unknown Error --- {}".format(error_type)) # send result self._sendResult(success=False, error=self.err, command=self.current_command)
def generateTrajectoryFromFrames(self, q_in, frame1, frame2, hz, time): """ Generates a linear trajectory between two cartesian frames """ cartesian_trajectory = self.generateCartesianFrames( frame1, frame2, hz, time) trajectory_points = [] if not self.checkIKService(): self.setCurrentTrajectory(trajectory_points) return trajectory_points else: current_q = q_in last_q = q_in success_counter = 0.0 for frame in cartesian_trajectory: trajectory_point = TrajectoryPoint(frame, last_q, 0.0, invalid=True) response = self.robot.getIKService().simple_ik(frame, last_q) if response.status == IKServiceResponse.STATUS_OK: trajectory_point.q = response.q_out.data trajectory_point.invalid = False success_counter += 1.0 trajectory_points.append(trajectory_point) last_q = trajectory_point.q success_counter = success_counter / float( len(cartesian_trajectory)) Logger.log("Trajectory Computation Success {}%".format( success_counter * 100)) if success_counter > 0.99: self.status = FuzzyTrajectoryGenerator.STATUS_OK else: self.status = FuzzyTrajectoryGenerator.STATUS_INVALID_POINTS self.setCurrentTrajectory(trajectory_points) return trajectory_points
def on_enter_action(self): Logger.log("State: ACTION")
def on_enter_preparing(self): Logger.log("State: PREPARING") self._sendFeedback(state="preparing") self.ee_frame = self.robot.getEEFrame() self.vel_noise_samples = []
def on_enter_sendcommand(self): Logger.log("State: SENDCOMMAND") self._sendFeedback(state="sendcommand")
def start(self): Logger.log("\n\n ************* SFM start ***********") self.sfm.getModel().idle()
def on_enter_checkcommand(self): Logger.log("State: CHECKCOMMAND") self._sendFeedback(state="checkcommand", command=self.current_command) self.target = self.interpreter.interpretCommand(self.current_command)
def on_enter_alarm(self): Logger.log("State: ALARM") self._reject_input_command(GlobalErrorList.ROBOT_ALARM)
def on_enter_trajectory(self): Logger.log("State: TRAJECTORY")
def on_enter_direct(self): Logger.log("State: DIRECT")
def on_enter_controlling(self): Logger.log("State: CONTROLLING")
def on_enter_idle(self): Logger.log("State: IDLE")
def command_callback(self, msg): try: self.command_success_flg = True if msg.isValid(): receiver = msg.getReceiver() if msg.getReceiver() == "{}_direct_commander".format( self.robot_name): command = msg.getCommand() #⬢⬢⬢⬢⬢➤ ON if command == "_on_": Logger.log( " ******** Direct Commander: ENABLE ******** ") self._reset() self._removeController() self.active = True #⬢⬢⬢⬢⬢➤ OFF elif command == "_off_": Logger.log( " ******** Direct Commander: DISABLE ******** ") self.current_tf = None self._reset() self._removeController() self.active = False #⬢⬢⬢⬢⬢➤ RESET elif command == "_reset_": Logger.log(" ******** RESET ******** ") self._reset() #⬢⬢⬢⬢⬢➤ JOYSTICK elif command == "_joyon_": Logger.log(" Joystick: ENABLE ") self.joystick_flg = True elif command == "_joyoff_": Logger.log(" Joystick: DISABLE ") self.joystick_flg = False #⬢⬢⬢⬢⬢➤ SELECT CONTROLLER elif command.startswith("controllerselect"): ctrl_id_list = msg.getData("id") Logger.log( " Controller Selection: {}".format(ctrl_id_list)) for ctrl_id in ctrl_id_list: if ctrl_id == "none": Logger.log(" Neutral Controller ") self._removeController() else: try: self._addController(ctrl_id) except: self.command_success_flg = False Logger.error( "Wrong Controller ID: {}".format( ctrl_id)) #⬢⬢⬢⬢⬢➤ CONTROLLER PARAMETERS elif command.startswith("controllerparameters"): parameters = msg.getData("parameters") Logger.log( " Parameters Update: {} ".format(parameters)) for ctrl_id in parameters.keys(): if ctrl_id in self.active_controllers.keys(): self.active_controllers[ctrl_id].setParameters( parameters[ctrl_id]) else: self.command_success_flg = False Logger.error( "Wrong Controller ID: {}".format(ctrl_id)) #⬢⬢⬢⬢⬢➤ REMOVE CONTROLLER elif command.startswith("controllerdisable"): Logger.log(" Removing Controller ") ctrl_id_list = msg.getData("id") for ctrl_id in ctrl_id_list: if ctrl_id == "_all_": self._reset() self._removeController() elif ctrl_id in self.active_controllers.keys(): self._reset(ctrl_id) self._removeController(ctrl_id) else: self.command_success_flg = False Logger.error( "Wrong Controller ID: {}".format(ctrl_id)) # ⬢⬢⬢⬢⬢➤ START CONTROLLERS elif command.startswith("controllerstart"): Logger.log(" Controllers Start {}".format( self.active_controllers.keys())) input_data = msg.getData("input_data") time.sleep(0.1) for ctrl_id in input_data.keys(): if ctrl_id in self.active_controllers.keys(): self.active_controllers[ctrl_id].start( input_data[ctrl_id]) else: self.command_success_flg = False Logger.error( "Wrong Controller ID: {}".format(ctrl_id)) else: self.command_success_flg = False Logger.warning("INVALID COMMAND: {}".format(command)) else: receiver = msg.getReceiver() if msg.getReceiver() == "{}_direct_commander".format( self.robot_name): self.command_success_flg = False except Exception as e: self.command_success_flg = False print(e) Logger.log(" \nActive Controllers: {}\n".format( self.active_controllers.keys()))
def on_enter_moving(self): Logger.log("State: MOVING") self._sendFeedback(state="moving", status=self.feedback.MOVING) delay = int(0.6 * float(self.trajectory_time * self.node_frequency)) self.end_moving_timer = self.trajectory_time * self.node_frequency + delay self.movement_progress = 0
def __init__(self, robot_name="", action_server=None): self.robot_name = robot_name self.robot = RobotMarket.createRobotByName(self.robot_name) # CREATE STATE MACHINE self.robotSFM = SFMachineRobotDefinition(self.robot) self.sfm = machines.SFMachine(name="sfm_" + self.robot_name, model=self.robotSFM) # DEFINE SM STATES self.sfm.addState("start") self.sfm.addState("idle") self.sfm.addState("preparing") self.sfm.addState("sendcommand") self.sfm.addState("checkcommand") self.sfm.addState("generatetrajectory") self.sfm.addState("checktrajectory") self.sfm.addState("ready") self.sfm.addState("moving") self.sfm.addState("steady") self.sfm.addState("error") # DEFINE SM TRANSITIONS # start ... self.sfm.addTransition("prepare", "start", "preparing") self.sfm.addTransition("sendcommand", "start", "sendcommand") # idle ... self.sfm.addTransition("sendcommand", "idle", "sendcommand") # preparing ... self.sfm.addTransition("idle", "preparing", "idle") # sendcommand ... self.sfm.addTransition("checkcommand", "sendcommand", "checkcommand") # checkcommand ... self.sfm.addTransition("generatetrajectory", "checkcommand", "generatetrajectory") # generatetrajectory ... self.sfm.addTransition("checktrajectory", "generatetrajectory", "checktrajectory") # checktrajectory ... self.sfm.addTransition("ready", "checktrajectory", "ready") self.sfm.addTransition("steady", "checktrajectory", "steady") # ready ... self.sfm.addTransition("moving", "ready", "moving") self.sfm.addTransition("steady", "ready", "steady") # moving ... self.sfm.addTransition("steady", "moving", "steady") self.sfm.addTransition("sendcommand", "moving", "sendcommand") # steady ... self.sfm.addTransition("idle", "steady", "idle") # ... in error self.sfm.addTransition("error", "start", "error") self.sfm.addTransition("error", "idle", "error") self.sfm.addTransition("error", "preparing", "error") self.sfm.addTransition("error", "checkcommand", "error") self.sfm.addTransition("error", "sendcommand", "error") self.sfm.addTransition("error", "generatetrajectory", "error") self.sfm.addTransition("error", "checktrajectory", "error") self.sfm.addTransition("error", "ready", "error") self.sfm.addTransition("error", "moving", "error") self.sfm.addTransition("error", "steady", "error") self.sfm.addTransition("idle", "error", "idle") self.sfm.addTransition("error", "error", "error") self.sfm.create() self.sfm.set_state("start") Logger.log("\n\n ************* SFM ready to start ***********")
def on_enter_next(self): Logger.log("State: NEXT") self.nextInstruction()