C++ private_nh函数代码示例

int main(int argc, char* argv[] ) {
    ros::init(argc, argv, "sdf_2d");
    ros::NodeHandle nh("sdf_main");

    ros::NodeHandle private_nh("~");
    bool p_bag_mode;
    private_nh.param("p_bag_mode", p_bag_mode, false);

    sdfslam::SignedDistanceField sdf;

    if (!p_bag_mode)
        ros::spin();
    else {
        bool cont = true;
        unsigned int microseconds = 10000;

        while (ros::ok && cont) {
            cont = sdf.checkTimeout();
            ros::spinOnce();
            usleep(microseconds);
        }
    }
    ROS_INFO("shutting down sdf slam..");

    return 0;
}

void OrientGoal::initialize(std::string name,tf::TransformListener* tf,
    costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap,
    geometry_msgs::Quaternion* goal_orientation ){
  if(!initialized_){
    name_ = name;
    tf_ = tf;
    global_costmap_ = global_costmap;
    local_costmap_ = local_costmap;
    goal_orientation_ = goal_orientation;

    //get some parameters from the parameter server
    ros::NodeHandle private_nh("~/" + name_);
    ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");

    //we'll simulate every degree by default
    private_nh.param("sim_granularity", sim_granularity_, 0.017);
    private_nh.param("frequency", frequency_, 20.0);

    blp_nh.param("acc_lim_th", acc_lim_th_, 1.0);
    blp_nh.param("max_rotational_vel", max_rotational_vel_, 1.0);
    blp_nh.param("min_in_place_rotational_vel", min_rotational_vel_, 0.4);
    blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10);

    local_costmap_->getCostmapCopy(costmap_);
    world_model_ = new base_local_planner::CostmapModel(costmap_);

    initialized_ = true;
  }
  else{
    ROS_ERROR("You should not call initialize twice on this object, doing nothing");
  }
}

void BoxDetectionNode::loadParameters()
{
    ros::NodeHandle private_nh("~");
#define GOP(key, default_value) getOptionalParameter(private_nh, #key, parameters_.key##_, default_value)
#define GRP(key) getRequiredParameter(private_nh, #key, parameters_.key##_)
    GOP(point_cloud_topic, std::string("/camera/depth/points"));
    GOP(target_frame_id, std::string("base_link"));
    GOP(have_action_server_debug_output, true);
    GOP(have_box_detection_debug_output, true);

    GOP(box_plane_points_min, 100);
    GRP(box_plane_size_min);
    GRP(box_plane_size_max);
    GOP(detection_timeout, 30.0);
    GOP(plane_fitting_distance_threshold, 0.02);
    GOP(plane_fitting_max_iterations, 50);
    GOP(downsampling_leaf_size, 0.005);
    GOP(clusterization_tolerance, 0.02);

    GOP(have_plane_publisher, true);
    GOP(plane_publishing_rate, 2.0);

    GOP(have_collision_object_publisher, true);
    GOP(collision_objects_basename, std::string("box"));

    GOP(have_visualization_marker_publisher, true);
    GOP(visualization_marker_namespace, std::string("tedusar_box_detection"));
#undef GOP
#undef GRP
}

int main(int argc, char** argv){
  ros::init(argc, argv, "turtlebot_car");
  
  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  //we get the name of the odometry topic
  private_nh.getParam("odometry_topic_name", mOdometry_topic_name);

  //we get paramaters that define the pose of the URG
  //position
  x_laser=0.0;  y_laser=0.0;  z_laser=0.0;
  private_nh.getParam("x_laser", x_laser);private_nh.getParam("y_laser", y_laser);private_nh.getParam("z_laser", z_laser);

  //orientation
  roll_laser=0.0;  pitch_laser=0.0;  yaw_laser=0.0;
  private_nh.getParam("roll_laser", roll_laser);private_nh.getParam("pitch_laser", pitch_laser);private_nh.getParam("yaw_laser", yaw_laser);
  
  //we define the subscriber to odometry
  ros::Subscriber sub = nh.subscribe<nav_msgs::Odometry>(mOdometry_topic_name, 10, &odometryCallback);

  std::cout<<"***************[ STARING turtlebot_car_node]***************** "<<std::endl;

  ros::spin();
  return 0;
};

  void DWAPlannerROS::initialize(
      std::string name,
      tf::TransformListener* tf,
      costmap_2d::Costmap2DROS* costmap_ros) {
    if (! isInitialized()) {

      ros::NodeHandle private_nh("~/" + name);
      g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
      l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
      tf_ = tf;
      costmap_ros_ = costmap_ros;
      costmap_ros_->getRobotPose(current_pose_);

      // make sure to update the costmap we'll use for this cycle
      costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();

      planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());

      //create the actual planner that we'll use.. it'll configure itself from the parameter server
      dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));
      
      initialized_ = true;

      dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);
      dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2);
      dsrv_->setCallback(cb);
    }
    else{
      ROS_WARN("This planner has already been initialized, doing nothing.");
    }
  }

