Python Marker.ADD属性代码示例

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ADD [as 别名]
def publishShape(self, position, rx, ry, tag, namespace):
        marker = Marker()
        marker.header.frame_id = '/map'
        marker.header.stamp = rospy.Time.now()

        marker.ns = namespace
        marker.id = int(tag)
        marker.action = Marker.ADD
        marker.type = self.shape

        self.setPosition(marker, position)
        self.setRadius(marker, rx, ry)
        self.setColor(marker, tag)

        marker.lifetime = rospy.Duration(5)

        self.publisher.publish(marker) 

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ADD [as 别名]
def publishShape(self, position, measurement, tag, namespace):
        marker = Marker()
        marker.header.frame_id = '/map'
        marker.header.stamp = rospy.Time.now()

        marker.ns = namespace
        marker.id = int(tag)
        marker.action = Marker.ADD
        marker.type = self.shape

        self.setPosition(marker, position)
        self.setRadius(marker, measurement)
        self.setColor(marker, tag)

        marker.lifetime = rospy.Duration(5)

        self.publisher.publish(marker) 

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ADD [as 别名]
def handle_image_msg(msg):
    assert msg._type == 'sensor_msgs/Image'
    
    with g_fusion_lock:
        g_fusion.filter(EmptyObservation(msg.header.stamp.to_sec()))
                    
        if g_fusion.last_state_mean is not None:
            pose = g_fusion.lidar_observation_function(g_fusion.last_state_mean)
                        
            yaw_q = ros_tf.transformations.quaternion_from_euler(0, 0, pose[3])
            br = ros_tf.TransformBroadcaster()
            br.sendTransform(tuple(pose[:3]), tuple(yaw_q), rospy.Time.now(), 'obj_fuse_centroid', 'velodyne')
            
            if last_known_box_size is not None:
                # bounding box
                marker = Marker()
                marker.header.frame_id = "obj_fuse_centroid"
                marker.header.stamp = rospy.Time.now()
                marker.type = Marker.CUBE
                marker.action = Marker.ADD
                marker.scale.x = last_known_box_size[2]
                marker.scale.y = last_known_box_size[1]
                marker.scale.z = last_known_box_size[0]
                marker.color = ColorRGBA(r=0., g=1., b=0., a=0.5)
                marker.lifetime = rospy.Duration()
                pub = rospy.Publisher("obj_fuse_bbox", Marker, queue_size=10)
                pub.publish(marker) 

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ADD [as 别名]
def create_markers(self):
       # Set up our waypoint markers
        marker_scale        = 0.6
        marker_lifetime     = 0 # 0 is forever
        marker_ns           = 'waypoints'
        marker_id           = 0
        marker_color        = {'r': 0.2, 'g': 0.8, 'b': 1.0, 'a': 1.0}
        
        # Define a marker publisher list.
        waypoint_name_list       = list()
        waypoint_name_list       = self.waypoint_list.keys()

        for waypoint in self.waypoint_list:

            marker                  = Marker()
            marker.ns               = marker_ns
            marker.id               = marker_id
            marker.type             = Marker.TEXT_VIEW_FACING
            marker.action           = Marker.ADD
            marker.lifetime         = rospy.Duration(marker_lifetime)
            marker.scale.x          = marker_scale
            marker.scale.y          = marker_scale
            marker.scale.z          = marker_scale

            marker.color.r          = marker_color['r']
            marker.color.g          = marker_color['g']
            marker.color.b          = marker_color['b']
            marker.color.a          = marker_color['a']

            marker.header.frame_id  = 'map'
            marker.header.stamp     = rospy.Time.now()
            marker.pose             = copy.deepcopy(self.waypoint_list[waypoint])
            marker.pose.position.z  = 0.65
            marker.text             = waypoint

            self.makerArray.markers.append(marker)

            marker_id               = marker_id + 1

