def update(self): markers = [] robot = self.get_robot() get_fk = robot.get_fk_pose links = [x for x in self.get_robot().get_link_names() if robot.has_link_visuals(x)] for i, link_name in enumerate(links): marker = robot.link_as_marker(link_name) if marker is None: continue marker.header.frame_id = self.robot_base marker.action = Marker.ADD marker.id = int(hashlib.md5(link_name).hexdigest()[:6], 16) # FIXME find a better way to give the same link the same id marker.ns = u'planning_visualization' marker.header.stamp = rospy.Time() origin = robot.get_urdf_link(link_name).visual.origin fk = get_fk(self.robot_base, link_name).pose if origin is not None: marker.pose.position = Point(*origin.xyz) marker.pose.orientation = Quaternion(*quaternion_from_euler(*origin.rpy)) marker.pose = kdl_to_pose(pose_to_kdl(fk) * pose_to_kdl(marker.pose)) else: marker.pose = fk markers.append(marker) self.publisher.publish(markers) return py_trees.common.Status.SUCCESS
def update(self): markers = [] time_stamp = rospy.Time() objects_dict = self.get_world().get_objects() # If objects were added, update the namespace and objects if self.has_environment_changed(): self.set_full_link_names_for_objects() # Creating of marker for every link in an object for object_name, object in objects_dict.items(): for link_name in object.get_link_names(): if object.has_link_visuals(link_name): # Simple objects (containing only one link) are skipped, since they are already managed # in plugin_pybullet.py and as marker encoded with the function as_marker_msg from urdf_object.py if link_name == object_name and len( object.get_link_names()) == 1: continue # More complex objects will be published here: else: marker = object.link_as_marker(link_name) if marker is None: continue marker.header.frame_id = self.map_frame id_str = self.get_id_str(object_name, link_name) marker.id = int( hashlib.md5(id_str).hexdigest()[:6], 16 ) # FIXME find a better way to give the same link the same id self.ids.add(marker.id) marker.ns = self.marker_namespace marker.header.stamp = time_stamp try: full_link_name = self.links_full_frame_name[ link_name] except KeyError: continue pose = lookup_pose(self.map_frame, full_link_name).pose if object.has_non_identity_visual_offset(link_name): marker.pose = kdl_to_pose( pose_to_kdl(pose) * pose_to_kdl(marker.pose)) else: marker.pose = pose markers.append(marker) self.publisher.publish(markers) if self.ensure_publish: rospy.sleep(0.1) return py_trees.common.Status.SUCCESS
def set_cart_goal_test(): giskard = GiskardWrapper() poseStamp = lookup_pose("odom_combined", "iai_kitchen/kitchen_island_left_lower_drawer_handle") h_g = np_to_kdl(np.array([[-1, 0, 0, 0], [0, 0, 1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])) pose = pose_to_kdl(poseStamp.pose) pose = pose * h_g poseStamp.pose = kdl_to_pose(pose) giskard.set_cart_goal("odom_combined", "r_gripper_tool_frame", poseStamp) giskard.allow_all_collisions() giskard.plan_and_execute()
def update(self): markers = [] time_stamp = rospy.Time() robot = self.get_robot() get_fk = robot.get_fk_pose links = [ x for x in self.get_robot().get_link_names() if robot.has_link_visuals(x) ] for i, link_name in enumerate(links): marker = robot.link_as_marker(link_name) if marker is None: continue marker.header.frame_id = self.robot_base marker.action = Marker.ADD marker.id = int( hashlib.md5(link_name.encode('utf-8')).hexdigest()[:6], 16 ) # FIXME find a better way to give the same link the same id self.ids.add(marker.id) marker.ns = u'planning_visualization' marker.header.stamp = time_stamp fk = get_fk(self.robot_base, link_name).pose if robot.has_non_identity_visual_offset(link_name): marker.pose = kdl_to_pose( pose_to_kdl(fk) * pose_to_kdl(marker.pose)) else: marker.pose = fk markers.append(marker) self.publisher.publish(markers) if self.ensure_publish: rospy.sleep(0.1) return py_trees.common.Status.SUCCESS