int main(int argc, char **argv)
{
  ros::init(argc, argv, "waypoints_marker_publisher");
  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  //subscribe traffic light
  ros::Subscriber light_sub = nh.subscribe("light_color", 10, receiveAutoDetection);
  ros::Subscriber light_managed_sub = nh.subscribe("light_color_managed", 10, receiveManualDetection);

  //subscribe global waypoints
  ros::Subscriber lane_array_sub = nh.subscribe("lane_waypoints_array", 10, laneArrayCallback);
  ros::Subscriber traffic_array_sub = nh.subscribe("traffic_waypoints_array", 10, laneArrayCallback);

  //subscribe local waypoints
  ros::Subscriber temporal_sub = nh.subscribe("temporal_waypoints", 10, temporalCallback);
  ros::Subscriber closest_sub = nh.subscribe("closest_waypoint", 10, closestCallback);

  //subscribe config
  ros::Subscriber config_sub = nh.subscribe("config/lane_stop", 10, configParameter);

  g_local_mark_pub = nh.advertise<visualization_msgs::MarkerArray>("local_waypoints_mark", 10, true);
  g_global_mark_pub = nh.advertise<visualization_msgs::MarkerArray>("global_waypoints_mark", 10, true);

  //initialize path color
  _initial_color.b = 1.0;
  _initial_color.g = 0.7;
  _global_color = _initial_color;
  _global_color.a = g_global_alpha;
  g_local_color = _initial_color;
  g_local_color.a = g_local_alpha;

  ros::spin();

}

void JointStatePublisher::init()
{
    ros::NodeHandle private_nh("~");

    std::string yaw_joint_name, pitch_joint_name;
    private_nh.param<std::string>("yaw_joint_name", yaw_joint_name, std::string("sh_yaw_joint"));
    private_nh.param<std::string>("pitch_joint_name", pitch_joint_name, std::string("sh_pitch_joint"));

    double frequency;
    private_nh.param<double>("frequency", frequency, 30.0);

    sh_joint_states_.name.resize(2);
    sh_joint_states_.position.resize(2);
    sh_joint_states_.name[0] = yaw_joint_name;
    sh_joint_states_.position[0] = 0.0;
    sh_joint_states_.name[1] = pitch_joint_name;
    sh_joint_states_.position[1] = 0.0;

    joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("sh_joint_state", 1);

    servo_1_state_sub_ = nh_.subscribe("servo_yaw_state", 1, &JointStatePublisher::servo1StateMsgCB, this);
    servo_2_state_sub_ = nh_.subscribe("servo_pitch_state", 1, &JointStatePublisher::servo2StateMsgCB, this);

    publish_joint_state_timer_ = nh_.createTimer(ros::Duration(1/frequency), &JointStatePublisher::publishJointStateTimerCB, this, false );
}

int main(int argc, char *argv[])
{
#if defined(USE_POSIX_SHARED_MEMORY)
  attach_ShareMem();

  while (1)
    {
      CvMemStorage *houghStorage = cvCreateMemStorage(0);
      IplImage *frame = getImage_fromSHM();
      process_image_common(frame);
      cvReleaseImage(frame);
    }

  detach_ShareMem();
#else
  ros::init(argc, argv, "line_ocv");
  ros::NodeHandle n;
  ros::NodeHandle private_nh("~");
  std::string image_topic_name;
  private_nh.param<std::string>("image_raw_topic", image_topic_name, "/image_raw");
  ROS_INFO("Setting image topic to %s", image_topic_name.c_str());

  ros::Subscriber subscriber = n.subscribe(image_topic_name, 1, lane_cannyhough_callback);

  image_lane_objects = n.advertise<lane_detector::ImageLaneObjects>("lane_pos_xy", 1);

  ros::spin();
#endif

  return 0;
}

