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

ShapeGraspPlanner::ShapeGraspPlanner(ros::NodeHandle& nh)
{
  /*
   * Gripper model is based on having two fingers, and assumes
   * that the robot is using the moveit_simple_controller_manager
   * gripper interface, with "parallel" parameter set to true.
   */
  nh.param<std::string>("gripper/left_joint", left_joint_, "l_gripper_finger_joint");
  nh.param<std::string>("gripper/right_joint", right_joint_, "r_gripper_finger_joint");
  nh.param("gripper/max_opening", max_opening_, 0.110);
  nh.param("gripper/max_effort", max_effort_, 50.0);
  nh.param("gripper/finger_depth", finger_depth_, 0.02);
  nh.param("gripper/grasp_duration", grasp_duration_, 2.0);
  nh.param("gripper/gripper_tolerance", gripper_tolerance_, 0.02);

  /*
   * Approach is usually aligned with wrist_roll
   */
  nh.param<std::string>("gripper/approach/frame", approach_frame_, "wrist_roll_link");
  nh.param("gripper/approach/min", approach_min_translation_, 0.145);
  nh.param("gripper/approach/desired", approach_desired_translation_, 0.15);

  /*
   * Retreat is usually aligned with wrist_roll
   */
  nh.param<std::string>("gripper/retreat/frame", retreat_frame_, "wrist_roll_link");
  nh.param("gripper/retreat/min", retreat_min_translation_, 0.145);
  nh.param("gripper/retreat/desired", retreat_desired_translation_, 0.15);

  // Distance from tool point to planning frame
  nh.param("gripper/tool_to_planning_frame", tool_offset_, 0.165);
}

    /**
    * Initialize parameters from launch file
    * @param node a ROS node handle
    */
    void Circular::initializeParameters(ros::NodeHandle &node){   
       Descriptor base, range;
       // Loading parameters from a launch file
	   node.param("threshold", this->threshold_, 100); 
       node.param("min_score", this->blob_.min_score, 7);
       node.getParam("landmark_diagonal", this->landmark_diag_); 

        // Base descriptor parameters
       node.getParam("base_shape", base.invariant.shape);
       node.getParam("base_area", base.invariant.area);
       node.getParam("base_hu", base.invariant.hu);
        
       // (Similarity) range descriptor parameters
       node.getParam("range_shape", range.invariant.shape);
       node.getParam("range_area", range.invariant.area);
       node.getParam("range_hu", range.invariant.hu);

       // Rotation of camera in respect to imu
       double roll_deg, pitch_deg, yaw_deg;   
       node.param("rot_x", roll_deg, 0.0);
       node.param("rot_y", pitch_deg, 0.0);
       node.param("rot_z", yaw_deg, 0.0);
       
       this->imuToCamRot_.setRPY(roll_deg*M_PI/180, pitch_deg*M_PI/180, yaw_deg*M_PI/180); 
        
        
       this->blob_.setBase(base);
       this->blob_.setRange(range);

    }

	void initialize(UAS &uas_,
			ros::NodeHandle &nh,
			diagnostic_updater::Updater &diag_updater)
	{
		double linear_stdev, angular_stdev, orientation_stdev, mag_stdev;

		uas = &uas_;

		nh.param<std::string>("imu/frame_id", frame_id, "fcu");
		nh.param("imu/linear_acceleration_stdev", linear_stdev, 0.0003); // check default by MPU6000 spec
		nh.param("imu/angular_velocity_stdev", angular_stdev, 0.02 * (M_PI / 180.0)); // check default by MPU6000 spec
		nh.param("imu/orientation_stdev", orientation_stdev, 1.0);
		nh.param("imu/magnetic_stdev", mag_stdev, 0.0);

		setup_covariance(linear_acceleration_cov, linear_stdev);
		setup_covariance(angular_velocity_cov, angular_stdev);
		setup_covariance(orientation_cov, orientation_stdev);
		setup_covariance(magnetic_cov, mag_stdev);
		setup_covariance(unk_orientation_cov, 0.0);

		imu_pub = nh.advertise<sensor_msgs::Imu>("imu/data", 10);
		magn_pub = nh.advertise<sensor_msgs::MagneticField>("imu/mag", 10);
		temp_pub = nh.advertise<sensor_msgs::Temperature>("imu/temperature", 10);
		press_pub = nh.advertise<sensor_msgs::FluidPressure>("imu/atm_pressure", 10);
		imu_raw_pub = nh.advertise<sensor_msgs::Imu>("imu/data_raw", 10);
	}

