camera_contains_plugin.cpp

Go to the documentation of this file.

 
#include <algorithm>
 
#include <yaml-cpp/yaml.h>
 
#include <gazebo_video_monitor_plugins/camera_contains_plugin.h>
#include <gazebo_video_monitor_plugins/internal/utils.h>
 
namespace gazebo {
 
CameraContainsPlugin::CameraContainsPlugin()
    : logger_prefix_(getClassName<CameraContainsPlugin>()),
      spinner_(1, &callback_queue_) {}
 
void CameraContainsPlugin::Load(physics::WorldPtr _world,
                                sdf::ElementPtr _sdf) {
  logger_prefix_ += _sdf->Get<std::string>("name") + ": ";
  world_ = _world;
 
  // Get the list of track models
  if (not _sdf->HasElement("trackedModels"))
    gzthrow(logger_prefix_ + "Failed to get trackedModels");
  YAML::Node node = YAML::Load(_sdf->Get<std::string>("trackedModels"));
  for (const auto &model : node)
    tracked_models_.push_back(model.as<std::string>());
 
  // Get the list of cameras
  if (not _sdf->HasElement("cameras"))
    gzthrow(logger_prefix_ + "Failed to get cameras");
  node = YAML::Load(_sdf->Get<std::string>("cameras"));
  for (const auto &camera : node) cameras_.push_back(camera.as<std::string>());
 
  // Get container pose
  if (not _sdf->HasElement("pose"))
    gzthrow(logger_prefix_ + "Failed to get pose");
  auto pose = _sdf->Get<ignition::math::Pose3d>("pose");
 
  // Get container size
  if (not _sdf->HasElement("size"))
    gzthrow(logger_prefix_ + "Failed to get size");
  auto size = _sdf->Get<ignition::math::Vector3d>("size");
 
  // Initialize container
  container_ = ignition::math::OrientedBoxd(size, pose);
 
  if (not ros::isInitialized()) {
    ROS_FATAL_STREAM(
        "A ROS node for Gazebo has not been initialized, unable to load "
        "plugin. Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' "
        "in the gazebo_ros package");
    return;
  }
 
  nh_ = boost::make_shared<ros::NodeHandle>();
  nh_->setCallbackQueue(&callback_queue_);
  spinner_.start();
 
  // Initialize publisher
  if (not _sdf->HasElement("topic"))
    gzthrow(logger_prefix_ + "Failed to get topic");
  publisher_ = nh_->advertise<gazebo_video_monitor_msgs::Strings>(
      _sdf->Get<std::string>("topic"), 10);
  msg_.names = cameras_;
 
  // Initialize container visualizer
  bool visualize = false;
  if (_sdf->HasElement("visualize")) visualize = _sdf->Get<bool>("visualize");
  if (visualize) {
    container_visualizer_ =
        std::make_shared<BoxMarkerVisualizer>(_sdf->Get<std::string>("name"));
    container_visualizer_->spawnMarker(4, size, pose);
  }
 
  // Get update rate
  if (not _sdf->HasElement("updateRate"))
    gzthrow(logger_prefix_ + "Failed to get updateRate");
  double update_rate = _sdf->Get<double>("updateRate");
  update_period_ = update_rate > 0 ? 1 / update_rate : 0;
 
  update_connection_ = event::Events::ConnectWorldUpdateBegin(
      std::bind(&CameraContainsPlugin::onUpdate, this, std::placeholders::_1));
}
 
bool CameraContainsPlugin::contains(const std::string &name) const {
  auto model = world_->ModelByName(name);
  if (not model) return false;
  auto position = model->WorldPose().Pos();
  return container_.Contains(position);
}
 
void CameraContainsPlugin::onUpdate(const common::UpdateInfo &info) {
  if (info.simTime - last_update_time_ < update_period_) return;
  last_update_time_ = info.simTime;
 
  bool contains_model = std::any_of(
      tracked_models_.begin(), tracked_models_.end(),
      [&](const auto &name) -> bool { return this->contains(name); });
 
  if (contains_model) {
    if (not contains_model_) {
      publisher_.publish(msg_);
      contains_model_ = true;
    }
  } else {
    contains_model_ = false;
  }
}
 
GZ_REGISTER_WORLD_PLUGIN(CameraContainsPlugin)
 
}  // namespace gazebo