SteeringDriver::SteeringDriver(int argc, char** argv, std::string node_name) {
    // Setup ROS stuff
    ros::init(argc, argv, "steering_driver");
    ros::NodeHandle private_nh("~");
    ros::Rate loop_rate(10);

    // Setup Subscriber(s)
    twist_subscriber =
    private_nh.subscribe("/cmd_vel", 10, &SteeringDriver::twistCallback, this);

    // Get Params
    SB_getParam(private_nh, "port", port, (std::string) "/dev/ttyACM0");
    SB_getParam(private_nh, "max_abs_linear_speed", max_abs_linear_speed, 2.0);
    SB_getParam(
    private_nh, "max_abs_angular_speed", max_abs_angular_speed, 1.0);
    // Ensure that the values given for max absolute speeds are positive
    max_abs_linear_speed  = fabs(max_abs_linear_speed);
    max_abs_angular_speed = fabs(max_abs_angular_speed);

    // Setup Arduino stuff
    // Get the Arduino port from a ros param
    // TODO - Give some indication when we attach/detach from the Steering
    // Controller. Could be as simple as checking how long it was since we last
    // received a message
    // Open the given serial port
    arduino.Open(port);
    arduino.SetBaudRate(LibSerial::SerialStreamBuf::BAUD_9600);
    arduino.SetCharSize(LibSerial::SerialStreamBuf::CHAR_SIZE_8);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "laserscan2costmap");

  tf::TransformListener tf_listener;
  g_tf_listenerp = &tf_listener;

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  private_nh.param<double>("resolution", g_resolution, 0.1);
  private_nh.param<int>("scan_size_x", g_scan_size_x, 1000);
  private_nh.param<int>("scan_size_y", g_scan_size_y, 1000);
  private_nh.param<int>("map_size_x", g_map_size_x, 500);
  private_nh.param<int>("map_size_y", g_map_size_y, 500);
  private_nh.param<std::string>("scan_topic", g_scan_topic, "/scan");
  private_nh.param<std::string>("sensor_frame", g_sensor_frame, "/velodyne");

  ros::Subscriber laserscan_sub = nh.subscribe(g_scan_topic, 1, laserScanCallback);

  g_map_pub = nh.advertise<nav_msgs::OccupancyGrid>("/ring_ogm", 1);

  ros::spin();

  return 0;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "voxel_grid_filter");

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  private_nh.getParam("points_topic", POINTS_TOPIC);
  private_nh.getParam("output_log", _output_log);
  if(_output_log == true){
	  char buffer[80];
	  std::time_t now = std::time(NULL);
	  std::tm *pnow = std::localtime(&now);
	  std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow);
	  filename = "voxel_grid_filter_" + std::string(buffer) + ".csv";
	  ofs.open(filename.c_str(), std::ios::app);
  }

  // Publishers
  filtered_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10);
  points_downsampler_info_pub = nh.advertise<points_downsampler::PointsDownsamplerInfo>("/points_downsampler_info", 1000);

  // Subscribers
  ros::Subscriber config_sub = nh.subscribe("config/voxel_grid_filter", 10, config_callback);
  ros::Subscriber scan_sub = nh.subscribe(POINTS_TOPIC, 10, scan_callback);

  ros::spin();

  return 0;
}

WmObjectCapture::WmObjectCapture(ros::NodeHandle private_nh_)
: nh_(),
  objectFrameId_("/object"),
  cameraFrameId_("/camera_link"),
  cameraTopicId_("/camera/depth_registered/points"),
  object_(new pcl::PointCloud<pcl::PointXYZRGB>),
  object_octree_(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB>(128.0f)),
  max_z_(0.5),
  making_object_(true),
  unique_shot_(false)
{
	ros::NodeHandle private_nh(private_nh_);
	private_nh.param("objectFrameId", objectFrameId_, objectFrameId_);
	private_nh.param("cameraFrameId", cameraFrameId_, cameraFrameId_);
	private_nh.param("cameraTopicId", cameraTopicId_, cameraTopicId_);
	private_nh.param("pointcloud_max_z", max_z_,max_z_);
	private_nh.param("make_object", making_object_,making_object_);
	private_nh.param("unique_shot", unique_shot_,unique_shot_);

	pointCloudSub_ = new message_filters::Subscriber<sensor_msgs::PointCloud2> (nh_, cameraTopicId_, 5);
	tfPointCloudSub_ = new tf::MessageFilter<sensor_msgs::PointCloud2> (*pointCloudSub_, tfListener_, cameraFrameId_, 5);
	tfPointCloudSub_->registerCallback(boost::bind(&WmObjectCapture::insertCloudCallback, this, _1));

	objectService_ = nh_.advertiseService("object", &WmObjectCapture::objectSrv, this);
	objectPub_ =  nh_.advertise<sensor_msgs::PointCloud2>("/object", 1, true);

	object_octree_->setInputCloud(object_);

	initMarkers();
}

