C++ Publisher::shutdown方法代码示例

int main(int argc, char **argv)
{
    ros::init(argc, argv, "image_to_laser_scan");
    ros::NodeHandle n;
    
    ros::Rate loop_rate(2);   
    
    //Subscribe to the message we need
    ros::Subscriber s_image = n.subscribe("image_stat", 1, &imageCallback);

    //Advertise our message
    p_laserScan = n.advertise<sensor_msgs::LaserScan>("/laser_scan", 5);
    p_laserScanImage = n.advertise<sensor_msgs::Image>("/laser_scan_image", 5);
    
    ros::spin();
    
    s_image.shutdown();
    p_laserScan.shutdown();
    
    return 0;
}

 ~BaseMotionController()
 {
      base_odom.shutdown(); 
      base_velocities_publisher.shutdown(); 
 }

// Main loop.
int main(int argc, char **argv)
{
  ros::init(argc, argv, "new_tagmapper_cu");
  ros::NodeHandle n;
  kill_pub = n.advertise<std_msgs::Int8>("/cu/killsg", 1000);
  pose_sub = n.subscribe("/cu/pose_cu", 1, pose_callback);
  marker_sub = n.subscribe("/cu/stargazer_marker_cu", 1, marker_callback);
  speeds_sub = n.subscribe("/speeds_bus", 1, speeds_callback);
  ros::Rate loop_rate(1000);

  isStopped = false;
  restart = false;
  poseSampleCount = 0;
  markerSampleCount = 0;
  lastTagLine = 0;
  memset(tagIDs, 0, PSEUDO_FILE_LINES*sizeof(int));
  tagCount = 0;
  currentTag = 0;

  // Open the file, read it in, extract the tag IDs, and determine the place to insert the new tag.
  int ifline = 0;
  std::ifstream psin;
  psin.open(pseudo_file, std::ifstream::in);
  if (!psin.good()) {
    return 1;
  }
  while (!psin.eof()) {
    psin.getline(pseudo_text[ifline], PSEUDO_FILE_LINE_WIDTH);
    ifline++;
  }
  psin.close();
  for (int i = 0; i < PSEUDO_FILE_LINES; i++) {
    if (strstr(pseudo_text[i], "/PseudoLiteMap")) {
      lastTagLine = i;
      break;
    }
  }
  for (int i = 0; i < lastTagLine; i++) {
    if (strstr(pseudo_text[i], "PseudoLite id")) {
      sscanf(pseudo_text[i], "    <PseudoLite id=\"%d\"", &tagIDs[tagCount]);
      tagCount++;
    }
  }

  // Run the main loop.  If the new tag samples have all been retrieved and the robot is stopped, save the tag, kill Stargazer, and then shut down.
  int count = 0;
  while (ros::ok())
  {
    if (poseSampleCount == SAMPLES && markerSampleCount == SAMPLES && isStopped) {
      saveTagInXml();
      publish_kill();
      break;
    }
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }

  kill_pub.shutdown();
  pose_sub.shutdown();
  marker_sub.shutdown();
  speeds_sub.shutdown();

  return 0;
}

