Пример #1
0
void Servo::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;
  signal = 0;

  // parse SDF Properries
  joint = model->GetJoint(sdf->Get<std::string>("joint"));
  if (sdf->HasElement("topic")) {
    topic = sdf->Get<std::string>("topic");
  } else {
    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
  }

  if (sdf->HasElement("torque")) {
    torque = sdf->Get<double>("torque");
  } else {
    torque = 5;
  }

  gzmsg << "initializing servo: " << topic << " joint=" << joint->GetName()
        << " torque=" << torque << std::endl;

  // Connect to Gazebo transport for messaging
  std::string scoped_name =
      model->GetWorld()->GetName() + "::" + model->GetScopedName();
  boost::replace_all(scoped_name, "::", "/");
  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
  node->Init(scoped_name);
  sub = node->Subscribe(topic, &Servo::Callback, this);

  // connect to the world update event
  // this will call update every iteration
  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
      boost::bind(&Servo::Update, this, _1));
}
Пример #2
0
void DCMotor::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
  this->model = model;
  signal = 0;

  // Parse SDF properties
  joint = model->GetJoint(sdf->Get<std::string>("joint"));
  if (sdf->HasElement("topic")) {
    topic = sdf->Get<std::string>("topic");
  } else {
    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
  }

  if (sdf->HasElement("multiplier")) {
    multiplier = sdf->Get<double>("multiplier");
  } else {
    multiplier = 1;
  }

  gzmsg << "Initializing motor: " << topic << " joint=" << joint->GetName()
        << " multiplier=" << multiplier << std::endl;

  // Connect to Gazebo transport for messaging
  std::string scoped_name =
      model->GetWorld()->GetName() + "::" + model->GetScopedName();
  boost::replace_all(scoped_name, "::", "/");
  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
  node->Init(scoped_name);
  sub = node->Subscribe(topic, &DCMotor::Callback, this);

  // Connect to the world update event.
  // This will trigger the Update function every Gazebo iteration
  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
      boost::bind(&DCMotor::Update, this, _1));
}
Пример #3
0
    virtual void Load( gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
        // Start up ROS
        int argc = 0;
        ros::init( argc, NULL, "pendulum_robot" );
        //ros::init( argc, NULL, "pendulum" );

        this->model = _model;
        this->world = _model->GetWorld();
        this->joints = this->model->GetJoints();

        // ROS Nodehandle
        //this->node = new ros::NodeHandle( "~" );
        this->node = new ros::NodeHandle( "pendulum" );

        // ROS Subscriber
        //this->sub = this->node->subscribe<pendulum::command>( "ros_pendulum_standup_controller", 1, &ros_pendulum_controller_c::ros_callback, this );

        //this->client = this->node->serviceClient<pendulum::command>( "ros_pendulum_standup_controller" );

        this->client = this->node->serviceClient<pendulum::rpc_command>( "pendulum_standup_command" );
/*
	if( !client.isValid() ) {
	    std::cerr << "client not valid\n";
	    client.waitForExistence();
        }
*/
        //this->sub = this->node->subscribe<pendulum::command>( "ros_pendulum_standup_controller_state", 1, ros_pendulum_controller_c::request_state )

	//this->server = this->node->advertiseService( "ros_pendulum_standup_controller_command", &ros_pendulum_controller_c::issue_command );

        start_time = world->GetSimTime();
        last_time = start_time;

        this->updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(
            boost::bind( &ros_pendulum_controller_c::Update, this ) );

        std::cerr << "Pendulum Loaded" << std::endl;
    }