def __init__(self, publish_name, xWid,yHei):
        self.tf_listener = tf.TransformListener()
        self.tf_name = ''

        self.bb_publisher = rospy.Publisher(publish_name, Convex_Space)
        self.marker_pub = rospy.Publisher('/look_for_this/found_it/bounding_pyramids/markers', Marker)
        
        self.xWid = xWid
        self.yHei = yHei

        self.aspectXtoZ = 1.0
        self.aspectYtoZ = 1.0
        
        self.K_dict = {}
        
       	global marker
	marker = Marker()
        marker.header.frame_id = "/map"
        marker.header.stamp = rospy.Time()
        marker.ns = ""
        marker.id = 0
        marker.type = 5 # 5 = LINELIST
        marker.action = 0 # 0 = ADD
        marker.pose.position.x = 0
        marker.pose.position.y = 0
        marker.pose.position.z = 0
        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 0.0
        marker.scale.x = 0.01
        marker.color.a = 1
        marker.color.r = 0
        marker.color.g = 0
        marker.color.b = 0
Example #2
0
    def __init__(self):
        self.layout = rospy.get_param('/thruster_layout/thrusters')  # Add thruster layout from ROS param set by mapper
        assert self.layout is not None, 'Could not load thruster layout from ROS param'

        '''
        Create MarkerArray with an arrow marker for each thruster at index node_id.
        The position of the marker is as specified in the layout, but the length of the arrow
        will be set at each thrust callback.
        '''
        self.markers = MarkerArray()
        for i in xrange(len(self.layout)):
            # Initialize each marker (color, scale, namespace, frame)
            m = Marker()
            m.header.frame_id = '/base_link'
            m.type = Marker.ARROW
            m.ns = 'thrusters'
            m.color.a = 0.8
            m.scale.x = 0.01  # Shaft diameter
            m.scale.y = 0.05  # Head diameter
            m.action = Marker.DELETE
            m.lifetime = rospy.Duration(0.1)
            self.markers.markers.append(m)
        for key, layout in self.layout.iteritems():
            # Set position and id of marker from thruster layout
            idx = layout['node_id']
            pt = numpy_to_point(layout['position'])
            self.markers.markers[idx].points.append(pt)
            self.markers.markers[idx].points.append(pt)
            self.markers.markers[idx].id = idx

        # Create publisher for marker and subscribe to thrust
        self.pub = rospy.Publisher('/thrusters/markers', MarkerArray, queue_size=5)
        self.thrust_sub = rospy.Subscriber('/thrusters/thrust', Thrust, self.thrust_cb, queue_size=5)
Example #3
0
def publish(anns, data):
    wall_list = WallList()
    marker_list = MarkerArray()    

    marker_id = 1
    for a, d in zip(anns, data):
        
        # Walls
        object = deserialize_msg(d.data, Wall)
        wall_list.obstacles.append(object)
        
        # Markers
        marker = Marker()
        marker.id = marker_id
        marker.header = a.pose.header
        marker.type = a.shape
        marker.ns = "wall_obstacles"
        marker.action = Marker.ADD
        marker.lifetime = rospy.Duration.from_sec(0)
        marker.pose = copy.deepcopy(a.pose.pose.pose)
        marker.scale = a.size
        marker.color = a.color

        marker_list.markers.append(marker)

        marker_id = marker_id + 1

    marker_pub = rospy.Publisher('wall_marker',    MarkerArray, latch=True, queue_size=1)
    wall_pub   = rospy.Publisher('wall_pose_list', WallList,    latch=True, queue_size=1)

    wall_pub.publish(wall_list)
    marker_pub.publish(marker_list)
    
    return
 def publish_prob2(self, waypoints, objects, probs):
     prob_msg = MarkerArray() 
     i = 0
     idx = 0
     n_waypoints = len(waypoints)
     n_objects = len(objects)        
     scaling_factor = max(probs) 
     current_probs = [0 for foo in objects]
     for node in self._topo_map:
         if node.name in waypoints:
             for j in range(0, n_objects):
                 marker = Marker()
                 marker.header.frame_id = 'map'
                 marker.id = idx
                 marker.type = Marker.CYLINDER
                 marker.action = Marker.ADD
                 marker.pose = node.pose
                 prob = probs[n_objects*i + j]
                 prob = prob/(scaling_factor)
                 print "AHAHHAHBHBHBHBHBHB", prob
                 marker.pose.position.z  = marker.pose.position.z + current_probs[j]
                 marker.scale.x = 1*prob
                 marker.scale.y = 1*prob                
                 marker.scale.z = 1*prob
                 current_probs[j] = current_probs[j] + prob + 0.1
                 marker.color.a = 1.0
                 marker.color.r = 1.0*prob
                 marker.color.g = 1.0*prob
                 marker.color.b = 1.0*prob
                 prob_msg.markers.append(marker)
                 idx = idx + 1
             i = i + 1
     self._prob_marker_pub.publish(prob_msg)    
