Python Marker.CUBE屬性代碼示例

# 需要導入模塊: from visualization_msgs.msg import Marker [as 別名]
# 或者: from visualization_msgs.msg.Marker import CUBE [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 CUBE [as 別名]
def to_marker(self, robot):
        marker = Marker()
        marker.header.frame_id = "map"
        marker.header.stamp = rospy.Time.now()
        marker.pose.position.x = self.x
        marker.pose.position.y = self.y
        marker.pose.position.z = 0

        quaternion = tf.transformations.quaternion_from_euler(0, 0, self.theta)
        marker.pose.orientation.x = quaternion[0]
        marker.pose.orientation.y = quaternion[1]
        marker.pose.orientation.z = quaternion[2]
        marker.pose.orientation.w = quaternion[3]

        marker.scale.x = robot.width
        marker.scale.y = robot.height
        marker.scale.z = 0.2

        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
        marker.color.a = 1.0

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

        return marker 

# 需要導入模塊: from visualization_msgs.msg import Marker [as 別名]
# 或者: from visualization_msgs.msg.Marker import CUBE [as 別名]
def sync_callback(msg1, msg2):
    # msg1: /image_raw   # msg2: /velodyne_points: velodyne_points
    func_start = time.time()
    timestamp1 = msg1.header.stamp.to_nsec()
    print('image_callback: msg : seq=%d, timestamp=%19d' % (msg1.header.seq, timestamp1))
    timestamp2 = msg2.header.stamp.to_nsec()
    print('velodyne_callback: msg : seq=%d, timestamp=%19d' % (msg2.header.seq, timestamp2))

    arr = msg_to_arr(msg2)
    lidar = np.array([[item[0], item[1], item[2], item[3]] for item in arr])

    camera_image = bridge.imgmsg_to_cv2(msg1, "bgr8")
    print("camera_image is {}".format(camera_image.shape))

    top_view = point_cloud_2_top(lidar, res=0.2, zres=0.5, side_range=(-45,45), fwd_range=(-45,45),
                                 height_range=(-3, 0.5))
    top_image = draw_top_image(top_view[:, :, -1])

    if 0:           # if show the images
        cemara_show_image = cv2.resize(camera_image,(camera_image.shape[1]//2, camera_image.shape[0]//2))
        top_show_image_width = camera_image.shape[0]//2
        top_show_image = cv2.resize(top_image,(top_show_image_width, top_show_image_width))
        show_image = np.concatenate((top_show_image, cemara_show_image), axis=1)
        cv2.imshow("top", show_image)
        cv2.waitKey(1)

    # use test data until round2 pipeline is ok
    np_reshape = lambda np_array: np_array.reshape(1, *(np_array.shape))
    top_view = np_reshape(top)
    front_view = np_reshape(front)
    rgb_view = np_reshape(rgb)

    np.save(os.path.join(sys.path[0], "../MV3D/data/", "top.npy"), top_view)
    np.save(os.path.join(sys.path[0], "../MV3D/data/", "rgb.npy"), rgb_view)

    start = time.time()
    boxes3d = rpc.predict()
    end = time.time()
    print("predict boxes len={} use predict time: {} seconds.".format(len(boxes3d), end-start))

    if len(boxes3d) > 0:
        translation, size, rotation = boxes3d_decompose(np.array(boxes3d))
        # publish (boxes3d) to tracker_node
        markerArray = MarkerArray()
        for i in range(len(boxes3d)):
            m = Marker()
            m.type = Marker.CUBE
            m.header.frame_id = "velodyne"
            m.header.stamp = msg2.header.stamp
            m.scale.x, m.scale.y, m.scale.z = size[i][0],size[i][1],size[i][2]
            m.pose.position.x, m.pose.position.y, m.pose.position.z = \
                translation[i][0], translation[i][1], translation[i][2]
            m.pose.orientation.x, m.pose.orientation.y, m.pose.orientation.z, m.pose.orientation.w = \
                rotation[i][0], rotation[i][1], rotation[i][2], 0.
            m.color.a, m.color.r, m.color.g, m.color.b = \
                1.0, 0.0, 1.0, 0.0
            markerArray.markers.append(m)
        pub.publish(markerArray)

    func_end = time.time()
    print("sync_callback use {} seconds".format(func_end - func_start)) 

# 需要導入模塊: from visualization_msgs.msg import Marker [as 別名]
# 或者: from visualization_msgs.msg.Marker import CUBE [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 CUBE [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 CUBE [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 CUBE [as 別名]
def _img_callback(self, msg):
        frame_index = self.timestamp_map[msg.header.stamp.to_nsec()]
        for i, f in enumerate(self.frame_map[frame_index]):
            trans = f.trans
            rotq = f.rotq
            h, w, l = f.size
            if f.gt:
                # This is a bit hackish but will show one (the first) gt with fixed color
                # and will show multiple predicted objects, but colors per object will change
                # if the number of objects predicted changes.
                bid = 16
                oid = 17
                color = kelly_colors_dict['very_light_blue']
            else:
                bid = i * 2
                oid = i * 2 + 1
                color = kelly_colors_list[i % len(kelly_colors_list)]

            if self.sphere:
                s = max(l, w, h)
                self._publish_marker(
                    Marker.SPHERE, bid, msg.header.stamp,
                    trans, rotq, [s, s, s],
                    color)
                self._publish_marker(
                    Marker.ARROW, oid, msg.header.stamp,
                    trans, rotq, [l/2, w/2, h/2],
                    color)    
            else:
                #FIXME change Marker types based on object type (ie car vs pedestrian)
                if f.object_type == 'Pedestrian':
                    self._publish_marker(
                        Marker.CYLINDER, bid, msg.header.stamp,
                        trans, rotq, [w, w, h],
                        color)
                    self._publish_marker(
                        Marker.ARROW, oid, msg.header.stamp,
                        trans, rotq, [w/2, w/2, w/2],
                        color)
                else:
                    self._publish_marker(
                        Marker.CUBE, bid, msg.header.stamp,
                        trans, rotq, [l, w, h],
                        color)
                    self._publish_marker(
                        Marker.ARROW, oid, msg.header.stamp,
                        trans, rotq, [l/2, w/2, h/2],
                        color)

            self._send_transform(trans, rotq, i) 

# 需要導入模塊: from visualization_msgs.msg import Marker [as 別名]
# 或者: from visualization_msgs.msg.Marker import CUBE [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)