utils.py 文件源码

def publish_speeds(self, duration=0.0, scale=0.7):
        should_publish = len(self.speed_profile) > 1
        if self.visualize and self.speed_pub.get_num_connections() > 0:
            if self.dirty():
                self.make_np_array()
            markers = [marker_clear_all("/map")]
            normed_speeds = np.array(self.speed_profile) / np.max(self.speed_profile)
            last_speed = 0.0
            for i, speed in enumerate(normed_speeds):
                if speed >= last_speed * 0.99:
                    color = ColorRGBA(0, 1, 0, 0.8)
                else:
                    color = ColorRGBA(1, 0, 0, 0.8)
                last_speed = speed
                markers.append(marker_from_point_radius(self.np_points[i,:], np.power(speed, 0.8) * scale,
                    index=i, linewidth=0.05, color=color, lifetime=duration))

            marker_array = MarkerArray(markers=markers)
            self.speed_pub.publish(marker_array)