void readParameters(ros::NodeHandle pnh){
	pnh.param("save_map_launch_package", save_map_launch_package , string("mapping_controller"));
	pnh.param("save_map_launch_file", save_map_launch_file , string("save_map"));
	pnh.param("map_file_path", map_file_path , string("/home/pi"));
	pnh.param("robot_position_topic", robot_position_topic, string("/agent1/amcl_pose"));
	pnh.param("robot_initial_position_topic", robot_initial_position_topic, string("/agent1/initialpose"));
}

bool JacoManipulation::loadParameters(const ros::NodeHandle n)
{
    ROS_DEBUG("Loading parameters");

    n.param("wpi_jaco/arm_name", arm_name_, std::string("jaco"));
    n.param("wpi_jaco/gripper_closed", gripper_closed_, 0.0);
    n.param("wpi_jaco/gripper_open", gripper_open_, 65.0);
    n.param("wpi_jaco/num_fingers", num_fingers_, 3);

    // Update topic prefix
    if (arm_name_ == "jaco2")
      topic_prefix_ = "jaco";
    else
      topic_prefix_ = arm_name_;

    if (kinova_gripper_)
      num_joints_ = num_fingers_ + NUM_JACO_JOINTS;
    else
      num_joints_ = NUM_JACO_JOINTS;

    joint_pos_.resize(num_joints_);

    ROS_INFO("arm_name: %s", arm_name_.c_str());

    ROS_INFO("Parameters loaded.");

    //! @todo MdL [IMPR]: Return is values are all correctly loaded.
    return true;
}

  CameraImpl(ros::NodeHandle& nh, ros::NodeHandle& nh_private, const openni::DeviceInfo& device_info) :
    rgb_sensor_(new SensorStreamManagerBase()),
    depth_sensor_(new SensorStreamManagerBase()),
    ir_sensor_(new SensorStreamManagerBase()),
    reconfigure_server_(nh_private)
  {
    device_.open(device_info.getUri());

    printDeviceInfo();
    printVideoModes();
    buildResolutionMap();

    device_.setDepthColorSyncEnabled(true);

    std::string rgb_frame_id, depth_frame_id;
    nh_private.param(std::string("rgb_frame_id"), rgb_frame_id, std::string("camera_rgb_optical_frame"));
    nh_private.param(std::string("depth_frame_id"), depth_frame_id, std::string("camera_depth_optical_frame"));

    if(device_.hasSensor(SENSOR_COLOR))
    {
      rgb_sensor_.reset(new SensorStreamManager(nh, device_, SENSOR_COLOR, "rgb", rgb_frame_id, resolutions_[Camera_RGB_640x480_30Hz]));
    }

    if(device_.hasSensor(SENSOR_DEPTH))
    {
      depth_sensor_.reset(new DepthSensorStreamManager(nh, device_, rgb_frame_id, depth_frame_id, resolutions_[Camera_DEPTH_640x480_30Hz]));
    }

    if(device_.hasSensor(SENSOR_IR))
    {
      ir_sensor_.reset(new SensorStreamManager(nh, device_, SENSOR_IR, "ir", depth_frame_id, resolutions_[Camera_IR_640x480_30Hz]));
    }

    reconfigure_server_.setCallback(boost::bind(&CameraImpl::configure, this, _1, _2));
  }

  MyClass()
    : p_nh("~")
  {
    sub = nh.subscribe("/humans/kinect_v2", 1, &MyClass::callback, this);
    recog_sub = nh.subscribe("/humans/recog_info", 1, &MyClass::recogCallback, this);
    dispub =  nh.advertise<std_msgs::Float64>("/displacement", 1); 

    srv = nh.advertiseService("displace", &MyClass::Service, this);    

    pt.x = 5.0;
    pt.y = 0;
    pt.z = 0;
    cv::namedWindow(OPENCV_WINDOW);

    index = 0;
    attention_t_id = 0;
    srv_switch = false;
    p_nh.param("width", width, 1000);
    p_nh.param("height", height, 500);

    o_id = 0;

    prev_time = ros::Time::now();
    now_time = ros::Time::now();
    count_time = 0;
  }

        HDPCDrive(ros::NodeHandle & nh) : geom(nh) {
            command_pub = nh.advertise<hdpc_com::Commands>("/hdpc_com/commands",1);
            reading_sub = nh.subscribe("/hdpc_com/readings",1,&HDPCDrive::readingsCallback,this);
            state_machine_client = nh.serviceClient<hdpc_com::ChangeStateMachine>("/hdpc_com/changeState");


            control_mode_serv = nh.advertiseService("set_control_mode",&HDPCDrive::set_mode,this);
            direct_command_sub = nh.subscribe("direct",1,&HDPCDrive::directDriveCallback,this);
            ackermann_command_sub = nh.subscribe("ackermann",1,&HDPCDrive::ackermannCallback,this);
            status_pub = nh.advertise<Status>("status",1);

            // TODO: change default value to something that makes sense
            nh.param("max_rotation_speed_rad_per_s",max_rotation_speed_rad_per_s,1.0);
            nh.param("max_linear_speed_m_per_s",max_linear_speed_m_per_s,0.9);
            nh.param("synchronise_steering",synchronise_steering,false);
            nh.param("zero_on_init",zero_on_init,false);

            watchdog = 0;
            desired_elevation = M_PI/2;
            control_mode = HDPCModes::MODE_INIT;
            commands.header.stamp = ros::Time::now();
            for (unsigned int i=0;i<10;i++) {
                commands.isActive[i] = false;
                commands.velocity[i] = 0;
                commands.position[i] = 0.0;
            }
        }

	CylinderDetection() :
		m_NodeHandle("~")
	{
			m_NodeHandle.param("base_frame", m_strBaseFrame, std::string("/body"));
			m_NodeHandle.param("max_range", m_dMaxRange, 5.0);
			m_NodeHandle.param("n_samples", m_nSamples, 1000);
			m_NodeHandle.param("ransac_tolerance", m_dRansacTolerance, 1.0);
			m_NodeHandle.param("parse_plane_lower_bound_threshhold", THRESHOLD_LOWER_TO_PARSE_PLANE, 10);
			m_NodeHandle.param("parse_plane_upper_bound_threshhold", THRESHOLD_UPPER_TO_PARSE_PLANE, 600);
			m_NodeHandle.param("accept_circle_threshhold", THRESHOLD_TO_ACCEPT_CIRCLE_FOUND, 10);
			m_NodeHandle.param("discretization_precision", m_dDiscretizationPrecision, 0.025);
			m_NodeHandle.param("min_num_valid_sections_to_accept_cylinder", MIN_NUM_VALID_SECTIONS_TO_ACCEPT_CYLINDER, 4);
			m_NodeHandle.param("tolerane_for_plane_concatenation", TOLERANCE_FOR_PLANE_CONCATENATION, 0.01);
			m_NodeHandle.param("maximum_radius", MAXIMUM_RADIUS, 1.0);

			ROS_INFO("Searching for vertical cylinders");
			ROS_INFO("RANSAC: %d iteration with %f tolerance", m_nSamples, m_dRansacTolerance);
			assert(m_nSamples > 0);

			// Make sure TF is ready
			ros::Duration(0.5).sleep();

			m_Subscriber = m_NodeHandle.subscribe("/vrep/depthSensor", 1, &CylinderDetection::pc_callback, this);

			// Initialize the random seed for RANSAC
			srand(time(NULL));

	}

    FollowerFSM() : hold(true),
        currentState(STOPPED),
        nextState(STOPPED),
        nPriv("~")


    {
        //hold = true;
        //currentState = STOPPED;
        //nextState = STOPPED;

        nPriv.param<std::string>("world_frame", world_frame, "world_lol");
        nPriv.param<std::string>("robot_frame", robot_frame, "robot");
        nPriv.param<std::string>("fixed_frame", fixed_frame_id, "odom");
        nPriv.param<double>("minimum_distance", minimum_distance, 2);
        nPriv.param<double>("planner_activation_distance", planner_activation_distance, 1.7);
        nPriv.param("minimum_planner_distance", minimum_planner_distance, 1.7);
        nPriv.param("distance_to_target", distance_to_target, 1.5);
        nPriv.param<double>("kv", kv, 0.03);
        nPriv.param<double>("kalfa", kalfa, 0.08);
        //nPriv.param<double>("rate", frequency, 10);
        rate = new ros::Rate(10.0);

        nPriv.param<std::string>("control_type", cType, "planner");

        if(cType == "planner")
        { ac = new MoveBaseClient("move_base", true);}

        notReceivedNumber = 0;
        sub = n.subscribe("person_position", 1, &FollowerFSM::receiveInformation, this);
        cmdPub = n.advertise<geometry_msgs::Twist>("twist_topic", 1);

        timer = n.createTimer(ros::Duration(0.1), &FollowerFSM::checkInfo, this); //If we didn't receive any position on 2 seconds STOP the robot!
        timer.start();
    }

