Пример #1
0
  GripperRosControl(const std::vector<std::string>& actr_names, const std::vector<std::string>& jnt_names,
                    const std::vector<std::string>& controller_names, const std::vector<double>& reducers)
    : actr_names_(actr_names)
    , jnt_names_(jnt_names)
    , controller_names_(controller_names)
  {
    for (int i = 0; i < jnt_names_.size(); i++)
    {
      std::string actr = actr_names_[i];
      std::string jnt = jnt_names_[i];

      // Get transmission
      boost::shared_ptr<transmission_interface::SimpleTransmission> t_ptr(
          new transmission_interface::SimpleTransmission(reducers[i]));
      trans_.push_back(t_ptr);

      // Initialize and wrap raw current data
      actr_curr_data_[actr].position.push_back(&actr_curr_pos_[actr]);
      actr_curr_data_[actr].velocity.push_back(&actr_curr_vel_[actr]);
      actr_curr_data_[actr].effort.push_back(&actr_curr_eff_[actr]);
      jnt_curr_data_[jnt].position.push_back(&jnt_curr_pos_[jnt]);
      jnt_curr_data_[jnt].velocity.push_back(&jnt_curr_vel_[jnt]);
      jnt_curr_data_[jnt].effort.push_back(&jnt_curr_eff_[jnt]);

      // Initialize and wrap raw command data
      actr_cmd_data_[actr].position.push_back(&actr_cmd_pos_[actr]);
      jnt_cmd_data_[jnt].position.push_back(&jnt_cmd_pos_[jnt]);

      // Register transmissions to each interface
      actr_to_jnt_state_.registerHandle(transmission_interface::ActuatorToJointStateHandle(
          actr + "_trans", trans_[i].get(), actr_curr_data_[actr], jnt_curr_data_[jnt]));
      jnt_to_actr_pos_.registerHandle(transmission_interface::JointToActuatorPositionHandle(
          jnt + "_trans", trans_[i].get(), actr_cmd_data_[actr], jnt_cmd_data_[jnt]));

      // Connect and register the joint state handle
      hardware_interface::JointStateHandle state_handle(jnt, &jnt_curr_pos_[jnt], &jnt_curr_vel_[jnt],
                                                        &jnt_curr_eff_[jnt]);
      jnt_state_interface_.registerHandle(state_handle);

      // Connect and register the joint position handle
      hardware_interface::JointHandle pos_handle(jnt_state_interface_.getHandle(jnt), &jnt_cmd_pos_[jnt]);
      pos_jnt_interface_.registerHandle(pos_handle);

      // ROS publishers and subscribers
      actr_cmd_pub_[actr] = nh_.advertise<std_msgs::Float64>("dxl/" + controller_names_[i] + "/command", 5);
      actr_state_sub_[actr] =
          nh_.subscribe("dxl/" + controller_names_[i] + "/state", 1, &GripperRosControl::actrStateCallback, this);
    }

    // Register interfaces
    registerInterface(&jnt_state_interface_);
    registerInterface(&pos_jnt_interface_);

    // Start spinning
    nh_.setCallbackQueue(&subscriber_queue_);
    subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_));
    subscriber_spinner_->start();
  }
  MyRobot() 
 { 
   // connect and register the joint state interface
   hardware_interface::JointStateHandle state_handle_lwa1("lwa/joint_1", &pos[0], &vel[0], &eff[0]);
   jnt_state_interface.registerHandle(state_handle_lwa1);

   hardware_interface::JointStateHandle state_handle_lwa2("lwa/joint_2", &pos[1], &vel[1], &eff[1]);
   jnt_state_interface.registerHandle(state_handle_lwa2);

   hardware_interface::JointStateHandle state_handle_lwa3("lwa/joint_3", &pos[2], &vel[2], &eff[2]);
   jnt_state_interface.registerHandle(state_handle_lwa3);

   hardware_interface::JointStateHandle state_handle_lwa4("lwa/joint_4", &pos[3], &vel[3], &eff[3]);
   jnt_state_interface.registerHandle(state_handle_lwa4);

   hardware_interface::JointStateHandle state_handle_lwa5("lwa/joint_5", &pos[4], &vel[4], &eff[4]);
   jnt_state_interface.registerHandle(state_handle_lwa5);

   hardware_interface::JointStateHandle state_handle_lwa6("lwa/joint_6", &pos[5], &vel[5], &eff[5]);
   jnt_state_interface.registerHandle(state_handle_lwa6);

   hardware_interface::JointStateHandle state_handle_lwa7("lwa/joint_7", &pos[6], &vel[6], &eff[6]);
   jnt_state_interface.registerHandle(state_handle_lwa7);   

   registerInterface(&jnt_state_interface);

   // connect and register the joint position interface
   hardware_interface::JointHandle pos_handle_lwa1(jnt_state_interface.getHandle("lwa/joint_1"), &cmd[0]);
   jnt_pos_interface.registerHandle(pos_handle_lwa1);

   hardware_interface::JointHandle pos_handle_lwa2(jnt_state_interface.getHandle("lwa/joint_2"), &cmd[1]);
   jnt_pos_interface.registerHandle(pos_handle_lwa2);

   hardware_interface::JointHandle pos_handle_lwa3(jnt_state_interface.getHandle("lwa/joint_3"), &cmd[2]);
   jnt_pos_interface.registerHandle(pos_handle_lwa3);

   hardware_interface::JointHandle pos_handle_lwa4(jnt_state_interface.getHandle("lwa/joint_4"), &cmd[3]);
   jnt_pos_interface.registerHandle(pos_handle_lwa4);

   hardware_interface::JointHandle pos_handle_lwa5(jnt_state_interface.getHandle("lwa/joint_5"), &cmd[4]);
   jnt_pos_interface.registerHandle(pos_handle_lwa5);

   hardware_interface::JointHandle pos_handle_lwa6(jnt_state_interface.getHandle("lwa/joint_6"), &cmd[5]);
   jnt_pos_interface.registerHandle(pos_handle_lwa6);

   hardware_interface::JointHandle pos_handle_lwa7(jnt_state_interface.getHandle("lwa/joint_7"), &cmd[6]);
   jnt_pos_interface.registerHandle(pos_handle_lwa7);

   registerInterface(&jnt_pos_interface);

  }
