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)