pr2_pose_test.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2011 Nate Koenig & Andrew Howard
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 #include <boost/bind.hpp>
00018 #include "physics/physics.hh"
00019 #include "transport/Node.hh"
00020 #include "transport/TransportTypes.hh"
00021 #include "msgs/MessageTypes.hh"
00022 #include "common/Time.hh"
00023 #include "common/Plugin.hh"
00024 #include "common/Events.hh"
00025 
00026 namespace gazebo
00027 {
00028   class PR2PoseTest : public ModelPlugin
00029   {
00030     public: void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00031     {
00032       // Get then name of the parent model
00033       std::string modelName = _sdf->GetParent()->GetValueString("name");
00034 
00035       // Get the world name.
00036       this->world = _parent->GetWorld();
00037 
00038       // Get a pointer to the model
00039       this->model = this->world->GetModel(modelName);
00040 
00041       // Error message if the model couldn't be found
00042       if (!this->model)
00043         gzerr << "Unable to get parent model\n";
00044 
00045       // Listen to the update event. This event is broadcast every
00046       // simulation iteration.
00047       this->updateConnection = event::Events::ConnectWorldUpdateBegin(
00048           boost::bind(&PR2PoseTest::OnUpdate, this));
00049       gzdbg << "plugin model name: " << modelName << "\n";
00050 
00051 
00052       this->node = transport::NodePtr(new transport::Node());
00053       this->node->Init(this->world->GetName());
00054       this->statsSub = this->node->Subscribe("~/world_stats", &PR2PoseTest::OnStats, this);
00055 
00056     }
00057 
00058     // Called by the world update start event
00059     public: void OnUpdate()
00060     {
00061       this->simTime  = this->world->GetSimTime();
00062 
00063       math::Pose orig_pose = this->model->GetWorldPose();
00064       math::Pose pose = orig_pose;
00065       pose.pos.x = 5.0*sin(0.1*this->simTime.Double());
00066       pose.rot.SetFromEuler(math::Vector3(0,0,2.0*this->simTime.Double()));
00067 
00068       if (this->simTime.Double() > 10.0 && this->simTime.Double() < 300.0)
00069       {
00070         this->model->SetWorldPose( pose );
00071         printf("test plugin OnUpdate simTime [%f] update pose [%f,%f,%f:%f,%f,%f,%f] orig pose.x [%f]\n",
00072                this->simTime.Double(), pose.pos.x, pose.pos.y, pose.pos.z, pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w, orig_pose.pos.x);
00073         if (this->simTime.Double() > 20.0)
00074           this->model->SetWorldTwist( math::Vector3(0,0,0),math::Vector3(0,0,0),true );
00075       }
00076     }
00077 
00078     public: void OnStats( const boost::shared_ptr<msgs::WorldStatistics const> &_msg)
00079     {
00080       static double fake_time = 0; fake_time = fake_time+0.2;
00081 
00082       math::Pose orig_pose = this->model->GetWorldPose();
00083       math::Pose pose = orig_pose;
00084       pose.pos.x = 0.5*sin(0.1*fake_time);
00085       pose.rot.SetFromEuler(math::Vector3(0,0,fake_time));
00086 
00087       if (this->world->GetSimTime().Double() < 10.0)
00088       {
00089         this->model->SetWorldPose( pose );
00090         printf("test plugin OnStats simTime [%f] update pose [%f,%f,%f:%f,%f,%f,%f] orig pose.x [%f]\n",
00091                fake_time, pose.pos.x, pose.pos.y, pose.pos.z, pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w, orig_pose.pos.x);
00092       }
00093 
00094     }
00095 
00096     // Pointer to the model
00097     private: physics::ModelPtr model;
00098     private: physics::WorldPtr world;
00099 
00100     // Pointer to the update event connection
00101     private: event::ConnectionPtr updateConnection;
00102 
00103     // subscribe to world stats
00104     private: transport::NodePtr node;
00105     private: transport::SubscriberPtr statsSub;
00106     private: common::Time simTime;
00107 
00108   };
00109 
00110   // Register this plugin with the simulator
00111   GZ_REGISTER_MODEL_PLUGIN(PR2PoseTest)
00112 }


gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Mon Oct 6 2014 12:15:44