def __init__(self): self.command_proxy_server_name = "grasshopper_grasp_supervisor" self.command_server = CommandProxyServer(self.command_proxy_server_name) self.command_server.registerCallback(self.command_action_callback) self.command_server_last_message = None self.message_proxy = SimpleMessageProxy() self.message_proxy.register(self.command_callback)
def __init__(self, sensor_name): self.sensor_name = sensor_name self.message_proxy = SimpleMessageProxy() self.message_proxy.register(self.command_callback) self.last_msg = None # robot outer command self.command_server = CommandProxyServer("{}_supervisor".format( self.sensor_name)) self.command_server.registerCallback(self.command_action_callback) self.command_server_last_message = None
def __init__(self): self.filtered_tf_map = {} self.filtered_tf_slots = {} self.current_tf_name = "" self.default_sls = 100 self.command_server = CommandProxyServer("tf_filter_supervisor") self.command_server.registerCallback(self.command_action_callback) self.command_server_last_message = None self.message_proxy = SimpleMessageProxy() self.message_proxy.register(self.command_callback)
def __init__(self, robot_name): self.robot_name = robot_name self.listener = tf.TransformListener() self.message_database = MessageStorage() # tool self.tools_list = Parameters.get(obj=self.robot_name, param="TOOLS") print self.tools_list self.new_tools = {} self.new_tools["dynamic"] = FrameVectorToKDL( self.tools_list["gripper"]) # alarm self.alarm_proxy = AlarmProxy(self.robot_name) self.alarm_proxy.registerAlarmCallback(self.alarm_callback) # target follower proxy self.target_follower = TargetFollowerProxy(self.robot_name) # robot outer message (no feedback/response) self.outer_message_proxy = SimpleMessageProxy() self.outer_message_proxy.register(self.command_callback) # robot outer command self.command_server = CommandProxyServer("{}_supervisor".format( self.robot_name)) self.command_server.registerCallback(self.command_action_callback) self.command_server_last_message = None self.command_category = "unknown" # robot inner message self.inner_message_proxy = SimpleMessageProxy( "{}_inner_message".format(self.robot_name)) # robot inner command self.robot_trajectory_client = RobotMotionClient( self.robot_name, ext_callback=self.robot_trajectory_callback) self.robot_control_client = CommandProxyClient( "{}_direct_commander".format(self.robot_name)) self.robot_control_client.registerDoneCallback( self.control_done_callback)
class ConnectionsScanProxyServer(object): def __init__(self): self.command_proxy_server_name = "grasshopper_grasp_supervisor" self.command_server = CommandProxyServer(self.command_proxy_server_name) self.command_server.registerCallback(self.command_action_callback) self.command_server_last_message = None self.message_proxy = SimpleMessageProxy() self.message_proxy.register(self.command_callback) def command_action_callback(self, cmd): self.command_server_last_message = cmd cmd_msg = cmd.getSentMessage() self.command_callback(cmd_msg) 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 sendResponse(self, success): if self.command_server_last_message: print("\n" + self.command_proxy_server_name.replace("_supervisor", "") + ": " + Hg_open + "Task Ended!!!" + H_close + " \t Success: {}".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 sleepAndResolve(message): if random.randint(0, 100) < 10: time.sleep(random.uniform(0.01, 5.0)) print("Resolved...") message.addResponseData("pino", 1) message.addResponseData("aaa", 2) command_proxy_server.resolveCommand(message) return def example_callback(message): print("Received", str(message)) p = random.randint(0, 100) if p > 10: print("Resolving...") t = threading.Thread(target=sleepAndResolve, args=(message,)) t.start() else: print("Rejecting...") command_proxy_server.rejectCommand( message, reject_message="RANDOM_ERROR!") command_proxy_server = CommandProxyServer("pino") command_proxy_server.registerCallback(example_callback) while node.isActive(): node.tick()
class SFMachineRobotSupervisorDefinition(object): DIRECT_COMMANDER = 100 TRAJECTORY_COMMANDER = 101 def __init__(self, robot_name): self.robot_name = robot_name self.listener = tf.TransformListener() self.message_database = MessageStorage() # tool self.tools_list = Parameters.get(obj=self.robot_name, param="TOOLS") print self.tools_list self.new_tools = {} self.new_tools["dynamic"] = FrameVectorToKDL( self.tools_list["gripper"]) # alarm self.alarm_proxy = AlarmProxy(self.robot_name) self.alarm_proxy.registerAlarmCallback(self.alarm_callback) # target follower proxy self.target_follower = TargetFollowerProxy(self.robot_name) # robot outer message (no feedback/response) self.outer_message_proxy = SimpleMessageProxy() self.outer_message_proxy.register(self.command_callback) # robot outer command self.command_server = CommandProxyServer("{}_supervisor".format( self.robot_name)) self.command_server.registerCallback(self.command_action_callback) self.command_server_last_message = None self.command_category = "unknown" # robot inner message self.inner_message_proxy = SimpleMessageProxy( "{}_inner_message".format(self.robot_name)) # robot inner command self.robot_trajectory_client = RobotMotionClient( self.robot_name, ext_callback=self.robot_trajectory_callback) self.robot_control_client = CommandProxyClient( "{}_direct_commander".format(self.robot_name)) self.robot_control_client.registerDoneCallback( self.control_done_callback) # self.robot_control_client.registerFeedbackCallback( # self.control_feedback_callback) # TODO # ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ CALLBACKS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ def alarm_callback(self, alarm_info): if alarm_info != self.alarm_proxy.NONE_ALARM: Logger.error(" !!!!!!!! ALARM !!!!!!!! ") self.alarm() def robot_trajectory_callback(self, cb_type, msg): if cb_type == self.robot_trajectory_client.FEEDBACK: self.trajectory_sm_feedback = msg # self._send_trajectory_feedback(msg) # TODO elif cb_type == self.robot_trajectory_client.RESULT: self._send_trajectory_command_result(msg) if self.state != "alarm": self.idle() def control_done_callback(self, command): # print command if command: success = (command.status == CommandMessage.COMMAND_STATUS_OK) command = command.getSentMessage().getCommand() else: command = "NONE" success = False self._send_command_result(success) print("controller: {} >>> SUCCESS={}".format(command, success)) # if self.state != "alarm": # self.idle() def command_action_callback(self, cmd): self.command_server_last_message = cmd cmd_msg = cmd.getSentMessage() self.command_callback(cmd_msg) def command_callback(self, msg): if msg.isValid(): if msg.getReceiver() == "{}_supervisor".format(self.robot_name): print msg.toString() command = msg.getCommand() try: #⬢⬢⬢⬢⬢➤ RESET if command == "reset": self.command_category = "setting" Logger.warning(" !!!!!!!! ALARM RESET !!!!!!!! ") self.alarm_proxy.resetAlarm() self.target_follower.resetAlarm() self._reject_input_command(GlobalErrorList.ROBOT_ALARM) self.idle() elif self.state != "alarm": #⬢⬢⬢⬢⬢➤ SET TOOL if command == "settool": # change multiple data structure in a single multi-entry dictionary self.command_category = "setting" tool_name = msg.getData("tool_name") new_tool_name = msg.getData("new_tool_name") transf = msg.getData("transformation") tool = self._getToolFromName(tool_name) transf = FrameVectorToKDL(transf) self.new_tools[new_tool_name] = tool * transf print("Tools: {}".format(self.new_tools)) self._send_command_result(True) self.idle() #⬢⬢⬢⬢⬢➤ SELECT TOOL elif command == "selecttool": self.command_category = "setting" tool_name = msg.getData("tool_name") tool = self._getToolFromName(tool_name) self._setTool(tool) self._send_command_result(True) self.idle() #⬢⬢⬢⬢⬢➤ SET TRAJECTORY elif command == "settrajectory": self.command_category = "setting" points = msg.getData( "points" ) # if it returns none, this wasn't the set parameter time = msg.getData("time") if points is not None: print Parameters.get(obj=self.robot_name, param="TRAJECTORY_POINTS") Parameters.change(obj=self.robot_name, param="TRAJECTORY_POINTS", value=int(points)) print Parameters.get(obj=self.robot_name, param="TRAJECTORY_POINTS") elif time is not None: Parameters.change(obj=self.robot_name, param="TRAJECTORY_TIME", value=int(points)) self._send_command_result(True) self.idle() #⬢⬢⬢⬢⬢➤ MOVE TO TF elif command == "movetotf": self.command_category = "trajectory" tf_name = msg.getData("tf_name") tool_name = msg.getData("tool_name") self._toTf(tf_name, tool_name, mode=command) print("Tf: \033[1m\033[4m\033[94m{}\033[0m".format( tf_name)) self.trajectory() #⬢⬢⬢⬢⬢➤ JUMP TO TF elif command == "jumptotf": self.command_category = "trajectory" tf_name = msg.getData("tf_name") tool_name = msg.getData("tool_name") self._toTf(tf_name, tool_name, mode=command) print("Tf: \033[1m\033[4m\033[94m{}\033[0m".format( tf_name)) self.trajectory() #⬢⬢⬢⬢⬢➤ GO TO TF elif command == "gototf": self.command_category = "trajectory" tf_name = msg.getData("tf_name") tool_name = msg.getData("tool_name") self._toTf(tf_name, tool_name, mode=command) print("Tf: \033[1m\033[4m\033[94m{}\033[0m".format( tf_name)) self.trajectory() #⬢⬢⬢⬢⬢➤ GO TO JOINTS elif command == "gotojoints": self.command_category = "trajectory" joints_str = msg.getData("joints_vector") self._toJoints(joints_str) print("Joints: \033[1m\033[4m\033[94m{}\033[0m". format(joints_str)) self.trajectory() #⬢⬢⬢⬢⬢➤ GO TO SHAPE elif command == "gotoshape": self.command_category = "trajectory" shape_name = msg.getData("shape_name", str) self._toShape(shape_name) print("Shape: \033[1m\033[4m\033[94m{}\033[0m". format(shape_name)) self.trajectory() #⬢⬢⬢⬢⬢➤ RESET CONTROLLERS elif command == "directreset": self.command_category = "setting" self._resetDirectCommander() #⬢⬢⬢⬢⬢➤ ENABLE CONTROLLERS elif command == "direct": self.command_category = "setting" active = msg.getData("active") if active: self._switchCommander(self.DIRECT_COMMANDER) self.direct() else: self._switchCommander( self.TRAJECTORY_COMMANDER) self.idle() #⬢⬢⬢⬢⬢➤ START CONTROLLERS elif command == "controllerstart": self.command_category = "setting" self._sendCommandActionToController(msg) print("Inputs: {}".format( msg.getData("input_data"))) self.controlling() #⬢⬢⬢⬢⬢➤ UPDATE CONTROLLERS PARAMETERS elif command == "controllerparameters": self.command_category = "setting" self._sendCommandActionToController(msg) print("Params: {}".format( msg.getData("parameters"))) #⬢⬢⬢⬢⬢➤ SELECT CONTROLLERS elif command == "controllerselect": self.command_category = "setting" self._sendCommandActionToController(msg) print("Controller selected: {}".format( msg.getData("id"))) #⬢⬢⬢⬢⬢➤ DISABLE CONTROLLERS elif command == "controllerdisable": self.command_category = "setting" self._sendCommandActionToController(msg) print("Controllers removed: {}".format( msg.getData("id"))) else: self.command_category = "unknown" Logger.error("INVALID input") self._reject_input_command() except Exception as e: Logger.error(e) self._reject_input_command() # ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ PRIVATE METHODS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ def _getToolFromName(self, tool_name): tool_v = self.tools_list[tool_name] tool = FrameVectorToKDL(tool_v) return tool def _sendCommandActionToController(self, msg): msg.setReceiver("{}_direct_commander".format(self.robot_name)) msg.setData("type", "force") self.robot_control_client.sendCommand(msg.toString()) def _sendMessageToController(self, msg): msg.setReceiver("{}_direct_commander".format(self.robot_name)) msg.setData("type", "force") self.inner_message_proxy.send(msg) def _resetDirectCommander(self): simple_message = SimpleMessage(command="_reset_", receiver="{}_direct_commander".format( self.robot_name)) self._sendCommandActionToController(simple_message) def _setTool(self, tool): """ tool is a PyKDL frame""" self.target_follower.setTool(tool) def _toJoints(self, joint_str): self._switchCommander(self.TRAJECTORY_COMMANDER, expect_done_response=False) q_str = joint_str txt_cmd = "joints {}".format(q_str) self.robot_trajectory_client.generateNewGoal(txt_cmd) def _toShape(self, shape_name): self._switchCommander(self.TRAJECTORY_COMMANDER, expect_done_response=False) txt_cmd = self._getCommandFromShape(shape_name) self.robot_trajectory_client.generateNewGoal(txt_cmd) def _toTf(self, tf_name, tool_name, mode="goto"): self._switchCommander(self.TRAJECTORY_COMMANDER, expect_done_response=False) if tool_name.startswith("dynamic"): tool = self.new_tools[tool_name] else: tool = self._getToolFromName(tool_name) self._setTool(tool) txt_cmd = "{} {}".format(mode, tf_name) self.robot_trajectory_client.generateNewGoal(txt_cmd) def _getCommandFromShape(self, shape_name): db_msg = self.message_database.searchByName(shape_name, JointState, single=False) q_str = str(db_msg[0][0].position).split("(")[1].split(")")[0].replace( " ", "") txt_cmd = "joints {}".format(q_str) return txt_cmd def _switchCommander(self, id, expect_done_response=True): if id == self.DIRECT_COMMANDER: simple_message = SimpleMessage( command="_on_", receiver="{}_direct_commander".format(self.robot_name)) self.target_follower.setSource(TargetFollowerProxy.SOURCE_DIRECT) elif id == self.TRAJECTORY_COMMANDER: simple_message = SimpleMessage( command="_off_", receiver="{}_direct_commander".format(self.robot_name)) self.target_follower.setSource( TargetFollowerProxy.SOURCE_TRAJECTORY) if expect_done_response: self._sendCommandActionToController(simple_message) else: self._sendMessageToController(simple_message) def _send_trajectory_command_result(self, msg): if self.command_server_last_message: if msg is None: error = GlobalErrorList.TRAJECTORY_ACTION_RESPONSE_NONETYPE Logger.error("Trajectory action response None-type") trajectory_done = False q_target_distance = -999 else: error = msg.error q_target_distance = msg.q_target_distance trajectory_done = msg.success self.command_server_last_message.addResponseData("error", error) self.command_server_last_message.addResponseData( "q_target_distance", q_target_distance) self.command_server_last_message.addResponseData( "trajectory_done", trajectory_done) self._send_command_result(trajectory_done) def _reject_input_command(self, error_type=GlobalErrorList.UNKNOWN_ERROR): if self.command_server_last_message: self.command_server_last_message.addResponseData( "error", error_type) if self.command_category == "trajectory": self._send_trajectory_command_result(None) else: self._send_command_result(False) def _send_command_result(self, success): if self.command_server_last_message: 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 # ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ STATES ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ # #⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ IDLE ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ # # ➤ ➤ ➤ ➤ ➤ IDLE: enter def on_enter_idle(self): Logger.log("State: IDLE") # ➤ ➤ ➤ ➤ ➤ IDLE: loop def on_loop_idle(self): pass # #⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ CONTROLLING ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ # # ➤ ➤ ➤ ➤ ➤ CONTROLLING: enter def on_enter_controlling(self): Logger.log("State: CONTROLLING") # ➤ ➤ ➤ ➤ ➤ CONTROLLING: loop def on_loop_controlling(self): pass # #⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ DIRECT ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ # # ➤ ➤ ➤ ➤ ➤ DIRECT: enter def on_enter_direct(self): Logger.log("State: DIRECT") # ➤ ➤ ➤ ➤ ➤ DIRECT: loop def on_loop_direct(self): if not rosnode.rosnode_ping("{}_direct_motion".format( self.robot_name)): self.alarm_proxy.setAlarm(AlarmProxy.NODE_DIED) self.alarm() pass # #⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ TRAJECTORY ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ # # ➤ ➤ ➤ ➤ ➤ TRAJECTORY: enter def on_enter_trajectory(self): Logger.log("State: TRAJECTORY") # self._switchCommander(self.TRAJECTORY_COMMANDER) # ➤ ➤ ➤ ➤ ➤ TRAJECTORY: loop def on_loop_trajectory(self): if not rosnode.rosnode_ping("{}_trajectory_motion".format( self.robot_name)): self.alarm_proxy.setAlarm(AlarmProxy.NODE_DIED) self.alarm() pass # #⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ ALARM ⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢⬢ # # ➤ ➤ ➤ ➤ ➤ ALARM: enter def on_enter_alarm(self): Logger.log("State: ALARM") self._reject_input_command(GlobalErrorList.ROBOT_ALARM) # ➤ ➤ ➤ ➤ ➤ ALARM: loop def on_loop_alarm(self): self._reject_input_command(GlobalErrorList.ROBOT_ALARM)
def __init__(self, robot_name, controllers_dict=None): self.robot_name = robot_name self.current_tf = None self.target_tf = None self.tf_target_name = "follow_target" self.active = False self.command_success_flg = False # target follower self.target_follower = TargetFollowerProxy(self.robot_name) # alarm self.alarm_proxy = AlarmProxy(self.robot_name) self.alarm_proxy.registerAlarmCallback(self.alarm_callback) self.alarm_proxy.registerResetCallback(self.alarm_reset_callback) # command message self.message_proxy = SimpleMessageProxy("{}_inner_message".format( self.robot_name)) self.message_proxy.register(self.command_callback) # command action self.command_server_last_message = None self.command_server = CommandProxyServer("{}_direct_commander".format( self.robot_name)) self.command_server.registerCallback(self.command_action_callback) # tf self.br = tf.TransformBroadcaster() self.listener = tf.TransformListener() # Joystick Contorol joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback, queue_size=1) self.joystick = Joystick(translation_mag=0.0005, rotation_mag=0.0008) self.joystick.registerCallback(self.joystick_key_callback) self.joystick_flg = False # Disable by default # Closed-Loop Control if controllers_dict is None or controllers_dict == {}: controllers_dict = {"none": NeutralController()} self.controllers_dict = controllers_dict self.active_controllers = {} self.active_controllers["none"] = NeutralController() Logger.warning("READY!!!") # Feedback self.feedback_data = {} # Force rospy.Subscriber('/atift', Twist, self.force_feedback_callback, queue_size=1) # Tactile rospy.Subscriber('/tactile', Float64MultiArray, self.tactile_feedback_callback, queue_size=1)
class DirectCommander(object): def __init__(self, robot_name, controllers_dict=None): self.robot_name = robot_name self.current_tf = None self.target_tf = None self.tf_target_name = "follow_target" self.active = False self.command_success_flg = False # target follower self.target_follower = TargetFollowerProxy(self.robot_name) # alarm self.alarm_proxy = AlarmProxy(self.robot_name) self.alarm_proxy.registerAlarmCallback(self.alarm_callback) self.alarm_proxy.registerResetCallback(self.alarm_reset_callback) # command message self.message_proxy = SimpleMessageProxy("{}_inner_message".format( self.robot_name)) self.message_proxy.register(self.command_callback) # command action self.command_server_last_message = None self.command_server = CommandProxyServer("{}_direct_commander".format( self.robot_name)) self.command_server.registerCallback(self.command_action_callback) # tf self.br = tf.TransformBroadcaster() self.listener = tf.TransformListener() # Joystick Contorol joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback, queue_size=1) self.joystick = Joystick(translation_mag=0.0005, rotation_mag=0.0008) self.joystick.registerCallback(self.joystick_key_callback) self.joystick_flg = False # Disable by default # Closed-Loop Control if controllers_dict is None or controllers_dict == {}: controllers_dict = {"none": NeutralController()} self.controllers_dict = controllers_dict self.active_controllers = {} self.active_controllers["none"] = NeutralController() Logger.warning("READY!!!") # Feedback self.feedback_data = {} # Force rospy.Subscriber('/atift', Twist, self.force_feedback_callback, queue_size=1) # Tactile rospy.Subscriber('/tactile', Float64MultiArray, self.tactile_feedback_callback, queue_size=1) # ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ CALLBACKS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ # TODO problema: OGNI VOLTA CHE SI AGGIUNGE UN FEEDBACK OCCORRE CREARE UNA CALLBACK!!!! def force_feedback_callback(self, msg): msg = self._getForceOnTool(msg) self.feedback_data["atift"] = msg def tactile_feedback_callback(self, msg): self.feedback_data["tactile"] = msg def joy_callback(self, msg): self.joystick.update(msg) 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 command_action_callback(self, cmd): self.command_server_last_message = cmd cmd_msg = cmd.getSentMessage() self.command_callback(cmd_msg) time.sleep(0.1) self._send_command_result(self.command_success_flg) 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 alarm_callback(self, alarm_info): if alarm_info != self.alarm_proxy.NONE_ALARM: Logger.error(" !!!!!!!! ALARM !!!!!!!! ") self.target_tf = self.current_tf self.active = False def alarm_reset_callback(self): Logger.warning(" ******** ALARM CLEAR ******** ") self.target_tf = self.current_tf self._reset() # ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ PRIVATE METHODS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ 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 _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 _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 _reset(self, ctrlid=None): self.target_tf = self.current_tf if ctrlid is None: for ctrl_id in self.active_controllers.keys(): self.active_controllers[ctrl_id].reset() else: self.active_controllers[ctrlid].reset() def _getForceOnTool(self, twist_on_sensor): linear_force_on_sensor = PyKDL.Frame(TwistToKDLVector(twist_on_sensor)) tr_link6_fs = PyKDL.Frame( PyKDL.Vector(WRIST_FT_SENSOR_POSITION[0], WRIST_FT_SENSOR_POSITION[1], WRIST_FT_SENSOR_POSITION[2])) tr_link6_fs.M = tr_link6_fs.M.RotX(-scipy.pi) tr_tool_link6 = transformations.retrieveTransform( self.listener, self.robot_name + "/tool", self.robot_name + "/link6", none_error=True, time=rospy.Time(0), print_error=True) # TODO aggiungere una tf eef (in comau coincide con link6 in shunk con finger1 (me media tra i 2)) try: tr_tool_fs = tr_tool_link6 * tr_link6_fs linear_force_on_tool = tr_tool_fs * linear_force_on_sensor twist_on_tool = TwistFormKDLVector(linear_force_on_tool.p) return twist_on_tool except Exception as e: print(e) def _derivateLinearForce(self, twist): dT = Twist() dT.linear.x = self.force_calculator.derivate(twist.linear.x) dT.linear.y = self.force_calculator.derivate(twist.linear.y) dT.linear.z = self.force_calculator.derivate(twist.linear.z) np.savetxt(self.file_handler, [[time.time(), dT.linear.x, dT.linear.y, dT.linear.z]], fmt='%.18e %.18e %.18e %.18e', delimiter=' ') return dT # ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ PUBLIC METHODS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ def stepForward(self): try: if self.active: self.current_tf = transformations.retrieveTransform( self.listener, self.robot_name + "/base_link", # TODO aggiungere base_link in shunck self.robot_name + "/tool", none_error=True, time=rospy.Time(0), print_error=True) if self.current_tf is not None: if self.target_tf is None: self.target_tf = self.current_tf #➤ Joystick if self.joystick_flg: self.target_tf = self.joystick.transformT( self.target_tf) #➤ Control Action else: self.feedback_data["current_tf"] = self.current_tf self.feedback_data["target_tf"] = self.target_tf for ctrl_id in self.active_controllers.keys(): self.target_tf = self.active_controllers[ ctrl_id].output(self.feedback_data) ####### Publish and Set Target ####### transformations.broadcastTransform(self.br, self.target_tf, self.tf_target_name, self.robot_name + "/base_link", time=rospy.Time.now()) dist = 1 # diff_tf = self.current_tf.Inverse() * self.target_tf # transformations.broadcastTransform(self.br, diff_tf, "diff_tf", # self.robot_name + "/base_link", time = rospy.Time.now()) # dist_lin, distr_ang=transformations.frameDistance(diff_tf, # PyKDL.Frame()) # distr_ang=np.linalg.norm(distr_ang) # dist_lin=np.linalg.norm( # transformations.KDLVectorToNumpyArray(dist_lin)) # dist=distr_ang + dist_lin # print dist if dist > 0.0003 or self.joystick_flg: self.target_follower.setTarget( target=self.target_tf, target_type=TargetFollowerProxy.TYPE_POSE, target_source=TargetFollowerProxy.SOURCE_DIRECT) else: Logger.error("tf not ready!!") else: pass except Exception as e: Logger.warning(e) pass
class SensorManager(object): def __init__(self, sensor_name): self.sensor_name = sensor_name self.message_proxy = SimpleMessageProxy() self.message_proxy.register(self.command_callback) self.last_msg = None # robot outer command self.command_server = CommandProxyServer("{}_supervisor".format( self.sensor_name)) self.command_server.registerCallback(self.command_action_callback) self.command_server_last_message = None def update(self, msg): self.last_msg = msg def uploadResetPublisher(self, pub): self.reset_publisher = pub def command_action_callback(self, cmd): self.command_server_last_message = cmd cmd_msg = cmd.getSentMessage() self.command_callback(cmd_msg) 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 _send_command_result(self, success): if self.command_server_last_message: 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
class TfFilter(object): def __init__(self): self.filtered_tf_map = {} self.filtered_tf_slots = {} self.current_tf_name = "" self.default_sls = 100 self.command_server = CommandProxyServer("tf_filter_supervisor") self.command_server.registerCallback(self.command_action_callback) self.command_server_last_message = None self.message_proxy = SimpleMessageProxy() self.message_proxy.register(self.command_callback) def resetFiltering(self): Logger.warning("FILTER RESET") self.filtered_tf_map = {} self.filtered_tf_slots = {} # self._send_command_result(True) def clearFiltering(self, name): Logger.warning("FILTER CLEAR: {}".format(name)) self.filtered_tf_map[name] = [] self.filtered_tf_slots[name] = 0 # self._send_command_result(True) def startFiltering(self, name, slot_size): Logger.warning("FILTER START: {}".format(name)) self.current_tf_name = name self.filtered_tf_slots[name] = slot_size if name not in self.filtered_tf_map: self.filtered_tf_map[name] = [] def stopFiltering(self): Logger.warning("FILTER STOP") self.current_tf_name = '' # self._send_command_result(True) def updateAndBroadcast(self, node, current_tf, size): if self.current_tf_name != '': if current_tf: if self.filtered_tf_slots[self.current_tf_name] > 0: self.filtered_tf_slots[self.current_tf_name] -= 1 print(self.current_tf_name, self.filtered_tf_slots[self.current_tf_name]) if len(self.filtered_tf_map[self.current_tf_name]) < size: self.filtered_tf_map[self.current_tf_name].append( current_tf) else: Logger.warning( "TF Buffer for '{}' is full! Clear it...".format( current_tf_name)) else: self._send_command_result(True) p = np.array([0, 0, 0]) q = np.array([0, 0, 0, 0]) for tf_name, tf_list in self.filtered_tf_map.iteritems(): if len(tf_list) <= 0: continue for tf in tf_list: newp = np.array([tf.p.x(), tf.p.y(), tf.p.z()]) qx, qy, qz, qw = tf.M.GetQuaternion() newq = np.array([qx, qy, qz, qw]) p = p + newp q = q + newq p = p / float(len(tf_list)) q = q / float(len(tf_list)) q = q / np.linalg.norm(q) filtered_tf = PyKDL.Frame() filtered_tf.p = PyKDL.Vector(p[0], p[1], p[2]) filtered_tf.M = PyKDL.Rotation.Quaternion(q[0], q[1], q[2], q[3]) node.broadcastTransform(filtered_tf, tf_name + "_filtered", node.getParameter("world_tf"), node.getCurrentTime()) # return filtered_tf def _send_command_result(self, success): if self.command_server_last_message: print("\nTask Ended!!! \t Success: {}".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 # ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ CALLBACKS ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ def command_action_callback(self, cmd): self.command_server_last_message = cmd cmd_msg = cmd.getSentMessage() self.command_callback(cmd_msg) 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)