C++ NodeHandle::getParamCached方法代码示例

    void simulateOneCycle()
    {
	tf::StampedTransform transform;
        try{
	  listener.waitForTransform("map","EEframe", ros::Time(0), ros::Duration(1.0));
          listener.lookupTransform( "map","EEframe", ros::Time(0), transform);
        }
        catch (tf::TransformException ex){
            ROS_ERROR("%s",ex.what());
        }
	// This is correct, but does not work.
	// state_ = transform;

	KDL::Frame state_copy;
	tf::transformTFToKDL(state_, state_copy);

	nh_.getParamCached("/cds_controller_pr2/run_controller", run_controller_flag);
	if (run_controller_flag == 1.0){  // Controller running -> Update next desired position
	    tf::transformKDLToTF(cds_.update(state_copy), state_);
	} else {
	    state_ = transform;
	}

	broadcastTF();
    }

 /** Update reconfigurable parameter (for strings). */
 bool inline updateParam(const std::string &name, std::string &val)
 {
   bool changed = false;
   std::string prev = val;
   if (nh_.getParamCached(name, val) && (val != prev))
     changed = true;
   return changed;
 }

 inline std::string getStringParam(std::string name)
 {
   std::string value;
   if (!root_nh_.getParamCached(name, value))
   {
     ROS_ERROR("Hand description: could not find parameter %s", name.c_str());
   }
   //ROS_INFO_STREAM("Hand description param " << name << " resolved to " << value);
   return value;
 }

  inline std::vector<std::string> getVectorParam(std::string name)
  {
    std::vector<std::string> values;
    XmlRpc::XmlRpcValue list;
    if (!root_nh_.getParamCached(name, list)) 
    {
      ROS_ERROR("Hand description: could not find parameter %s", name.c_str());
    }
    if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
      ROS_ERROR("Hand description: bad parameter %s", name.c_str());
    }
    //ROS_INFO_STREAM("Hand description vector param " << name << " resolved to:");
    for (int32_t i=0; i<list.size(); i++)
    {
      if (list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
      {
	ROS_ERROR("Hand description: bad parameter %s", name.c_str());
      }
      values.push_back( static_cast<std::string>(list[i]) );
      //ROS_INFO_STREAM("  " << values.back());
    }
    return values;	
  }

    void executeActionCB(const OrientToBaseGoalConstPtr& goal) {

        BaseScanLinearRegression srv;

        double min_angle = -M_PI/8.0, max_angle=M_PI/8.0, min_dist=0.02, max_dist=0.8;
        nh_.getParamCached("/laser_linear_regression/min_angle", min_angle);
        nh_.getParamCached("/laser_linear_regression/max_angle", max_angle);
        nh_.getParamCached("/laser_linear_regression/min_dist", min_dist);
        nh_.getParamCached("/laser_linear_regression/max_dist", max_dist);

        srv.request.filter_minAngle = angles::from_degrees(min_angle);
        srv.request.filter_maxAngle = angles::from_degrees(max_angle);
        srv.request.filter_minDistance = min_dist;
        srv.request.filter_maxDistance = max_dist;
        std::cout << "min_dist " << min_dist << ", max_dist " << max_dist << std::endl;
        //srv.request.filter_minAngle = -M_PI / 8.0;
        //srv.request.filter_maxAngle = M_PI / 8.0;
        //srv.request.filter_minDistance = 0.02;
        //srv.request.filter_maxDistance = 0.80;

        target_distance = goal->distance;

        ros::Duration max_time(50.0);
        ros::Time stamp = ros::Time::now();
        OrientToBaseResult result;
        bool oriented = false;
        int  iterator = 0;

        while (ros::ok()) {
            ROS_INFO("Call service Client");

            if(client.call(srv)) {
                ROS_INFO("Called service LaserScanLinearRegressionService");
                //std::cout << "result: " << srv.response.center << ", " << srv.response.a << ", " << srv.response.b << std::endl;


                geometry_msgs::Twist cmd = calculateVelocityCommand(srv.response.center, srv.response.a, srv.response.b,oriented,iterator);
                cmd_pub.publish(cmd);

                std::cout << "cmd x:" << cmd.linear.x << ", y: "  << cmd.linear.y << ", z: " << cmd.angular.z << std::endl;

                if ((fabs(cmd.angular.z)  + fabs(cmd.linear.x) ) < 0.001) {

                    ROS_INFO("Point reached");
                    result.succeed = true;
                    as_.setSucceeded(result);
                    geometry_msgs::Twist zero_vel;
                    cmd_pub.publish(zero_vel);
                    break;

                }

                if  (stamp + max_time < ros::Time::now()) {
                    result.succeed = false;
                    as_.setAborted(result);
                    geometry_msgs::Twist zero_vel;
                    cmd_pub.publish(zero_vel);
                    break;
                }

            } else {
                ROS_ERROR("Failed to call service LaserScanLinearRegressionService");

                if  (stamp + max_time < ros::Time::now()) {
                    result.succeed = false;
                    as_.setAborted(result);
                    break;
                }
            }
        }

    }