VisualizationTool.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2022 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#ifndef GZ_SIM_SYSTEMS_ENVIRONMENTPRELOAD_VIZTOOL_HH_
#define GZ_SIM_SYSTEMS_ENVIRONMENTPRELOAD_VIZTOOL_HH_
 
#include <atomic>
#include <memory>
#include <mutex>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include <gz/common/CSVStreams.hh>
#include <gz/common/DataFrame.hh>
 
#include <gz/transport/Node.hh>
 
#include <gz/msgs/float_v.pb.h>
#include <gz/msgs/pointcloud_packed.pb.h>
#include <gz/msgs/PointCloudPackedUtils.hh>
#include <gz/msgs/Utility.hh>
 
#include "gz/sim/components/Environment.hh"
#include "gz/sim/Util.hh"
 
using namespace gz;
using namespace sim;
 
namespace gz
{
namespace sim
{
inline namespace GZ_SIM_VERSION_NAMESPACE
{
 
class EnvironmentVisualizationTool
{
  public: EnvironmentVisualizationTool();
 
  private: std::mutex mutex;
 
  private: bool first{true};
 
  public: std::atomic<bool> resample{true};
 
  private: bool finishedTime{false};
 
  private: void CreatePointCloudTopics(
    const std::shared_ptr<components::EnvironmentalData> &_data,
    const UpdateInfo &_info);
 
  public: void FileReloaded();
 
  public: void Step(
    const UpdateInfo &_info,
    const EntityComponentManager &_ecm,
    const std::shared_ptr<components::EnvironmentalData> &_data,
    unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples);
 
  private: void Visualize(
    const std::shared_ptr<components::EnvironmentalData> &_data,
    unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples);
 
  private: void Publish();
 
  private: void ResizeCloud(
    const std::shared_ptr<components::EnvironmentalData> &_data,
    const EntityComponentManager &_ecm,
    unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples);
 
  private: transport::Node::Publisher pcPub;
 
  private: std::unordered_map<std::string, transport::Node::Publisher> pubs;
 
  private: std::unordered_map<std::string, gz::msgs::Float_V> floatFields;
 
  private: transport::Node node;
 
  private: gz::msgs::PointCloudPacked pcMsg;
 
  private: std::unordered_map<std::string,
    gz::math::InMemorySession<double, double>> sessions;
 
  private:  std::chrono::time_point<std::chrono::steady_clock> lastTick;
};
}
}
}
#endif