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

void CalcMin::chatterCallback(const cob_roboskin::roboskinCAN2 msg)
{
  n.setParam("/data1", msg.data1);
  n.setParam("/data2", msg.data2);
  n.setParam("/data3", msg.data3);
  n.setParam("/data4", msg.data4);
}

  void processGoal(const geometry_msgs::Pose& start_block_pose, const geometry_msgs::Pose& end_block_pose )
  {
    // Change the goal constraints on the servos to be less strict, so that the controllers don't die. this is a hack
    nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/elbow_pitch_joint/goal", 2); // originally it was 0.45
    nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/shoulder_pan_joint/goal", 2); // originally it was 0.45
    nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/wrist_pitch_joint/goal", 2); // originally it was 0.45
    nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/gripper_roll_joint/goal", 2); // originally it was 0.45
    nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/wrist_pitch_joint/goal", 2); // originally it was 0.45

    if( !pickAndPlace(start_block_pose, end_block_pose) )
    {
      ROS_ERROR_STREAM_NAMED("pick_place_moveit","Pick and place failed");

      if(action_server_.isActive()) // Make sure we haven't sent a fake goal
      {
        // Report failure
        result_.success = false;
        action_server_.setSucceeded(result_);
      }
    }
    else
    {
      if(action_server_.isActive()) // Make sure we haven't sent a fake goal
      {
        // Report success
        result_.success = true;
        action_server_.setSucceeded(result_);
      }
    }

    // TODO: remove
    ros::shutdown();
  }

void BallDetector::updateLowThreshold(const geometry_msgs::Vector3::ConstPtr& thresh){
  int h = thresh->x-1, s = thresh->y-1, v = thresh->z-1;
  ROS_INFO("Updated ball low HSV threshold to h,s,v: %d,%d,%d",h,s,v);
  lowThresh = cv::Scalar(h,s,v,0);
  nh.setParam("thresh/high/h",h);
  nh.setParam("thresh/high/s",s);
  nh.setParam("thresh/high/v",v);
}

void BallDetector::updateHighThreshold(const geometry_msgs::Vector3::ConstPtr& thresh){
  int h = thresh->x+1, s = thresh->y+1, v = thresh->z+1;
  ROS_INFO("Updated ball high HSV threshold to h,s,v: %d,%d,%d",h,s,v);
  highThresh = cv::Scalar(h,s,v,0);
  nh.setParam("thresh/low/h",h);
  nh.setParam("thresh/low/s",s);
  nh.setParam("thresh/low/v",v);
}