Example #5
0
   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)


      self.marker_pub.publish(marker_array)
    def __setMarker(self, id, waypoint, colors = [1,0,0,0]):
        try:
            marker = Marker()
            marker.header.frame_id = '/map'
            marker.header.stamp = rospy.Time.now()
            marker.ns = 'patrol'
            marker.id = id
            marker.type = marker.SPHERE
            marker.action = marker.ADD
            marker.scale.x = 0.2
            marker.scale.y = 0.2
            marker.scale.z = 0.2
            marker.color.a = colors[0]
            marker.color.r = colors[1]
            marker.color.b = colors[2]
            marker.color.g = colors[3]

            marker.pose.orientation.w = 1.0
            marker.pose.position.x = waypoint.target_pose.pose.position.x
            marker.pose.position.y = waypoint.target_pose.pose.position.y
            marker.pose.position.z = waypoint.target_pose.pose.position.z

            return marker

        except Exception as ex:
            rospy.logwarn('PatrolNode.__setMarker- ', ex.message)
Example #7
0
	def publish_marker(self):
		# create marker
		marker = Marker()
		marker.header.frame_id = "/base_link"
		marker.header.stamp = rospy.Time.now()
		marker.ns = "color"
		marker.id = 0
		marker.type = 2 # SPHERE
		marker.action = 0 # ADD
		marker.pose.position.x = 0
		marker.pose.position.y = 0
		marker.pose.position.z = 1.5
		marker.pose.orientation.x = 0.0
		marker.pose.orientation.y = 0.0
		marker.pose.orientation.z = 0.0
		marker.pose.orientation.w = 1.0
		marker.scale.x = 0.1
		marker.scale.y = 0.1
		marker.scale.z = 0.1
		marker.color.a = self.color.a #Transparency
		marker.color.r = self.color.r
		marker.color.g = self.color.g
		marker.color.b = self.color.b
		# publish marker
		self.pub_marker.publish(marker)
Example #8
0
	def draw_base(self):
		marker = Marker()
		ratio = (self.rod_position[CENTER][1] - self.rod_position[LEFT][1]) / (self.rod_position[CENTER][0] - self.rod_position[LEFT][0]) 
		alpha = numpy.arctan(ratio) - numpy.pi / 2.0

		marker.header.frame_id = "/pl_base"
		marker.ns = "basic_shapes"
		marker.id = 5
		marker.type = marker.CUBE
		marker.action = marker.ADD
		marker.pose.position.x = self.rod_position[CENTER][0]
		marker.pose.position.y = self.rod_position[CENTER][1]
		marker.pose.position.z = self.position_z + DIFF_DISTANCE_BASE
		marker.pose.orientation.x = 0.0
		marker.pose.orientation.y = 0.0		
		marker.pose.orientation.z = numpy.sin(alpha / 2.0)
		marker.pose.orientation.w = numpy.cos(alpha / 2.0) 		
		marker.scale.x = BASE_SCALE_XYZ[0]
		marker.scale.y = BASE_SCALE_XYZ[1]
		marker.scale.z = BASE_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)
Example #9
0
def createMarkerLine(pos_list, color = (.1, .1, 1), ID = 0, alpha = 1., size = 0.5):
    marker = Marker()
    marker.header.frame_id = "/openni_depth_frame"
    marker.id = ID
    marker.type = marker.LINE_STRIP
    marker.action = marker.ADD

    marker.scale.x = size
    marker.scale.y = size
    marker.scale.z = size
    marker.color.a = alpha

    marker.color.r = color[0]
    marker.color.g = color[1]
    marker.color.b = color[2]
    marker.pose.orientation.w = 1.0

    marker.pose.position.x = 0
    marker.pose.position.y = 0
    marker.pose.position.z = 0

    for p in pos_list:
        marker.points.append( Point(p[0],p[1],0) )

    return marker
