SHARE
TWEET

move_model.cc

a guest Nov 6th, 2013 7 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <boost/bind.hpp>
  2. #include <gazebo/gazebo.hh>
  3. #include <gazebo/physics/physics.hh>
  4. #include <gazebo/common/common.hh>
  5. #include <stdio.h>
  6. #include <time.h>
  7. #include <stdlib.h>
  8.  
  9. namespace gazebo
  10. {
  11.     class MoveModel : public ModelPlugin
  12.     {
  13.     public:
  14.         void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
  15.             {
  16.                 srand(time(NULL));
  17.                 // Store the pointer to the model
  18.                 this->model = _parent;
  19.  
  20.                 // Listen to the update event. This event is broadcast every
  21.                 // simulation iteration.
  22.                 this->updateConnection = event::Events::ConnectWorldUpdateBegin(
  23.                     boost::bind(&MoveModel::OnUpdate, this, _1));
  24.             };
  25.  
  26.         // Called by the world update start event
  27.         void OnUpdate(const common::UpdateInfo & /*_info*/)
  28.             {
  29.                 if(iteration < 1000) {
  30.                     // Apply a small linear velocity to the model.
  31.                     if(!appliedRight) {
  32.                         std::cout<<"Right"<<std::endl;
  33.                         this->model->SetLinearVel(math::Vector3(
  34.                                                       .03,
  35.                                                       (rand() % 5) * 0.01,
  36.                                                       (rand() % 5) * 0.01));
  37.                         appliedRight = true;
  38.                         appliedLeft = false;
  39.                     }
  40.                     ++iteration;
  41.                 } else {
  42.                     if(iteration > 2000) {
  43.                         iteration = 0;
  44.                     }
  45.                     if(!appliedLeft) {
  46.                         std::cout<<"Left"<<std::endl;
  47.                         this->model->SetLinearVel(math::Vector3(
  48.                                                       -.03,
  49.                                                       (rand() % 5) * -0.01,
  50.                                                       (rand() % 5) * -0.01));
  51.                         appliedLeft = true;
  52.                         appliedRight = false;
  53.                     }
  54.                     ++iteration;
  55.                 }
  56.             };
  57.  
  58.     private:
  59.         // Pointer to the model
  60.         physics::ModelPtr model;
  61.         // Pointer to the update event connection
  62.         event::ConnectionPtr updateConnection;
  63.  
  64.         bool appliedRight = false;
  65.         bool appliedLeft = false;
  66.         int iteration = 0;
  67.  
  68.     };
  69.  
  70.     // Register this plugin with the simulator
  71.     GZ_REGISTER_MODEL_PLUGIN(MoveModel)
  72. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top