Esempi in C++ (Cpp) per sim_time_ros

Esempio n. 1

File: gazebo_ros_control_plugin.cpp Progetto: iit-DLSLab/gazebo_ros_pkgs

// Called by the world update start event
void GazeboRosControlPlugin::Update()
{
// Get the simulation time and period
  gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime();
  ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
  ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;

  robot_hw_sim_->eStopActive(e_stop_active_);

  // Check if we should update the controllers
  if(sim_period >= motor_period_) {
    // Store this simulation time
    last_update_sim_time_ros_ = sim_time_ros;

    // Update the robot simulation with the state of the gazebo model
    robot_hw_sim_->readSim(sim_time_ros, sim_period);

    // Compute the controller commands
    bool reset_ctrlrs;
    if (e_stop_active_)
    {
      reset_ctrlrs = false;
      last_e_stop_active_ = true;
    }
    else
    {
      if (last_e_stop_active_)
      {
        reset_ctrlrs = true;
        last_e_stop_active_ = false;
      }
      else
      {
        reset_ctrlrs = false;
      }
    }

    static unsigned int task_counter = 0;
    if(task_counter == 0){
      controller_manager_->update(sim_time_ros,
          ros::Duration(0,sim_period.toNSec()*control_ratio_), reset_ctrlrs);
      task_counter++;
    }else if(task_counter == (control_ratio_ -1))
    {
      task_counter = 0;
    }else{
      task_counter++;
    }
  }

  // Update the gazebo model with the result of the controller
  // computation
  robot_hw_sim_->writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_);
  last_write_sim_time_ros_ = sim_time_ros;
}

Esempio n. 2

File: ros_control_plugin.cpp Progetto: jbohren-forks/gazebo_ros_control

    // Called by the world update start event
    void Update()
    {
      // Get the simulation time and period
      gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime();
      ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
      ros::Duration sim_period = sim_time_ros - last_sim_time_ros_;

      // Check if we should update the controllers
      if(sim_period >= control_period_) {
        // Store this simulation time
        last_sim_time_ros_ = sim_time_ros;

        // Update the robot simulation with the state of the gazebo model
        robot_sim_->readSim(sim_time_ros, sim_period);

        // Compute the controller commands
        controller_manager_->update(sim_time_ros, sim_period);

        // Update the gazebo model with the result of the controller
        // computation
        robot_sim_->writeSim(sim_time_ros, sim_period);
      }
    }