void load_params(ros::NodeHandle &nh) {
	string str_calib;
	stringstream ss_calib;

	nh.param("D", str_calib, string(""));
	restoreVec(str_calib, cam_info.D);

	nh.param("K", str_calib, string(""));
	 (str_calib, cam_info.K);

	nh.param("R", str_calib, string(""));
	restoreVec(str_calib, cam_info.R);

	nh.param("P", str_calib, string(""));
	restoreVec(str_calib, cam_info.P);
	
	std::string frame_id;
	nh.param<std::string>("frame_id",frame_id,"/camera");// /viz/uav_cam_front/camera
	cam_info.header.frame_id = frame_id;

	cout
			<< "Current cam_info:"
			<< endl;
	cout << "    <param name=\"D\" type=\"string\" value=\"" << arrayToString(
			cam_info.D) << "\"/>" << endl;
	cout << "    <param name=\"K\" type=\"string\" value=\"" << arrayToString(
			cam_info.K) << "\"/>" << endl;
	cout << "    <param name=\"R\" type=\"string\" value=\"" << arrayToString(
			cam_info.R) << "\"/>" << endl;
	cout << "    <param name=\"P\" type=\"string\" value=\"" << arrayToString(
			cam_info.P) << "\"/>" << endl;
}

int NodeClass::init() 
{
	if (n.hasParam("ComPort"))
	{
		n.getParam("ComPort", sComPort);
		ROS_INFO("Loaded ComPort parameter from parameter server: %s",sComPort.c_str());
	}
	else
	{
		sComPort ="/dev/ttyUSB0";
		ROS_WARN("ComPort Parameter not available, using default Port: %s",sComPort.c_str());
	}

	n.param("message_timeout", relayboard_timeout_, 2.0);

	n.param("protocol_version", protocol_version_, 1);
    
	m_SerRelayBoard = new SerRelayBoard(sComPort, protocol_version_);
	ROS_INFO("Opened Relayboard at ComPort = %s", sComPort.c_str());

	m_SerRelayBoard->init();

	// Init member variable for EM State
	EM_stop_status_ = ST_EM_ACTIVE;
	duration_for_EM_free_ = ros::Duration(1);

	return 0;
}

