def load(self, name, path): if not os.path.exists(path): return False if not name.startswith('/'): name = '/' + name f = file(path, 'r') try: params = {} for doc in yaml.load_all(f.read(), Loader=yaml.FullLoader): params.update(doc) finally: f.close() try: self.client = DynamicReconfigureClient(name) try: self.client.update_configuration(params) except DynamicReconfigureCallbackException as err: logger.log_error(str(err)) return False except DynamicReconfigureParameterException as err: logger.log_error(str(err)) return False return True finally: self.client.close()
def main(): topic_in = rospy.get_param(node_name + '/topic_in', '/joint_states') try: manipulator_controller = ManipulatorController(topic_in) manipulator_controller.spinner() rospy.signal_shutdown(node_name + ' node is shoting down') except rospy.ROSInternalException as e: logger.log_error(e + " program interrupted before completion")
def configuration(self, config, level): self.axes_button_trigger = {} # get button_names from dynamic-reconfigure server button_names = str(config['button_names']) self.button_names = [ button_name.strip() for button_name in button_names.split(',') ] self.axes_threshold = config['axes_threshold'] # get axes_names from dynamic-reconfigure server axes_names = str(config['axes_names']) self.axes_names = [ axes_name.strip() for axes_name in axes_names.split(',') ] # get axes_names from dynamic-reconfigure server button_to_axes = str(config['button_to_axes']) self.button_to_axes = [ button_name.strip() for button_name in button_to_axes.split(',') ] self.last_button_to_axes_states = {} # validate and extract axes_to_button config # if result is None it means that configuration string # is bad. axes_to_button = str(config['axes_to_button']) if len(axes_to_button) > 2: axes_to_button = self._extract_axes_to_button_config( axes_to_button) if not axes_to_button: logger.log_error('bad axes_to_button configuration.') else: # the structure of each item is like: # { # 'axes_idx': <number>, # 'mode': <'less-than', 'greater-than'>, # 'threshold': <number>, # 'button_name': <string> # } self.axes_to_button = axes_to_button else: self.axes_to_button = [] # validate and extract map_axes config, like axes_to_button map_axes = self._extract_map_axes_config(str(config['map_axes'])) if not map_axes: logger.log_error('bad map_axes configuration.') else: self.map_axes = map_axes return config
def init_params(self): out = subprocess.Popen(['rospack', 'find', 'arka_manipulator'], stdout=subprocess.PIPE, stderr=subprocess.STDOUT) base_path, _ = out.communicate() config_path = os.path.join(str(base_path[:-1]), 'config/manual_calibration.yaml') if not os.path.exists(config_path): logger.log_error( 'manual calibration file {} not exist please check the path'. format(config_path)) self.path_to_read_arm_calibration = rospy.get_param( '~arm_calibration_path', config_path)
self.manipulator_joy_commander.manipulator_protocol.angularspeed. joint1, self.manipulator_joy_commander.manipulator_protocol. angularspeed.joint2, self.manipulator_joy_commander. manipulator_protocol.angularspeed.joint3)) @staticmethod def rad_per_secn_rpm(value): return value * 60 / (pi * 2) def spin(self): rate = rospy.Rate(40) while not rospy.is_shutdown(): self.manipulator_joy_commander.manipulator_protocol.send_data_old_main_board( ) self.manipulator_joy_commander.manipulator_protocol.send_sensor_board_manipulator_data( ) rate.sleep() if __name__ == "__main__": rospy.init_node(node_name, anonymous=True) logger.stack_print(True) manipulator = Manipulator() try: # rospy.spin() manipulator.spin() except KeyboardInterrupt: rospy.signal_shutdown('keyboard interrupt') except rospy.ROSException as err: logger.log_error("{}".format(err))