Python Marker.id方法代码示例

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
	def draw_rod(self, rod):
		marker = Marker()
		
		marker.header.frame_id = "/pl_base"
		marker.ns = "basic_shapes"
		marker.id = 10 + rod
		marker.type = marker.CYLINDER
		marker.action = marker.ADD
		marker.pose.position.x = self.rod_position[rod][0]
		marker.pose.position.y = self.rod_position[rod][1]
		marker.pose.position.z = self.position_z
		marker.pose.orientation.w = 1.0
		marker.scale.x = ROD_SCALE_XYZ[0]
		marker.scale.y = ROD_SCALE_XYZ[1]
		marker.scale.z = ROD_SCALE_XYZ[2]
		marker.color.r = ROD_COLOR_RGB[0]
		marker.color.g = ROD_COLOR_RGB[1]
		marker.color.b = ROD_COLOR_RGB[2]
		marker.color.a = 1.0

		self.rviz_pub.publish(marker)

		# pink top of the rod
		marker.id = 30 + rod
		marker.type = marker.SPHERE
		marker.pose.position.z = marker.pose.position.z + 0.0475
		marker.scale.z = TOP_SCALE_Z
		marker.color.r = TOP_COLOR_RGB[0]
		marker.color.g = TOP_COLOR_RGB[1]
		marker.color.b = TOP_COLOR_RGB[2]

		self.rviz_pub.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
    def publish_debug_info(self, footprint_point, distance, scan_point):
        array = MarkerArray()

        fp = Marker()
        fp.id = 1
        fp.type = fp.SPHERE
        scale = 0.1
        fp.pose.position.x,fp.pose.position.y, fp.pose.position.z = footprint_point[0], footprint_point[1], 0
        fp.scale.x, fp.scale.y, fp.scale.z = scale, scale, scale
        fp.color.r, fp.color.g, fp.color.b, fp.color.a = (0, 0, 1, 1)
        fp.header.frame_id = "{}/base_link".format(self.robot.robot_name)
        fp.frame_locked = True
        fp.action = fp.ADD
        fp.ns = "door_opening"
        array.markers += [fp]

        sp = Marker()
        sp.id = 2
        sp.type = sp.SPHERE
        scale = 0.1
        sp.pose.position.x,sp.pose.position.y, sp.pose.position.z = scan_point[0], scan_point[1], 0
        sp.scale.x, sp.scale.y, sp.scale.z = scale, scale, scale
        sp.color.r, sp.color.g, sp.color.b, sp.color.a = (1, 0, 0, 1)
        sp.header.frame_id = "{}/base_link".format(self.robot.robot_name)
        sp.frame_locked = False
        sp.action = sp.ADD
        sp.ns = "door_opening"
        array.markers += [sp]

        self.debug_vizualizer.publish(array)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
