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)
#!/usr/bin/env python
import rospy
from giskardpy.garden import grow_tree
from giskardpy.utils import check_dependencies
from giskardpy import logging, identifier
# TODO add pytest to package xml
# TODO add transform3d to package xml
if __name__ == u'__main__':
rospy.init_node(u'giskard')
check_dependencies()
tree = grow_tree()
tree_tick_rate = 1. / rospy.get_param(
rospy.get_name() + u'/' + u'/'.join(identifier.tree_tick_rate[1:]))
sleeper = rospy.Rate(tree_tick_rate)
logging.loginfo(u'giskard is ready')
while not rospy.is_shutdown():
try:
tree.tick()
sleeper.sleep()
except KeyboardInterrupt:
break
logging.loginfo(u'\n')