#thread -> publish marks 

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ADD [as 别名]
def init_markers(self):
        # Set up our waypoint markers
        marker_scale = 0.2
        marker_lifetime = 0 # 0 is forever
        marker_ns = 'waypoints'
        marker_id = 0
        marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}
        
        # Define a marker publisher.
        self.marker_pub = rospy.Publisher('waypoint_markers', Marker, queue_size=5)
        
        # Initialize the marker points list.
        self.markers = Marker()
        self.markers.ns = marker_ns
        self.markers.id = marker_id
        self.markers.type = Marker.CUBE_LIST
        self.markers.action = Marker.ADD
        self.markers.lifetime = rospy.Duration(marker_lifetime)
        self.markers.scale.x = marker_scale
        self.markers.scale.y = marker_scale
        self.markers.color.r = marker_color['r']
        self.markers.color.g = marker_color['g']
        self.markers.color.b = marker_color['b']
        self.markers.color.a = marker_color['a']
        
        self.markers.header.frame_id = 'odom'
        self.markers.header.stamp = rospy.Time.now()
        self.markers.points = list() 

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ADD [as 别名]
def addWaypointMarker(self, keyPoint):
        rospy.loginfo('Publishing marker')
        # Set up our waypoint markers
        marker_scale = 0.2
        marker_lifetime = 0 # 0 is forever
        marker_ns = 'waypoints'
        marker_id = keyPoint.id
        marker_color = {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0}
        
        # Initialize the marker points list.
        self.waypoint_markers = Marker()
        self.waypoint_markers.ns = marker_ns
        self.waypoint_markers.id = marker_id
        self.waypoint_markers.type = Marker.CUBE_LIST
        self.waypoint_markers.action = Marker.ADD
        self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime)
        self.waypoint_markers.scale.x = marker_scale
        self.waypoint_markers.scale.y = marker_scale
        self.waypoint_markers.color.r = marker_color['r']
        self.waypoint_markers.color.g = marker_color['g']
        self.waypoint_markers.color.b = marker_color['b']
        self.waypoint_markers.color.a = marker_color['a']
        
        self.waypoint_markers.header.frame_id = 'map'
        self.waypoint_markers.header.stamp = rospy.Time.now()
        self.waypoint_markers.points = list()
        p = Point(keyPoint.pose.position.x, keyPoint.pose.position.y, keyPoint.pose.position.z)
        self.waypoint_markers.points.append(p)

        # Publish the waypoint markers
        self.marker_pub = rospy.Publisher('waypoint_markers', Marker)

        self.marker_pub.publish(self.waypoint_markers) 

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ADD [as 别名]
def init_waypoint_markers(self):
    # Set up our waypoint markers
    marker_scale = 0.2
    marker_lifetime = 0 # 0 is forever
    marker_ns = 'waypoints'
    marker_id = 0
    marker_color = {'r': 1.0, 'g': 0.0, 'b': 0.0, 'a': 1.0}
    
    # Define a marker publisher.
    self.marker_pub = rospy.Publisher('waypoint_markers', Marker)
    
    # Initialize the marker points list.
    self.waypoint_markers = Marker()
    self.waypoint_markers.ns = marker_ns
    self.waypoint_markers.id = marker_id
    self.waypoint_markers.type = Marker.CUBE_LIST
    self.waypoint_markers.action = Marker.ADD
    self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime)
    self.waypoint_markers.scale.x = marker_scale
    self.waypoint_markers.scale.y = marker_scale
    self.waypoint_markers.color.r = marker_color['r']
    self.waypoint_markers.color.g = marker_color['g']
    self.waypoint_markers.color.b = marker_color['b']
    self.waypoint_markers.color.a = marker_color['a']
    
    self.waypoint_markers.header.frame_id = 'map'
    self.waypoint_markers.header.stamp = rospy.Time.now()
    self.waypoint_markers.points = list() 

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ADD [as 别名]
def simple_obs(self):
        self.no_obs()
        pose_stamped = geometry_msgs.msg.PoseStamped()
        pose_stamped.header.frame_id = "/world_link"
        pose_stamped.header.stamp = rospy.Time(0)

        pose_stamped.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.25, 0.2)) )
        self.scene.add_box("obs1", pose_stamped,(0.1,0.1,1))
        self.scene.add_box("box1", pose_stamped,(0.05, 0.05, 0.95))

        obs = MarkerArray()
        obj = Marker()
        obj.header.frame_id = "/world_link"
        obj.header.stamp = rospy.Time(0)
        obj.ns = 'obstacle'
        obj.id = 1
        obj.type = Marker.CUBE
        obj.action = Marker.ADD
        obj.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.25, 0.2)) )
        obj.scale.x = 0.1
        obj.scale.y = 0.1
        obj.scale.z = 1.0
        obj.color.r = 0.0
        obj.color.g = 1.0
        obj.color.b = 0.0
        obj.color.a = 1.0
        obs.markers.append(obj)
        self.marker_pub.publish(obs) 

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ADD [as 别名]
def handle_msg_car(msg, who):
    assert isinstance(msg, Odometry)

    global last_cap_r, last_cap_f, last_cap_yaw

    if who == 'cap_r':
        last_cap_r = rtk_position_to_numpy(msg)
    elif who == 'cap_f' and last_cap_r is not None:
        cap_f = rtk_position_to_numpy(msg)
        cap_r = last_cap_r

        last_cap_f = cap_f
        last_cap_yaw = get_yaw(cap_f, cap_r)
    elif who == 'obs_r' and last_cap_f is not None and last_cap_yaw is not None:
        md = None
        for obs in metadata:
            if obs['obstacle_name'] == 'obs1':
                md = obs
        assert md, 'obs1 metadata not found'

        # find obstacle rear RTK to centroid vector
        lrg_to_gps = [md['rear_gps_l'], -md['rear_gps_w'], md['rear_gps_h']]
        lrg_to_centroid = [md['l'] / 2., -md['w'] / 2., md['h'] / 2.]
        obs_r_to_centroid = np.subtract(lrg_to_centroid, lrg_to_gps)

        # in the fixed GPS frame 
        cap_f = last_cap_f
        obs_r = rtk_position_to_numpy(msg)
        
        # in the capture vehicle front RTK frame
        cap_to_obs = np.dot(rotMatZ(-last_cap_yaw), obs_r - cap_f)
        cap_to_obs_centroid = cap_to_obs + obs_r_to_centroid

        br = tf.TransformBroadcaster()
        br.sendTransform(tuple(cap_to_obs_centroid), (0,0,0,1), rospy.Time.now(), 'obs_centroid', 'gps_antenna_front')

        # publish obstacle bounding box
        marker = Marker()
        marker.header.frame_id = "obs_centroid"
        marker.header.stamp = rospy.Time.now()

        marker.type = Marker.CUBE
        marker.action = Marker.ADD

        marker.scale.x = md['l']
        marker.scale.y = md['w']
        marker.scale.z = md['h']

        marker.color.r = 0.2
        marker.color.g = 0.5
        marker.color.b = 0.2
        marker.color.a = 0.5

        marker.lifetime = rospy.Duration()

        pub = rospy.Publisher("obs_bbox", Marker, queue_size=10)
        pub.publish(marker) 

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ADD [as 别名]
def handle_radar_msg(msg, dont_fuse):
    assert msg._md5sum == '6a2de2f790cb8bb0e149d45d297462f8'
    
    publish_rviz_topics = True
    
    with g_fusion_lock:
        # do we have any estimation?
        if g_fusion.last_state_mean is not None:
            pose = g_fusion.lidar_observation_function(g_fusion.last_state_mean)
    
            observations = RadarObservation.from_msg(msg, RADAR_TO_LIDAR, 0.9115)
            
            # find nearest observation to current object position estimation
            distance_threshold = 4.4
            nearest = None
            nearest_dist = 1e9
            for o in observations:
                dist = [o.x - pose[0], o.y - pose[1], o.z - pose[2]]
                dist = np.sqrt(np.array(dist).dot(dist))
                
                if dist < nearest_dist and dist < distance_threshold:
                    nearest_dist = dist
                    nearest = o
            
            if nearest is not None:
                # FUSION
                if not dont_fuse:
                    g_fusion.filter(nearest)
                
                if publish_rviz_topics:
                    header = Header()
                    header.frame_id = '/velodyne'
                    header.stamp = rospy.Time.now()
                    point = np.array([[nearest.x, nearest.y, nearest.z]], dtype=np.float32)
                    rospy.Publisher(name='obj_radar_nearest',
                      data_class=PointCloud2,
                      queue_size=1).publish(pc2.create_cloud_xyz32(header, point))
                    
                    #pose = g_fusion.lidar_observation_function(g_fusion.last_state_mean)
                    
                    #br = ros_tf.TransformBroadcaster()
                    #br.sendTransform(tuple(pose), (0,0,0,1), rospy.Time.now(), 'car_fuse_centroid', 'velodyne')
                    