Example #10
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 #11
0
    def line(self, frame_id, p, r, t=[0.0, 0.0], key=None):
        """
        line: t r + p

        This will be drawn for t[0] .. t[1]
        """

        r = np.array(r)
        p = np.array(p)

        m = Marker()
        m.header.frame_id = frame_id
        m.ns = key or 'lines'
        m.id = 0 if key else len(self.lines)
        m.type = Marker.LINE_STRIP
        m.action = Marker.MODIFY
        m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1))

        m.scale = Vector3(0.005, 0.005, 0.005)
        m.color = ColorRGBA(0,0.8,0.8,1)

        m.points = [Point(*(p+r*t)) for t in np.linspace(t[0], t[1], 10)]
        m.colors = [m.color] * 10

        key = key or element_key(m)

        with self.mod_lock:
            self.lines[key] = m

        return key
    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 #13
0
 def _delete_markers(self):
     marker = Marker()
     marker.action = 3
     marker.header.frame_id = "map"
     markerarray = MarkerArray()
     markerarray.markers.append(marker)
     self.markerpub.publish(markerarray)
Example #14
0
    def create_object_marker(self, soma_obj, roi, soma_type, pose,markerno):
        # create an interactive marker for our server
        marker = Marker()
        marker.header.frame_id = "map"
        #int_marker.name = soma_obj+'_'+str(markerno)
       #int_marker.description = soma_type + ' (' + roi +'_'+str(markerno)+  ')'
        marker.pose = pose
        marker.id = markerno;
       # print marker.pose
        marker.pose.position.z = 0.01


        #marker = Marker()
        marker.type = Marker.SPHERE
        marker.action = 0
        marker.scale.x = 0.25
        marker.scale.y = 0.25
        marker.scale.z = 0.25
        marker.pose.position.z = (marker.scale.z / 2)

        random.seed(soma_type)
        val = random.random()
        marker.color.r = r_func(val)
        marker.color.g = g_func(val)
        marker.color.b = b_func(val)
        marker.color.a = 1.0
        #marker.pose = pose

        return marker
 def init_int_marker(self):
     self.ims = InteractiveMarkerServer("simple_marker")
     self.im = InteractiveMarker()
     self.im.header.frame_id = "/ned"
     self.im.name = "my_marker"
     self.im.description = "Simple 1-DOF control"
     
     bm = Marker()
     bm.type = Marker.CUBE
     bm.scale.x = bm.scale.y = bm.scale.z = 0.45
     bm.color.r = 0.0
     bm.color.g = 0.5
     bm.color.b = 0.5
     bm.color.a = 1.0
     
     bc = InteractiveMarkerControl()
     bc.always_visible = True
     bc.markers.append(bm)
     
     self.im.controls.append(bc)
     
     rc = InteractiveMarkerControl()
     rc.name = "move_x"
     rc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
     
     self.im.controls.append(rc)
     
     self.ims.insert(self.im, self.process_marker_feedback)
     self.ims.applyChanges()
Example #16
0
def createArrowMarker(points, color, offset=(0,0,0), orientation=(0,0,0,1)):
    marker = Marker()
    marker.header.frame_id = "/map"
    marker.type = marker.ARROW
    marker.scale.x = 0.003
    marker.scale.y = 0.005
    marker.scale.z = 0
    
    n = len(points)//3
    marker.id = 2
    for i in range(n):
        p = Point()
        p.x = points[i*3+0]
        p.y = points[i*3+1]
        p.z = points[i*3+2]
        marker.points.append(p)
        
    marker.color.r = color[0]
    marker.color.g = color[1]
    marker.color.b = color[2]
    marker.color.a = color[3]
        
    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 #17
0
def publish(anns, data):
    ar_mk_list = AlvarMarkers()
    marker_list = MarkerArray()    

    marker_id = 1
    for a, d in zip(anns, data):
        
        # AR markers
        object = deserialize_msg(d.data, AlvarMarker)
        ar_mk_list.markers.append(object)
        
        # Visual markers
        marker = Marker()
        marker.id = marker_id
        marker.header = a.pose.header
        marker.type = a.shape
        marker.ns = "ar_mk_obstacles"
        marker.action = Marker.ADD
        marker.lifetime = rospy.Duration.from_sec(0)
        marker.pose = copy.deepcopy(a.pose.pose.pose)
        marker.scale = a.size
        marker.color = a.color

        marker_list.markers.append(marker)

        marker_id = marker_id + 1

    marker_pub = rospy.Publisher('ar_mk_marker',    MarkerArray, latch=True, queue_size=1)
    ar_mk_pub  = rospy.Publisher('ar_mk_pose_list', AlvarMarkers,latch=True, queue_size=1)

    ar_mk_pub.publish(ar_mk_list)
    marker_pub.publish(marker_list)
    
    return
