def ros_load_robot_config(config_file, test=False):
config = load_robot_yaml(utils.get_ros_pkg_path(u'giskardpy') + u'/config/' + config_file)
if test:
config = utils.update_nested_dicts(deepcopy(config),
load_robot_yaml(utils.get_ros_pkg_path(u'giskardpy') + u'/config/test.yaml'))
if config and not rospy.is_shutdown():
rospy.set_param('~', config)
return True
return False
def __init__(self, config_file):
with open(get_ros_pkg_path(u'giskardpy') + u'/config/' +
config_file) as f:
config = yaml.load(f)
rospy.set_param(u'~', config)
rospy.set_param(u'~path_to_data_folder', u'tmp_data/')
rospy.set_param(u'~enable_gui', False)
self.sub_result = rospy.Subscriber(u'/giskardpy/command/result',
MoveActionResult,
self.cb,
queue_size=100)
self.tree = grow_tree()
self.loop_once()
# rospy.sleep(1)
self.wrapper = GiskardWrapper(ns=u'tests')
self.results = Queue(100)
self.default_root = self.get_robot().get_root()
self.map = u'map'
self.simple_base_pose_pub = rospy.Publisher(u'/move_base_simple/goal',
PoseStamped,
queue_size=10)
self.set_base = rospy.ServiceProxy(u'/base_simulator/set_joint_states',
SetJointState)
self.tick_rate = 10
def create_publisher(topic):
p = rospy.Publisher(topic, JointState, queue_size=10)
rospy.sleep(.2)
return p
self.joint_state_publisher = KeyDefaultDict(create_publisher)
def __init__(self, stream):
"""
Initialise Loader by setting the root directory, specifying keywords for finding config files from other
ROS packages.
"""
try:
self.config_root = os.path.split(stream.name)[0]
self.ros_package_keywords = [u'ros://', u'package://']
self.giskardpy_root = utils.get_ros_pkg_path(u'giskardpy')
except AttributeError:
self.config_root = os.path.curdir
super(Loader, self).__init__(stream)
def get_filename(loader, node, root):
"""Returns file name referenced at given node by using the given loader."""
file_or_ros_path_str = loader.construct_scalar(node)
indices = [i for i, x in enumerate(loader.ros_package_keywords) if x in file_or_ros_path_str]
if indices:
if len(indices) != 1:
raise SyntaxError(u'Invalid ros package path: please use ros:// or package:// as path prefix.')
removed_key_word = file_or_ros_path_str.replace(loader.ros_package_keywords[indices[0]], '')
path_split = removed_key_word.split('/')
package_path = utils.get_ros_pkg_path(path_split[0])
filename = package_path + removed_key_word.replace(path_split[0], '')
else:
filename = os.path.abspath(os.path.join(root, file_or_ros_path_str))
return filename