bool MitsubishiArmInterface::init(hardware_interface::JointStateInterface  & jnt_state_interface_,
                                  hardware_interface::PositionJointInterface & jnt_pos_interface_)
{

    // connect and register the joint state interface
    hardware_interface::JointStateHandle state_handle_j1("j1", &pos[0], &vel[0], &eff[0]);
    jnt_state_interface_.registerHandle(state_handle_j1);

    hardware_interface::JointStateHandle state_handle_j2("j2", &pos[1], &vel[1], &eff[1]);
    jnt_state_interface_.registerHandle(state_handle_j2);

    hardware_interface::JointStateHandle state_handle_j3("j3", &pos[2], &vel[2], &eff[2]);
    jnt_state_interface_.registerHandle(state_handle_j3);

    hardware_interface::JointStateHandle state_handle_j4("j4", &pos[3], &vel[3], &eff[3]);
    jnt_state_interface_.registerHandle(state_handle_j4);

    hardware_interface::JointStateHandle state_handle_j5("j5", &pos[4], &vel[4], &eff[4]);
    jnt_state_interface_.registerHandle(state_handle_j5);

    hardware_interface::JointStateHandle state_handle_j6("j6", &pos[5], &vel[5], &eff[5]);
    jnt_state_interface_.registerHandle(state_handle_j6);

    registerInterface(&jnt_state_interface_);

    // connect and register the joint position interface
    hardware_interface::JointHandle pos_handle_j1(jnt_state_interface_.getHandle("j1"), &cmd[0]);
    jnt_pos_interface_.registerHandle(pos_handle_j1);

    hardware_interface::JointHandle pos_handle_j2(jnt_state_interface_.getHandle("j2"), &cmd[1]);
    jnt_pos_interface_.registerHandle(pos_handle_j2);

    hardware_interface::JointHandle pos_handle_j3(jnt_state_interface_.getHandle("j3"), &cmd[2]);
    jnt_pos_interface_.registerHandle(pos_handle_j3);

    hardware_interface::JointHandle pos_handle_j4(jnt_state_interface_.getHandle("j4"), &cmd[3]);
    jnt_pos_interface_.registerHandle(pos_handle_j4);

    hardware_interface::JointHandle pos_handle_j5(jnt_state_interface_.getHandle("j5"), &cmd[4]);
    jnt_pos_interface_.registerHandle(pos_handle_j5);

    hardware_interface::JointHandle pos_handle_j6(jnt_state_interface_.getHandle("j6"), &cmd[5]);
    jnt_pos_interface_.registerHandle(pos_handle_j6);

    registerInterface(&jnt_pos_interface_);

}