C++ ros::NodeHandle类代码示例

void CoaxSimpleControl::loadParams(ros::NodeHandle &n) {
  n.getParam("motorconst/const1",motor_const1);
  n.getParam("motorconst/const2",motor_const2);
  n.getParam("rollconst/const",servo1_const);
  n.getParam("pitchconst/const",servo2_const);
  n.getParam("yawcoef/coef1",yaw_coef1);
  n.getParam("yawcoef/coef2",yaw_coef2);
  n.getParam("yawcoef/offset", yaw_offset);
  n.getParam("throttlecoef/coef1",thr_coef1);
  n.getParam("throttlecoef/coef2",thr_coef2);
  n.getParam("rollrccoef/coef",r_rc_coef);
  n.getParam("pitchrccoef/coef",p_rc_coef);
  n.getParam("yawcontrol/proportional",kp_yaw);
  n.getParam("yawcontrol/differential",kd_yaw);
  n.getParam("rollcontrol/proportional",kp_roll);
  n.getParam("rollcontrol/differential",kd_roll);
  n.getParam("pitchcontrol/proportional",kp_pitch);
  n.getParam("pitchcontrol/differential",kd_pitch);
  n.getParam("altitude/base",range_base);
  n.getParam("altitude/trim", altitude_trim);
  n.getParam("altitudecontrol/proportional",kp_altitude);
  n.getParam("altitudecontrol/derivative",kd_altitude);
  n.getParam("errorparams/K1_pitch", K1_pitch);
  n.getParam("errorparams/K1_roll", K1_roll);
  n.getParam("auxparams/aux1", aux1);
  n.getParam("auxparams/aux2", aux2);
  n.getParam("auxparams/aux3", aux3);
  n.getParam("auxparams/aux4", aux4);
  n.getParam("auxparams/aux5", aux5);
  n.getParam("auxparams/aux6", aux6);
  n.getParam("auxparams/aux7", aux7);
  n.getParam("auxparams/aux8", aux8);
  n.getParam("auxparams/aux9", aux9);
  n.getParam("auxparams/aux10", aux10);
  n.getParam("auxparams/aux11", aux11);
  n.getParam("auxparams/aux12", aux12);
  n.getParam("auxparams/aux13", aux13);
  n.getParam("auxparams/aux14", aux14);
  n.getParam("auxparams/aux15", aux15);
  n.getParam("auxparams/aux16", aux16);
  n.getParam("auxparams/aux17", aux17);
  n.getParam("auxparams/aux18", aux18);
  n.getParam("auxparams/aux19", aux19);
  n.getParam("auxparams/aux20", aux20);
  n.getParam("auxparams/aux21", aux21);
  n.getParam("auxparams/aux22", aux22);
  n.getParam("auxparams/aux23", aux23);
  n.getParam("auxparams/aux24", aux24);
  n.getParam("auxparams/aux25", aux25);
  n.getParam("auxparams/aux26", aux26);
  n.getParam("auxparams/aux27", aux27);
  n.getParam("auxparams/aux28", aux28);
  n.getParam("auxparams/aux29", aux29);
  n.getParam("auxparams/aux30", aux30);
  n.getParam("auxparams/aux31", aux31);
  n.getParam("auxparams/aux32", aux32);
  n.getParam("auxparams/aux33", aux33);
  n.getParam("auxparams/aux34", aux34);
  n.getParam("auxparams/aux35", aux35);
  n.getParam("auxparams/aux36", aux36);
  
  n.getParam("auxparams/aux_int1", aux_int1);
  n.getParam("auxparams/aux_int2", aux_int2);
  n.getParam("auxparams/aux_int3", aux_int3);
  n.getParam("auxparams/aux_int4", aux_int4);
  n.getParam("auxparams/aux_int5", aux_int5);
  n.getParam("auxparams/aux_int6", aux_int6);
  n.getParam("trims/roll", roll_trim);
  n.getParam("trims/pitch", pitch_trim);
  n.getParam("error_limits/velocity", lim_vel_error);  
  n.getParam("error_limits/position", lim_pos_error);
  n.getParam("error_limits/angle", lim_ang_error); 
  
}

