def _handle_stop_clicked(self): """ GUI Envent handler Stops V-REP """ msg = Empty() self.pub_reset.publish(msg) vrep_controll.TriggerSimualtion() vrep_controll.stopSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion()
def _handle_reset_clicked(self): """ GUI Envent handler, resets gui/node and V-REP """ msg = Empty() self.pub_reset.publish(msg) vrep_controll.TriggerSimualtion() vrep_controll.stopSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.startSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.startSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.startSimualtion() vrep_controll.TriggerSimualtion()
def init_params(self, msg): """ init parameters for main node of youbot remote writing @return <b><i><c> [void]: </c></i></b> nothing """ self.rate = 100 self.res = 1000 self.disable = False self.status = 1 # 0= error 1= no error self.status_string = "no error" self.status_vrep = 0 # 0=doing nothing 1= in progress 2= movement done self.kinematics = kin.Kinematics_geom() self.kin_num = kinematics_nummeric.Kinematics_num() self.config_trgt_pos = np.array([np.nan, np.nan, np.nan]) self.hoverOffset = 0.01 self.line = 0 self.line_ending_threshold = 0.1 self.start_line_offset = -0.00 self.nextLetterPos = np.array([self.start_line_offset, -self.line_ending_threshold, 0]) # z coordinate # parse xml letter database script_path = os.path.dirname(os.path.abspath(__file__)) print("script dir: "), script_path tmp_use_rosparam_path = rospy.get_param("/youbot_rw_node/general/USE_ROSPARAM_PATH_FOR_DATABASE") if (tmp_use_rosparam_path): db_path = rospy.get_param("/youbot_rw_node/general/PATH_OF_LETTER_DATABASE") else: db_path = script_path + '/../../../../../material/YouBot_RW_Material/Buchstaben_Datenbank/letter_database.xml' print("opening following letter_database: "), db_path self.letter_database = ET.parse(db_path) self.ldb_root = self.letter_database.getroot() # print loaded ldb elements print("Loaded "), self.ldb_root.tag, (": "), [x.tag for x in self.ldb_root] sync.init_sync() vrep_controll.TriggerSimualtion() vrep_controll.unsetSyncSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.stopSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.startSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.startSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.setSyncSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() vrep_controll.TriggerSimualtion() self.config_thetas_bogen = np.array(sync.getJointPostition()) self.desired_thetas_bogen = self.config_thetas_bogen self.config_cur_pos = self.kinematics.direct_kin(self.config_thetas_bogen)