Example #1
0
    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
Example #2
0
    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
Example #3
0
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]
Example #4
0
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]
Example #7
0
    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,))
Example #9
0
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
Example #10
0
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
Example #11
0
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()
Example #12
0
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
Example #13
0
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
Example #15
0
    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)
Example #16
0
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")
Example #17
0
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)
Example #18
0
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
Example #20
0
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)
Example #22
0
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
Example #23
0
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
Example #24
0
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)
Example #26
0
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
Example #27
0
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
Example #29
0
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")
Example #30
0
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)
Example #32
0
 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)
Example #33
0
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
Example #34
0
    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
Example #35
0
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
Example #36
0
    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)
Example #37
0
    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 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 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
Example #40
0
    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, ))
Example #41
0
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    
Example #42
0
 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)
Example #44
0
    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
Example #46
0
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)
Example #51
0
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)
Example #53
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)
Example #57
0
    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)
Example #58
0
    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)
Example #59
0
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()
Example #60
0
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()