//.........这里部分代码省略.........
          continue;
        }

        s_start = &graph[(int)robot_pos_y + ud][(int)robot_pos_x + lr];
         
        computeShortestPath();  // this updates the cost field to this node
        ud++;  
      }
      lr++;
    }  
       
    // extract the path, this will be used to figure out where to move the robot  
    //printf("extract path1 %d \n", time_counter);
    MapPath* pathToGoalWithLookAhead = extractPathOneLookahead();
    double path1_max_single_grid;
    double path1_cost = calculatePathCost(pathToGoalWithLookAhead, path1_max_single_grid);

    //printf("extract path2 %d \n", time_counter);
    MapPath* pathToGoalMine = extractPathMine(0); // this uses gradient descent where possible
    double path2_max_single_grid;
    double path2_cost = calculatePathCost(pathToGoalMine, path2_max_single_grid);

    
    double old_path_cost = LARGE;
    double old_path_max_single_grid;
    if(old_path != NULL)
      old_path_cost = calculatePathCost(old_path, old_path_max_single_grid);
    
    change_token_used = false;

    // use better path of the two to move the robot (or retain the old path if it is still better)
    if(old_path != NULL && path1_cost*old_path_discount <= old_path_cost && old_path_cost <= path1_cost/old_path_discount
                        && path2_cost*old_path_discount <= old_path_cost && old_path_cost <= path2_cost/old_path_discount 
                        && old_path_max_single_grid < OBSTACLE_COST)
    {
      publish_global_path(old_path);
      //printf("old path is about the same or better %d [%f vs %f] %f \n", time_counter, old_path_cost, (path1_cost+path2_cost)/2, old_path_max_single_grid);   
        
      deleteMapPath(pathToGoalMine);  
      deleteMapPath(pathToGoalWithLookAhead);
    }
    else if(path1_cost < path2_cost && path1_max_single_grid < OBSTACLE_COST)
    {         
      publish_global_path(pathToGoalWithLookAhead); 
      //printf("path1 is better %d %f %f\n", time_counter, path1_cost, path1_max_single_grid);

      
      if(old_path != NULL)
      {
        deleteMapPath(old_path);
        old_path = NULL;
      }
      
      old_path = pathToGoalWithLookAhead;
      deleteMapPath(pathToGoalMine);
    }
    else if(path2_max_single_grid < OBSTACLE_COST)
    {
      publish_global_path(pathToGoalMine);  
      //printf("path2 is better %d %f %f\n", time_counter, path2_cost, path2_max_single_grid);
      
      if(old_path != NULL)
      {
        deleteMapPath(old_path);
        old_path = NULL;
      }
      
      old_path = pathToGoalMine;
      deleteMapPath(pathToGoalWithLookAhead);
    }
    else // all paths go through obstacles
    {
     printf("Base Planner: No safe path exists to goal %f %f %f\n", old_path_cost, path1_cost, path2_cost);   
     publish_system_update(1);   
     reload_map = true;
    }

    ros::spinOnce();
    loop_rate.sleep();
    
    //printf("done %d \n", time_counter);
  }
    
  if(old_path != NULL)
    deleteMapPath(old_path);
  
  pose_sub.shutdown();
  goal_sub.shutdown();
  map_changes_sub.shutdown();
  global_path_pub.shutdown();
  system_update_pub.shutdown();
    
  destroy_pose(robot_pose);
  destroy_pose(goal_pose);
    
  cpDeleteHeap(primaryCellPathHeap);
  cpDeleteHeap(secondaryCellPathHeap);
  deleteHeap();   
  deleteGraphAndMap();
}

	/**
	 *	Destructor
	 */
	~RosPublisher()
	{
		_pub.shutdown();
	}

 /// Destructor.
 ~CobFiducialsNode()
 {
     fiducials_marker_array_publisher_.shutdown();
 }

int main(int argc, char** argv) 
{
    ros::init(argc, argv, "goal_server_cu");
    ros::NodeHandle nh;
    ros::Rate loop_rate(100);
    
     // load globals from parameter server
    double param_input;
    bool given_goal = false;
    if(ros::param::get("goal_server_cu/goal_pose_x_init", param_input)) 
    {
      goal_pose_x_init = (float)param_input;
      given_goal = true;
    }
    if(ros::param::get("goal_server_cu/goal_pose_y_init", param_input)) 
    {
      goal_pose_y_init = (float)param_input;
      given_goal = true;
    }
    if(ros::param::get("goal_server_cu/goal_pose_z_init", param_input)) 
    {
      goal_pose_z_init = (float)param_input;                                 // z is basically unused
      given_goal = true;
    }
    if(ros::param::get("goal_server_cu/goal_pose_theta_init", param_input)) 
    {
      goal_pose_theta_init = (float)param_input;
      given_goal = true;
    }
    
    // if the goal was sent in, then init goal pose
    if(given_goal)
    {
      printf("goal recieved at startup: [%f %f %f] \n", goal_pose_x_init, goal_pose_y_init, goal_pose_theta_init);
      goal = make_pose(goal_pose_x_init, goal_pose_y_init, goal_pose_z_init, goal_pose_theta_init);
    }
    
    // set up publisher
    goal_pub = nh.advertise<geometry_msgs::PoseStamped>("/cu/goal_cu", 1);
    
    // set up subscribers
    reset_goal_sub = nh.subscribe("/cu/reset_goal_cu", 1, reset_goal_callback);
    
    // set up service servers
    get_goal_srv = nh.advertiseService("/cu/get_goal_cu", get_goal_callback);
    
    while (ros::ok()) 
    {
      //printf(" This is the goal server \n");
      //print_pose(goal);
        
      publish_goal();
        
      ros::spinOnce();
      loop_rate.sleep();
    }
    
    
    // destroy publisher
    goal_pub.shutdown();
    reset_goal_sub.shutdown();
    get_goal_srv.shutdown();
        
    destroy_pose(goal);
}