def visualize_traj(points):

    traj = Marker()
    traj.header.frame_id = FRAME
    traj.header.stamp = rospy.get_rostime()
    traj.ns = "love_letter"
    traj.action = Marker.ADD
    traj.pose.orientation.w = 1.0
    traj.type = Marker.LINE_STRIP
    traj.scale.x = 0.001 # line width
    traj.color.r = 1.0
    traj.color.b = 0.0
    traj.color.a = 1.0
    
    if(WRITE_MULTIPLE_SHAPES):
        traj.id = shapeCount;
    else:
        traj.id = 0; #overwrite any existing shapes
        traj.lifetime.secs = 1; #timeout for display
    
    traj.points = list(points)
    
    # use interactive marker from place_paper instead
    #pub_markers.publish(a4_sheet()) 
    pub_markers.publish(traj)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
   def plot_3d_vel(self, p_arr, v_arr, v_scale=1.0):

      marker_array = MarkerArray()

      for i in xrange(len(p_arr)):
	 p = p_arr[i]
	 v = vx,vy,vz = v_arr[i]
	 marker = Marker()
	 marker.header.frame_id = "/openni_rgb_optical_frame"
	 marker.type = marker.ARROW
	 marker.action = marker.ADD
	 marker.color.a = 1.0
	 marker.color.r = 1.0
	 marker.color.g = 0.0
	 marker.color.b = 0.0
	 marker.points.append(Point(p[0],p[1],p[2]))
	 marker.points.append(Point(p[0]+vx*v_scale,p[1]+vy*v_scale,p[2]+vz*v_scale)) 
	 marker.scale.x=0.05
	 marker.scale.y=0.1
	 marker.id = i 

	 marker_array.markers.append(marker)
      
      for i in xrange(len(p_arr),self.max_features):
	 marker = Marker()
	 marker.action = marker.DELETE
	 marker.id = i
	 marker_array.markers.append(marker)


      self.marker_pub.publish(marker_array)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
    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

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
    def make_marker(self, robot_id, x, y, ns):
        """ Create a Marker message with the given x,y coordinates """
        m = Marker()
        m.header.stamp = rospy.Time.now()
        m.header.frame_id = 'map'
        m.ns = ns
        m.action = m.ADD
        m.pose.position.x = x
        m.pose.position.y = y
        if robot_id == 0:
            m.color.r = 255
            m.color.g = 0
            m.color.b = 0
        else:
            m.color.r = 0
            m.color.g = 0
            m.color.b = 255
        m.color.a = 1.0

        if ns == 'mess':
            m.type = m.CUBE
            m.id = len(self.messes)
            m.scale.x = m.scale.y = m.scale.z = .15
            self.messes.append(m)
        else:
            m.type = m.SPHERE
            m.id = robot_id
            m.scale.x = m.scale.y = m.scale.z = .35
        return m

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
 def add_marker(self, array, track, color, tid):
     m1 = Marker()
     m1.header.frame_id = "camera_rgb_optical_frame"
     m1.ns = "line"
     m1.id = tid
     m1.type = 4 #lines
     m1.action = 0
     m1.scale.x = .002
     m1.color.a = 1.
     m1.color.r = color[0]/255.
     m1.color.g = color[1]/255.
     m1.color.b = color[2]/255.
     m1.points = track
     array.append(m1)
     m2 = Marker()
     m2.header.frame_id = "camera_rgb_optical_frame"
     m2.ns = "point"
     m2.id = tid
     m2.type = 8 #points
     m2.action = 0
     m2.scale.x = .008
     m2.scale.y = .008
     m2.color.a = 1.
     m2.color.r = color[0]/255.
     m2.color.g = color[1]/255.
     m2.color.b = color[2]/255.
     m2.points = [ track[-1] ]
     array.append(m2)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