WebVideoServer::WebVideoServer(ros::NodeHandle &nh, ros::NodeHandle &private_nh) :
    nh_(nh), image_transport_(nh),
    handler_group_(http_server::HttpReply::stock_reply(http_server::HttpReply::not_found))
{
  cleanup_timer_ = nh.createTimer(ros::Duration(0.5), boost::bind(&WebVideoServer::cleanup_inactive_streams, this));

  int port;
  private_nh.param("port", port, 8080);

  int server_threads;
  private_nh.param("server_threads", server_threads, 1);

  private_nh.param("ros_threads", ros_threads_, 2);

  stream_types_["mjpeg"] = boost::shared_ptr<ImageStreamerType>(new MjpegStreamerType());
  stream_types_["vp8"] = boost::shared_ptr<ImageStreamerType>(new LibavStreamerType("webm", "libvpx", "video/webm"));

  handler_group_.addHandlerForPath("/", boost::bind(&WebVideoServer::handle_list_streams, this, _1, _2));
  handler_group_.addHandlerForPath("/stream", boost::bind(&WebVideoServer::handle_stream, this, _1, _2));
  handler_group_.addHandlerForPath("/stream_viewer", boost::bind(&WebVideoServer::handle_stream_viewer, this, _1, _2));
  handler_group_.addHandlerForPath("/snapshot", boost::bind(&WebVideoServer::handle_snapshot, this, _1, _2));

  server_.reset(new http_server::HttpServer("0.0.0.0", boost::lexical_cast<std::string>(port),
      boost::bind(ros_connection_logger, handler_group_, _1, _2), server_threads));
}

