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 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 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 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 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 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 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): 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 _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 _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 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 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 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 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 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.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 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, 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 _sendFeedback(self, text="", state="", command="", status=0, progress=-1.0, warning=0): if self.feedback_ext_callback: self.feedback.current_state = state self.feedback.text = text self.feedback.executing_command = command self.feedback.status = status self.feedback.progress = progress self.feedback.warning = warning self.feedback_ext_callback(self.feedback) else: Logger.error("External callback for FEEDBACK update NOT found!")
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 command_cb(message): global current_tf_name, publishing, all_tfs if message.isValid(): receiver = message.getReceiver() command = message.getCommand() if receiver == node.getName(): print("New command received", message.toString()) #⬢⬢⬢⬢⬢➤ UPDATE if command == 'update': all_tfs = tf_storage.allTfs() return #⬢⬢⬢⬢⬢➤ START PUBLISH if command == 'start_publish': publishing = True return #⬢⬢⬢⬢⬢➤ STOP PUBLISH if command == 'stop_publish': publishing = False return #⬢⬢⬢⬢⬢➤ DELETE_ALL if command == 'delete_all': tf_storage.deleteAll() all_tfs = [] return #⬢⬢⬢⬢⬢➤ DELETE if command == 'delete': saving_name = message.getData("saving_name") tf_storage.removeFrameByName(saving_name) all_tfs = tf_storage.allTfs() return #⬢⬢⬢⬢⬢➤ SAVE if command == 'save': saving_name = message.getData("saving_name") tf_name = message.getData("tf_name") tf_parent = message.getData("tf_parent") iterations = node.getParameter("max_save_iterations") if tf_name == None: Logger.error("'tf_name' must be not None") return if tf_parent == None: Logger.error("'tf_parent' must be not None") return if saving_name == None: Logger.error("'saving_name' must be not None") return while iterations > 0: frame = node.retrieveTransform(tf_name, tf_parent, -1) if frame: tf_storage.saveFrame(frame, saving_name, tf_parent) print("Saving ", tf_name, tf_parent) return Logger.error( "Max iterations reached, retrieving '{}'".format(tf_name))
def simple_ik(self, target_frame, q_current, ik_type=0): """ Calls a simple_ik service """ try: p = Pose() p.position.x = target_frame.p.x() p.position.y = target_frame.p.y() p.position.z = target_frame.p.z() qu = target_frame.M.GetQuaternion() p.orientation.x = qu[0] p.orientation.y = qu[1] p.orientation.z = qu[2] p.orientation.w = qu[3] q_in = Float64MultiArray() q_in.data = q_current response = self.service(p, q_in, 0) return response except rospy.ServiceException, e: Logger.error("GeneralIKService: Service call failed: % s" % e)
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 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 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_callback(self, alarm_info): if alarm_info != self.alarm_proxy.NONE_ALARM: Logger.error(" !!!!!!!! ALARM !!!!!!!! ") self.alarm()
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()
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