PopulationPlugin.cc

Go to the documentation of this file.

/*
 * Copyright (C) 2016 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.
 *
*/

#include <algorithm>
#include <mutex>
#include <ostream>
#include <string>
#include <vector>
#include <gazebo/common/Assert.hh>
#include <gazebo/common/Console.hh>
#include <gazebo/common/Events.hh>
#include <gazebo/math/Pose.hh>
#include <gazebo/msgs/gz_string.pb.h>
#include <gazebo/physics/Link.hh>
#include <gazebo/physics/PhysicsTypes.hh>
#include <gazebo/physics/World.hh>
#include <gazebo/transport/transport.hh>
#include <ignition/math/Matrix4.hh>
#include <sdf/sdf.hh>

#include "osrf_gear/PopulationPlugin.hh"

namespace gazebo
{
  struct PopulationPluginPrivate
  {
    public: physics::WorldPtr world;

    public: sdf::ElementPtr sdf;

    public: class Object
            {
             public: bool operator<(const Object &_obj) const
             {
               return this->time < _obj.time;
             }

             public: friend std::ostream &operator<<(std::ostream &_out,
                                                     const Object &_obj)
             {
               _out << _obj.type << std::endl;
               _out << "  Time: [" << _obj.time << "]" << std::endl;
               _out << "  Pose: [" << _obj.pose << "]" << std::endl;
               return _out;
             }

              public: double time;

              public: std::string type;

              public: math::Pose pose;
            };

    public: std::vector<Object> objects;

    public: std::vector<Object> initialObjects;

    public: event::ConnectionPtr connection;

    public: common::Time startTime;

    public: bool loopForever = false;

    public: physics::EntityPtr frame;

    public: transport::NodePtr node;

    public: transport::SubscriberPtr activationSub;

    public: bool enabled = false;

    public: std::mutex mutex;

    public: common::Time elapsedWhenPaused;
  };
}

using namespace gazebo;

GZ_REGISTER_WORLD_PLUGIN(PopulationPlugin)


PopulationPlugin::PopulationPlugin()
  : dataPtr(new PopulationPluginPrivate)
{
}

PopulationPlugin::~PopulationPlugin()
{
}

void PopulationPlugin::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
{
  GZ_ASSERT(_world, "PopulationPlugin world pointer is NULL");
  GZ_ASSERT(_sdf, "PopulationPlugin sdf pointer is NULL");
  this->dataPtr->world = _world;
  this->dataPtr->sdf = _sdf;

  // Read the SDF parameters
  if (_sdf->HasElement("loop_forever"))
  {
    sdf::ElementPtr loopElem = _sdf->GetElement("loop_forever");
    this->dataPtr->loopForever = loopElem->Get<bool>();
  }

  if (_sdf->HasElement("frame"))
  {
    std::string frameName = _sdf->Get<std::string>("frame");
    this->dataPtr->frame = this->dataPtr->world->GetEntity(frameName);
    if (!this->dataPtr->frame) {
      gzthrow(std::string("The frame '") + frameName + "' does not exist");
    }
    if (!this->dataPtr->frame->HasType(physics::Base::LINK) &&
      !this->dataPtr->frame->HasType(physics::Base::MODEL))
    {
      gzthrow("'frame' tag must list the name of a link or model");
    }
  }

  if (!_sdf->HasElement("object_sequence"))
  {
    gzerr << "PopulationPlugin: Unable to find <object_sequence> element\n";
    return;
  }

  sdf::ElementPtr sequence = _sdf->GetElement("object_sequence");

  sdf::ElementPtr objectElem = NULL;
  if (sequence->HasElement("object"))
  {
    objectElem = sequence->GetElement("object");
  }

  while (objectElem)
  {
    // Parse the time.
    if (!objectElem->HasElement("time"))
    {
      gzerr << "PopulationPlugin: Unable to find <time> in object\n";
      objectElem = objectElem->GetNextElement("object");
      continue;
    }
    sdf::ElementPtr timeElement = objectElem->GetElement("time");
    double time = timeElement->Get<double>();

    // Parse the object type.
    if (!objectElem->HasElement("type"))
    {
      gzerr << "PopulationPlugin: Unable to find <type> in object.\n";
      objectElem = objectElem->GetNextElement("object");
      continue;
    }
    sdf::ElementPtr typeElement = objectElem->GetElement("type");
    std::string type = typeElement->Get<std::string>();

    // Parse the object pose (optional).
    math::Pose pose;
    if (objectElem->HasElement("pose"))
    {
      sdf::ElementPtr poseElement = objectElem->GetElement("pose");
      pose = poseElement->Get<math::Pose>();
    }

    // Add the object to the collection.
    PopulationPluginPrivate::Object obj = {time, type, pose};
    this->dataPtr->initialObjects.push_back(obj);

    objectElem = objectElem->GetNextElement("object");
  }
  std::sort(this->dataPtr->initialObjects.begin(),
    this->dataPtr->initialObjects.end());

  // Listen on the activation topic, if present. This topic is used for
  // manual activation.
  if (_sdf->HasElement("activation_topic"))
  {
    // Create and initialize the node.
    this->dataPtr->node = transport::NodePtr(new transport::Node());
    this->dataPtr->node->Init();

    // Subscribe to the activation topic.
    this->dataPtr->activationSub = this->dataPtr->node->Subscribe(
        _sdf->Get<std::string>("activation_topic"),
        &PopulationPlugin::OnActivation, this);
  }
  else
    this->Restart();

  this->dataPtr->connection = event::Events::ConnectWorldUpdateEnd(
      boost::bind(&PopulationPlugin::OnUpdate, this));
}