//! Initialize plugin - called in server constructor
void srs_env_model::CCMapPlugin::init(ros::NodeHandle & node_handle)
{
	m_collisionMapVersion = 0;
	m_dataBuffer->boxes.clear();
	m_data->boxes.clear();

	// Read parameters

	// Get collision map radius
	node_handle.param("collision_map_radius", m_collisionMapLimitRadius, COLLISION_MAP_RADIUS_LIMIT );

	// Get collision map topic name
	node_handle.param("collision_map_publisher", m_cmapPublisherName, COLLISION_MAP_PUBLISHER_NAME );

	// Get FID to which will be points transformed when publishing collision map
	node_handle.param("collision_map_frame_id", m_cmapFrameId, COLLISION_MAP_FRAME_ID ); //

	// Create and publish service - get collision map
	m_serviceGetCollisionMap = node_handle.advertiseService( GetCollisionMap_SRV, 	&CCMapPlugin::getCollisionMapSrvCallback, this);

	// Create and publish service - is new collision map
	m_serviceIsNewCMap = node_handle.advertiseService( IsNewCMap_SRV, &CCMapPlugin::isNewCmapSrvCallback, this );

	// Create and publish service - lock map
	m_serviceLockCMap = node_handle.advertiseService( LockCMap_SRV, &CCMapPlugin::lockCmapSrvCallback, this );

	// Create and publish service - remove cube from map
	m_serviceRemoveCube = node_handle.advertiseService( RemoveCubeCMP_SRV, &CCMapPlugin::removeBoxCallback, this );

	// Create and publish service - add cube to map
	m_serviceAddCube = node_handle.advertiseService( AddCubeCMP_SRV, &CCMapPlugin::addBoxCallback, this );

	// Connect publishing services
	pause( false, node_handle );
}

void FreeFloatingPids::InitPID(control_toolbox::Pid &_pid, const ros::NodeHandle&_node, const bool &_use_dynamic_reconfig)
{
    if(_use_dynamic_reconfig)
    {
        // classical PID init
        _pid.init(_node);
    }
    else
    {
        control_toolbox::Pid::Gains gains;

        // Load PID gains from parameter server
        if (!_node.getParam("p", gains.p_gain_))
        {
          ROS_ERROR("No p gain specified for pid.  Namespace: %s", _node.getNamespace().c_str());
          return;
        }
        // Only the P gain is required, the I and D gains are optional and default to 0:
        _node.param("i", gains.i_gain_, 0.0);
        _node.param("d", gains.d_gain_, 0.0);

        // Load integral clamp from param server or default to 0
        double i_clamp;
        _node.param("i_clamp", i_clamp, 0.0);
        gains.i_max_ = std::abs(i_clamp);
        gains.i_min_ = -std::abs(i_clamp);
        _pid.setGains(gains);
    }
}