def publishPositions(self): if not self.initialized: return nr = 0 markerArray = MarkerArray() marker = Marker() marker.header.frame_id = "/map" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.mesh_use_embedded_materials = True marker.mesh_resource = "package://pacman_controller/meshes/pacman.dae" marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation = self.pacman["orientation"] marker.pose.position.x = self.pacman["x"] marker.pose.position.y = self.pacman["y"] marker.pose.position.z = 0.0 marker.id = nr markerArray.markers.append(marker) for ghost in self.ghosts: curGhost = self.ghosts[ghost] if not curGhost["initialized"]: continue nr += 1 marker = Marker() marker.header.frame_id = "/map" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.mesh_use_embedded_materials = True if curGhost["eaten"]: marker.mesh_resource = "package://pacman_controller/meshes/dead.dae" elif self.state == State.FLEEING: marker.mesh_resource = "package://pacman_controller/meshes/ghost_catchable.dae" else: marker.mesh_resource = "package://pacman_controller/meshes/%s.dae" % ghost marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation = curGhost["orientation"] marker.pose.position.x = curGhost["x"] marker.pose.position.y = curGhost["y"] marker.pose.position.z = 0.0 marker.id = nr markerArray.markers.append(marker) self.pubPositions.publish(markerArray) return
def publishPositions(self): if not self.initialized: return nr = 0 markerArray = MarkerArray() marker = Marker() marker.header.frame_id = "/map" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.mesh_use_embedded_materials = True marker.mesh_resource = "package://pacman_controller/meshes/pacman.dae" marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation = self.pacman['orientation'] marker.pose.position.x = self.pacman['x'] marker.pose.position.y = self.pacman['y'] marker.pose.position.z = 0.0 marker.id = nr markerArray.markers.append(marker) for ghost in self.ghosts: curGhost = self.ghosts[ghost] if not curGhost["initialized"]: continue nr += 1 marker = Marker() marker.header.frame_id = "/map" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.mesh_use_embedded_materials = True if curGhost["eaten"]: marker.mesh_resource = "package://pacman_controller/meshes/dead.dae" elif self.state == State.FLEEING: marker.mesh_resource = "package://pacman_controller/meshes/ghost_catchable.dae" else: marker.mesh_resource = "package://pacman_controller/meshes/%s.dae" % ghost marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation = curGhost['orientation'] marker.pose.position.x = curGhost['x'] marker.pose.position.y = curGhost['y'] marker.pose.position.z = 0.0 marker.id = nr markerArray.markers.append(marker) self.pubPositions.publish(markerArray) return
def draw_grasps(grasps): """ Draws grasps in RVIZ Parameters: grasps: a list of grasps """ # TODO: add fingers to gripper publisher = rospy.Publisher("visualization_marker", Marker) for i, g in enumerate(grasps): marker = Marker() marker.header.frame_id = "base_link" marker.header.stamp = rospy.Time.now() marker.ns = "grasps" marker.type = Marker.MESH_RESOURCE marker.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae" marker.action = marker.ADD marker.id = i marker.pose.position = g.grasp_pose.position marker.pose.orientation = g.grasp_pose.orientation marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 marker.mesh_use_embedded_materials = True publisher.publish(marker)
def get_trafficsign_marker(marker_id, x, y, q, marker_type): if marker_type == "T-intersection": marker_type = "T-intersect" marker = Marker() marker.header.frame_id = "/map" marker.id = marker_id marker.ns = "traffic_signs" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.mesh_resource = "package://duckietown_visualization/meshes/traffic-signs/" + \ "sign_" + marker_type.replace('-','_') + ".dae" marker.mesh_use_embedded_materials = True marker.pose.position.x = x marker.pose.position.y = y marker.pose.position.z = -0.04 marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 (_,_,yaw) = tf.transformations.euler_from_quaternion(q) q = tf.transformations.quaternion_from_euler(0, 0, yaw-math.pi/2) marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] return marker
def publish_people_marker(people_pos, nav_map, timestamp, publisher): """ Get rviz visualization marker for people poses. :param people_pos: cell coordinates of people :param nav_map: map of the environment :param timestamp: ros timestamp :param publisher: ros publisher """ people_markers = MarkerArray() person_marker = Marker() person_marker.header.frame_id = nav_map.frame_id person_marker.header.stamp = timestamp person_marker.type = Marker.MESH_RESOURCE person_marker.mesh_resource = "package://learning-nav-irl/meshes/human/meshes/standing.dae" person_marker.mesh_use_embedded_materials = True person_marker.scale.x = 1.0 person_marker.scale.y = 1.0 person_marker.scale.z = 1.0 for i, person_pos in enumerate(people_pos): person_marker.id = i person_x, person_y = nav_map.cell2world(person_pos[0], person_pos[1]) person_marker.pose.position.x = person_x person_marker.pose.position.y = person_y # person orientation hard-coded for visualization person_marker.pose.orientation.z = 0.7071068 person_marker.pose.orientation.w = 0.7071068 people_markers.markers.append(copy.deepcopy(person_marker)) publisher.publish(people_markers)
def get_watchtower_marker(marker_id, x, y, q): marker = Marker() marker.header.frame_id = "/map" marker.id = marker_id marker.ns = "watchtowers" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.mesh_resource = "package://duckietown_visualization/meshes/watchtower/watchtower.dae" marker.mesh_use_embedded_materials = True marker.pose.position.x = x marker.pose.position.y = y marker.pose.position.z = 0 marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 (_,_,yaw) = tf.transformations.euler_from_quaternion(q) q = tf.transformations.quaternion_from_euler(0, 0, yaw) marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] return marker
def make_kin_tree_from_link(ps,linkname, ns='default_ns',valuedict=None): markers = [] U = get_pr2_urdf() link = U.links[linkname] if link.visual and link.visual.geometry and isinstance(link.visual.geometry,urdf.Mesh): rospy.logdebug("%s is a mesh. drawing it.", linkname) marker = Marker(type = Marker.MESH_RESOURCE, action = Marker.ADD) marker.ns = ns marker.header = ps.header linkFromGeom = conversions.trans_rot_to_hmat(link.visual.origin.position, transformations.quaternion_from_euler(*link.visual.origin.rotation)) origFromLink = conversions.pose_to_hmat(ps.pose) origFromGeom = np.dot(origFromLink, linkFromGeom) marker.pose = conversions.hmat_to_pose(origFromGeom) marker.scale = gm.Vector3(1,1,1) marker.color = stdm.ColorRGBA(1,1,0,.5) marker.id = 0 marker.lifetime = rospy.Duration() marker.mesh_resource = str(link.visual.geometry.filename) marker.mesh_use_embedded_materials = False markers.append(marker) else: rospy.logdebug("%s is not a mesh", linkname) if U.child_map.has_key(linkname): for (cjoint,clink) in U.child_map[linkname]: markers.extend(make_kin_tree_from_joint(ps,cjoint,ns=ns,valuedict=valuedict)) return markers
def main(): # Initialize ROS stuff rospy.init_node("optimization_node") pub = rospy.Publisher('robot_meshes', MarkerArray, queue_size=0, latch=True) # Draw the chessboard markers = MarkerArray() m = Marker( header=Header(frame_id='base_link', stamp=rospy.Time.now()), ns='charuco', id=0, frame_locked=True, type=Marker.MESH_RESOURCE, action=Marker.ADD, lifetime=rospy.Duration(0), pose=Pose( position=Point(x=-0.9, y=-0.176 * 2, z=0.5), orientation=Quaternion( *trf.quaternion_from_euler(math.pi * 0.5, 0, math.atan2(-0.176, -1)) - math.pi * 0.1)), scale=Vector3(x=1.5, y=1.5, z=1.5), color=ColorRGBA(r=.5, g=.5, b=.5, a=1)) m.mesh_resource = 'package://ur_e_description/meshes/objects/charuco_5x5.dae' m.mesh_use_embedded_materials = True markers.markers.append(m) rate = rospy.Rate(10) while not rospy.is_shutdown(): pub.publish(markers) rate.sleep()
def _make_mesh_marker(self): '''Creates a mesh marker''' mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 alpha = 0.6 if self.is_dimmed: alpha = 0.1 if self.is_fake: alpha = 0.4 mesh.color = ColorRGBA(0.3, 0.3, 0.3, alpha) return mesh if self._is_reachable(): # Original: some kinda orange # r,g,b = 1.0, 0.5, 0.0 # New: rainbow! See method comment for details. r,g,b = self.get_marker_color() mesh.color = ColorRGBA(r, g, b, alpha) else: mesh.color = ColorRGBA(0.5, 0.5, 0.5, alpha) return mesh
def pub_at_position(self, odom_msg): """ Handles necessary information for displaying things at ACCEPTED (NOVATEL) POSITION SOLUTION: -vehicle mesh !!!this should only be invoked when the accepted (novatel) position is updated """ ### G35 Mesh ############################################################# marker = Marker() pub = self.curpos_publisher marker.header = odom_msg.header marker.id = 0 # enumerate subsequent markers here marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY marker.pose = odom_msg.pose.pose marker.pose.position.z = 1.55 marker.lifetime = rospy.Duration() # will last forever unless modified marker.ns = "vehicle_model" marker.type = Marker.MESH_RESOURCE marker.scale.x = 0.0254 # artifact of sketchup export marker.scale.y = 0.0254 # artifact of sketchup export marker.scale.z = 0.0254 # artifact of sketchup export marker.color.r = .05 marker.color.g = .05 marker.color.b = .05 marker.color.a = .2 marker.mesh_resource = self.veh_mesh_resource marker.mesh_use_embedded_materials = False pub.publish(marker)
def get_apriltag_marker(marker_id, trans, q): marker = Marker() marker.header.frame_id = "/map" marker.header.stamp = rospy.Time.now() marker.id = marker_id marker.ns = "ground_tags" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.mesh_resource = "package://duckietown_visualization/meshes/apriltags/apriltag.dae" marker.mesh_use_embedded_materials = True marker.pose.position.x = trans[0] marker.pose.position.y = trans[1] marker.pose.position.z = trans[2] marker.scale.x = 0.064 marker.scale.y = 0.064 marker.scale.z = 1 # (_,_,yaw) = tf.transformations.euler_from_quaternion(q) # q = tf.transformations.quaternion_from_euler(0, 0, yaw) marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] return marker
def publish_shelf(publisher, pose_stamped): """Publishes a shelf marker at a given pose. The pose is assumed to represent the bottom center of the shelf, with the +x direction pointing along the depth axis of the bins and +z pointing up. Args: publisher: A visualization_msgs/Marker publisher pose_stamped: A PoseStamped message with the location, orientation, and reference frame of the shelf. """ marker = Marker() marker.header.frame_id = pose_stamped.header.frame_id marker.header.stamp = rospy.Time().now() marker.ns = 'shelf' marker.id = 0 marker.type = Marker.MESH_RESOURCE marker.mesh_resource = 'package://pr2_pick_perception/models/shelf/shelf.ply' marker.mesh_use_embedded_materials = True marker.action = Marker.ADD marker.pose = pose_stamped.pose marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.lifetime = rospy.Duration() _publish(publisher, marker, "shelf")
def publishVisualizationMessage(pub, posX, posY, posZ, oriW, oriX, oriY, oriZ, objType, mid, mesh): # build object ... msg = Marker() msg.header.stamp = rospy.Time.now() msg.header.frame_id = '/PTU' msg.ns = 'textured' msg.id = mid msg.type = 10 msg.action = 0 msg.pose.position.x = posX msg.pose.position.y = posY msg.pose.position.z = posZ msg.pose.orientation.w = oriW msg.pose.orientation.x = oriX msg.pose.orientation.y = oriY msg.pose.orientation.z = oriZ msg.scale.x = 0.001 msg.scale.y = 0.001 msg.scale.z = 0.001 msg.mesh_resource = mesh msg.mesh_use_embedded_materials = True # ... and publish it pub.publish(msg)
def get_apriltag_marker(x, y, angle, marker_type, marker_id, marker_scale): marker = Marker() marker.header.frame_id = "/map" marker.id = marker_id marker.ns = 'apriltags' marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.pose.position.x = x marker.pose.position.y = y marker.pose.position.z = -0.04 marker.scale.x = 0.065 marker.scale.y = 0.065 marker.scale.z = 1 marker.mesh_resource = "package://duckietown_visualization/meshes/apriltags/" + \ marker_type + ".dae" marker.mesh_use_embedded_materials = True q = quaternion_from_euler(0, 0, angle) marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] return marker
def copy_marker_attributes(sample): marker = Marker() marker.header.frame_id = sample.header.frame_id marker.header.stamp = sample.header.stamp marker.id = sample.id marker.type = sample.type marker.action = sample.action marker.scale.x = sample.scale.x marker.scale.y = sample.scale.y marker.scale.z = sample.scale.z marker.pose.position.x = sample.pose.position.x marker.pose.position.y = sample.pose.position.y marker.pose.position.z = sample.pose.position.z marker.pose.orientation.x = sample.pose.orientation.x marker.pose.orientation.y = sample.pose.orientation.y marker.pose.orientation.z = sample.pose.orientation.z marker.pose.orientation.w = sample.pose.orientation.w marker.color.a = sample.color.a marker.color.r = sample.color.r marker.color.g = sample.color.g marker.color.b = sample.color.b marker.text = sample.text marker.mesh_resource = sample.mesh_resource marker.mesh_use_embedded_materials = sample.mesh_use_embedded_materials marker.points = sample.points marker.colors = sample.colors return marker
def rosbagVisualizationMessage(bag, posX, posY, posZ, oriW, oriX, oriY, oriZ, objType, mid, mesh): # build object ... msg = Marker() msg.header.stamp = rospy.Time.now() msg.header.frame_id = '/PTU' msg.ns = 'textured' msg.id = mid msg.type = 10 msg.action = 0 msg.pose.position.x = posX msg.pose.position.y = posY msg.pose.position.z = posZ msg.pose.orientation.w = oriW msg.pose.orientation.x = oriX msg.pose.orientation.y = oriY msg.pose.orientation.z = oriZ msg.scale.x = 0.001 msg.scale.y = 0.001 msg.scale.z = 0.001 msg.mesh_resource = mesh msg.mesh_use_embedded_materials = True # ... and write it into bag file bag.write('/stereo/visualization_marker', msg)
def get_current_position_marker(self, link, offset=None, root="", scale=1, color=(0, 1, 0, 1), idx=0): (mesh, pose) = self.get_link_data(link) marker = Marker() if offset == None: marker.pose = pose else: marker.pose = toMsg(fromMsg(offset) * fromMsg(pose)) marker.header.frame_id = root marker.header.stamp = rospy.get_rostime() marker.ns = self.robot_name marker.mesh_resource = mesh marker.type = Marker.MESH_RESOURCE marker.action = Marker.MODIFY marker.scale.x = scale marker.scale.y = scale marker.scale.z = scale marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] marker.text = link marker.id = idx marker.mesh_use_embedded_materials = True return marker
def draw_quad_base(self, quad): quad_id = self.hash32(quad) if not rospy.is_shutdown(): marker = Marker() marker.header.frame_id = "/my_frame" marker.lifetime = rospy.Duration(self.duration) marker.type = marker.MESH_RESOURCE marker.mesh_resource = "package://hector_quadrotor_description/"\ + "meshes/quadrotor/quadrotor_base.dae" marker.action = marker.ADD marker.scale.x = 80 marker.scale.y = 80 marker.scale.z = 80 marker.mesh_use_embedded_materials = True marker.pose.orientation.w = math.cos( math.radians(quad.get_orientation() / 2) ) marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = math.sin( math.radians(quad.get_orientation() / 2) ) marker.pose.position.x = quad.get_x() marker.pose.position.y = quad.get_y() marker.pose.position.z = quad.get_z() marker.id = quad_id self.markers.append(marker)
def get_marker(marker_id, x, y, q): marker = Marker() marker.header.frame_id = "/duckiebot_link" marker.id = marker_id marker.ns = "duckiebots" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.mesh_resource = "package://duckietown_visualization/meshes/duckiebot/duckiebot.dae" marker.mesh_use_embedded_materials = True marker.pose.position.x = x marker.pose.position.y = y marker.pose.position.z = 0 marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] return marker
def add_marker(self, mesh_frame, marker_name, mesh_file, use_materials=False, color=None): marker_msg = Marker() marker_msg.frame_locked = True marker_msg.id = 0 marker_msg.action = Marker.ADD marker_msg.mesh_use_embedded_materials = use_materials if marker_msg.mesh_use_embedded_materials: marker_msg.color.r = 0 marker_msg.color.g = 0 marker_msg.color.b = 0 marker_msg.color.a = 0 elif color: marker_msg.color = color else: marker_msg.color.r = 0.6 marker_msg.color.g = 0.6 marker_msg.color.b = 0.6 marker_msg.color.a = 1.0 marker_msg.header.stamp = rospy.get_rostime() #marker_msg.lifetime = #marker_msg.pose = marker_msg.type = Marker.MESH_RESOURCE marker_msg.header.frame_id = mesh_frame marker_msg.ns = marker_name marker_msg.mesh_resource = 'file://%s' % (os.path.abspath(mesh_file)) scale = 1.0 marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale, scale, scale marker_msg.pose.position.x, marker_msg.pose.position.y, marker_msg.pose.position.z = 0, 0, 0 marker_msg.pose.orientation.x, marker_msg.pose.orientation.y, marker_msg.pose.orientation.z, marker_msg.pose.orientation.w = 0, 0, 0, 1 self.pub_marker_sync(marker_msg) if not self.marker_thread: self.marker_thread = thread.start_new_thread(MarkerPublisher.publish_markers, (self,))
def add_marker(self, mesh_frame, marker_name, mesh_file): marker_msg = Marker() marker_msg.frame_locked = True marker_msg.id = 0 marker_msg.action = Marker.ADD marker_msg.mesh_use_embedded_materials = False marker_msg.color.a = 1.0 marker_msg.color.r = 0.6 marker_msg.color.g = 0.6 marker_msg.color.b = 0.6 marker_msg.header.stamp = rospy.get_rostime() #marker_msg.lifetime = #marker_msg.pose = marker_msg.type = Marker.MESH_RESOURCE marker_msg.header.frame_id = mesh_frame marker_msg.ns = marker_name marker_msg.mesh_resource = 'file://%s' % (os.path.abspath(mesh_file)) scale = 1.0 marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale, scale, scale marker_msg.pose.position.x, marker_msg.pose.position.y, marker_msg.pose.position.z = 0, 0, 0 marker_msg.pose.orientation.x, marker_msg.pose.orientation.y, marker_msg.pose.orientation.z, marker_msg.pose.orientation.w = 0, 0, 0, 1 self.pub_marker_sync(marker_msg) if not self.marker_thread: self.marker_thread = thread.start_new_thread( MarkerPublisher.publish_markers, (self, ))
def print_points(self): rate = rospy.Rate(10) # iterations = 0 while not rospy.is_shutdown(): marker = Marker() marker.header.frame_id = "/base" marker.header.stamp = rospy.Time() marker.ns = "primitive_surfaces" marker.id = 10 marker.mesh_use_embedded_materials = True marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.mesh_resource = "file:///home/pratique/drone_course_data/CC_tag_detection/Submission/cctag.dae" self.tag_pub.publish(marker) rate.sleep()
def get_current_position_marker(self, link, offset=None, root="", scale=1, color=(0,1,0,1), idx=0): (mesh, pose) = self.get_link_data(link) marker = Marker() if offset==None : marker.pose = pose else : marker.pose = toMsg(fromMsg(offset)*fromMsg(pose)) marker.header.frame_id = root marker.header.stamp = rospy.get_rostime() marker.ns = self.robot_name marker.mesh_resource = mesh marker.type = Marker.MESH_RESOURCE marker.action = Marker.MODIFY marker.scale.x = scale marker.scale.y = scale marker.scale.z = scale marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] marker.text = link marker.id = idx marker.mesh_use_embedded_materials = True return marker
def getMarker(self, source, markerID): mType = source.get('marker_type', Marker.SPHERE) mType = mType if not isinstance(mType, str) else getattr( Marker, mType.upper()) mAction = source.get('marker_action', Marker.ADD) mAction = mAction if not isinstance(mType, str) else getattr( Marker, mType.upper()) marker = Marker() marker.header.stamp = rospy.Time(0) marker.header.frame_id = source.get('marker_frame', 'map') marker.ns = source.get('marker_ns', '') marker.id = int(source.get('markerID', markerID)) marker.type = mType marker.action = mAction marker.pose.position.x = source.get('marker_position', {}).get('x', 0.0) marker.pose.position.y = source.get('marker_position', {}).get('y', 0.0) marker.pose.position.z = source.get('marker_position', {}).get('z', 0.0) marker.pose.orientation = self._getOrientation( source.get('marker_orientation', None)) marker.scale.x = source.get('marker_scale', {}).get('x', 0.1) marker.scale.y = source.get('marker_scale', {}).get('y', 0.1) marker.scale.z = source.get('marker_scale', {}).get('z', 0.1) marker.color.r = source.get('marker_color', {}).get('r', 1.0) marker.color.g = source.get('marker_color', {}).get('g', 1.0) marker.color.b = source.get('marker_color', {}).get('b', 1.0) marker.color.a = source.get('marker_color', {}).get('a', 1.0) marker.lifetime = rospy.Duration(source.get('marker_lifetime', 0)) marker.frame_locked = bool(source.get('marker_framelocked', False)) # TODO: points # TODO: colors marker.text = source.get('marker_text', '') marker.mesh_resource = source.get('marker_mesh', '') marker.mesh_use_embedded_materials = bool( source.get('marker_mesh_resources', True)) # Second (default hidden) marker as a label label = copy.deepcopy(marker) label.action = Marker.DELETE label.ns += ('/' if label.ns else '') + 'labels' label.scale.z = 0.1 label.pose.position.z += marker.scale.z + label.scale.z * 0.5 label.type = Marker.TEXT_VIEW_FACING label.color.r = source.get('label_color', {}).get('r', 1.0) label.color.g = source.get('label_color', {}).get('g', 1.0) label.color.b = source.get('label_color', {}).get('b', 1.0) label.color.a = source.get('label_color', {}).get('a', 1.0) return marker, label
def load_markers(): publisher = rospy.Publisher('~marker', MarkerArray, queue_size=1, latch=True) markerArray = MarkerArray(); file_path = rospy.get_param('~world_file') if not file_path: rospy.loginfo('World description file path is empty.') return if not isfile(file_path): rospy.logwarn('World description file with path "' + file_path + '" does not exists.') return parameters = load_file(file_path) if not parameters: return for model in parameters[0][0]['models']: marker = Marker(); if 'box' in model: marker.type = Marker.CUBE; model = model['box'] marker.color.a = 1.0; marker.color.r = 0.5; marker.color.g = 0.5; marker.color.b = 0.5; elif 'cylinder' in model: marker.type = Marker.CYLINDER; model = model['cylinder'] marker.color.a = 1.0; marker.color.r = 0.5; marker.color.g = 0.5; marker.color.b = 0.5; elif 'mesh' in model: marker.type = Marker.MESH_RESOURCE; model = model['mesh'] marker.mesh_resource = model['mesh_resource'] marker.mesh_use_embedded_materials = True else: continue marker.ns = model['name'] marker.header.frame_id = model['frame_id'] # marker.id = i # TODO Required? marker.action = Marker.ADD marker.pose.position.x = model['position']['x'] marker.pose.position.y = model['position']['y'] marker.pose.position.z = model['position']['z'] marker.pose.orientation.x = model['orientation']['x'] marker.pose.orientation.y = model['orientation']['y'] marker.pose.orientation.z = model['orientation']['z'] marker.pose.orientation.w = model['orientation']['w'] marker.scale.x = model['scale']['x'] marker.scale.y = model['scale']['y'] marker.scale.z = model['scale']['z'] markerArray.markers.append(marker) rospy.loginfo("World `%s` is published as marker.", marker.ns) publisher.publish(markerArray) rospy.spin()
def makeTable(id, frame_id, scale, pose, file_name): marker = Marker(type=Marker.MESH_RESOURCE, id=id, header=Header(frame_id=frame_id)) marker.scale = scale marker.pose = pose marker.mesh_resource = file_name marker.mesh_use_embedded_materials = True return marker
def interactive_gripper_marker(pose_stamped, finger_distance): # gripper marker gripper_marker = Marker() gripper_marker.pose.position.x = 0.166 # wrist_roll_link/gripper_link Translation: [0.166, 0.000, 0.000] gripper_marker.pose.orientation.w = 1 # wrist_roll_link/gripper_link Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000] gripper_marker.type = Marker.MESH_RESOURCE gripper_marker.mesh_resource = GRIPPER_MESH gripper_marker.mesh_use_embedded_materials = True finger_distance = max(CLOSE_FINGER_POS, min(finger_distance, OPEN_FINGER_POS)) # left finger marker l_marker = Marker() l_marker.pose.position.x = 0.166 l_marker.pose.position.y = -finger_distance / 2.0 l_marker.pose.orientation.w = 1 l_marker.type = Marker.MESH_RESOURCE l_marker.mesh_resource = L_FINGER_MESH l_marker.mesh_use_embedded_materials = True # right finger marker r_marker = Marker() r_marker.pose.position.x = 0.166 r_marker.pose.position.y = finger_distance / 2.0 r_marker.pose.orientation.w = 1 r_marker.type = Marker.MESH_RESOURCE r_marker.mesh_resource = R_FINGER_MESH r_marker.mesh_use_embedded_materials = True control = InteractiveMarkerControl() control.orientation.w = 1 control.interaction_mode = InteractiveMarkerControl.NONE control.always_visible = True control.markers.append(gripper_marker) control.markers.append(l_marker) control.markers.append(r_marker) interactive_marker = InteractiveMarker() interactive_marker.header = pose_stamped.header interactive_marker.pose = pose_stamped.pose interactive_marker.controls.append(control) interactive_marker.scale = 0.3 return interactive_marker
def test(): rospy.init_node('intersect_plane_test') marker_pub = rospy.Publisher('table_marker', Marker) pose_pub = rospy.Publisher('pose', PoseStamped) int_pub = rospy.Publisher('intersected_points', PointCloud2) tfl = tf.TransformListener() # Test table table = Table() table.pose.header.frame_id = 'base_link' table.pose.pose.orientation.x, table.pose.pose.orientation.y, table.pose.pose.orientation.z, table.pose.pose.orientation.w = (0,0,0,1) table.x_min = -0.5 table.x_max = 0.5 table.y_min = -0.5 table.y_max = 0.5 # A marker for that table marker = Marker() marker.header.frame_id = table.pose.header.frame_id marker.id = 1 marker.type = Marker.LINE_STRIP marker.action = 0 marker.pose = table.pose.pose marker.scale.x, marker.scale.y, marker.scale.z = (0.005,0.005,0.005) marker.color.r, marker.color.g, marker.color.b, marker.color.a = (0.0,1.0,1.0,1.0) marker.frame_locked = False marker.ns = 'table' marker.points = [ Point(table.x_min,table.y_min, table.pose.pose.position.z), Point(table.x_min,table.y_max, table.pose.pose.position.z), Point(table.x_max,table.y_max, table.pose.pose.position.z), Point(table.x_max,table.y_min, table.pose.pose.position.z), Point(table.x_min,table.y_min, table.pose.pose.position.z), ] marker.colors = [] marker.text = '' # marker.mesh_resource = '' marker.mesh_use_embedded_materials = False marker.header.stamp = rospy.Time.now() # A test pose pose = PoseStamped() pose.header = table.pose.header pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = (0,0,0.5) pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(0,-pi/5,pi/5) intersection = cast_ray(pose, table, tfl) cloud = xyz_array_to_pointcloud2(np.array([[intersection.point.x, intersection.point.y, intersection.point.z]])) cloud.header = pose.header while not rospy.is_shutdown(): marker_pub.publish(marker) pose_pub.publish(pose) int_pub.publish(cloud) rospy.loginfo('published') rospy.sleep(0.1)
def gripper_interactive_marker(pose_stamped, finger_distance): gripper_marker = Marker() gripper_marker.pose.position.x = 0.166 gripper_marker.pose.orientation.w = 1 gripper_marker.type = Marker.MESH_RESOURCE gripper_marker.mesh_resource = GRIPPER_MESH gripper_marker.mesh_use_embedded_materials = True finger_distance = max(0, min(finger_distance, 0.1)) l_marker = Marker() l_marker.pose.position.x = 0.166 l_marker.pose.position.y = finger_distance / 2.0 - 0.1 l_marker.pose.orientation.w = 1 l_marker.type = Marker.MESH_RESOURCE l_marker.mesh_resource = L_FINGER_MESH l_marker.mesh_use_embedded_materials = True r_marker = Marker() r_marker.pose.position.x = 0.166 r_marker.pose.position.y = -finger_distance / 2.0 + 0.1 r_marker.pose.orientation.w = 1 r_marker.type = Marker.MESH_RESOURCE r_marker.mesh_resource = R_FINGER_MESH r_marker.mesh_use_embedded_materials = True control = InteractiveMarkerControl() control.orientation.w = 1 control.interaction_mode = InteractiveMarkerControl.NONE control.always_visible = True control.markers.append(gripper_marker) control.markers.append(l_marker) control.markers.append(r_marker) interactive_marker = InteractiveMarker() interactive_marker.header = pose_stamped.header interactive_marker.pose = pose_stamped.pose interactive_marker.controls.append(control) interactive_marker.scale = 0.25 return interactive_marker
def link_as_marker(self, link_name): if link_name not in self._link_to_marker: marker = Marker() geometry = self.get_urdf_link(link_name).visual.geometry if isinstance(geometry, up.Mesh): marker.type = Marker.MESH_RESOURCE marker.mesh_resource = geometry.filename if geometry.scale is None: marker.scale.x = 1.0 marker.scale.z = 1.0 marker.scale.y = 1.0 else: marker.scale.x = geometry.scale[0] marker.scale.z = geometry.scale[1] marker.scale.y = geometry.scale[2] marker.mesh_use_embedded_materials = True elif isinstance(geometry, up.Box): marker.type = Marker.CUBE marker.scale.x = geometry.size[0] marker.scale.y = geometry.size[1] marker.scale.z = geometry.size[2] elif isinstance(geometry, up.Cylinder): marker.type = Marker.CYLINDER marker.scale.x = geometry.radius * 2 marker.scale.y = geometry.radius * 2 marker.scale.z = geometry.length elif isinstance(geometry, up.Sphere): marker.type = Marker.SPHERE marker.scale.x = geometry.radius * 2 marker.scale.y = geometry.radius * 2 marker.scale.z = geometry.radius * 2 else: return None marker.header.frame_id = self.get_root() marker.action = Marker.ADD marker.color.a = 0.5 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.scale.x *= 0.99 marker.scale.y *= 0.99 marker.scale.z *= 0.99 marker.pose = Pose() if self.has_non_identity_visual_offset(link_name): marker.pose = self.get_visual_pose(link_name) self._link_to_marker[link_name] = marker return deepcopy(self._link_to_marker[link_name])
def _make_mesh_marker(self): '''Creates a mesh marker''' mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 if self._is_reachable(): mesh.color = ColorRGBA(1.0, 0.5, 0.0, 0.6) else: mesh.color = ColorRGBA(0.5, 0.5, 0.5, 0.6) return mesh
def CreateMeshMarker(pose, mesh_path, alpha = 1, scaleFactor=1, id=randint(0,10000)): marker = Marker() marker.ns = "visual" marker.id = id marker.scale.x = scaleFactor marker.scale.y = scaleFactor marker.scale.z = scaleFactor marker.pose = pose marker.type = Marker.MESH_RESOURCE marker.mesh_use_embedded_materials = True marker.mesh_resource = mesh_path marker.frame_locked = True return marker
def to_ros_markers(shape, stamp, frame_name, X_FG, color_rgba): assert isinstance(shape, Shape), shape assert isinstance(frame_name, str), frame_name assert isinstance(X_FG, RigidTransform), X_FG marker = Marker() marker.header.stamp = stamp marker.header.frame_id = frame_name marker.pose = to_ros_pose(X_FG) marker.action = Marker.ADD marker.lifetime = Duration(0.) marker.frame_locked = True def use_color(): nonlocal color_rgba if color_rgba is None: color_rgba = DEFAULT_RGBA (marker.color.r, marker.color.g, marker.color.b, marker.color.a) = (color_rgba.r(), color_rgba.g(), color_rgba.b(), color_rgba.a()) if type(shape) == Box: marker.type = Marker.CUBE marker.scale.x, marker.scale.y, marker.scale.z = shape.size() use_color() return [marker] if type(shape) == Sphere: marker.type = Marker.SPHERE marker.scale.x = shape.radius() marker.scale.y = shape.radius() marker.scale.z = shape.radius() use_color() return [marker] elif type(shape) == Cylinder: marker.type = Marker.CYLINDER marker.scale.x = shape.radius() marker.scale.y = shape.radius() marker.scale.z = shape.length() use_color() return [marker] elif type(shape) in (Mesh, Convex): marker.type = Marker.MESH_RESOURCE marker.mesh_resource = f"file://{shape.filename()}" # TODO(eric.cousineau): Is this the correct way to handle color vs. # texture? use_color() marker.mesh_use_embedded_materials = True marker.scale.x, marker.scale.y, marker.scale.z = 3 * [shape.scale()] return [marker] else: # TODO(eric): Support Capsule, Ellipse, all dat jazz. assert False, f"Unsupported type: {shape}"
def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6) ik_solution = self.ik.get_ik_for_ee(self.ee_pose) if (ik_solution == None): mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh
def get_mesh_marker(): marker = Marker() marker.type = marker.MESH_RESOURCE marker.pose = Pose() marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.pose.orientation.w = 1. marker.mesh_resource = "package://point_cloud_tools/data/mesh.stl" marker.mesh_use_embedded_materials = True marker.header.frame_id = "world" return marker
def _make_mesh_marker(self): '''Creates and returns a mesh marker. Returns: Marker ''' mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 mesh.color = self._get_mesh_marker_color() return mesh
def callback_tag(self, data): marker_tag = Marker() marker_tag.header.frame_id = self.frame_id marker_tag.header.stamp = rospy.Time.now() marker_tag.id = 0 marker_tag.action = Marker().ADD if self.isNinebot: marker_tag.type = Marker().MESH_RESOURCE marker_tag.mesh_resource = "package://tms_ss_ninebot_pozyx/meshes/ninebot_v2.dae" marker_tag.mesh_use_embedded_materials = True marker_tag.scale.x = 0.01 marker_tag.scale.y = 0.01 marker_tag.scale.z = 0.01 elif self.isHuman: marker_tag.type = Marker().MESH_RESOURCE marker_tag.mesh_resource = "package://tms_ss_ninebot_pozyx/meshes/WalkingMan4.dae" marker_tag.mesh_use_embedded_materials = True marker_tag.scale.x = 0.025 marker_tag.scale.y = 0.025 marker_tag.scale.z = 0.025 else: marker_tag.type = Marker().CYLINDER marker_tag.scale.x = 0.2 marker_tag.scale.y = 0.2 marker_tag.scale.z = 1.0 marker_tag.color.r = 1.0 marker_tag.color.g = 0.0 marker_tag.color.b = 0.0 marker_tag.color.a = 1.0 marker_tag.pose = data.pose marker_tag.lifetime = rospy.Duration() self.publish_anchors() self.tag_pub.publish(marker_tag)
def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 target_pose = GripperMarkers._offset_pose(self.ee_pose) ik_sol = self._ik_service.get_ik_for_ee(target_pose) if ik_sol == None: mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh
def getDroneMarker(self, pose): marker = Marker() marker.id = 1 marker.ns = "mesh_" + self.name marker.header.frame_id = "world" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.pose = pose marker.lifetime = rospy.Duration.from_sec(0.0) marker.mesh_use_embedded_materials = True marker.mesh_resource = "package://acl_sim/meshes/quadrotor/quadrotor.dae" marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 return marker
def _make_mesh_marker(color): '''Creates and returns a mesh marker. Args: color (ColorRGBA) Returns: Marker ''' mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 mesh.color = color return mesh
def publish_rviz_markers(frames, frame_id, handarm_params): timestamp = rospy.Time.now() global markers_rviz global frames_rviz markers_rviz = MarkerArray() for i, f in enumerate(frames): msg = Marker() msg.header.stamp = timestamp msg.header.frame_id = frame_id msg.frame_locked = True # False msg.id = i msg.type = Marker.MESH_RESOURCE msg.action = Marker.ADD msg.lifetime = rospy.Duration() msg.color.r = msg.color.g = msg.color.b = msg.color.a = 0 msg.mesh_use_embedded_materials = True msg.mesh_resource = handarm_params["mesh_file"] msg.scale.x = msg.scale.y = msg.scale.z = handarm_params[ "mesh_file_scale"] #msg.mesh_resource = mesh_resource msg.pose = homogenous_tf_to_pose_msg(f) markers_rviz.markers.append(msg) for f1, f2 in zip(frames, frames[1:]): msg = Marker() msg.header.stamp = timestamp msg.header.frame_id = frame_id msg.frame_locked = True # False msg.id = markers_rviz.markers[-1].id + 1 msg.action = Marker.ADD msg.lifetime = rospy.Duration() msg.type = Marker.ARROW msg.color.g = msg.color.b = 0 msg.color.r = msg.color.a = 1 msg.scale.x = 0.01 # shaft diameter msg.scale.y = 0.03 # head diameter msg.points.append(homogenous_tf_to_pose_msg(f1).position) msg.points.append(homogenous_tf_to_pose_msg(f2).position) markers_rviz.markers.append(msg) frames_rviz = frames
def make_model_marker(model): marker = Marker() marker.type = Marker.MESH_RESOURCE marker.scale.x = 0.01 marker.scale.y = 0.01 marker.scale.z = 0.01 model_type = '_'.join( model.split('_')[:-1]) # remove instance index to get the model type marker.mesh_resource = "package://thorp_simulation/worlds/gazebo/models/" + model_type + "/meshes/Cat_v1_l3.obj" marker.mesh_use_embedded_materials = True # TODO: get mesh path and mesh pose from the model SDF file to make this generic and avoid the hack below if model_type == 'cat_black': marker.pose.position.x = -0.09 marker.pose.orientation = quaternion_msg_from_yaw(math.pi / 2.0) if model_type == 'cat_orange': marker.pose.orientation = quaternion_msg_from_yaw(math.pi / 2.0) return marker
def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 target_pose = GripperMarkers._offset_pose(self.marker_pose, GripperMarkers._offset) print target_pose ik_solution = self.ik.get_ik_for_ee(target_pose) if (ik_solution == None): mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh
def create_marker_for_link(self, link, T, scale=1, color=(0,1,0,1), idx=0) : marker = Marker() if not link in self.urdf.link_map : print "EndEffectorHelper::create_marker_for_link() -- link: ", link, " not found in URDF model" return marker model_link = self.urdf.link_map[link] if model_link : if model_link.visual : if model_link.visual.geometry : if model_link.visual.geometry.filename : mesh = model_link.visual.geometry.filename p = geometry_msgs.msg.Pose() q = (kdl.Rotation.RPY(model_link.visual.origin.rpy[0],model_link.visual.origin.rpy[1],model_link.visual.origin.rpy[2])).GetQuaternion() p.position.x = model_link.visual.origin.xyz[0] p.position.y = model_link.visual.origin.xyz[1] p.position.z = model_link.visual.origin.xyz[2] p.orientation.x = q[0] p.orientation.y = q[1] p.orientation.z = q[2] p.orientation.w = q[3] marker.pose = toMsg(T*fromMsg(p)) marker.header.frame_id = self.root_frame marker.ns = self.robot_name marker.mesh_resource = mesh marker.type = Marker.MESH_RESOURCE marker.action = Marker.MODIFY marker.scale.x = scale marker.scale.y = scale marker.scale.z = scale marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] marker.text = link marker.id = idx marker.mesh_use_embedded_materials = True return marker
def talker(): pub = rospy.Publisher('marker_chatter', Marker) rospy.init_node('marker_talker') while not rospy.is_shutdown(): m = Marker() m.header.frame_id = '/camera_rgb_optical_frame' m.type = 10 m.id = 2 m.action = 0 m.scale.x = 1 m.scale.y = 1 m.scale.z = 1 m.color.r = 0.3 m.color.g = 1 m.color.b = 0.2 m.color.a = 0.7 m.lifetime.secs = 1 m.frame_locked = False m.mesh_resource= 'http://localhost:5984/object_recognition/cd4c27a2dd307cda79b49ee31494ba08/mesh.stl' m.mesh_use_embedded_materials= False pub.publish(m) rospy.sleep(1.0)
for x in xrange(0,num_obs): kind=int(data[3*x]) px=float(data[3*x+1]) py=float(data[3*x+2]) filename="package://dynamic_3d_view/dynamic_model/"+switch[kind] # print filename marker = Marker() marker.id = x marker.header.frame_id = "/map" marker.ns='put_obstacles' marker.type=10 marker.scale.x=1 marker.scale.y=1 marker.scale.z=1 # marker.header.stamp =rospy.Time.now() marker.mesh_resource=filename marker.mesh_use_embedded_materials=1 marker.action=0 # marker.lifetime = rospy.Duration(0) marker.pose.position.x = px-6.5 marker.pose.position.y = py-6.5 marker.pose.position.z = 0 markerArray.markers.append(marker) while not rospy.is_shutdown(): # Publish the MarkerArray publisher.publish(markerArray) rospy.sleep(1)
def publish_object_poses(input_markers): output_markers = MarkerArray() tag_detections = {} for marker in input_markers.markers: tag_id = marker.id position_data = marker.pose.position orientation_data = marker.pose.orientation tag_pose = numpy.matrix(quaternion_matrix([orientation_data.x, orientation_data.y, orientation_data.z, orientation_data.w])) tag_pose[0,3] = position_data.x tag_pose[1,3] = position_data.y tag_pose[2,3] = position_data.z tag_detections[tag_id] = [tag_pose] obj_tag_poses = {} for tag in tag_detections: if tag not in tag_to_obj: #Not a noted marker continue #Get the object for that tag tag_obj = tag_to_obj[tag] # Multiply apriltags value with pre-stored value tag_to_cam = tag_detections[tag]*obj_to_tags[tag_obj][tag] if tag_obj not in obj_tag_poses: #Only append obj_tag_poses[tag_obj] = {} obj_tag_poses[tag_obj][tag] = tag_to_cam print(obj_tag_poses) for obj in obj_tag_poses: for tag in obj_tag_poses[obj]: obj_pose_transform = obj_tag_poses[obj][tag] output_marker = Marker() output_marker.header.stamp = rospy.Time.now() output_marker.header.frame_id = input_markers.markers[0].header.frame_id output_marker.ns = obj output_marker.id = tag output_marker.type = Marker.CUBE output_marker.action = output_marker.ADD output_marker.pose.position.x = obj_pose_transform[0,3] output_marker.pose.position.y = obj_pose_transform[1,3] output_marker.pose.position.z = obj_pose_transform[2,3] output_marker.scale.x = 0.01 output_marker.scale.y = 0.01 output_marker.scale.z = 0.01 output_marker.color.r = 1.0 output_marker.color.a = 1.0 output_quat = quaternion_from_matrix(obj_pose_transform) output_marker.pose.orientation.x = output_quat[0] output_marker.pose.orientation.y = output_quat[1] output_marker.pose.orientation.z = output_quat[2] output_marker.pose.orientation.w = output_quat[3] output_marker.mesh_use_embedded_materials = False output_markers.markers.append(output_marker) obj_pose_pub.publish(output_markers)
from __future__ import print_function import copy import rospy from rospkg import RosPack, ResourceNotFound from visualization_msgs.msg import Marker import pysdf import os.path protoMarkerMsg = Marker() protoMarkerMsg.frame_locked = True protoMarkerMsg.id = 0 protoMarkerMsg.action = Marker.ADD protoMarkerMsg.mesh_use_embedded_materials = True protoMarkerMsg.color.a = 0.0 protoMarkerMsg.color.r = 0.0 protoMarkerMsg.color.g = 0.0 protoMarkerMsg.color.b = 0.0 supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box'] gazebo_rospack = RosPack() def link2marker_msg(link, full_linkname, use_collision = False, lifetime = rospy.Duration(0)): marker_msg = None linkpart = None if use_collision: linkparts = getattr(link, 'collisions') else: # visual
def publish_sim_path_as_markers(self, sim_path, frame_id): """Publishes the given sim apth as a set of ROS markers""" msg = MarkerArray() now = rospy.Time.now() if not sim_path or len(sim_path) < self.last_marker_length: diff = self.last_marker_length offset = 0 if sim_path: offset = len(sim_path) diff -= offset for index in range(diff+1): marker = Marker() marker.header.frame_id = frame_id marker.ns = 'sim_path' marker.id = offset + index - 1 marker.action = Marker.DELETE if sim_path: for index, waypoint in enumerate(sim_path): marker = Marker() marker.header.frame_id = frame_id marker.ns = 'sim_path' marker.id = index marker.type = Marker.ARROW marker.action = Marker.MODIFY marker.pose.position.x = waypoint[1] marker.pose.position.y = waypoint[2] quat = qfe(0, 0, waypoint[3]) marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] marker.scale.x = 0.25 marker.scale.y = 0.25 marker.scale.z = 0.25 marker.color.r = 0.25 marker.color.g = 0.25 marker.color.b = 0.25 marker.color.a = 0.25 msg.markers.append(marker) waypoint = sim_path[-1] marker = Marker() marker.header.frame_id = frame_id marker.ns = 'prediction' marker.id = 0 marker.type = Marker.MESH_RESOURCE marker.action = Marker.MODIFY marker.pose.position.x = waypoint[1] marker.pose.position.y = waypoint[2] marker.pose.position.z = 0.13 quat = qfe(0, 0, waypoint[3]) marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.color.r = 1 marker.color.g = 0 marker.color.b = 0 marker.color.a = 1 marker.mesh_resource = 'package://gavlab_atrv_description/meshes/atrv_chassis.dae' marker.mesh_use_embedded_materials = True msg.markers.append(marker) self.marker_array_pub.publish(msg)
def makeGripperMarker(angle=0.541, color=None, scale=1.0): """ Creates an InteractiveMarkerControl with the PR2 gripper shape. Parameters: angle: the aperture angle of the gripper (default=0.541) color: (r,g,b,a) tuple or None (default) if using the material colors scale: the scale of the gripper, default is 1.0 Returns: The new gripper InteractiveMarkerControl """ T1 = euclid.Matrix4() T2 = euclid.Matrix4() T1.translate(0.07691, 0.01, 0.) T1.rotate_axis(angle, euclid.Vector3(0,0,1)) T2.translate(0.09137, 0.00495, 0.) T1.rotate_axis(-angle, euclid.Vector3(0,0,1)) T_proximal = T1.copy() T_distal = T1 * T2 control = InteractiveMarkerControl() mesh = Marker() mesh.type = Marker.MESH_RESOURCE mesh.scale.x = scale mesh.scale.y = scale mesh.scale.z = scale if color is not None: mesh.color.r = color[0] mesh.color.g = color[1] mesh.color.b = color[2] mesh.color.a = color[3] mesh.mesh_use_embedded_materials = False else: mesh.mesh_use_embedded_materials = True mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae" mesh.pose.orientation.w = 1 control.markers.append(copy.deepcopy(mesh)) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae" mesh.pose = matrix4ToPose(T_proximal) control.markers.append(copy.deepcopy(mesh)) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae" mesh.pose = matrix4ToPose(T_distal) control.markers.append(copy.deepcopy(mesh)) T1 = euclid.Matrix4() T2 = euclid.Matrix4() T1.translate(0.07691, -0.01, 0.) T1.rotate_axis(numpy.pi, euclid.Vector3(1,0,0)) T1.rotate_axis(angle, euclid.Vector3(0,0,1)) T2.translate(0.09137, 0.00495, 0.) T1.rotate_axis(-angle, euclid.Vector3(0,0,1)) T_proximal = T1.copy() T_distal = T1 * T2 mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae" mesh.pose = matrix4ToPose(T_proximal) control.markers.append(copy.deepcopy(mesh)) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae" mesh.pose = matrix4ToPose(T_distal) control.markers.append(copy.deepcopy(mesh)) control.interaction_mode = InteractiveMarkerControl.BUTTON return control
def create_map(self): """ This function collects the survey data and puts it into marker arrays ready to be published in rviz """ stripes, centers = self.survey() self.map_stripe_array = MarkerArray() self.map_lane_array = MarkerArray() NCAT_id = 0 ### Stripes ################################################################ for pt in stripes: lat = float(pt[0]) lon = float(pt[1]) (east, nrth, _), info = self.gps.lla2utm((lat, lon, 0)) # convert to UTM marker = Marker() marker.header.frame_id = 'odom' marker.id = NCAT_id # enumerate subsequent markers here marker.action = Marker.ADD # can be ADD, REMOVE, or MODIFY marker.lifetime = rospy.Duration() # will last forever unless modified marker.ns = "stripes" marker.type = Marker.CUBE marker.pose.position.x = east - float(self.UTMdatum['E']) marker.pose.position.y = nrth - float(self.UTMdatum['N']) marker.pose.position.z = 0 # zero is a novatel mount level marker.color.r = 255 marker.color.g = 255 marker.color.b = 0 marker.color.a = 1.0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.4 marker.mesh_use_embedded_materials = False self.map_stripe_array.markers.append(marker) NCAT_id += 1 self.map_stripe_publisher.publish(self.map_stripe_array) # publish stripes as markers print('mapBridge: Stripe Markers have been printed') del marker ### Lane Centers ########################################################### for pt in centers: lat = float(pt[0]) lon = float(pt[1]) (east, nrth, _), _ = self.gps.lla2utm((lat, lon, 0)) # convert to UTM marker = Marker() marker.header.frame_id = 'odom' marker.id = NCAT_id # enumerate subsequent markers here marker.action = Marker.ADD # can be ADD, REMOVE, or MODIFY marker.lifetime = rospy.Duration() # will last forever unless modified marker.ns = "lane_centers" marker.type = Marker.SPHERE marker.pose.position.x = east - float(self.UTMdatum['E']) marker.pose.position.y = nrth - float(self.UTMdatum['N']) marker.pose.position.z = 0 # zero is a novatel mount level marker.color.r = 255 marker.color.g = 255 marker.color.b = 255 marker.color.a = 0.75 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.mesh_use_embedded_materials = False self.map_lane_array.markers.append(marker) NCAT_id += 1 self.map_lane_publisher.publish(self.map_lane_array) # publish lane centers as markers print('mapBridge: Lane Center Markers have been printed')
def create_map_mesh(self): """Puts a blender mesh of the paement and stripes (continuous) into rviz""" marker_array = MarkerArray() ### Track Pavement Publisher ########################################### if self.track_mesh_resource: marker = Marker() marker.header.frame_id = 'odom' # publish in static frame marker.id = 0 marker.action = Marker.ADD marker.lifetime = rospy.Duration() # immortal unless changed marker.ns = "track_mesh_pavement" marker.type = Marker.MESH_RESOURCE marker.mesh_use_embedded_materials = False marker.mesh_resource = '//'.join(['file:', self.track_mesh_resource]) marker.pose.position.x = 0# - float(self.UTMdatum['E']) marker.pose.position.y = 0# - float(self.UTMdatum['N']) marker.pose.position.z = 0 marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.color.r = 0.3 marker.color.g = 0.3 marker.color.b = 0.3 marker.color.a = 1.0 marker_array.markers.append(marker) ### Lane Marking Publisher ############################################## if self.marking_mesh_resource: marker = Marker() marker.header.frame_id = 'odom' # publish in static frame marker.id = 1 marker.action = Marker.ADD marker.lifetime = rospy.Duration() # immortal unless changed marker.ns = "track_mesh_lane_markings" marker.type = Marker.MESH_RESOURCE marker.mesh_use_embedded_materials = False marker.mesh_resource = self.marking_mesh_resource marker.pose.position.x = 0# - float(self.UTMdatum['E']) marker.pose.position.y = 0# - float(self.UTMdatum['N']) marker.pose.position.z = 0 marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.color.r = 255 marker.color.g = 255 marker.color.b = 0 marker.color.a = 1.0 marker_array.markers.append(marker) if self.sign_mesh_resource: marker = Marker() marker.header.frame_id = 'odom' # publish in static frame marker.id = 1 marker.action = Marker.ADD marker.lifetime = rospy.Duration() # immortal unless changed marker.ns = "track_mesh_section_signs" marker.type = Marker.MESH_RESOURCE marker.mesh_use_embedded_materials = False marker.mesh_resource = '//'.join(['file:',self.sign_mesh_resource]) marker.pose.position.x = 0# - float(self.UTMdatum['E']) marker.pose.position.y = 0# - float(self.UTMdatum['N']) marker.pose.position.z = 0 marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.color.r = 0.1 marker.color.g = 0.6 marker.color.b = 0.1 marker.color.a = 1.0 marker_array.markers.append(marker) if self.stop_mesh_resource: marker = Marker() marker.header.frame_id = 'odom' # publish in static frame marker.id = 1 marker.action = Marker.ADD marker.lifetime = rospy.Duration() # immortal unless changed marker.ns = "track_mesh_stop_signs" marker.type = Marker.MESH_RESOURCE marker.mesh_use_embedded_materials = False marker.mesh_resource = '//'.join(['file:',self.stop_mesh_resource]) marker.pose.position.x = 0# - float(self.UTMdatum['E']) marker.pose.position.y = 0# - float(self.UTMdatum['N']) marker.pose.position.z = 0 marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.color.r = 0.7 marker.color.g = 0.1 marker.color.b = 0.1 marker.color.a = 1.0 marker_array.markers.append(marker) self.track_mesh_publisher.publish(marker_array)
def publish_gripper(server, pose_stamped, name): """Publishes a marker representing a gripper. Code taken from action_step_marker.py in PR2/PbD. Args: server: An InteractiveMarkerServer pose_stamped: A PoseStamped giving the wrist_roll_link pose. name: string, a unique name for this gripper. """ # Set angle of meshes based on gripper open vs closed. angle = 28 * math.pi / 180.0 # Fully open. STR_MESH_GRIPPER_FOLDER = 'package://pr2_description/meshes/gripper_v0/' STR_GRIPPER_PALM_FILE = STR_MESH_GRIPPER_FOLDER + 'gripper_palm.dae' STR_GRIPPER_FINGER_FILE = STR_MESH_GRIPPER_FOLDER + 'l_finger.dae' STR_GRIPPER_FINGERTIP_FILE = STR_MESH_GRIPPER_FOLDER + 'l_finger_tip.dae' # Make transforms in preparation for meshes 1, 2, and 3. # NOTE(mbforbes): There are some magic numbers in here that are # used a couple times. Seems like a good candidate for # refactoring to constants, but I think they're more clear if # left in here as (a) they likely won't be changed, and (b) it's # easier to understand the computations with them here. transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) # Create mesh 1 (palm). mesh1 = Marker() mesh1.header.frame_id = pose_stamped.header.frame_id mesh1.mesh_use_embedded_materials = True mesh1.type = Marker.MESH_RESOURCE mesh1.scale.x = 1.0 mesh1.scale.y = 1.0 mesh1.scale.z = 1.0 mesh1.mesh_resource = STR_GRIPPER_PALM_FILE mesh1.pose = pose_stamped.pose # Create mesh 2 (finger). mesh2 = Marker() mesh2.mesh_use_embedded_materials = True mesh2.type = Marker.MESH_RESOURCE mesh2.scale.x = 1.0 mesh2.scale.y = 1.0 mesh2.scale.z = 1.0 mesh2.mesh_resource = STR_GRIPPER_FINGER_FILE mesh2.pose = _get_pose_from_transform(t_proximal) # Create mesh 3 (fingertip). mesh3 = Marker() mesh3.mesh_use_embedded_materials = True mesh3.type = Marker.MESH_RESOURCE mesh3.scale.x = 1.0 mesh3.scale.y = 1.0 mesh3.scale.z = 1.0 mesh3.mesh_resource = STR_GRIPPER_FINGERTIP_FILE mesh3.pose = _get_pose_from_transform(t_distal) # Make transforms in preparation for meshes 4 and 5. quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(math.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) # Create mesh 4 (other finger). mesh4 = Marker() mesh4.mesh_use_embedded_materials = True mesh4.type = Marker.MESH_RESOURCE mesh4.scale.x = 1.0 mesh4.scale.y = 1.0 mesh4.scale.z = 1.0 mesh4.mesh_resource = STR_GRIPPER_FINGER_FILE mesh4.pose = _get_pose_from_transform(t_proximal) # Create mesh 5 (other fingertip). mesh5 = Marker() mesh5.mesh_use_embedded_materials = True mesh5.type = Marker.MESH_RESOURCE mesh5.scale.x = 1.0 mesh5.scale.y = 1.0 mesh5.scale.z = 1.0 mesh5.mesh_resource = STR_GRIPPER_FINGERTIP_FILE mesh5.pose = _get_pose_from_transform(t_distal) # Append all meshes we made. control = InteractiveMarkerControl() control.markers = [mesh1, mesh2, mesh3, mesh4, mesh5] control.interaction_mode = InteractiveMarkerControl.NONE interactive_marker = InteractiveMarker() interactive_marker.controls = [control] interactive_marker.header.frame_id = pose_stamped.header.frame_id interactive_marker.pose = pose_stamped.pose interactive_marker.name = name server.insert(interactive_marker) server.applyChanges()
def simulate_quad(self): # simulator node rospy.init_node('simulate_quad', anonymous=True) #-----------------------------------------------------------------------# #-----------------------------------------------------------------------# # Simulator subscribes to command inputs, published by a controller rospy.Subscriber("quad_cmd", quad_cmd, self.get_input) #-----------------------------------------------------------------------# #-----------------------------------------------------------------------# # this node is a simulator, thus it will publish the state of the quad # it uses the commands -- that it is subscribed to -- to solve differential equations pub = rospy.Publisher('quad_state', quad_state, queue_size=10) #-----------------------------------------------------------------------# #-----------------------------------------------------------------------# # TO SAVE FLAG # by default, no dynamics: everything stopped self.StartFlag = False # Service is created, so that simulation can be started or stopped Start_service = rospy.Service('StartSimulator', StartSim, self.handle_Start_service) # Service is created, so that simulation can be reseted Reset_service = rospy.Service('ResetSimulator', StartSim, self.handle_Reset_service) # NOTICE THAT SERVICE TYPE IS THE SAME FOR BOTH SERVICES ABOVE #-----------------------------------------------------------------------# #-----------------------------------------------------------------------# #-----------------------------------------------------------------------# #-----------------------------------------------------------------------# # Service is created, so that user can change simulator on GUI Chg_Simulator = rospy.Service('ServiceChangeSimulator', SrvCreateJsonableObjectByStr, self.__handle_simulator_change_service) # solve differential equations at frequency rate = rospy.Rate(self.frequency) pub_rviz = rospy.Publisher("visualization_marker",Marker, queue_size=10); marker = Marker() marker.header.frame_id = "map"; marker.header.stamp = rospy.Time(); marker.ns = "my_namespace"; marker.id = 0; # marker.type = Marker.SPHERE; # marker.action = Marker.ADD; marker.pose.position.y = 0.1; marker.pose.position.z = 0.1; marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; marker.pose.orientation.w = 1.0; # marker.scale.x = 1; # marker.scale.y = 0.1; # marker.scale.z = 0.1; # marker.color.a = 1.0; #Don't forget to set the alpha! # marker.color.r = 0.0; # marker.color.g = 1.0; # marker.color.b = 0.0; # //only if using a MESH_RESOURCE marker type: # marker.color.r = 1.0; # marker.color.g = 1.0; # marker.color.b = 1.0; marker.scale.x = 1; marker.scale.y = 1; marker.scale.z = 1; marker.type = Marker.MESH_RESOURCE marker.mesh_use_embedded_materials = True marker.mesh_resource = "package://rotors_description/meshes/firefly.dae" # marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae"; while not rospy.is_shutdown(): # WARNING: IT IS VERY IMPORTANT THAT U0, U1, U2 AND U3 ARE PROVIDED THIS WAY # I CANNOT PROVIDE SELF.U TO THE INTEGRATION BECAUSE IT IS CHANGING # AND IT MESSES UP THE INTEGRATION!!! # thid DOES NOT work: r.set_f_params(deepcopy(self.U),parameters) # we delay system the initialization of the system by a TimeDelay simtime = self.sim.get_time() simstate = self.sim.get_state() if (simtime >= self.TimeDelay and self.StartFlag): self.sim.run(1.0/self.frequency) # set dynamics vector accordinf to current input vector # input vector is assumed constant during integration #self.r.set_f_params(Input(self.U)) # integrate equation for period of loop #self.r.integrate(self.r.t + 1.0/self.frequency); # reset initial state and initial time #self.r.set_initial_value(self.r.y, self.r.t) else: # need to update initial state and time #self.r.set_initial_value(self.r.y, self.r.t + 1.0/self.frequency) self.sim.reset( initial_time=simtime+1.0/self.frequency, initial_state=simstate ) # create a message of type quad_state with current state state = self.write_state() # publish current state # rospy.logwarn(state) # rospy.logwarn('aaaaaaaaaaaaaaaaaaaaaaaaa') pub.publish(state) # marker.pose.position.x = 0.1 + numpy.cos(2*3.14/5*rospy.get_time()); marker.pose.position.x = simstate[0] marker.pose.position.y = simstate[1] marker.pose.position.z = simstate[2] # quaternion # marker.pose.orientation.x = simstate[0] # marker.pose.orientation.y = simstate[1] # marker.pose.orientation.z = simstate[2] # marker.pose.orientation.w = simstate[2] pub_rviz.publish( marker ) # rospy.logwarn(marker) # let node sleep rate.sleep() # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def __init__(self, input_ref_frame_id, input_frame_id): self.input_ref_frame_id = input_ref_frame_id self.input_frame_id = input_frame_id # Parameters self.tip_link = rospy.get_param('~tip_link') self.cmd_frame_id = rospy.get_param('~cmd_frame', 'wam/cmd') self.scale = rospy.get_param('~scale', 1.0) self.use_hand = rospy.get_param('~use_hand', True) # TF structures self.last_time_check = rospy.Time.now() self.listener = tf.TransformListener() self.broadcaster = tf.TransformBroadcaster() # State self.cmd_origin = kdl.Frame() self.tip_origin = kdl.Frame() # Command state self.cmd_frame = None self.deadman_engaged = False self.engage_augmenter = False self.cmd_scaling = 0.0 # Hand structures if self.use_hand: # Hand state self.hand_cmd = oro_barrett_msgs.msg.BHandCmd() self.last_hand_cmd = rospy.Time.now() self.hand_position = [0, 0, 0, 0] self.hand_velocity = [0, 0, 0, 0] self.move_f = [True, True, True] self.move_spread = True self.move_all = True # Hand joint state and direct command self.hand_joint_state_sub = rospy.Subscriber( 'hand/joint_states', sensor_msgs.msg.JointState, self.hand_state_cb) self.hand_pub = rospy.Publisher('hand/cmd', oro_barrett_msgs.msg.BHandCmd, queue_size=1000) # ROS generic telemanip command self.telemanip_cmd_pub = rospy.Publisher('telemanip_cmd_out', TelemanipCommand, queue_size=1000) # goal marker self.marker_pub = rospy.Publisher('master_target_markers', MarkerArray, queue_size=1000) self.master_target_markers = MarkerArray() self.color_blue = ColorRGBA(0.0, 0.66, 1.0, 1.0) self.color_gray = ColorRGBA(0.5, 0.5, 0.5, 1.0) self.color_orange = ColorRGBA(1.0, 0.5, 0.0, 1.0) self.color_green = ColorRGBA(0.0, 1.0, 0.0, 1.0) self.color_red = ColorRGBA(1.0, 0.0, 0.0, 1.0) m = Marker() m.header.frame_id = self.cmd_frame_id m.ns = 'finger_marker' m.id = 0 m.type = m.MESH_RESOURCE m.action = 0 p = m.pose.position o = m.pose.orientation p.x, p.y, p.z = finger_point(0.0) o.x, o.y, o.z, o.w = (0, 0, 0, 1.0) m.color = self.color_gray m.scale.x = m.scale.y = m.scale.z = 0.02 m.frame_locked = True m.mesh_resource = 'package://lcsr_barrett/models/teleop_finger_marker.dae' m.mesh_use_embedded_materials = False self.master_target_markers.markers.append(m) m = Marker() m.header.frame_id = self.cmd_frame_id m.ns = 'finger_marker' m.id = 1 m.type = m.MESH_RESOURCE m.action = 0 p = m.pose.position o = m.pose.orientation p.x, p.y, p.z = finger_point(0.0) o.x, o.y, o.z, o.w = (0, 0, 0, 1.0) m.color = self.color_gray m.scale.x = m.scale.y = m.scale.z = 0.02 m.frame_locked = True m.mesh_resource = 'package://lcsr_barrett/models/teleop_finger_marker.dae' m.mesh_use_embedded_materials = False self.master_target_markers.markers.append(m) m = Marker() m.header.frame_id = self.cmd_frame_id m.ns = 'finger_marker' m.id = 2 m.type = m.MESH_RESOURCE m.action = 0 p = m.pose.position o = m.pose.orientation p.x, p.y, p.z = (0.0, 0.08, -0.08) o.x, o.y, o.z, o.w = (0, 0, 0, 1.0) m.color = self.color_gray m.scale.x = m.scale.y = m.scale.z = 0.02 m.frame_locked = True m.mesh_resource = 'package://lcsr_barrett/models/teleop_finger_marker.dae' m.mesh_use_embedded_materials = False self.master_target_markers.markers.append(m) m = Marker() m.header.frame_id = self.cmd_frame_id m.ns = 'master_target_markers' m.id = 3 m.type = m.MESH_RESOURCE m.action = 0 o = m.pose.orientation o.x, o.y, o.z, o.w = (-0.7071, 0.0, 0.0, 0.7071) m.color = self.color_gray m.scale.x = m.scale.y = m.scale.z = 0.02 m.frame_locked = True m.mesh_resource = 'package://lcsr_barrett/models/teleop_target.dae' m.mesh_use_embedded_materials = False self.master_target_markers.markers.append(m)
def makeGripperMarker(pose, angle=0.0, color=None, scale=1.0): """ Creates an MarkerArray with the PR2 gripper shape. Parameters: pose: the position of the marker, a Pose instance angle: the aperture angle of the gripper (default=0.541) color: (r,g,b,a) tuple or None (default) if using the material colors scale: the scale of the gripper, default is 1.0 Returns: The new gripper MarkerArray """ assert isinstance(pose, PoseStamped) pose_T = poseTomatrix(pose.pose) gripper_marker = MarkerArray() T1 = transformations.translation_matrix((0.07691, 0.01, 0.)) T1 = T1.dot(transformations.rotation_matrix(angle, (0, 0, 1))) T2 = transformations.translation_matrix((0.09137, 0.00495, 0.)) T2 = T2.dot(transformations.rotation_matrix(angle, (0, 0, 1))) #T1 = euclid.Matrix4() #T2 = euclid.Matrix4() #T1.translate(0.07691, 0.01, 0.) #T1.rotate_axis(angle, euclid.Vector3(0,0,1)) #T2.translate(0.09137, 0.00495, 0.) #T1.rotate_axis(-angle, euclid.Vector3(0,0,1)) T_proximal = T1.copy() T_distal = T1.dot(T2) mesh = Marker() mesh.header.frame_id = pose.header.frame_id mesh.type = Marker.MESH_RESOURCE mesh.scale.x = scale mesh.scale.y = scale mesh.scale.z = scale if color is not None: mesh.color.r = color[0] mesh.color.g = color[1] mesh.color.b = color[2] mesh.color.a = color[3] mesh.mesh_use_embedded_materials = False else: mesh.mesh_use_embedded_materials = True mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae" mesh.pose = matrixToPose(pose_T) gripper_marker.markers.append(copy.deepcopy(mesh)) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae" mesh.pose = matrixToPose(pose_T.dot(T_proximal)) gripper_marker.markers.append(copy.deepcopy(mesh)) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae" mesh.pose = matrixToPose(pose_T.dot(T_distal)) gripper_marker.markers.append(copy.deepcopy(mesh)) #T1 = euclid.Matrix4() #T2 = euclid.Matrix4() #T1.translate(0.07691, -0.01, 0.) #T1.rotate_axis(np.pi, euclid.Vector3(1,0,0)) #T1.rotate_axis(angle, euclid.Vector3(0,0,1)) #T2.translate(0.09137, 0.00495, 0.) #T1.rotate_axis(-angle, euclid.Vector3(0,0,1)) T1 = transformations.translation_matrix((0.07691, -0.01, 0.)) T1 = T1.dot(transformations.rotation_matrix(np.pi, (1, 0, 0))) T1 = T1.dot(transformations.rotation_matrix(angle, (0, 0, 1))) T2 = transformations.translation_matrix((0.09137, 0.00495, 0.)) T2 = T2.dot(transformations.rotation_matrix(-angle, (0, 0, 1))) T_proximal = T1.copy() T_distal = T1.dot(T2) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae" mesh.pose = matrixToPose(pose_T.dot(T_proximal)) gripper_marker.markers.append(copy.deepcopy(mesh)) mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae" mesh.pose = matrixToPose(pose_T .dot(T_distal)) gripper_marker.markers.append(copy.deepcopy(mesh)) for i, marker in enumerate(gripper_marker.markers): marker.id = i return gripper_marker
import sys frame_name = sys.argv[1] rate = rospy.Rate(2) counter_id = 0 while not rospy.is_shutdown(): m = Marker() m.id = counter_id m.ns = frame_name m.action = m.ADD m.type = m.MESH_RESOURCE m.header.stamp = rospy.Time.now() m.header.frame_id = frame_name m.color = ColorRGBA(255.0/255.0, 224.0/255.00, 150.0/255.0, 1) m.lifetime = rospy.Duration(1) m.mesh_resource = "package://pr2_pouring_experiments/models/dinner_plate.dae"; m.scale = Vector3(0.0254,0.0254,0.0254) m.mesh_use_embedded_materials = True m.pose.position = Point(0, 0, 0) m.pose.orientation.w = 1 m.frame_locked = True pub.publish(m) rate.sleep()