void PopulationPlugin::Pause()
{
  if (!this->dataPtr->enabled)
    return;

  this->dataPtr->enabled = false;
  this->dataPtr->elapsedWhenPaused =
    this->dataPtr->world->GetSimTime() - this->dataPtr->startTime;

  gzmsg << "Object population paused" << std::endl;
}

void PopulationPlugin::Resume()
{
  if (this->dataPtr->enabled)
    return;

  this->dataPtr->enabled = true;
  this->dataPtr->startTime = this->dataPtr->world->GetSimTime() -
    this->dataPtr->elapsedWhenPaused;

   gzmsg << "Object population resumed" << std::endl;
}

void PopulationPlugin::Restart()
{
  this->dataPtr->enabled = true;
  this->dataPtr->startTime = this->dataPtr->world->GetSimTime();
  this->dataPtr->objects = this->dataPtr->initialObjects;

   gzmsg << "Object population restarted" << std::endl;
}

void PopulationPlugin::OnUpdate()
{
  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);

  this->Publish();

  if (!this->dataPtr->enabled)
    return;

  if (this->dataPtr->objects.empty())
  {
    if (this->dataPtr->loopForever)
      this->Restart();
    else
      return;
  }

  // Check whether spawn a new object from the list.
  auto elapsed = this->dataPtr->world->GetSimTime() - this->dataPtr->startTime;
  if (elapsed.Double() >= this->dataPtr->objects.front().time)
  {
    auto obj = this->dataPtr->objects.front();
    if (this->dataPtr->frame)
    {
      auto framePose = this->dataPtr->frame->GetWorldPose().Ign();
      ignition::math::Matrix4d transMat(framePose);
      ignition::math::Matrix4d pose_local(obj.pose.Ign());
      obj.pose = (transMat * pose_local).Pose();
    }
    std::string modelName = this->GetHandle() + "|" + obj.type;

    std::ostringstream newModelStr;
    newModelStr <<
      "<sdf version='" << SDF_VERSION << "'>\n"
      "  <include>\n"
      "    <pose>" << obj.pose << "</pose>\n"
      "    <name>" << modelName << "</name>\n"
      "    <uri>model://" << obj.type << "</uri>\n"
      "  </include>\n"
      "</sdf>\n";

    gzdbg << "Object spawned: " << modelName << std::endl;
    this->dataPtr->world->InsertModelString(newModelStr.str());
    this->dataPtr->objects.erase(this->dataPtr->objects.begin());
  }
}

void PopulationPlugin::OnActivation(ConstGzStringPtr &_msg)
{
  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);

  if (_msg->data() == "restart")
    this->Restart();
  else if (_msg->data() == "pause")
    this->Pause();
  else if (_msg->data() == "resume")
    this->Resume();
  else
    gzerr << "Unknown activation command [" << _msg->data() << "]" << std::endl;
}

bool PopulationPlugin::Enabled() const
{
  return this->dataPtr->enabled;
}

void PopulationPlugin::Publish() const
{
}