def __init__(self, node_name=u'giskard'):
giskard_topic = u'{}/command'.format(node_name)
if giskard_topic is not None:
self._client = SimpleActionClient(giskard_topic, MoveAction)
self._update_world_srv = rospy.ServiceProxy(
u'{}/update_world'.format(node_name), UpdateWorld)
self._get_object_names_srv = rospy.ServiceProxy(
u'{}/get_object_names'.format(node_name), GetObjectNames)
self._get_object_info_srv = rospy.ServiceProxy(
u'{}/get_object_info'.format(node_name), GetObjectInfo)
self._update_rviz_markers_srv = rospy.ServiceProxy(
u'{}/update_rviz_markers'.format(node_name), UpdateRvizMarkers)
self._get_attached_objects_srv = rospy.ServiceProxy(
u'{}/get_attached_objects'.format(node_name),
GetAttachedObjects)
self._marker_pub = rospy.Publisher(u'visualization_marker_array',
MarkerArray,
queue_size=10)
rospy.wait_for_service(u'{}/update_world'.format(node_name))
self._client.wait_for_server()
self.robot_urdf = URDFObject(rospy.get_param(u'robot_description'))
self.collisions = []
self.clear_cmds()
self._object_js_topics = {}
rospy.sleep(.3)
def load_urdf_string_into_bullet(urdf_string, pose=None):
"""
Loads a URDF string into the bullet world.
:param urdf_string: XML string of the URDF to load.
:type urdf_string: str
:param pose: Pose at which to load the URDF into the world.
:type pose: Pose
:return: internal PyBullet id of the loaded urdfs
:rtype: intload_urdf_string_into_bullet
"""
if pose is None:
pose = Pose()
pose.orientation.w = 1
if isinstance(pose, PoseStamped):
pose = pose.pose
object_name = URDFObject(urdf_string).get_name()
if object_name in get_body_names():
raise DuplicateNameException(
u'an object with name \'{}\' already exists in pybullet'.format(
object_name))
resolved_urdf = resolve_ros_iris_in_urdf(urdf_string)
filename = write_to_tmp(u'{}.urdfs'.format(random_string()), resolved_urdf)
with NullContextManager(
) if giskardpy.PRINT_LEVEL == DEBUG else suppress_stdout():
id = p.loadURDF(filename,
[pose.position.x, pose.position.y, pose.position.z], [
pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w
],
flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
os.remove(filename)
return id
def get_attached_objects(self, req):
original_robot = URDFObject(self.get_robot().original_urdf)
link_names = self.get_robot().get_link_names()
original_link_names = original_robot.get_link_names()
attached_objects = list(set(link_names).difference(original_link_names))
attachment_points = [self.get_robot().get_parent_link_of_joint(object) for object in attached_objects]
res = GetAttachedObjectsResponse()
res.object_names = attached_objects
res.attachment_points = attachment_points
return res
def __init__(self, sim=True, move=True):
# rospy.init_node('tests')
# rospy.set_param(DummyInterfaceNodeName + '/initial_beliefstate', 'package://refills_perception_interface/owl/muh.owl')
# rospy.set_param(DummyInterfaceNodeName + '/initial_beliefstate',
# 'package://refills_rooming_in/data/augsburg_rooming_in_map_2018-11-08_10-17-17.owl')
# rospy.set_param(DummyInterfaceNodeName + '/initial_beliefstate',
# 'package://refills_rooming_in/data/rooming_in_map_2018-11-29_16-59-38.owl')
# rospy.set_param(DummyInterfaceNodeName + '/path_to_json',
# 'package://refills_rooming_in/data/augsburg_rooming_in_map_2018-11-08_10-17-17.json')
self.query_shelf_systems_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_shelf_systems', QueryShelfSystems)
self.query_shelf_layers_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_shelf_layers', QueryShelfLayers)
self.query_facings_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_facings', QueryFacings)
self.finish_perception_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/finish_perception', FinishPerception)
self.query_reset_belief_state_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/reset_beliefstate', Trigger)
self.query_shelf_detection_path_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_detect_shelf_layers_path',
QueryDetectShelfLayersPath)
self.query_facing_detection_path_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_detect_facings_path',
QueryDetectFacingsPath)
self.query_product_counting_path_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_count_products_posture',
QueryCountProductsPosture)
self.detect_shelves_ac = SimpleActionClient(
DummyInterfaceNodeName + '/detect_shelf_layers',
DetectShelfLayersAction)
self.detect_facings_ac = SimpleActionClient(
DummyInterfaceNodeName + '/detect_facings', DetectFacingsAction)
self.count_products_ac = SimpleActionClient(
DummyInterfaceNodeName + '/count_products', CountProductsAction)
# self.simple_base_goal_pub = rospy.Publisher('move_base_simple/goal', PoseStamped)
self.simple_joint_goal = rospy.ServiceProxy(
'refills_bot/set_joint_states', SetJointState)
self.sleep = sim
self.sleep_amount = 0
self.move = move
if URDFObject(rospy.get_param(
'robot_description')).get_name() == 'iai_donbot':
from refills_perception_interface.move_arm import MoveArm
self.giskard = MoveArm(avoid_self_collisinon=True)
else:
from refills_perception_interface.move_arm_kmr_iiwa import MoveArm
self.giskard = MoveArm(avoid_self_collisinon=True)
# self.base = MoveBase()
rospy.sleep(.5)
def __init__(self, giskard_topic=u'giskardpy/command', ns=u'giskard'):
if giskard_topic is not None:
self.client = SimpleActionClient(giskard_topic, MoveAction)
self.update_world = rospy.ServiceProxy(u'{}/update_world'.format(ns), UpdateWorld)
self.marker_pub = rospy.Publisher(u'visualization_marker_array', MarkerArray, queue_size=10)
rospy.wait_for_service(u'{}/update_world'.format(ns))
self.client.wait_for_server()
self.tip_to_root = {}
self.robot_urdf = URDFObject(rospy.get_param(u'robot_description'))
self.collisions = []
self.clear_cmds()
self.object_js_topics = {}
rospy.sleep(.3)
def dump_state_cb(self, data):
try:
path = self.get_god_map().unsafe_get_data(identifier.data_folder)
folder_name = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
folder_path = '{}{}'.format(path, folder_name)
os.mkdir(folder_path)
robot = self.unsafe_get_robot()
world = self.unsafe_get_world()
with open("{}/dump.txt".format(folder_path), u'w') as f:
tree_manager = self.get_god_map().unsafe_get_data(identifier.tree_manager) # type: TreeManager
joint_state_message = tree_manager.get_node(u'js1').lock.get()
f.write("initial_robot_joint_state_dict = ")
write_dict(to_joint_state_position_dict(joint_state_message), f)
f.write("try:\n" +
" x_joint = initial_robot_joint_state_dict[\"odom_x_joint\"]\n" +
" y_joint = initial_robot_joint_state_dict[\"odom_y_joint\"]\n" +
" z_joint = initial_robot_joint_state_dict[\"odom_z_joint\"]\n" +
" base_pose = PoseStamped()\n" +
" base_pose.header.frame_id = \"map\"\n" +
" base_pose.pose.position = Point(x_joint, y_joint, 0)\n" +
" base_pose.pose.orientation = Quaternion(*quaternion_about_axis(z_joint, [0, 0, 1]))\n" +
" zero_pose.teleport_base(base_pose)\n" +
"except:\n" +
" logging.loginfo(\'no x,y and z joint\')\n\n")
f.write("zero_pose.send_and_check_joint_goal(initial_robot_joint_state_dict)\n")
robot_base_pose = PoseStamped()
robot_base_pose.header.frame_id = 'map'
robot_base_pose.pose = robot.base_pose
f.write("map_odom_transform_dict = ")
write_dict(convert_ros_message_to_dictionary(robot_base_pose), f)
f.write("map_odom_pose_stamped = convert_dictionary_to_ros_message(\'geometry_msgs/PoseStamped\', map_odom_transform_dict)\n")
f.write("map_odom_transform = Transform()\n" +
"map_odom_transform.rotation = map_odom_pose_stamped.pose.orientation\n" +
"map_odom_transform.translation = map_odom_pose_stamped.pose.position\n\n")
f.write("set_odom_map_transform = rospy.ServiceProxy('/map_odom_transform_publisher/update_map_odom_transform', UpdateTransform)\n")
f.write("set_odom_map_transform(map_odom_transform)\n")
original_robot = URDFObject(robot.original_urdf)
link_names = robot.get_link_names()
original_link_names = original_robot.get_link_names()
attached_objects = list(set(link_names).difference(original_link_names))
for object_name in attached_objects:
parent = robot.get_parent_link_of_joint(object_name)
pose = robot.get_fk_pose(parent, object_name)
world_object = robot.get_sub_tree_at_joint(object_name)
f.write("#attach {}\n".format(object_name))
with open("{}/{}.urdf".format(folder_path, object_name), u'w') as f_urdf:
f_urdf.write(world_object.original_urdf)
f.write('with open(\'{}/{}.urdf\', \"r\") as f:\n'.format(folder_path, object_name))
f.write(" {}_urdf = f.read()\n".format(object_name))
f.write("{0}_name = \"{0}\"\n".format(object_name))
f.write("{}_pose_stamped_dict = ".format(object_name))
write_dict(convert_ros_message_to_dictionary(pose), f)
f.write("{0}_pose_stamped = convert_dictionary_to_ros_message('geometry_msgs/PoseStamped', {0}_pose_stamped_dict)\n".format(object_name))
f.write(
"zero_pose.add_urdf(name={0}_name, urdf={0}_urdf, pose={0}_pose_stamped)\n".format(object_name))
f.write(
"zero_pose.attach_existing(name={0}_name, frame_id=\'{1}\')\n".format(object_name, parent))
for object_name, world_object in world.get_objects().items(): # type: (str, WorldObject)
f.write("#add {}\n".format(object_name))
with open("{}/{}.urdf".format(folder_path, object_name), u'w') as f_urdf:
f_urdf.write(world_object.original_urdf)
f.write('with open(\'{}/{}.urdf\', \"r\") as f:\n'.format(folder_path,object_name))
f.write(" {}_urdf = f.read()\n".format(object_name))
f.write("{0}_name = \"{0}\"\n".format(object_name))
f.write("{0}_js_topic = \"{0}_js_topic\"\n".format(object_name))
f.write("{}_pose_dict = ".format(object_name))
write_dict(convert_ros_message_to_dictionary(world_object.base_pose), f)
f.write("{0}_pose = convert_dictionary_to_ros_message('geometry_msgs/Pose', {0}_pose_dict)\n".format(object_name))
f.write("{}_pose_stamped = PoseStamped()\n".format(object_name))
f.write("{0}_pose_stamped.pose = {0}_pose\n".format(object_name))
f.write("{0}_pose_stamped.header.frame_id = \"map\"\n".format(object_name))
f.write("zero_pose.add_urdf(name={0}_name, urdf={0}_urdf, pose={0}_pose_stamped, js_topic={0}_js_topic, set_js_topic=None)\n".format(object_name))
f.write("{}_joint_state = ".format(object_name))
write_dict(to_joint_state_position_dict((dict_to_joint_states(world_object.joint_state))), f)
f.write("zero_pose.set_object_joint_state({0}_name, {0}_joint_state)\n\n".format(object_name))
last_goal = self.get_god_map().unsafe_get_data(identifier.next_move_goal)
if last_goal:
f.write(u'last_goal_dict = ')
write_dict(convert_ros_message_to_dictionary(last_goal), f)
f.write("last_goal_msg = convert_dictionary_to_ros_message('giskard_msgs/MoveCmd', last_goal_dict)\n")
f.write("last_action_goal = MoveActionGoal()\n")
f.write("last_move_goal = MoveGoal()\n")
f.write("last_move_goal.cmd_seq = [last_goal_msg]\n")
f.write("last_move_goal.type = MoveGoal.PLAN_AND_EXECUTE\n")
f.write("last_action_goal.goal = last_move_goal\n")
f.write("zero_pose.send_and_check_goal(goal=last_action_goal)\n")
else:
f.write(u'#no goal\n')
logging.loginfo(u'saved dump to {}'.format(folder_path))
except:
logging.logerr('failed to dump state pls try again')
res = TriggerResponse()
res.message = 'failed to dump state pls try again'
return TriggerResponse()
res = TriggerResponse()
res.success = True
res.message = 'saved dump to {}'.format(folder_path)
return res
while not rospy.has_param('/urdf_merger/urdf_sources'):
logging.loginfo('waiting for rosparam /urdf_merger/urdf_sources')
rospy.sleep(1.0)
while not rospy.has_param('/urdf_merger/robot_name'):
logging.loginfo('waiting for rosparam /urdf_merger/robot_name')
rospy.sleep(1.0)
urdf_sources = rospy.get_param('/urdf_merger/urdf_sources')
robot_name = rospy.get_param('/urdf_merger/robot_name')
merged_object = None
if rospy.has_param(urdf_sources[0]['source']):
urdf_string = rospy.get_param(urdf_sources[0]['source'])
merged_object = URDFObject(urdf_string)
else:
merged_object = URDFObject.from_urdf_file(urdf_sources[0]['source'])
for source in urdf_sources[1:]:
if rospy.has_param(source['source']):
urdf_string = rospy.get_param(source['source'])
urdf_object = URDFObject(urdf_string)
else:
urdf_object = URDFObject.from_urdf_file(source['source'])
positon = Point()
rotation = Quaternion()
if 'position' in source.keys():
position = Point(source['position'][0], source['position'][1],
source['position'][2])