std::vector<std::string> ThrusterAllocator::initControl(ros::NodeHandle &nh, double map_threshold)
{
  std::vector<std::string> controlled_axes;
  const std::vector<std::string> axes{"x", "y", "z", "roll", "pitch", "yaw"};

  for(size_t axis = 0; axis < 6; axis++)
  {
    if(max_wrench[axis] > 1e-6)
    {
      char param[FILENAME_MAX];
      sprintf(param, "controllers/%s", axes[axis].c_str());
      if(nh.hasParam(param))
      {
        controlled_axes.push_back(axes[axis]);
        // push to position PID
        sprintf(param, "controllers/%s/position/i_clamp", axes[axis].c_str());
        nh.setParam(param, max_wrench[axis]);
        // push to velocity PID
        sprintf(param, "controllers/%s/velocity/i_clamp", axes[axis].c_str());
        nh.setParam(param, max_wrench[axis]);
      }
    }
  }

  // threshold on map before pseudo-inverse
  for(int r = 0; r < 6; ++r)
  {
    for(int c = 0; c < map.cols(); ++c)
    {
      if(std::abs(map(r,c)) < map_threshold)
        map(r,c) = 0;
    }
  }

  inverse_map.resize(map.cols(), map.rows());
  Eigen::JacobiSVD<Eigen::MatrixXd> svd_M(map, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::VectorXd dummy_in(map.rows());
  Eigen::VectorXd dummy_out(map.cols());
  unsigned int i,j;
  for(i=0;i<map.rows();++i)
  {
    dummy_in.setZero();
    dummy_in(i) = 1;
    dummy_out = svd_M.solve(dummy_in);
    for(j = 0; j<map.cols();++j)
      inverse_map(j,i) = dummy_out(j);
  }
  return controlled_axes;
}

    BaseCamera *createCamera()
    {
        std::string cameraType;
        node.param("camera_type", cameraType, std::string("usb"));

        node.param("video_device", videoDevice, std::string(""));
        std::string resolution;
        node.getParam("resolution", resolution);
        cameraWidth = std::stoi(resolution.substr(0, resolution.find('x')));
        cameraHeight = std::stoi(resolution.substr(resolution.find('x') + 1));
        node.param("framerate", framerate, 30);
        node.param("media_path", mediaPath, std::string("~/.rospilot/media"));
        wordexp_t p;
        wordexp(mediaPath.c_str(), &p, 0);
        if (p.we_wordc != 1) {
            ROS_ERROR("Got too many words when expanding media path: %s",
                    mediaPath.c_str());
        }
        else {
            mediaPath = p.we_wordv[0];
        }
        wordfree(&p);

        if (videoDevice.size() == 0) {
            videoDevice = findCameraDevice();
            node.setParam("video_device", videoDevice);
        }

        if (cameraType == "ptp") {
            return new PtpCamera();
        }
        else if (cameraType == "usb") {
            ROS_INFO("Requesting camera res %dx%d", cameraWidth, cameraHeight);
            UsbCamera *camera = new UsbCamera(videoDevice, cameraWidth, cameraHeight, framerate);
            // Read the width and height, since the camera may have altered it to
            // something it supports
            cameraWidth = camera->getWidth();
            cameraHeight = camera->getHeight();
            std::stringstream ss;
            ss << cameraWidth << "x" << cameraHeight;
            node.setParam("resolution", ss.str());
            ROS_INFO("Camera selected res %dx%d", cameraWidth, cameraHeight);
            return camera;
        }
        else {
            ROS_FATAL("Unsupported camera type: %s", cameraType.c_str());
            return nullptr;
        }
    }

  void PluginPlanner::resetOldParameters(ros::NodeHandle& nh)
  {
    ROS_INFO("Loading from pre-hydro parameter style");
    std::vector < XmlRpc::XmlRpcValue > plugins;
    plugins.resize(6);
    plugins[0] = "Oscillation"; // discards oscillating motions (assisgns cost -1)
    plugins[1] = "Obstacle";    // discards trajectories that move into obstacles
    plugins[2] = "GoalAlign";   // prefers trajectories that make the nose go towards (local) nose goal
    plugins[3] = "PathAlign";   // prefers trajectories that keep the robot nose on nose path
    plugins[4] = "PathDist";    // prefers trajectories on global path
    plugins[5] = "GoalDist";    // prefers trajectories that go towards (local) goal, based on wave propagation

    XmlArray critics;
    critics.setArray(&plugins);
    nh.setParam("critics", critics);

    move_parameter(nh, "path_distance_bias", "PathAlign/scale", 32.0, false);
    move_parameter(nh, "goal_distance_bias", "GoalAlign/scale", 24.0, false);
    move_parameter(nh, "path_distance_bias", "PathDist/scale", 32.0);
    move_parameter(nh, "goal_distance_bias", "GoalDist/scale", 24.0);
    move_parameter(nh, "occdist_scale",      "Obstacle/scale", 0.01);

    move_parameter(nh, "max_scaling_factor", "Obstacle/max_scaling_factor", 0.2);
    move_parameter(nh, "scaling_speed",      "Obstacle/scaling_speed", 0.25);

}

// Subscribe to models of interest.  Currently, we find and subscribe
// to the first 'laser' model and the first 'position' model.  Returns
// 0 on success (both models subscribed), -1 otherwise.
//
// Eventually, we should provide a general way to map stage models onto ROS
// topics, similar to Player .cfg files.
int
StageNode::SubscribeModels()
{
  n_.setParam("/use_sim_time", true);

  for (size_t r = 0; r < this->positionmodels.size(); r++)
  {
    if(this->lasermodels[r])
    {
      this->lasermodels[r]->Subscribe();
    }
    else
    {
      ROS_ERROR("no laser");
      return(-1);
    }
    if(this->positionmodels[r])
    {
      this->positionmodels[r]->Subscribe();
    }
    else
    {
      ROS_ERROR("no position");
      return(-1);
    }
    laser_pubs_.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN,r), 10));
    odom_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(ODOM,r), 10));
    ground_truth_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH,r), 10));
    cmdvel_subs_.push_back(n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL,r), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1)));
  }
  clock_pub_ = n_.advertise<rosgraph_msgs::Clock>("/clock",10);
  return(0);
}

