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))