def command_callback(self, msg): try: if msg.isValid(): if msg.getReceiver() == "{}_supervisor".format( self.sensor_name): command = msg.getCommand() self.result_flg = True if command == "reset": self.reset_publisher.publish("") time.sleep(0.5) Logger.warning("Sensor '{}': Reset".format( self.sensor_name)) elif command == "filteron": Logger.warning("Sensor '{}': Filter ON".format( self.sensor_name)) # TODO elif command == "filteroff": Logger.warning("Sensor '{}': Filter OFF".format( self.sensor_name)) # TODO elif command == "filterreset": Logger.warning("Sensor '{}': Filter Reset".format( self.sensor_name)) # TODO else: self.result_flg = False Logger.error("INVALID input") self._send_command_result(self.result_flg) except Exception as e: print(e)
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 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 shape(self, shape_name, time=rospy.Time(0)): """ Applies a Shape to robot """ if self.robot_shapes.get(shape_name): self.getController().applyShape(self.robot_shapes.get(shape_name), time) else: Logger.warning("No shape with name '{}' for robot '{}'".format( shape_name, self.robot_name))
def _sendResult(self, success=False, command="", error=0): if self.current_command != None: if self.result_ext_callback: q = self.robot.getController().getQ() pose = geometry_msgs.msg.Pose() eef_frame = self.robot.getEEFrame() pose.position.x = eef_frame.p.x() pose.position.y = eef_frame.p.y() pose.position.z = eef_frame.p.z() eef_quaternion = eef_frame.M.GetQuaternion() pose.orientation.x = eef_quaternion[0] pose.orientation.y = eef_quaternion[1] pose.orientation.z = eef_quaternion[2] pose.orientation.w = eef_quaternion[3] self.result.pose = pose self.result.q.data = list(q) self.result.success = success self.result.error = error self.result.executed_command = command self.result.q_target_distance = self.dist_target self.result_ext_callback(self.result) succ = "\033[91m" + "\033[92m" * success + "\033[1m\033[4m{}\033[0m".format( success) print("{}: {}".format(self.current_command, succ)) else: Logger.error("External callback for RESULT update NOT found!") self.current_command = None
def setParameters(self, params_dict): try: self.dampedforward_controller.setParameters(self._param("dampedforward_parameters", params_dict)) self.spring_controller.setParameters(self._param("spring_parameters", params_dict)) except Exception as e: Logger.error(e)
def feedback_callback(self, feedback): """ Callback chiamata sui feedback """ if self.ext_callback: self.ext_callback(msg=feedback, cb_type=RobotMotionClient.FEEDBACK) else: Logger.error("External callback for FEEDBACK update NOT found!") self.current_feedback = feedback self.progress = feedback.progress
def done_callback(self, status, result): """ Callback chiamata al completamento del goal """ if self.ext_callback: self.ext_callback(msg=result, cb_type=RobotMotionClient.RESULT) else: Logger.error("External callback for RESULT update NOT found!") self.done_flg = True self.current_result = result
def reset(self): self.is_active = False self.done = False # self.velocity = 0 # self.axis = PyKDL.Vector() thr = self._estimateDisturbance(20) * 1.3 self.setParameters({"damp_force_threshold": thr}) Logger.warning("damp_force_threshold={}".format(thr))
def deleteByName(self, name, message_type): query = self.searchByName(name, message_type, single=False) if query: for item in query: try: self.msg_store.delete(item.id) except rospy.ServiceException, e: Logger.error(e)
def setParameters(self, params_dict=None, standard_index="default"): ''' Set the parameters stored by name in the dictionary "params_dict".''' if params_dict is not None: try: self.gain = self._param("gain", params_dict) # @@@ add here the other parameters... except Exception as e: Logger.error(e)
def _generateTrajectoryFromCommand(self): self.initial_q = self.robot.getController().getQ() self.trajectory_generator.clearTrajectory() self.generate_trajectory_failure = False steps = self.trajectory_time * self.node_frequency if self.target: self.trajectory_points = Parameters.get(obj=self.robot_name, param="TRAJECTORY_POINTS") if self.trajectory_points is None: self.trajectory_points = self.node_frequency * self.trajectory_time try: if self.interpreter.trajectory_specs == "linear_joints": self.trajectory = self.trajectory_generator.generateTrajectoryFromShapes( self.initial_q, self.target, self.trajectory_points, 1) elif self.interpreter.trajectory_specs == "linear_frames_to_free_joints": self.target = self.target * self.robot.getCurrentTool( ).Inverse() self.trajectory = self.trajectory_generator.generateTrajectoryFromFrames( self.initial_q, self.robot.getEEFrame(), self.target, self.node_frequency, self.trajectory_time) elif self.interpreter.trajectory_specs == "free_frames_to_linear_joints": self.target = self.target * self.robot.getCurrentTool( ).Inverse() self.trajectory = self.trajectory_generator.generateTrajectoryFreeFromFrames( self.initial_q, self.robot.getEEFrame(), self.target, steps=steps, agumented_middle_joints=True, middle_frames=10, number_of_tries=1, perturbate_middle_frames=False) elif self.interpreter.trajectory_specs == "target_frame_to_target_joints": self.target = self.target * self.robot.getCurrentTool( ).Inverse() self.trajectory = self.trajectory_generator.generateTrajectoryFreeFromFrames( self.initial_q, self.robot.getEEFrame(), self.target, steps=1, agumented_middle_joints=False, middle_frames=20, number_of_tries=5, perturbate_middle_frames=False) else: self.trajectory = [] except Exception as e: Logger.error(e) self.trajectory = [] else: self.trajectory = [] # Trajectory target update if len(self.trajectory) > 0: self.target_q = self.trajectory_generator.getTargetQ() else: self.generate_trajectory_failure = True self.target_q = self.initial_q
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 _addController(self, ctrl_id=None): if ctrl_id: self.active_controllers[ctrl_id] = self.controllers_dict[ctrl_id] if "none" in self.active_controllers.keys(): self._removeController("none") else: Logger.error( "Trying to activate an unlisted controller: {}".format( ctrl_id))
def stepForward(self): """ Moves one step forward (1 cycle) the robot's state machine """ try: self.sfm.loop() if self.robotSFM.isTrajectoryAdmissible() and self.sfm.getModel( ).state == "moving": self.robotSFM.applyTrajectory() except Exception as e: Logger.warning("Exception: {}".format(e))
def _send_command_result(self, success): if self.command_server_last_message: Logger.warning("Command result:{}".format(success)) if success: self.command_server.resolveCommand( self.command_server_last_message) else: self.command_server.rejectCommand( self.command_server_last_message) self.command_server_last_message = None
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 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 searchByName(self, name, message_type, single=True): try: return self.msg_store.query(message_type._type, {"_meta.name": { '$regex': name }}, single=single) except rospy.ServiceException, e: Logger.error(e) return None
def serachByCustomMeta(self, key, value, message_type): try: return self.msg_store.query( message_type._type, {"_meta.{}".format(key): { '$regex': value }}, single=False) except rospy.ServiceException, e: Logger.error(e) return None
def generateNewGoal(self, goal): """ Genera un nuovo goal e lo invia al server""" self.done_flg = False if goal: self.current_goal = RobotMotionGoal(robot_command=goal) self.action_client.send_goal(self.current_goal, done_cb=self.done_callback, active_cb=self.active_callback, feedback_cb=self.feedback_callback) else: Logger.error("Empty Goal")
def setSource(self, source): """ Set the target 'source' as string """ self.follow_msg.action = RobotFollow.ACTION_SETSOURCE if source == self.SOURCE_TRAJECTORY: self.follow_msg.target_source = RobotFollow.TARGET_FROM_TRAJECTORY_PLANNER elif source == self.SOURCE_DIRECT: self.follow_msg.target_source = RobotFollow.TARGET_FROM_EXTERNAL_FEEDBACK else: Logger.error("Invalid Source") return self._publishToFollow(self.follow_msg, sleep_time=0.1)
def setParameters(self, params_dict): try: self.translation_mag = self._param("translation_mag", params_dict) self.rotation_mag = self._param("rotation_mag", params_dict) self.thresholds = self._param("thresholds", params_dict) self.forceSensor.setMagnitude(self.translation_mag, self.rotation_mag) self.forceSensor.setTransThreshold(self.thresholds) except Exception as e: Logger.error(e)
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 setParameters(self, params_dict): try: self.damp_force_threshold = self._param("damp_force_threshold", params_dict) self.damp_magnitude = self._param("damp_magnitude", params_dict) self.initial_velocity = self._param("velocity", params_dict) print(self.initial_velocity) except Exception as e: Logger.error(e)
def _removeController(self, ctrl_id=None): if ctrl_id is None: self.active_controllers = {} self.active_controllers["none"] = NeutralController() else: if ctrl_id in self.active_controllers.keys(): self.active_controllers.pop(ctrl_id) if self.active_controllers == {}: self.active_controllers["none"] = NeutralController() else: Logger.error( "Trying to remove a deactivate controller: {}".format( ctrl_id))
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 createRobotByName(name, tf_listener=None): cfg = ROBOT_MARKET_LIBRARY.get(name) if cfg != None: robot = controllers.Robot(configuration=cfg, tf_listener=tf_listener) shapes = ROBOT_MARKET_SHAPE_LIBRARIES.get(name) if shapes: for s in shapes: robot_shape = controllers.RobotShape(s["name"], s["q"]) robot.addRobotShape(robot_shape) return robot else: Logger.error("No Robot with name '{}'".format(name)) return None
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()