std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
{
  std::string full_param_name;
  std::string full_radius_param_name;
  std::vector<geometry_msgs::Point> points;

  if (nh.searchParam("footprint", full_param_name))
  {
    XmlRpc::XmlRpcValue footprint_xmlrpc;
    nh.getParam(full_param_name, footprint_xmlrpc);
    if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString)
    {
      if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
      {
        writeFootprintToParam(nh, points);
      }
    }
    else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
    {
      points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
      writeFootprintToParam(nh, points);
    }
  }
  else if (nh.searchParam("robot_radius", full_radius_param_name))
  {
    double robot_radius;
    nh.param(full_radius_param_name, robot_radius, 1.234);
    points = makeFootprintFromRadius(robot_radius);
    nh.setParam("robot_radius", robot_radius);
  }
  // Else neither param was found anywhere this knows about, so
  // defaults will come from dynamic_reconfigure stuff, set in
  // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
  return points;
}

int main(int argc, char **argv) {

	std::vector<double> state_probability(7, 0.0);

	//TODO: Should we think about introducing some "undefined" state ?
	//or is it of no use?

	//make the robot follow the right wall at the beginning of the algorithm
	state current_state = FOLLOW_RIGHT;
	state_probability[current_state] = 1.0;

	ros::init(argc, argv, "WallFollowerController"); // Name of the node is WallFollowerController

	n.setParam("/param_gain", 5.0);

	ir_sub = n.subscribe("/sensors/transformed/ADC", 1, ir_readings_update); // Subscribing to the processed ir values topic.

	ros::Rate loop_rate(UPDATE_RATE);

	// Creates a WallFollower object.
	WallFollower wf;
	// Runs the initiation method (initializes the variable) on the WallFollower object.
	wf.init();

	while (ros::ok()) {
		ros::spinOnce();
		loop_rate.sleep();
		// Runs the step method of the wallfollower object, which remembers the state through fields (variables).
		wf.step();
	}
}

void CalcMin::chatterCallback1(const fts_Omega160::fts msg)
{
int data1;
int data2;
int data3;
int data4;
int stop;

  n.setParam("/stop", 0);
  req.data = std::vector<int>(8);
  req.data[4] = msg.Fx;
  req.data[5] = msg.Fy;
  req.data[6] = msg.Fz;

	if (n.getParam("/data1", data1)){
		  req.data[0] = data1;
		}
	if (n.getParam("/data2", data2)){
		  req.data[1] = data2;
		}
	if (n.getParam("/data3", data3)){
		  req.data[2] = data3;
		}
	if (n.getParam("/data4", data4)){
		  req.data[3] = data4;
		}
	if (n.getParam("/stop", stop)){
		  req.data[7] = stop;
		}


  pub = n.advertise<std_msgs::Int32MultiArray>("/min_pub",1000); 
  pub.publish(req);
}

	bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res)
	{
		mav_frame = static_cast<MAV_FRAME>(req.mav_frame);
		const std::string mav_frame_str = utils::to_string(mav_frame);
		sp_nh.setParam("mav_frame", mav_frame_str);
		res.success = true;
		return true;
	}

		/*!
		* \brief Executes the service callback for set_operation_mode.
		*
		* Changes the operation mode.
		* \param req Service request
		* \param res Service response
		*/
		bool srvCallback_SetOperationMode(	cob_srvs::SetOperationMode::Request &req,
											cob_srvs::SetOperationMode::Response &res )
		{
			ROS_INFO("Set operation mode to [%s]", req.operation_mode.data.c_str());
			n_.setParam("OperationMode", req.operation_mode.data.c_str());
			res.success.data = true; // 0 = true, else = false
			return true;
		}

void InfoPublisher::reset( ros::NodeHandle& nh )
{
  // We latch as we only publish once
  pub_ = nh.advertise<naoqi_bridge_msgs::StringStamped>( topic_, 1, true );

  std::string robot_desc = naoqi::tools::getRobotDescription(robot_);
  nh.setParam("/robot_description", robot_desc);
  std::cout << "load robot description from file" << std::endl;

  is_initialized_ = true;
}

void AmclNode::savePoseToServer()
{
  // We need to apply the last transform to the latest odom pose to get
  // the latest map pose to store.  We'll take the covariance from
  // last_published_pose.
  tf::Pose map_pose = latest_tf_.inverse() * latest_odom_pose_;
  double yaw,pitch,roll;
  map_pose.getBasis().getEulerYPR(yaw, pitch, roll);

  ROS_DEBUG("Saving pose to server. x: %.3f, y: %.3f", map_pose.getOrigin().x(), map_pose.getOrigin().y() );

  private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x());
  private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y());
  private_nh_.setParam("initial_pose_a", yaw);
  private_nh_.setParam("initial_cov_xx",
                                  last_published_pose.pose.covariance[6*0+0]);
  private_nh_.setParam("initial_cov_yy",
                                  last_published_pose.pose.covariance[6*1+1]);
  private_nh_.setParam("initial_cov_aa",
                                  last_published_pose.pose.covariance[6*5+5]);
}