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 createMarker(x_pos, z_pos): # gen marker gripper_center = Marker() gripper_left = Marker() gripper_right = Marker() #mesh resource gripper_center.type = Marker.MESH_RESOURCE gripper_left.type = Marker.MESH_RESOURCE gripper_right.type = Marker.MESH_RESOURCE # add resource gripper_center.mesh_resource = GRIPPER_MESH gripper_left.mesh_resource = L_FINGER_MESH gripper_right.mesh_resource = R_FINGER_MESH # add color to gripper gripper_center.color.r = 1.0 gripper_left.color.r = 1.0 gripper_right.color.r = 1.0 gripper_center.color.a = 1.0 gripper_left.color.a = 1.0 gripper_right.color.a = 1.0 gripper_center.scale.x = 1.0 gripper_center.scale.y = 1.0 gripper_center.scale.z = 1.0 gripper_center.pose.position.x += GRIPPER_OFFSET + x_pos gripper_left.pose.position.x += GRIPPER_OFFSET + x_pos gripper_right.pose.position.x += GRIPPER_OFFSET + x_pos gripper_center.pose.position.z += z_pos gripper_left.pose.position.z += z_pos gripper_right.pose.position.z += z_pos gripper_left.pose.position.y = gripper_center.pose.position.y - 0.05 gripper_right.pose.position.y = gripper_center.pose.position.y + 0.05 return [gripper_center, gripper_left, gripper_right]
def createMarker(): gripper_center = Marker() gripper_left = Marker() gripper_right = Marker() gripper_center.type = Marker.MESH_RESOURCE gripper_left.type = Marker.MESH_RESOURCE gripper_right.type = Marker.MESH_RESOURCE gripper_center.mesh_resource = GRIPPER_MESH gripper_left.mesh_resource = L_FINGER_MESH gripper_right.mesh_resource = R_FINGER_MESH return [gripper_center, gripper_left, gripper_right]
def init_markers(xOffset, yOffset, zOffset): """ Returns either 1 InteractiveMarker or list of 3 Markers tbd """ gripper = Marker() l_finger = Marker() r_finger = Marker() # setup meshes gripper.type = Marker.MESH_RESOURCE gripper.mesh_resource = GRIPPER_MESH l_finger.type = Marker.MESH_RESOURCE l_finger.mesh_resource = L_FINGER_MESH r_finger.type = Marker.MESH_RESOURCE r_finger.mesh_resource = R_FINGER_MESH # # gripper gripper.pose.position.x += xOffset gripper.pose.position.y += yOffset gripper.pose.position.z += zOffset gripper.pose.orientation.w = 1.0 gripper.color.r = 1 gripper.color.g = 0.0 gripper.color.b = 0.0 gripper.color.a = 0.7 # # l finger stuff l_finger.pose.position.x += xOffset l_finger.pose.position.y += -0.05 + yOffset l_finger.pose.position.z += zOffset l_finger.pose.orientation.w = 1.0 l_finger.color.r = 1 l_finger.color.g = 0.0 l_finger.color.b = 0.0 l_finger.color.a = 0.7 # # r finger stuff r_finger.pose.position.x += xOffset r_finger.pose.position.y += 0.05 + yOffset r_finger.pose.position.z += zOffset r_finger.pose.orientation.w = 1.0 r_finger.color.r = 1 r_finger.color.g = 0.0 r_finger.color.b = 0.0 r_finger.color.a = 0.7 markers = [gripper, l_finger, r_finger] return markers
def createMarkers(ps): name1 = "left" marker1 = Marker() # marker1.header.frame_id = "base_link" marker1.type = Marker.MESH_RESOURCE marker1.mesh_resource = L_FINGER_MESH marker1.pose = copy.deepcopy(ps.pose) marker1.pose.position.x += 0.166 marker1.pose.position.y -= 0.05 # marker1.pose.orientation.w = 1 marker1.scale.x = 1 marker1.scale.y = 1 marker1.scale.z = 1 marker1.color.r = 0.0 marker1.color.g = 0.5 marker1.color.b = 0.5 marker1.color.a = 1.0 #marker1.markers.append(create_box_marker()) name2 = "right" marker2 = Marker() # marker2.header.frame_id = "base_link" marker2.type = Marker.MESH_RESOURCE marker2.mesh_resource = R_FINGER_MESH marker2.pose = copy.deepcopy(ps.pose) marker2.pose.position.x += 0.166 marker2.pose.position.y += 0.05 # marker2.pose.orientation.w = 1 marker2.scale.x = 1 marker2.scale.y = 1 marker2.scale.z = 1 marker2.color.r = 0.0 marker2.color.g = 0.5 marker2.color.b = 0.5 marker2.color.a = 1.0 name3 = "gripperer" marker3 = Marker() # marker3.header.frame_id = "base_link" marker3.type = Marker.MESH_RESOURCE marker3.mesh_resource = GRIPPER_MESH marker3.pose = copy.deepcopy(ps.pose) # marker3.pose.position.x += 0.166 marker3.scale.x = 1 marker3.scale.y = 1 marker3.scale.z = 1 marker3.color.r = 0.0 marker3.color.g = 0.5 marker3.color.b = 0.5 marker3.color.a = 1.0 return [marker1, marker2, marker3]
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 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 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 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 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 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 visualize_object( pose, filepath="package://config/descriptions/meshes/objects/object.stl", name="/object", color=(0., 0., 1., 1.), frame_id="/yumi_body", scale=(1., 1., 1.)): marker_pub = rospy.Publisher(name, Marker, queue_size=1) marker_type = Marker.MESH_RESOURCE marker = Marker() marker.header.frame_id = frame_id marker.ns = name marker.header.stamp = rospy.Time(0) marker.action = Marker.ADD marker.pose.orientation.w = 1.0 marker.type = marker_type marker.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] marker.lifetime.secs = 1 marker.pose = pose.pose marker.mesh_resource = filepath marker.lifetime = rospy.Duration(10000) for i in range(10): marker_pub.publish(marker)
def getMarker(self): marker = Marker() ns = rospy.get_namespace() marker.header.frame_id = rospy.get_param('~grid_frame') marker.header.stamp = rospy.Time.now() marker.ns = "shape_namespace" marker.id = self.idnum marker.type = 1 marker.action = 0 marker.pose.position.x = self.position[0] marker.pose.position.y = self.position[1] marker.pose.position.z = self.position[2] marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 0 marker.pose.orientation.w = 1 marker.scale.x = self.size[0] marker.scale.y = self.size[1] marker.scale.z = self.size[2] marker.color.a = 0.9 marker.color.r = 1 marker.color.g = 0 marker.color.b = 0 marker.lifetime = rospy.Duration(0.6) marker.mesh_resource = "" return marker
def init(self): for poseview,mesh_path,mesh_scale,mesh_rgba in zip(self.ros_pose_viewers, self.mesh_paths, self.mesh_scales, self.mesh_rgbas): marker = Marker() marker.ns = self.namespace marker.id = self.last_used_id+1 self.last_used_id += 1 marker.action = 0 # 10 for mesh marker.type = 10 marker.pose = poseview.pose x,y,z = mesh_scale marker.scale.x = x marker.scale.y = y marker.scale.z = z r,g,b,a = mesh_rgba marker.color.a = a marker.color.r = r marker.color.g = g marker.color.b = b marker.mesh_resource = 'file://'+mesh_path marker.header.frame_id = '/world' self.marker_array.markers.append(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 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 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_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 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 publish_car_model(model_pub): marker = Marker() marker.header.frame_id = FRAME_ID marker.header.stamp = rospy.Time.now() marker.id = 100 marker.action = Marker.ADD marker.lifetime = rospy.Duration() marker.type = Marker.MESH_RESOURCE marker.mesh_resource = "package://kitti_test/car_model/car_model.dae" marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = -1.73 q = tf.transformations.quaternion_from_euler(0, 0, -np.pi) marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 1.0 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 model_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 add_text(frame_id='map', id_=0, x=0, y=0, r=0, g=0, b=0, scale=0.1): marker = Marker() marker.header.frame_id = frame_id marker.header.stamp = rospy.get_rostime() marker.ns = "my_namespace" marker.id = id_ marker.type = marker.TEXT_VIEW_FACING marker.action = marker.ADD marker.pose.position.x = x + 0.1 marker.pose.position.y = y + 0.1 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.text = "chair" + str(int(id_ / 2)) marker.scale.x = scale marker.scale.y = scale marker.scale.z = scale marker.color.a = 1.0 marker.color.r = r marker.color.g = g marker.color.b = b marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae" return marker
def world_body_to_marker_msg(world_body, id=1, ns=u''): """ :type world_body: WorldBody :rtype: Marker """ m = Marker() m.ns = u'{}/{}'.format(ns, world_body.name) m.id = id if world_body.type == WorldBody.URDF_BODY: raise Exception( u'can\'t convert urdf body world object to marker array') elif world_body.type == WorldBody.PRIMITIVE_BODY: if world_body.shape.type == SolidPrimitive.BOX: m.type = Marker.CUBE m.scale = Vector3(*world_body.shape.dimensions) elif world_body.shape.type == SolidPrimitive.SPHERE: m.type = Marker.SPHERE m.scale = Vector3(world_body.shape.dimensions[0], world_body.shape.dimensions[0], world_body.shape.dimensions[0]) elif world_body.shape.type == SolidPrimitive.CYLINDER: m.type = Marker.CYLINDER m.scale = Vector3( world_body.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS], world_body.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS], world_body.shape.dimensions[SolidPrimitive.CYLINDER_HEIGHT]) else: raise Exception( u'world body type {} can\'t be converted to marker'.format( world_body.shape.type)) elif world_body.type == WorldBody.MESH_BODY: m.type = Marker.MESH_RESOURCE m.mesh_resource = world_body.mesh m.color = ColorRGBA(0, 1, 0, 0.8) return m
def publish_ego_car(ego_car_pub): """ Publish lef/right 45 degree FOV line use Marker http://wiki.ros.org/rviz/DisplayTypes/Marker """ ##camera FOV maker_array = MarkerArray() marker = Marker() marker.header.frame_id = FRAME_ID marker.header.stamp = rospy.Time.now() marker.id = 0 #must be different marker.action = Marker.ADD marker.lifetime = rospy.Duration() #forever show in screen marker.type = Marker.LINE_STRIP #you can change line style in here marker.color.r = 0.0 # 010 = green marker.color.g = 1.0 # marker.color.b = 0.0 # marker.color.a = 1.0 # Transparent 0 ~ 1 opaque marker.scale.x = 0.2 # Thickness marker.points = [] #Connection point coordinates marker.points.append(Point(10, -10, 0)) marker.points.append(Point(0, 0, 0)) marker.points.append(Point(10, 10, 0)) maker_array.markers.append(marker) ##car model mesh_marker = Marker() mesh_marker.header.frame_id = FRAME_ID mesh_marker.header.stamp = rospy.Time.now() mesh_marker.id = -1 mesh_marker.lifetime = rospy.Duration() #forever show in screen mesh_marker.type = Marker.MESH_RESOURCE #you can change line style in here mesh_marker.mesh_resource = "package://agv_object_dection/Land Rover Range Rover Sport HSE 2010.dae" mesh_marker.pose.position.x = 0.0 # set position mesh_marker.pose.position.y = 0.0 mesh_marker.pose.position.z = -1.73 #lidar = 0 0 0 car = 0 0 -1.73 q = tf.transformations.quaternion_from_euler(np.pi/2, 0, np.pi) #ratation value mesh_marker.pose.orientation.x = q[0] mesh_marker.pose.orientation.y = q[1] mesh_marker.pose.orientation.z = q[2] mesh_marker.pose.orientation.w = q[3] mesh_marker.color.r = 1.0 # mesh_marker.color.g = 1.0 # mesh_marker.color.b = 1.0 # mesh_marker.color.a = 1.0 # Transparent 0 ~ 1 opaque mesh_marker.scale.x = 0.9 # Thickness mesh_marker.scale.y = 0.9 # Thickness mesh_marker.scale.z = 0.9 # Thickness maker_array.markers.append(mesh_marker) ego_car_pub.publish(maker_array)
def createMeshMarker(resource, offset=(0,0,0), rgba=(1,0,0,1), orientation=(0,0,0,1), scale=1, scales=(1,1,1), frame_id="/map"): marker = Marker() marker.mesh_resource = resource; marker.header.frame_id = frame_id marker.type = marker.MESH_RESOURCE marker.scale.x = scale*scales[0] marker.scale.y = scale*scales[1] marker.scale.z = scale*scales[2] marker.color.a = rgba[3] marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.pose.position.x = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] obj_control = InteractiveMarkerControl() obj_control.always_visible = True obj_control.markers.append( marker ) return obj_control
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 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 publish_car_model(model_pub): marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.id = 0 marker.lifetime = rospy.Duration() marker.type = Marker.MESH_RESOURCE marker.mesh_resource = "package://kitti/bmw_x5/BMW X5 4.dae" # marker.mesh_use_embedded_materials = True marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = -1.73 q = tf.transformations.quaternion_from_euler(np.pi / 2, 0, np.pi) marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 0.8 marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 model_pub.publish(marker) rospy.loginfo("marker published")
def callback(msg): """""" global rate marker = Marker() marker.header.frame_id = "global_tank" marker.id = 0 marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.color.r = 1.0 marker.color.g = 0 marker.color.b = 0 marker.color.a = 1 # transparency marker.pose.orientation.w = msg.pose.orientation.w marker.pose.orientation.x = msg.pose.orientation.x marker.pose.orientation.y = msg.pose.orientation.y marker.pose.orientation.z = msg.pose.orientation.z marker.pose.position.x = msg.pose.position.x # x marker.pose.position.y = msg.pose.position.y # y marker.pose.position.z = msg.pose.position.z # z # marker.mesh_resource = "package://hippocampus_test/models/uuv_hippocampus.stl" marker.mesh_resource = "package://hippocampus_common/models/uuv_hippocampus.stl" publisher_marker.publish(marker)
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 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 createMeshMarker(resource, offset=(0, 0, 0), rgba=(1, 0, 0, 1), orientation=(0, 0, 0, 1), scale=1, scales=(1, 1, 1), frame_id="/map", marker_id=0): marker = Marker() marker.mesh_resource = resource marker.header.frame_id = frame_id marker.id = marker_id marker.type = marker.MESH_RESOURCE marker.scale.x = scale * scales[0] marker.scale.y = scale * scales[1] marker.scale.z = scale * scales[2] marker.color.a = rgba[3] marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.pose.position.x = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] return marker
def create_object_marker(self, soma_obj, soma_type, pose): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.name = soma_obj int_marker.description = "id" + soma_obj int_marker.pose = pose mesh_marker = Marker() mesh_marker.type = Marker.MESH_RESOURCE mesh_marker.scale.x = 1 mesh_marker.scale.y = 1 mesh_marker.scale.z = 1 random.seed(soma_type) val = random.random() mesh_marker.color.r = r_func(val) mesh_marker.color.g = g_func(val) mesh_marker.color.b = b_func(val) mesh_marker.color.a = 1.0 #mesh_marker.pose = pose mesh_marker.mesh_resource = self.mesh[soma_type] control = InteractiveMarkerControl() control.markers.append(mesh_marker) int_marker.controls.append(control) return int_marker
def mesh_marker(position, orientation=None, mesh_resource="", rgba=[1.0, 1.0, 1.0, 1.0], ns="", id=0, frame_id="", lifetime=None, stamp=None): marker = Marker() if stamp is None: marker.header.stamp = rospy.Time.now() else: marker.header.stamp = stamp marker.header.frame_id = frame_id marker.ns = ns marker.id = id marker.type = Marker.MESH_RESOURCE marker.mesh_resource = mesh_resource marker.action = Marker.ADD marker.pose.position.x = position[0] marker.pose.position.y = position[1] marker.pose.position.z = position[2] if orientation is not None: marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] #marker.points = [Point(*tuple(position)), Point(*tuple(position+direction*1.0))] marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.color.a = rgba[3] if lifetime is None: marker.lifetime = rospy.Duration() else: marker.lifetime = rospy.Duration(lifetime) return marker
def publishMarker(self, position, visual): markertypes = { "cube": Marker.CUBE, "sphere": Marker.SPHERE, "mesh": Marker.MESH_RESOURCE } marker = Marker() marker.header.frame_id = position.get("frame_id") marker.type = markertypes[visual.get("type")] marker.ns = visual.get("ns") marker.id = visual.get("id") marker.action = Marker.ADD marker.scale.x = visual.get("scale")[0] marker.scale.z = visual.get("scale")[1] marker.scale.y = visual.get("scale")[2] marker.color.r = visual.get("color")[0] marker.color.g = visual.get("color")[1] marker.color.b = visual.get("color")[2] marker.color.a = visual.get("color")[3] marker.pose.position.x = position.get("x") marker.pose.position.y = position.get("y") marker.pose.position.z = visual.get("z") orientation = self.eulerToQuaternion(visual.get("orientation")) marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.mesh_resource = visual.get("mesh_path") if marker.type == Marker.MESH_RESOURCE else '' self.MarkersPUBL.publish(marker)
def createMesh(self, name, frame_id='world', mesh_path='', transform=PyKDL.Frame(), color=Color(), scale=np.array([1, 1, 1])): """ Creates a Mesh Marker """ marker = Marker() marker.header.frame_id = frame_id marker.type = Marker.MESH_RESOURCE marker.ns = name marker.action = marker.ADD marker.mesh_resource = mesh_path marker.color.r = color.r marker.color.g = color.g marker.color.b = color.b marker.color.a = color.a marker.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.pose.position.x = transform.p.x() marker.pose.position.y = transform.p.y() marker.pose.position.z = transform.p.z() quat = transform.M.GetQuaternion() marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] self.objects_map[name] = marker return marker
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 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 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 publish_sub_marker(self, pos, ori): marker = Marker() #marker.header.frame_id = "/base_footprint" marker.header.frame_id = "/base_link" marker.header.stamp = rospy.Time() marker.id = 0 marker.type = Marker.MESH_RESOURCE marker.action = Marker.ADD marker.pose.position.x = pos[0] marker.pose.position.y = pos[1] marker.pose.position.z = pos[2] marker.pose.orientation.x = ori[0] marker.pose.orientation.y = ori[1] marker.pose.orientation.z = ori[2] marker.pose.orientation.w = ori[3] marker.color.a = 1. marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 if self.model == 'chair': name = 'subject_model' marker.mesh_resource = "package://hrl_base_selection/models/wheelchair_and_body_assembly_rviz.STL" marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 elif self.model == 'bed': name = 'subject_model' marker.mesh_resource = "package://hrl_base_selection/models/head_bed.dae" marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 elif self.model == 'autobed': name = 'subject_model' marker.mesh_resource = "package://hrl_base_selection/models/bed_and_body_v3_rviz.dae" marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 else: print 'I got a bad model. What is going on???' return None vis_pub = rospy.Publisher(''.join(['~',name]), Marker, queue_size=1, latch=True) marker.ns = ''.join(['base_service_',name]) vis_pub.publish(marker) print 'Published a model of the subject to rviz'
def publish_subject_marker(self, pos, ori): marker = Marker() marker.header.frame_id = "/base_link" marker.header.stamp = rospy.Time() marker.ns = "base_service_subject_model" marker.id = 0 marker.type = Marker.MESH_RESOURCE; marker.action = Marker.ADD marker.pose.position.x = pos[0] marker.pose.position.y = pos[1] marker.pose.position.z = 0 marker.pose.orientation.x = ori[0] marker.pose.orientation.y = ori[1] marker.pose.orientation.z = ori[2] marker.pose.orientation.w = ori[3] marker.scale.x = .0254 marker.scale.y = .0254 marker.scale.z = .0254 marker.color.a = 1. marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 if self.model=='chair': name = 'wc_model' marker.mesh_resource = "package://hrl_base_selection/models/ADA_Wheelchair.dae" marker.scale.x = .0254 marker.scale.y = .0254 marker.scale.z = .0254 elif self.model=='bed': name = 'bed_model' marker.mesh_resource = "package://hrl_base_selection/models/head_bed.dae" marker.scale.x = 1.0 marker.scale.y = 1. marker.scale.z = 1.0 elif self.model=='autobed': name = 'autobed_model' marker.mesh_resource = "package://hrl_base_selection/models/bed_and_body_v3_rviz.dae" marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 else: print 'I got a bad model. What is going on???' return None self.vis_pub.publish(marker)
def create_object_marker(self, soma_obj, soma_type, pose): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "map" int_marker.name = soma_obj int_marker.description = "id" + soma_obj int_marker.pose = pose int_marker.pose.position.z = 0.01 mesh_marker = Marker() mesh_marker.type = Marker.MESH_RESOURCE mesh_marker.scale.x = 1 mesh_marker.scale.y = 1 mesh_marker.scale.z = 1 random.seed(soma_type) val = random.random() mesh_marker.color.r = r_func(val) mesh_marker.color.g = g_func(val) mesh_marker.color.b = b_func(val) mesh_marker.color.a = 1.0 #mesh_marker.pose = pose mesh_marker.mesh_resource = self.mesh[soma_type] # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE if self._interactive: int_marker.controls.append(copy.deepcopy(control)) # add the control to the interactive marker if self.marker[soma_type] == '3D': control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) # add menu control menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True menu_control.markers.append( mesh_marker) #makeBox(int_marker) ) int_marker.controls.append(menu_control) return int_marker
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 draw_object(object_name, object): marker = Marker() marker.header.frame_id = "/BASE" marker.header.stamp = rospy.Time.now() marker.ns = object_name marker.id = 0 marker.type = Marker.MESH_RESOURCE marker.action = Marker.ADD marker.mesh_resource = "package://dec_object_models/objects/%s/%s.obj" % (object_name, object.name) marker.pose = object.pose.pose.pose marker.scale.x, marker.scale.y, marker.scale.z = 1.0, 1.0, 1.0 marker.color.r, marker.color.g, marker.color.b, marker.color.a = (1.0, 0.0, 0.0, 0.5) marker.lifetime = rospy.Duration() mesh_pub.publish(marker)
def mesh_resource(frame, ns, idnum, color, mesh_uri, pose): m = Marker() m.header.frame_id = frame m.header.stamp = rospy.Time.now() m.ns = ns m.id = idnum m.type = Marker.MESH_RESOURCE m.action = Marker.ADD m.mesh_resource = mesh_uri m.pose = pose m.scale.x, m.scale.y, m.scale.z = 1.0, 1.0, 1.0 m.color.r, m.color.g, m.color.b, m.color.a = color m.lifetime = rospy.Duration() return m
def make_darci_ee_marker(ps, color, orientation = None, marker_array=None, control=None, mesh_id_start = 0, ns = "darci_ee", offset=-0.14): mesh_id = mesh_id_start # this is the palm piece mesh = Marker() mesh.ns = ns #mesh.mesh_use_embedded_materials = True mesh.scale = Vector3(1.,1.,1.) mesh.color = ColorRGBA(*color) # stub_end_effector.dae # stub_end_effector_mini45.dae # tube_with_ati_collisions.dae # wedge_blender.dae # mesh.mesh_resource = "package://hrl_dynamic_mpc/meshes/tube_with_ati_collisions.dae" mesh.mesh_resource = "package://hrl_dynamic_mpc/meshes/Darci_Flipper.stl" mesh.type = Marker.MESH_RESOURCE #rot_default = tr.Ry(np.radians(-90))*tr.Rz(np.radians(90)) rot_default = tr.Ry(np.radians(0))*tr.Rz(np.radians(90)) if orientation == None: orientation = [0, 0, 0, 1] rot_buf = tr.quaternion_to_matrix(orientation) orientation = tr.matrix_to_quaternion(rot_buf*rot_default) mesh.pose.orientation = Quaternion(*orientation) mesh.pose.position.x = offset if control != None: control.markers.append( mesh ) elif marker_array != None: mesh.pose.position = ps.pose.position mesh.header.frame_id = ps.header.frame_id mesh.id = mesh_id marker_array.markers.append(mesh) if control != None: control.interaction_mode = InteractiveMarkerControl.BUTTON return control elif marker_array != None: return marker_array
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 callback(msgs): "msgs = ContactStatesStamped" global g_config if g_config.use_parent_link: urdf_robot = URDF.from_parameter_server() marker_array = MarkerArray() for msg, i in zip(msgs.states, range(len(msgs.states))): marker = Marker() link_name = msg.header.frame_id if g_config.use_parent_link: # lookup parent link chain = urdf_robot.get_chain(urdf_robot.get_root(), link_name) link_name = chain[-3] mesh_file, offset = find_mesh(link_name) marker.header.frame_id = link_name marker.header.stamp = rospy.Time.now() marker.type = Marker.MESH_RESOURCE if msg.state.state == ContactState.ON: marker.color.a = g_config.on_alpha marker.color.r = g_config.on_red marker.color.g = g_config.on_green marker.color.b = g_config.on_blue elif msg.state.state == ContactState.OFF: marker.color.a = g_config.off_alpha marker.color.r = g_config.off_red marker.color.g = g_config.off_green marker.color.b = g_config.off_blue marker.scale.x = g_config.marker_scale marker.scale.y = g_config.marker_scale marker.scale.z = g_config.marker_scale marker.pose = offset marker.mesh_resource = mesh_file marker.frame_locked = True marker.id = i if msg.state.state == ContactState.OFF: if not g_config.visualize_off: marker.action = Marker.DELETE marker_array.markers.append(marker) pub.publish(marker_array)
def createMeshMarker(resource, offset=(0,0,0), rgba=(1,0,0,1), orientation=(0,0,0,1), scale=1, scales=(1,1,1), frame_id="/map", marker_id = 0): marker = Marker() marker.mesh_resource = resource; marker.header.frame_id = frame_id marker.id = marker_id marker.type = marker.MESH_RESOURCE marker.scale.x = scale*scales[0] marker.scale.y = scale*scales[1] marker.scale.z = scale*scales[2] marker.color.a = rgba[3] marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.pose.position.x = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] 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)
def get_gripper_marker(self, pose_stamped, quality): m = Marker() m.header = pose_stamped.header m.ns = "gripper" m.id = self.marker_id self.marker_id += 1 m.type = m.MESH_RESOURCE m.action = m.ADD m.pose = pose_stamped.pose m.scale.x = 1.0 m.scale.y = 1.0 m.scale.z = 1.0 if quality < 0.5: m.color.r = quality * 2 m.color.g = 0 else: m.color.r = 0 m.color.g = (quality - 0.5) * 2 m.color.b = 0 m.color.a = 0.8 m.mesh_resource = "package://ubr1_description/meshes/gripper_link.STL" return m
def callback(msgs): "msgs = ContactStatesStamped" marker_array = MarkerArray() for msg, i in zip(msgs.states, range(len(msgs.states))): marker = Marker() mesh_file, offset = find_mesh(msg.header.frame_id) marker.header.frame_id = msg.header.frame_id marker.header.stamp = rospy.Time.now() marker.type = Marker.MESH_RESOURCE marker.color.a = alpha marker.color.r = rgb[0] marker.color.g = rgb[1] marker.color.b = rgb[2] marker.scale.x = scale marker.scale.y = scale marker.scale.z = scale marker.pose = offset marker.mesh_resource = mesh_file marker.frame_locked = True marker.id = i if msg.state.state == ContactState.OFF: marker.action = Marker.DELETE marker_array.markers.append(marker) pub.publish(marker_array)
def publish_wc_marker(self, pos, ori): marker = Marker() marker.header.frame_id = "/base_footprint" marker.header.stamp = rospy.Time() marker.ns = "base_service_wc_model" marker.id = 0 marker.type = Marker.MESH_RESOURCE; marker.action = Marker.ADD marker.pose.position.x = pos[0] marker.pose.position.y = pos[1] marker.pose.position.z = 0 marker.pose.orientation.x = ori[0] marker.pose.orientation.y = ori[1] marker.pose.orientation.z = ori[2] marker.pose.orientation.w = ori[3] marker.scale.x = .0254 marker.scale.y = .0254 marker.scale.z = .0254 marker.color.a = 1. marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.mesh_resource = "package://hrl_base_selection/models/ADA_Wheelchair.dae" self.vis_pub.publish(marker)
import math topic = "visualization_marker" publisher = rospy.Publisher(topic, Marker) rospy.init_node("register") markerArray = MarkerArray() count = 0 MARKERS_MAX = 100 while not rospy.is_shutdown(): marker = Marker() marker.mesh_resource = "package://apc_config/models/object_meshes/expo_dry_erase_board_eraser.stl" marker.header.frame_id = "/world" marker.type = marker.MESH_RESOURCE marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 1 publisher.publish(marker)
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 __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)
from std_msgs.msg import * from geometry_msgs.msg import Pose, Vector3, Point, Quaternion from visualization_msgs.msg import Marker MESH_FILENAME = 'package://grasper_plan/data/dae/'+sys.argv[1] if __name__ == '__main__': rospy.init_node('grasp_marker_pub') pub = rospy.Publisher('grasper_ctrl/marker_pub', Marker, queue_size=3) m = Marker() m.header.frame_id = 'graspable_object' m.header.stamp = rospy.Time() m.ns = '.'.join(sys.argv[1].split('.')[:-1]) # removes file extension from argv[1] m.id = 0 m.type = Marker.MESH_RESOURCE m.action = Marker.ADD m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1)) m.scale = Vector3(1,1,1) m.color = ColorRGBA(0.5,0.5,0.5,1) m.lifetime = rospy.Duration(3, 0) m.mesh_resource = MESH_FILENAME r = rospy.Rate(1) while not rospy.is_shutdown(): pub.publish(m) m.id += 1 # print m.id r.sleep()
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()