def publish_cluster(publisher, points, frame_id, namespace, cluster_id):
    """Publishes a marker representing a cluster.
    The x and y arguments specify the center of the target.

    Args:
      publisher: A visualization_msgs/Marker publisher
      points: A list of geometry_msgs/Point
      frame_id: The coordinate frame in which to interpret the points.
      namespace: string, a unique name for a group of clusters.
      cluster_id: int, a unique number for this cluster in the given namespace.
    """
    marker = Marker()
    # TODO(jstn): Once the point clouds have the correct frame_id,
    # use them here.
    marker.header.frame_id = frame_id
    marker.header.stamp = rospy.Time().now()
    marker.ns = namespace
    marker.id = 2 * cluster_id
    marker.type = Marker.POINTS
    marker.action = Marker.ADD
    marker.color.r = random.random()
    marker.color.g = random.random()
    marker.color.b = random.random()
    marker.color.a = 0.5
    marker.points = points
    marker.scale.x = 0.002
    marker.scale.y = 0.002
    marker.lifetime = rospy.Duration()

    center = [0, 0, 0]
    for point in points:
        center[0] += point.x
        center[1] += point.y
        center[2] += point.z
    center[0] /= len(points)
    center[1] /= len(points)
    center[2] /= len(points)

    text_marker = Marker()
    text_marker.header.frame_id = frame_id
    text_marker.header.stamp = rospy.Time().now()
    text_marker.ns = namespace
    text_marker.id = 2 * cluster_id + 1
    text_marker.type = Marker.TEXT_VIEW_FACING
    text_marker.action = Marker.ADD
    text_marker.pose.position.x = center[0] - 0.1
    text_marker.pose.position.y = center[1]
    text_marker.pose.position.z = center[2]
    text_marker.color.r = 1
    text_marker.color.g = 1
    text_marker.color.b = 1
    text_marker.color.a = 1
    text_marker.scale.z = 0.05
    text_marker.text = '{}'.format(cluster_id)
    text_marker.lifetime = rospy.Duration()

    _publish(publisher, marker, "cluster")
    _publish(publisher, text_marker, "text_marker")
    return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
    def publishMap(self):
        markerArray = MarkerArray()
        marker = Marker()
        marker.header.frame_id = "/map"
        marker.type = marker.SPHERE_LIST
        marker.action = marker.ADD
        marker.scale.x = 0.05
        marker.scale.y = 0.05
        marker.scale.z = 0.05
        marker.color.a = 1.0
        marker.color.r = 1.0
        marker.color.g = 1.0
        marker.color.b = 0.0
        marker.pose.orientation.w = 1.0
        marker.pose.position.x = 0.0
        marker.pose.position.y = 0.0
        marker.pose.position.z = 0.0
        marker.id = 0
        for p in self.mapPoints:
            if p["eaten"]:
                continue
            if p["powerup"]:
                continue
            point = Point()
            point.x = p["x"]
            point.y = p["y"]
            point.z = 0.15
            marker.points.append(point)
        markerArray.markers.append(marker)

        marker = Marker()
        marker.header.frame_id = "/map"
        marker.type = marker.SPHERE_LIST
        marker.action = marker.ADD
        marker.scale.x = 0.3
        marker.scale.y = 0.3
        marker.scale.z = 0.3
        marker.color.a = 1.0
        marker.color.r = 1.0
        marker.color.g = 1.0
        marker.color.b = 0.0
        marker.pose.orientation.w = 1.0
        marker.pose.position.x = 0.0
        marker.pose.position.y = 0.0
        marker.pose.position.z = 0.0
        marker.id = 1
        for p in self.mapPoints:
            if p["eaten"]:
                continue
            if not p["powerup"]:
                continue
            point = Point()
            point.x = p["x"]
            point.y = p["y"]
            point.z = 0.2
            marker.points.append(point)
        markerArray.markers.append(marker)

        self.pubMap.publish(markerArray)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
    def create_force_marker(self, force, axis, frame_id):
        m = Marker()
        m.header.frame_id = frame_id
        m.type = m.CYLINDER

        m.scale.x = 0.02  # diameter
        m.scale.y = 0.02  # also diameter
        m.scale.z = abs(force) / self.force_to_length  # length
        # MarkerArray visualizer in Rviz complains if a scale is 0
        if m.scale.z == 0.0:
            m.scale.z = 0.0001
        # namespaced so we can disable axis
        m.ns = axis
        if axis == 'x':
            m.id = 555
            m.color.r = 1.0
            m.color.g = 0.0
            m.color.b = 0.0
            m.pose.position.x = m.scale.z / 2.0
            # Aligned with the X axis of the frame
            if force > 0:
                m.pose.orientation = Quaternion(
                    *quaternion_from_euler(0, radians(90), 0))
            else:  # if the force is negative the cylinder goes to the other side
                m.pose.orientation = Quaternion(
                    *quaternion_from_euler(0, radians(-90), 0))
                m.pose.position.x *= -1.0
        elif axis == 'y':
            m.id = 666
            m.color.r = 0.0
            m.color.g = 1.0
            m.color.b = 0.0
            m.pose.position.y = m.scale.z / 2.0
            # Aligned with the Y axis of the frame
            if force > 0:
                m.pose.orientation = Quaternion(
                    *quaternion_from_euler(radians(90), 0, 0))
            else:
                m.pose.orientation = Quaternion(
                    *quaternion_from_euler(radians(-90), 0, 0))
                m.pose.position.y *= -1.0
        elif axis == 'z':
            m.id = 777
            m.color.r = 0.0
            m.color.g = 0.0
            m.color.b = 1.0
            m.pose.position.z = m.scale.z / 2.0
            # Aligned with the Z axis of the frame
            if force > 0:
                m.pose.orientation = Quaternion(
                    *quaternion_from_euler(0, 0, 0))
            else:
                m.pose.orientation = Quaternion(
                    *quaternion_from_euler(radians(180), 0, 0))
                m.pose.position.z *= -1.0
        m.color.a = 1.0
        return m

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
    def visualize_cluster(self, cluster, label=None):
        points = pc2.read_points(cluster.pointcloud, skip_nans=True)
        point_list = [Point(x=x, y=y-0.3, z=z) for x, y, z, rgb in points]
        if len(point_list) == 0:
            rospy.logwarn('Point list was of size 0, skipping.')
            return

        marker_id = len(self._current_markers)

        marker = Marker()
        marker.header.frame_id = 'map'
        marker.header.stamp = rospy.Time().now()
        marker.ns = 'clusters'
        marker.id = marker_id
        marker.type = Marker.POINTS
        marker.action = Marker.ADD
        marker.color.r = random.random()
        marker.color.g = random.random()
        marker.color.b = random.random()
        marker.color.a = 0.5 + random.random()
        marker.points = point_list
        marker.scale.x = 0.002
        marker.scale.y = 0.002
        marker.lifetime = rospy.Duration()
        self.visualize_marker(marker)
    
        if label is not None:
            center = [0, 0, 0]
            for point in point_list:
                center[0] += point.x
                center[1] += point.y
                center[2] += point.z
            center[0] /= len(point_list)
            center[1] /= len(point_list)
            center[2] /= len(point_list)
    
            text_marker = Marker()
            text_marker.header.frame_id = 'map'
            text_marker.header.stamp = rospy.Time().now()
            text_marker.ns = 'labels'
            text_marker.id = marker_id + 1
            text_marker.type = Marker.TEXT_VIEW_FACING
            text_marker.action = Marker.ADD
            text_marker.pose.position.x = center[1] - 0.05
            text_marker.pose.position.y = center[1]
            text_marker.pose.position.z = center[2]
            text_marker.color.r = 1
            text_marker.color.g = 1
            text_marker.color.b = 1
            text_marker.color.a = 1
            text_marker.scale.z = 0.05
            text_marker.text = label
            text_marker.lifetime = rospy.Duration()
    
            self.visualize_marker(text_marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
    def publish_waypoint_markers(self):
        pub_waypoint_markers = rospy.Publisher('waypoint_markers', MarkerArray)
        marker_array = MarkerArray()
        for i in range(len(self.waypoints)):
            waypoint = self.waypoints[i]
            waypoint_marker = Marker()
            waypoint_marker.id = i
            waypoint_marker.header.frame_id = "/map"
            waypoint_marker.header.stamp = rospy.Time.now()
            if (waypoint.type == 'P'):
                waypoint_marker.type = 5  # Line List
                waypoint_marker.text = 'waypoint_%s_point' % i
                waypoint_marker.color.r = 176.0
                waypoint_marker.color.g = 224.0
                waypoint_marker.color.b = 230.0
                waypoint_marker.color.a = 0.5
                waypoint_marker.scale.x = 0.5
                c = waypoint.coordinate
                waypoint_marker.points.append(Point(c.x, c.y, c.z))
                waypoint_marker.points.append(Point(c.x, c.y, c.z + 3.0))
            else:
                waypoint_marker.type = 3  # Cylinder
                waypoint_marker.text = 'waypoint_%s_cone' % i
                waypoint_marker.color.r = 255.0
                waypoint_marker.color.g = 69.0
                waypoint_marker.color.b = 0.0
                waypoint_marker.color.a = 1.0
                waypoint_marker.scale.x = 0.3
                waypoint_marker.scale.y = 0.3
                waypoint_marker.scale.z = 0.5
                waypoint_marker.pose.position = waypoint.coordinate
            marker_array.markers.append(waypoint_marker)

        if self.current_waypoint_idx != len(self.waypoints):
            current_waypoint_marker = Marker()
            current_waypoint_marker.id = 999
            current_waypoint_marker.header.frame_id = "/map"
            current_waypoint_marker.header.stamp = rospy.Time.now()
            current_waypoint_marker.type = 2  # Sphere
            current_waypoint_marker.text = 'current_waypoint'
            current_waypoint_marker.color.r = 255.0
            current_waypoint_marker.color.g = 0.0
            current_waypoint_marker.color.b = 0.0
            current_waypoint_marker.color.a = 1.0
            current_waypoint_marker.scale.x = 0.3
            current_waypoint_marker.scale.y = 0.3
            current_waypoint_marker.scale.z = 0.3
            current_waypoint = self.waypoints[self.current_waypoint_idx]
            current_waypoint_marker.pose.position.x = current_waypoint.coordinate.x
            current_waypoint_marker.pose.position.y = current_waypoint.coordinate.y
            current_waypoint_marker.pose.position.z = 1.0
            marker_array.markers.append(current_waypoint_marker)
 
        pub_waypoint_markers.publish(marker_array)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
def node():
    pub = rospy.Publisher('shapes', Marker, queue_size=10)
    rospy.init_node('plotter', anonymous=False)
    rate = rospy.Rate(1)
   

    points=Marker()
    line=Marker()
#Set the frame ID and timestamp.  See the TF tutorials for information on these.
    points.header.frame_id=line.header.frame_id="/map"
    points.header.stamp=line.header.stamp=rospy.Time.now()

    points.ns=line.ns = "markers"
    points.id = 0
    line.id =2

    points.type = Marker.POINTS
    line.type=Marker.LINE_STRIP
#Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    points.action = line.action = Marker.ADD;

    points.pose.orientation.w = line.pose.orientation.w = 1.0;

    line.scale.x = 0.02;
    points.scale.x=0.03; 
    line.scale.y= 0.02;
    points.scale.y=0.03; 

    line.color.r = 1.0;
    points.color.g = 1.0;
   
    points.color.a=line.color.a = 1.0;
    points.lifetime =line.lifetime = rospy.Duration();

    p=Point()
    p.x = 1;
    p.y = 1;
    p.z = 1;

    pp=[]
    
    pp.append(copy(p))
       
    while not rospy.is_shutdown():
        p.x+=0.1  
        pp.append(copy(p)) 
        points.points=pp
        #print points.points,'\n','----------------','\n'
        line.points=pp

        pub.publish(points)
        pub.publish(line)
        rate.sleep()

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
 def publish(self, target_frame, timestamp):
     ma = MarkerArray()
     for id in self.marker_list:
         marker = Marker()
         marker.header.stamp = timestamp
         marker.header.frame_id = target_frame
         marker.ns = "landmark_kf"
         marker.id = id
         marker.type = Marker.CYLINDER
         marker.action = Marker.ADD
         Lkf = self.marker_list[id]
         marker.pose.position.x = Lkf.L[0,0]
         marker.pose.position.y = Lkf.L[1,0]
         marker.pose.position.z = 0
         marker.pose.orientation.x = 0
         marker.pose.orientation.y = 0
         marker.pose.orientation.z = 1
         marker.pose.orientation.w = 0
         marker.scale.x = max(3*sqrt(Lkf.P[0,0]),0.05)
         marker.scale.y = max(3*sqrt(Lkf.P[1,1]),0.05)
         marker.scale.z = 0.5;
         marker.color.a = 1.0;
         marker.color.r = 1.0;
         marker.color.g = 1.0;
         marker.color.b = 0.0;
         marker.lifetime.secs=3.0;
         ma.markers.append(marker)
         marker = Marker()
         marker.header.stamp = timestamp
         marker.header.frame_id = target_frame
         marker.ns = "landmark_kf"
         marker.id = 1000+id
         marker.type = Marker.TEXT_VIEW_FACING
         marker.action = Marker.ADD
         Lkf = self.marker_list[id]
         marker.pose.position.x = Lkf.L[0,0]
         marker.pose.position.y = Lkf.L[1,0]
         marker.pose.position.z = 1.0
         marker.pose.orientation.x = 0
         marker.pose.orientation.y = 0
         marker.pose.orientation.z = 1
         marker.pose.orientation.w = 0
         marker.text = str(id)
         marker.scale.x = 1.0
         marker.scale.y = 1.0
         marker.scale.z = 0.2
         marker.color.a = 1.0;
         marker.color.r = 1.0;
         marker.color.g = 1.0;
         marker.color.b = 1.0;
         marker.lifetime.secs=3.0;
         ma.markers.append(marker)
     self.marker_pub.publish(ma)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import id [as 别名]
    def pubViz(self, ast, bst):

        rate = rospy.Rate(10)

        for i in range(self.winSize):

            msgs = MarkerArray()
            
            #use1について
            msg = Marker()
            #markerのプロパティ
            msg.header.frame_id = 'camera_link'
            msg.header.stamp = rospy.Time.now()
            msg.ns = 'j1'
            msg.action = 0
            msg.id = 1
            msg.type = 8
            msg.scale.x = 0.1
            msg.scale.y = 0.1
            msg.color = self.carray[2]
            #ジョイントポイントを入れる処理
            for j1 in range(self.jointSize):
                point = Point()
                point.x = self.pdata[0][ast+i][j1][0]
                point.y = self.pdata[0][ast+i][j1][1]
                point.z = self.pdata[0][ast+i][j1][2]
                msg.points.append(point) 
            msg.pose.orientation.w = 1.0
            msgs.markers.append(msg)    
            
            msg = Marker()
            msg.header.frame_id = 'camera_link'
            msg.header.stamp = rospy.Time.now()
            msg.ns = 'j2'
            msg.action = 0
            msg.id = 2
            msg.type = 8
            msg.scale.x = 0.1
            msg.scale.y = 0.1
            msg.color = self.carray[1]

            for j2 in range(self.jointSize):
                point = Point()
                point.x = self.pdata[1][bst+i][j2][0]
                point.y = self.pdata[1][bst+i][j2][1]
                point.z = self.pdata[1][bst+i][j2][2]
                msg.points.append(point) 
            msg.pose.orientation.w = 1.0

            msgs.markers.append(msg)

            self.mpub.publish(msgs)
            rate.sleep()