gazebo / plugins / ModelTrajectoryTestPlugin.cc

/*
 * Copyright 2011 Nate Koenig & Andrew Howard
 *
 * 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 "physics/physics.h"
#include "transport/transport.h"
#include "plugins/ModelTrajectoryTestPlugin.hh"

using namespace gazebo;

GZ_REGISTER_MODEL_PLUGIN(ModelTrajectoryTestPlugin)

/////////////////////////////////////////////////
ModelTrajectoryTestPlugin::ModelTrajectoryTestPlugin()
{
}

/////////////////////////////////////////////////
void ModelTrajectoryTestPlugin::Load(physics::ModelPtr _model,
                           sdf::ElementPtr /*_sdf*/)
{
  this->model = _model;

  this->node = transport::NodePtr(new transport::Node());
  this->node->Init(this->model->GetWorld()->GetName());

  this->trajSub = this->node->Subscribe(std::string("~/") +
      this->model->GetName() + "/pose_trajectory",
      &ModelTrajectoryTestPlugin::OnPoseTrajectoryMsg, this);

  this->updateConnection = event::Events::ConnectWorldUpdateStart(
          boost::bind(&ModelTrajectoryTestPlugin::OnUpdate, this));
}

/////////////////////////////////////////////////
void ModelTrajectoryTestPlugin::Init()
{
}

/////////////////////////////////////////////////
void ModelTrajectoryTestPlugin::OnPoseTrajectoryMsg(
    ConstPoseTrajectoryPtr &/*_msg*/)
{
}

/////////////////////////////////////////////////
void ModelTrajectoryTestPlugin::OnUpdate()
{
  common::Time currTime = this->model->GetWorld()->GetSimTime();
  common::Time stepTime = currTime - this->prevUpdateTime;
  this->prevUpdateTime = currTime;

  {
    // ignore everything else, get position and force only
    math::Pose pose;
    this->model->SetWorldPose(pose);
  }
}
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.