bool CarSlamAdapter_QBFD_Analyzer::init(const string base_name, const ros::NodeHandle &n)
{
  ROS_INFO("%s: init(): ", this->CLASSNAME);

  std::string path = ros::package::getPath("car_slam");
  path = path + "/model/";


  std::string  pathGroupParam = "";
  if (!n.getParam("path_group", pathGroupParam))
  {
    ROS_WARN ("No path parameter was specified in CarSlamAdapter_QBFD_Analyzer! Use no parent item.");
  } else {
    path_ = pathGroupParam ;
  }

  std::string  pathCapParam = "";
  if (!n.getParam("cap", pathCapParam))
  {
    ROS_WARN ("No cap parameter was specified in CarSlamAdapter_QBFD_Analyzer! Use no parent item.");
  } else {
    path_ = path_ + "/" + pathCapParam ;
  }
  std::string  pathFuncParam = "";
  if (!n.getParam("func", pathFuncParam))
  {
    ROS_WARN ("No func parameter was specified in CarSlamAdapter_QBFD_Analyzer! Use no parent item.");
  } else {
    path_ = path_ + "/" + pathFuncParam ;
  }

  ROS_INFO("%s: init: path is: %s", this->CLASSNAME, path_.c_str());

  if (!n.getParam("node_to_analyze", this->nodeToAnalyze))
  {
    ROS_ERROR(
        "No power board name was specified in CarSlamAdapter_QBFD_Analyzer! Power board must be \"Power board 10XX\". Namespace: %s", n.getNamespace().c_str());
    return false;
  }

  ROS_DEBUG("%s: init(): path_group: %s, cap: %s, func: %s, node_to_analyze: %s",
           this->CLASSNAME, pathGroupParam.c_str(), pathCapParam.c_str(), pathFuncParam.c_str(), this->nodeToAnalyze.c_str());

  //get a list from yaml
  n.getParam("systemNodesToAnalyze", systemNodesToAnalyze);
  ROS_ASSERT(systemNodesToAnalyze.getType() == XmlRpc::XmlRpcValue::TypeArray);
  //default
  if (systemNodesToAnalyze.size() == 0) {
    //set default
    systemNodesToAnalyze[0] = "SLAM_Adapter";
  }

  n.getParam("systemNodesStateToAnalyze", systemNodesStateToAnalyze);
  ROS_ASSERT(systemNodesStateToAnalyze.getType() == XmlRpc::XmlRpcValue::TypeArray);
  //default
  if (systemNodesStateToAnalyze.size() == 0) {
    //set default
    systemNodesStateToAnalyze[0] = "failure";
  }

  //both lists need to have the same length
  if (systemNodesToAnalyze.size() == systemNodesStateToAnalyze.size()) {

  for (int i = 0; i < systemNodesToAnalyze.size(); ++i)
    {
      string nodeName = (string) systemNodesToAnalyze[i];
      string nodeState = (string) systemNodesStateToAnalyze[i];
      ROS_DEBUG("%s init(): register request item (name: %s, state: %s).",
               this->CLASSNAME, nodeName.c_str(), nodeState.c_str());

      // specify the nodes and states to query by name
      BayesianQueryItem itemComp(systemNodesToAnalyze[i], -1, systemNodesStateToAnalyze[i], -1); //ids not known yet
      this->detailedQueryRequest.push_back(itemComp);
    }
  } else {
    ROS_ERROR("%s init(): parameter array lengths of \"itemNamesToAnalyze\" and \"systemNodesStateToAnalyze\" do not match!. Take the default (Robot; failure) ",
              this->CLASSNAME);
  }


  boost::shared_ptr<StatusItem> item(new StatusItem("Report"));
  report_item_ = item;

  has_initialized_ = true;

  std::string my_path;
  std::string nice_name = "huhn";
  if (base_name == "/")
    my_path = nice_name;
  else
    my_path = base_name + "/" + nice_name;

  int z;
  //z = gethostname(hostname, sizeof hostname);
  this->robotname =  supplementary::SystemConfig::getHostname();
  //char to string as stream operation
  this->fullName = "";
  stringstream ss;
  ss << this->robotname << "_" << this->nodeToAnalyze;
  ss >> this->fullName;
//.........这里部分代码省略.........

    bool moveRightArm ( const arm_navigation_msgs::MoveArmGoal * newArmGoal )
    {
      actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm",true);

      ROS_INFO("Waiting for server..");
      move_arm.waitForServer();
      ROS_INFO("Connected to server");

      arm_navigation_msgs::MoveArmGoal armGoal;
      std::vector<std::string> names(7);

      names[0] = "r_shoulder_pan_joint";
      names[1] = "r_shoulder_lift_joint";
      names[2] = "r_upper_arm_roll_joint";
      names[3] = "r_elbow_flex_joint";
      names[4] = "r_forearm_roll_joint";
      names[5] = "r_wrist_flex_joint";
      names[6] = "r_wrist_roll_joint";

      armGoal.motion_plan_request.group_name = "right_arm";
      armGoal.motion_plan_request.num_planning_attempts = 1;
      armGoal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

      armGoal.motion_plan_request.planner_id= std::string("");
      armGoal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
      armGoal.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());

      for (unsigned int i = 0 ; i < armGoal.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
      {
        armGoal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
        armGoal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
        armGoal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
        armGoal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
        // Set the joint values
        armGoal.motion_plan_request.goal_constraints.joint_constraints[i].position =
          newArmGoal->motion_plan_request.goal_constraints.joint_constraints[i].position;
      }

      bool result = true;
      bool success = true;

      if (nh_.ok())
      {
        bool finished_within_time = false;
        ROS_INFO("Sending arm goal..");
        move_arm.sendGoal(armGoal);
        finished_within_time = move_arm.waitForResult(ros::Duration(200.0));

        if (!finished_within_time)
        {
          move_arm.cancelGoal();
          ROS_INFO("Timed out achieving arm goal");
          success = false;
        }
        else
        {
          actionlib::SimpleClientGoalState state = move_arm.getState();
          success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
          if(success)
            ROS_INFO("Action finished: %s",state.toString().c_str());
          else
            ROS_INFO("Action failed: %s",state.toString().c_str());

        }
      }
      result = success;

      return result;
    }