#                     if last_known_box_size is not None:
#                         # bounding box
#                         marker = Marker()
#                         marker.header.frame_id = "car_fuse_centroid"
#                         marker.header.stamp = rospy.Time.now()
#                         marker.type = Marker.CUBE
#                         marker.action = Marker.ADD
#                         marker.scale.x = last_known_box_size[2]
#                         marker.scale.y = last_known_box_size[1]
#                         marker.scale.z = last_known_box_size[0]
#                         marker.color = ColorRGBA(r=1., g=1., b=0., a=0.5)
#                         marker.lifetime = rospy.Duration()
#                         pub = rospy.Publisher("car_fuse_bbox", Marker, queue_size=10)
#                         pub.publish(marker) 

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ADD [as 别名]
def init_markers(self):
       # Set up our waypoint markers
        marker_scale 		= 0.6
        marker_lifetime 	= 0 # 0 is forever
        marker_ns 			= 'waypoints'
        marker_id 			= 0
        marker_color 		= {'r': 1.0, 'g': 0.0, 'b': 0.0, 'a': 1.0}
        
        # Define a marker publisher list.
        location_list 		= list()
        location_list		= self.locations.keys()
        self.marker_pub_list = list()
        for i in location_list:
            self.marker_pub_list.append(rospy.Publisher('waypoint_' + i, Marker, queue_size=5))

        # Define a marker list.
        self.marker_list = list()
        for location in self.locations:

            marker                  = Marker()
            marker.ns               = marker_ns
            marker.id               = marker_id
            marker.type             = Marker.TEXT_VIEW_FACING
            marker.action           = Marker.ADD
            marker.lifetime         = rospy.Duration(marker_lifetime)
            marker.scale.x          = marker_scale
            marker.scale.y          = marker_scale
            marker.scale.z          = marker_scale

            marker.color.r          = marker_color['r']
            marker.color.g          = marker_color['g']
            marker.color.b          = marker_color['b']
            marker.color.a          = marker_color['a']

            marker.header.frame_id  = 'map'
            marker.header.stamp     = rospy.Time.now()

            # rospy.loginfo(self.locations['foyer'])
            marker.pose             = copy.deepcopy(self.locations[location])
            marker.pose.position.z  = 0.65
            marker.text             = location

            # rospy.loginfo(self.locations['foyer'])
            self.marker_list.append(marker)
    
    # Create the waypoints list from txt 

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ADD [as 别名]
def complex_obs(self):
        self.no_obs()
        pose_stamped = geometry_msgs.msg.PoseStamped()
        pose_stamped.header.frame_id = "/world_link"
        pose_stamped.header.stamp = rospy.Time(0)
        pose_stamped.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.25, 0.4)) )
        self.scene.add_box("obs1", pose_stamped,(0.1,0.1,0.8))
        self.scene.add_box("box1", pose_stamped,(0.05,0.05,0.75))
        pose_stamped.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.0, 0.8)) )
        self.scene.add_box("obs2", pose_stamped,(0.1,0.5,0.1))
        self.scene.add_box("box2", pose_stamped,(0.05,0.45,0.05))

        obs = MarkerArray()
        obj1 = Marker()
        obj1.header.frame_id = "/world_link"
        obj1.header.stamp = rospy.Time(0)
        obj1.ns = 'obstacle'
        obj1.id = 1
        obj1.type = Marker.CUBE
        obj1.action = Marker.ADD
        obj1.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.25, 0.4)) )
        obj1.scale.x = 0.1
        obj1.scale.y = 0.1
        obj1.scale.z = 0.8
        obj1.color.r = 0.0
        obj1.color.g = 1.0
        obj1.color.b = 0.0
        obj1.color.a = 1.0
        obs.markers.append(obj1)

        obj2 = Marker()
        obj2.header.frame_id = "/world_link"
        obj2.header.stamp = rospy.Time(0)
        obj2.ns = 'obstacle'
        obj2.id = 2
        obj2.type = Marker.CUBE
        obj2.action = Marker.ADD
        obj2.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.0, 0.8)) )
        obj2.scale.x = 0.1
        obj2.scale.y = 0.5
        obj2.scale.z = 0.1
        obj2.color.r = 0.0
        obj2.color.g = 1.0
        obj2.color.b = 0.0
        obj2.color.a = 1.0
        obs.markers.append(obj2)

        self.marker_pub.publish(obs)