Exemplo n.º 1
0
    bool initSim(const std::string& _robot_namespace,
      ros::NodeHandle _nh,
      gazebo::physics::ModelPtr _model,
      const urdf::Model* const _urdf_model,
      std::vector<transmission_interface::TransmissionInfo> _transmissions)
    {
        using gazebo::physics::JointPtr;

        this->CleanUp();

        // simulation joints
        sim_joints_ = _model->GetJoints();
        int n_dof = sim_joints_.size();

#ifdef MULTIPLE_JOINTS
        this->GetJointNames(_nh);
        this->Resize();
#endif
        this->RegisterHardwareInterfaces();
    }
//Here we already have the model!
bool robotSim::gazeboConfigureHook(gazebo::physics::ModelPtr model) {
    if (model.get() == NULL) {
        RTT::log(RTT::Error) << "No model could be loaded" << RTT::endlog();
        return false;
    }

    if (!_models_loaded) {
        RTT::log(RTT::Error) << "URDF and SRDF models has not been passed. Call loadURDFAndSRDF(URDF_path, SRDF_path)" << RTT::endlog();
        return false;
    }

    // Get the joints
    gazebo_joints_ = model->GetJoints();
    model_links_ = model->GetLinks();

    RTT::log(RTT::Info) << "Model name "<< model->GetName() << RTT::endlog();
    RTT::log(RTT::Info) << "Model has " << gazebo_joints_.size() << " joints" << RTT::endlog();
    RTT::log(RTT::Info) << "Model has " << model_links_.size() << " links" << RTT::endlog();

    for(unsigned int i = 0; i < _xbotcore_model.get_chain_names().size(); ++i){
        std::string chain_name = _xbotcore_model.get_chain_names()[i];
        std::vector<std::string> enabled_joints_in_chain;
        _xbotcore_model.get_enabled_joints_in_chain(chain_name,enabled_joints_in_chain);

        kinematic_chains.insert(std::pair<std::string, boost::shared_ptr<KinematicChain>>(
            chain_name, boost::shared_ptr<KinematicChain>(
                new KinematicChain(chain_name, enabled_joints_in_chain, *(this->ports()), model
#ifdef USE_INTROSPECTION
                ,this
#endif
                ))));
    }

    RTT::log(RTT::Info) << "Kinematic Chains map created!" << RTT::endlog();

    for(std::map<std::string, boost::shared_ptr<KinematicChain>>::iterator it = kinematic_chains.begin();
        it != kinematic_chains.end(); it++){
        if(!(it->second->initKinematicChain(gains.Gains))){
            RTT::log(RTT::Warning) << "Problem Init Kinematic Chain" <<
                it->second->getKinematicChainName() << RTT::endlog();
            return false;
        }
    }
    RTT::log(RTT::Info) << "Kinematic Chains Initialized!" << RTT::endlog();



    RTT::log(RTT::Info) << "Checking Sensors (URDF/SRDF)"<<RTT::endlog();
    std::map<std::string,int> ft_srdf = _xbotcore_model.get_ft_sensors();
    std::map<std::string,int> imu_srdf = _xbotcore_model.get_imu_sensors();


    RTT::log(RTT::Info) << "Force Update of SensorManager (before accessing sensors)!" << RTT::endlog();
    gazebo::sensors::SensorManager::Instance()->Update(true);

    RTT::log(RTT::Info) << "Checking Sensors (Gazebo)"<<RTT::endlog();
    gazebo::sensors::Sensor_V sensors = gazebo::sensors::SensorManager::Instance()->GetSensors();

    gazebo::sensors::Sensor_V sensors_attached_to_robot;
    for(unsigned int i = 0; i < sensors.size(); ++i){
        if(sensors[i]->ScopedName().find("::"+model->GetName()+"::") != std::string::npos)
            sensors_attached_to_robot.push_back(sensors[i]);
    }

    for(std::map<std::string,int>::iterator i = ft_srdf.begin(); i != ft_srdf.end(); i++)
    {
        force_torque_sensor ft(i->first, model, _xbotcore_model.get_urdf_model(),
                               sensors_attached_to_robot,
                               *(this->ports())
    #ifdef USE_INTROSPECTION
                               ,this
    #endif
                               );
        if(ft.isInited())
            force_torque_sensors.push_back(ft);
    }

    for(std::map<std::string,int>::iterator i = imu_srdf.begin(); i != imu_srdf.end(); ++i){
        imu_sensor imu_(i->first,sensors_attached_to_robot,*(this->ports()));
        if(imu_.isInited())
            imu_sensors.push_back(imu_);
    }



    RTT::log(RTT::Warning) << "Done Configuring Component" << RTT::endlog();
    return true;
}
  bool SteerBotHardwareGazebo::initSim(const std::string& robot_namespace,
      ros::NodeHandle nh,
      gazebo::physics::ModelPtr model,
      const urdf::Model* const urdf_model,
      std::vector<transmission_interface::TransmissionInfo> transmissions)
  {
    using gazebo::physics::JointPtr;

    nh_ = nh;

    // Simulation joints
    sim_joints_ = model->GetJoints();
    n_dof_ = sim_joints_.size();

    this->CleanUp();
    this->GetJointNames(nh_);
    this->RegisterHardwareInterfaces();

    nh_.param(ns_ + "wheel_separation_w", wheel_separation_w_);
    nh_.param(ns_ + "wheel_separation_h", wheel_separation_h_);
    ROS_INFO_STREAM("wheel_separation_w = " << wheel_separation_w_);
    ROS_INFO_STREAM("wheel_separation_h = " << wheel_separation_h_);

    nh_.param(ns_ + "enable_ackermann_link", true);
    ROS_INFO_STREAM("enable_ackermann_link = " << (enable_ackermann_link_ ? "true" : "false"));

#ifdef JOINT_LIMIT
    // Position joint limits interface
    std::vector<std::string> cmd_handle_names = steer_jnt_pos_cmd_interface_.getNames();
    for (size_t i = 0; i < n_dof_; ++i)
    {
      const std::string name = cmd_handle_names[i];

      // unless current handle is not pos interface for steer, skip
      if(name != virtual_steer_jnt_names_[INDEX_RIGHT] && name != virtual_steer_jnt_names_[INDEX_LEFT])
          continue;

      JointHandle cmd_handle = steer_jnt_pos_cmd_interface_.getHandle(name);

      using namespace joint_limits_interface;
      boost::shared_ptr<const urdf::Joint> urdf_joint = urdf_model->getJoint(name);
      JointLimits limits;
      SoftJointLimits soft_limits;
      if (!getJointLimits(urdf_joint, limits) || !getSoftJointLimits(urdf_joint, soft_limits))
      {
        ROS_WARN_STREAM("Joint limits won't be enforced for joint '" << name << "'.");
      }
      else
      {
        jnt_limits_interface_.registerHandle(
            PositionJointSoftLimitsHandle(cmd_handle, limits, soft_limits));

        ROS_DEBUG_STREAM("Joint limits will be enforced for joint '" << name << "'.");
      }
    }
#endif

    // PID controllers for wheel
    const int virtual_jnt_cnt_ = virtual_rear_wheel_jnt_names_.size();
    pids_.resize(virtual_jnt_cnt_);

    for (size_t i = 0; i < virtual_jnt_cnt_; ++i)
    {
      const std::string jnt_name = virtual_rear_wheel_jnt_names_[i];
      const ros::NodeHandle joint_nh(nh, "gains/" +  jnt_name);

      ROS_INFO_STREAM("Trying to set pid param of '" << jnt_name << " ' at PID proc in init()");
      if (!pids_[i].init(joint_nh))
      {
        ROS_INFO_STREAM("Faied to set pid param of '" << jnt_name << " ' at PID proc in init()");
        return false;
      }
      ROS_INFO_STREAM("Succeeded to set pid param of '" << jnt_name << " ' at PID proc in init()");
    }

    return true;
  }