Example #18
0
def createSphereMarker(color, scale, offset=(0,0,0), orientation=(0,0,0,1)):
    marker = Marker()
    marker.header.frame_id = "/map"
    marker.type = marker.SPHERE
    marker.scale.x = scale[0]
    marker.scale.y = scale[1]
    marker.scale.z = scale[2]
    marker.id = 1
        
    p = ColorRGBA()
    p.r = color[0]
    p.g = color[1]
    p.b = color[2]
    p.a = color[3]
    marker.color = p
        
    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 set_position_callback(self,marker,joy):
		print self.position_marker.ns
		print joy.buttons[3] == 1
		print marker.ns == "PEOPLE"
		if (self.position_marker.ns == "NONE" and joy.buttons[3] == 1 and marker.ns == "PEOPLE"):
			self.position_marker = marker
			print "set position"
			ref_marker = Marker()
			ref_marker.header.frame_id = "base_footprint"
			ref_marker.header.stamp = rospy.get_rostime()
			ref_marker.ns = "robot"
			ref_marker.type = 9
			ref_marker.action = 0
			ref_marker.pose.position.x = self.position_marker.pose.position.x
			ref_marker.pose.position.y = self.position_marker.pose.position.y
			ref_marker.pose.position.z = self.position_marker.pose.position.z
			ref_marker.scale.x = .25
			ref_marker.scale.y = .25
			ref_marker.scale.z = .25
			ref_marker.color.r = 1.0
			ref_marker.color.g = 0.0
			ref_marker.color.a = 1.0
			ref_marker.lifetime = rospy.Duration(0)
			self.ref_viz_pub.publish(ref_marker)
		else:
			pass
Example #20
0
def createPointMarker(points, colors, offset=(0,0,0), orientation=(0,0,0,1)):
    marker = Marker()
    marker.header.frame_id = "/map"
    marker.type = marker.POINTS
    marker.scale.x = 0.003
    marker.scale.y = 0.003
    marker.scale.z = 0.003
    marker.id = 1
    
    n = len(points)//3
    for i in range(n):
        p = Point()
        p.x = points[i*3+0]
        p.y = points[i*3+1]
        p.z = points[i*3+2]
        marker.points.append(p)
        
        p = ColorRGBA()
        p.r = colors[i*3+0]
        p.g = colors[i*3+1]
        p.b = colors[i*3+2]
        p.a = 0.5
        marker.colors.append(p)
        
    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 publish_marker(cls, x_cord, y_cord, mid):
     new_marker = Marker(type = Marker.CUBE, action=Marker.ADD)
     new_marker.header.frame_id = "/base_laser_front_link"
     new_marker.header.stamp = rospy.Time.now()
     new_marker.ns = "basic_shapes"
     new_marker.id = mid
     new_marker.pose.position.x = x_cord
     new_marker.pose.position.y = y_cord
     new_marker.pose.position.z = 0.0
     #pointxyz
     new_marker.pose.orientation.x = 0
     new_marker.pose.orientation.y = 0
     new_marker.pose.orientation.z = 0.0
     new_marker.pose.orientation.w = 1
     new_marker.scale.x = 0.005
     new_marker.scale.y = 0.005
     new_marker.scale.z = 0.005
     if mid == 0:
         new_marker.color.r = 1
     elif mid == 5:
         new_marker.color.g = 1
     elif mid == 10:
         new_marker.color.b = 1
     
     new_marker.color.a = 1
     new_marker.text = "marker"
     
     new_marker.lifetime = rospy.Duration(1)
     SmachGlobalData.pub_marker.publish(new_marker)