void StepbackRecovery::initialize(std::string name, tf::TransformListener* tf,
                                  costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap) {
    if(!initialized_) {
        name_ = name;
        tf_ = tf;
        global_costmap_ = global_costmap;
        local_costmap_ = local_costmap;

        //get some parameters from the parameter server
        ros::NodeHandle private_nh("~/" + name_);
        ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");

        //Stepping back 10 cm per default
        private_nh.param("stepback_length", stepback_length_, 0.1);
        private_nh.param("frequency", frequency_, 10.0);

        blp_nh.param("acc_lim_th", acc_lim_th_, 3.2);
        blp_nh.param("max_rotational_vel", max_rotational_vel_, 1.0);
        blp_nh.param("min_in_place_rotational_vel", min_rotational_vel_, 0.4);
        blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10);

        world_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap());

        initialized_ = true;
    }
    else {
        ROS_ERROR("You should not call initialize twice on this object, doing nothing");
    }
}

ScanManipulation::ScanManipulation(ros::Rate &publish_retry_rate) :
    publish_retry_rate_(publish_retry_rate)
{
    nh_ = ros::NodeHandle("");
    laser_sub_ = nh_.subscribe<sensor_msgs::LaserScan>("laser_in", 100,
                                                                    &ScanManipulation::laserCB, this);
    pointcloud_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("pointcloud_in", 10,
                                                                    &ScanManipulation::pointcloudCB, this);

    laser_pub_ = nh_.advertise<sensor_msgs::LaserScan>("laser_out", 100);
    pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("pointcloud_out", 10);

    ros::NodeHandle private_nh("~");
    reconf_server_.reset(new ReconfigureServer(private_nh));
    reconf_server_->setCallback(boost::bind(&ScanManipulation::configCB, this, _1, _2));

    private_nh.param("frame_id_new", frame_id_new_, frame_id_new_);
    private_nh.param("frame_id_to_replace", frame_id_to_replace_, frame_id_to_replace_);
    double msg_delay_in_milliseconds, time_offset_in_milliseconds;
    private_nh.param("msg_delay_milliseconds", msg_delay_in_milliseconds, msg_delay_.toSec() * 1000);
    private_nh.param("time_offset_milliseconds", time_offset_in_milliseconds, time_offset_.toSec() * 1000);
    msg_delay_ = ros::Duration(msg_delay_in_milliseconds/1000);
    time_offset_ = ros::Duration(time_offset_in_milliseconds/1000);
    if (private_nh.hasParam("publish_retry_rate"))
    {
        double retry_rate;
        private_nh.param("publish_retry_rate", retry_rate, 100.0);
        publish_retry_rate_ = ros::Rate(retry_rate);
    }
}

    AppLoader::AppLoader()
    {
        ros::NodeHandle private_nh("~");
        ros::NodeHandle nh;

        rtp_manager_destroy_ = false;

        //get app name and type
        private_nh.param("app_name", app_name_, std::string("app_demo"));
        private_nh.param("app_type", app_type_, std::string("micros_swarm/AppDemo"));

        ros::ServiceClient client = nh.serviceClient<app_loader::AppLoad>("app_loader_load_app");
        app_loader::AppLoad srv;
        srv.request.name = app_name_;
        srv.request.type = app_type_;
        if (client.call(srv))
        {
            ROS_INFO("[AppLoader]: App %s loaded successfully.", app_name_.c_str());
        }
        else
        {
            ROS_ERROR("[AppLoader]: Failed to load App %s.", app_name_.c_str());
        }

        //when the rtp manager was destroyed, automatically unload the apps
        std::string topic_name = "runtime_core_destroy_" + app_name_;
        rtp_manager_destroy_srv_ = nh.advertiseService(topic_name, &AppLoader::rtpManagerDestroyCB, this);
    }