Example #22
0
    def location_marker(self, ID, location):
        marker = Marker()
        marker.header.frame_id = "torso"
        marker.ns = "headl"
        marker.type = marker.SPHERE
        marker.action = marker.ADD
        marker.scale.x = 0.1
        marker.scale.y = 0.1
        marker.scale.z = 0.1
        marker.color.a = 1.0

        if ID == self.trackerTarget:
            marker.color.r = 1.0
            marker.color.g = 0.0
            marker.color.b = 0.0
        else:
            marker.color.r = 0.1
            marker.color.g = 1.0
            marker.color.b = 0.0
        marker.pose.orientation.w = 1.0
        marker.pose.position.x = location[0]
        marker.pose.position.y = location[1]
        marker.pose.position.z = location[2]
        marker.lifetime.secs = 1
        marker.id = int(ID)
        return marker
 def publish_prob(self, waypoints, objects, probs):
     prob_msg = MarkerArray() 
     i = 0
     n_waypoints = len(waypoints)
     n_objects = len(objects)        
     scaling_factor = max(probs)      
     for node in self._topo_map:
         if node.name in waypoints:
             marker = Marker()
             marker.header.frame_id = 'map'
             marker.id = i
             marker.type = Marker.CYLINDER
             marker.action = Marker.ADD
             marker.pose = node.pose
             prob = 1
             for j in range(0, n_objects):
                 prob = prob*probs[n_objects*i + j]
             prob = prob/(scaling_factor**2)
             print prob
             marker.scale.x = 1*prob
             marker.scale.y = 1*prob                
             marker.scale.z = 1
             marker.color.a = 1.0
             marker.color.r = 0.0
             marker.color.g = 1.0
             marker.color.b = 0.0
             prob_msg.markers.append(marker)
             i = i + 1
     self._prob_marker_pub.publish(prob_msg)
Example #24
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 #25
0
 def new_marker(self, ns="/debug", frame="enu", time=None, type = Marker.CUBE , position=(0,0,0), orientation=(0,0,0,1), color=(1,0,0)):
     marker = Marker()
     marker.ns = ns
     if time != None:
         marker.header.stamp = time
     marker.header.frame_id = frame
     marker.type = type
     marker.action = marker.ADD
     marker.scale.x = 1.0
     marker.scale.y = 1.0
     marker.scale.z = 1.0
     marker.color.r = color[0]
     marker.color.g = color[1]
     marker.color.b = color[2]
     marker.color.a = 1.0
     marker.id = self.last_id
     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 = position[0]
     marker.pose.position.y = position[1]
     marker.pose.position.z = position[2]
     self.last_id += 1
     self.markers.markers.append(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
Example #27
0
    def create_marker(self, markerArray, view, pose, view_values):
        marker1 = Marker()
        marker1.id = self.marker_id
        self.marker_id += 1
        marker1.header.frame_id = "/map"
        marker1.type = marker1.TRIANGLE_LIST
        marker1.action = marker1.ADD
        marker1.scale.x = 1
        marker1.scale.y = 1
        marker1.scale.z = 2
        marker1.color.a = 0.25

        vals = view_values.values()
        max_val = max(vals)
        non_zero_vals = filter(lambda a: a != 0, vals)
        min_val = min(non_zero_vals)
        
        print min_val, max_val, view_values[view.ID]
        
        marker1.color.r = r_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1)))
        marker1.color.g = g_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1)))
        marker1.color.b = b_func( float((view_values[view.ID] - min_val)) /  float((max_val - min_val + 1)))

        marker1.pose.orientation = pose.orientation
        marker1.pose.position = pose.position
        marker1.points = [Point(0,0,0.01),Point(2.5,-1.39,0.01),Point(2.5,1.39,0.01)]
        
        markerArray.markers.append(marker1)
def createCubeMarker(offset=(0,0,0), marker_id = 0, rgba=(1,0,0,1), orientation=(0,0,0,1), scale=(0.1,0.1,0.1), frame_id="/map"):
    marker = Marker()
    marker.header.frame_id = frame_id
    marker.type = marker.CUBE
    marker.id = marker_id
    marker.scale.x = scale[0]
    marker.scale.y = scale[1]
    marker.scale.z = scale[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 #29
0
    def publishObstacles(self):
        """
        Publishes the obstacles as markers
        """
        mk = Marker()
        mk.header.stamp = rospy.get_rostime()
        mk.header.frame_id = '/base_link'

        mk.ns='basic_shapes'
        mk.id = 0
        mk.type = Marker.POINTS
        mk.scale.x = 0.3
        mk.scale.y = 0.3
        mk.scale.z = 0.3
        mk.color.r = 1.0
        mk.color.a = 1.0

        for value in self.obstacle_map.obstacles_in_memory:
            p = Point()
            p.x = value[0]
            p.y = value[1]
            mk.points.append(p)


        self.obs_pub.publish(mk)
Example #30
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