void ObjRecInterface::recognize_objects() 
{
  while(ros::ok() && !time_to_stop_) {
    // Don't hog the cpu
    ros::Duration(0.03).sleep();

    // Scope for syncrhonization
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_full(new pcl::PointCloud<pcl::PointXYZRGB>());
    {
      // Lock the buffer mutex
      boost::mutex::scoped_lock buffer_lock(buffer_mutex_);

      // Continue if the cloud is empty
      static ros::Rate warn_rate(1.0);
      if(clouds_.empty()) {
        ROS_WARN("Pointcloud buffer is empty!");
        warn_rate.sleep();
        continue;
      }

      ros::Time time_back, time_front;
      time_back.fromNSec(clouds_.back()->header.stamp * 1000);
      time_front.fromNSec(clouds_.back()->header.stamp * 1000);
      ROS_INFO_STREAM("Computing objects from "
          <<scene_points_->GetNumberOfPoints()<<" points "
          <<"between "<<(ros::Time::now() - time_back)
          <<" to "<<(ros::Time::now() - time_front)<<" seconds after they were acquired.");

      // Copy references to the stored clouds
      cloud_full->header = clouds_.front()->header;

      while(!clouds_.empty()) {
        *cloud_full += *(clouds_.front());
        clouds_.pop();
      }
    }

    objrec_msgs::RecognizedObjects objects_msg = do_recognition(cloud_full);

    // Publish the visualization markers
    this->publish_markers(objects_msg);

    // Publish the recognized objects
    objects_pub_.publish(objects_msg);

    // Publish the points used in the scan, for debugging
    /**
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr foreground_points_pcl(new pcl::PointCloud<pcl::PointXYZRGB>());
    foreground_points_pcl->header = cloud->header;
    for(unsigned int i=0; i<foreground_points->GetNumberOfPoints(); i++) {
      double point[3];
      foreground_points->GetPoint(i,point);
      foreground_points_pcl->push_back(pcl::PointXYZ(point[0]/1000.0,point[1]/1000.0,point[2]/1000.0));
    }
    foreground_points_pub_.publish(foreground_points_pcl);
    **/
  }
}

    //================================================================
    // Close the gripper at the specified rate to the distance
    // specified
    //================================================================
    void closeToPosition(ros::Rate rate, double move_gripper_distance, 
                         double gripper_position)
    {
      // Get location of gripper currently 
      double current_gripper_position = simple_gripper->getGripperLastPosition();
      int pressure_max = 0;

      // Continue until position is achieved, ros cancel, or if
      // the pressure approaches something dangerous
      while (current_gripper_position > gripper_position 
            && pressure_max < 500 
            && current_gripper_position > 0.0 
            && ros::ok())
      {
        // Move the gripper by the specified amount
        simple_gripper->closeByAmount(move_gripper_distance);
        
        // Find and update if gripper moved
        current_gripper_position = simple_gripper->getGripperLastPosition();
      
        // Get pressure
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        // Wait set time and check again 
        ros::spinOnce();
        rate.sleep();
      }
    }

position_tracker::Position take_step(int action, ros::Rate loop_rate, ros::ServiceClient client)
{  
  printf("p take action\n");
  cout << "taking action " <<action << endl;
  /*Execute the Action */
  if(action==LEFT){
    left(client);
  }
  else if(action==FORWARD){
    forward(client);
  }
  else{
    right(client);
  }
  
  cout << "took an action" << endl;
  ros::spinOnce();
  loop_rate.sleep();
   
  //check to make sure this observation is ok
  
  position_tracker::Position observation= getObservation();
  return observation;
  /*
  cout << "going to convert"<< endl;
  convertObservation(observation);
  cout << "converted"<<endl;

  this_reward_observation.reward= calculateReward(observation);
  this_reward_observation.terminal=checkTerminal(observation);
  
    cout << "we took a step" << endl;
    return &this_reward_observation;
  */
}

    //================================================================
    // Close gripper to specified pressure
    //================================================================
    void closeToPressure(ros::Rate rate, int desired_pressure, 
                         double move_gripper_distance)
    {
      double current_gripper_position = simple_gripper->getGripperLastPosition();
      int pressure_max = 0;
      int pressure_min = 0;

      while (pressure_min < desired_pressure 
             && pressure_max < 500
             && ros::ok()
             && current_gripper_position > 0.0)
      {
        // Move the gripper by the specified amount
        simple_gripper->closeByAmount(move_gripper_distance);
        
        // Find and update if gripper moved
        current_gripper_position = simple_gripper->getGripperLastPosition();
      
        // Get pressure
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
        pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        // Wait set time and check again 
        ros::spinOnce();
        rate.sleep();
      }
    }

void turn(ros::Publisher& out, geometry_msgs::Twist& msg, ros::Rate& rate){
	int count = 0;
	while(ros::ok()){
		count++;
    	out.publish(msg);
		rate.sleep();
		if (count == 40)break; //after 45 updates break
	}	
}	

void moveForward(ros::Publisher& out, geometry_msgs::Twist& msg, ros::Rate& rate){
	int count =0; //number of updates
    while(ros::ok()){
		out.publish(msg);		
    	rate.sleep();
		count++;
		if (count == 10) break; //after a second break
    }
}

void Node::mapPublishThread(ros::Rate rate)
{
  while(ros::ok()) {
    {
#ifdef USE_HECTOR_TIMING
      hector_diagnostics::TimingSection section("map publish");
#endif
      publishMap();
    }
    rate.sleep();
  }
}

    //================================================================ 
    // Closes the gripper until a pressure is achieved, 
    // then opens again at the same rate the gripper closed at
    //================================================================
    void squeeze(ros::Rate rate, double move_gripper_distance)
    {
      int previous_pressure_max = 0; 
      int pressure_max = 0;
      int no_motion_counter = 0;

      // Close 
      while (pressure_max < SqueezePressureContact && ros::ok()
             && no_motion_counter < 250 
	           && simple_gripper->getGripperLastPosition() > 0.0)
      {
        previous_pressure_max = pressure_max;
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        // Checks if pressure has been "stuck" 
        if (abs(previous_pressure_max-pressure_max) < 1)
          no_motion_counter++;

        simple_gripper->closeByAmount(move_gripper_distance);
        //ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
   
      // Store the gripper position when max pressure is achieved
      gripper_max_squeeze_position = simple_gripper->getGripperLastPosition();

      // Open - 10 and not 0 because the values will drift
      while (pressure_max > 10 && ros::ok() 
	     && simple_gripper->getGripperLastPosition() < 0.08)
      {
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        simple_gripper->openByAmount(move_gripper_distance);
        //ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
    }

position_tracker::Position start_environment(ros::Rate loop_rate)
{
  /* see where the robot is.  And then send that position*/
  
  ros::spinOnce();
  loop_rate.sleep();
  
  position_tracker::Position observation= getObservation();
  
  cout << observation.x << " " << observation.y << " " << observation.theta << endl;  
  return observation;
  
}

    //================================================================
    // Open gripper by the rate and position specified.
    // This is necessary to keep opening the gripper until 
    // the bioTacs do not report any pressure 
    //================================================================
    void openUntilNoContact(ros::Rate rate, double gripper_position)
    {
      int pressure_max = LightPressureContact + 50;

      while (pressure_max > LightPressureContact && ros::ok())
      {
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        simple_gripper->open2Position(gripper_position);
        //ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
    }

//Moves forward for 3 seconds, moves backwards 3 seconds
//Press Enter for emergency land
void controller::testMove(ros::Rate loop_rate, ros::NodeHandle node)
{
	if (inFlight == 0)
	{
		controller::sendTakeoff(node);
		inFlight = 1;
	}

	start_time = (double)ros::Time::now().toSec();
	cout << "Started Time: " << start_time << "\n" << endl;

	linebufferedController(false);
	echoController(false);

	while ((double)ros::Time::now().toSec() < start_time + test_flight_time)
	{

		if ((double)ros::Time::now().toSec() < start_time + test_flight_time / 2)
		{
			controller::setMovement(5, 0, 0);
			controller::sendMovement(node);

		}
		else
		{
			controller::setMovement(-5,0, 0);
			controller::sendMovement(node);
		}

		ros::spinOnce();
		loop_rate.sleep();
		if (iskeypressed(500) && cin.get() == '\n') break;
	}

	controller::resetTwist();
	controller::sendMovement(node);
	controller::sendLand(node);

	echoController();
	linebufferedController();
}

void
computeBackgroundCloud (int frames, float voxel_size, int sr_conf_threshold, std::string frame_id, ros::Rate rate, PointCloudT::Ptr& background_cloud)
{
  std::cout << "Background acquisition..." << std::flush;

  // Create background cloud:
  PointCloudT::Ptr cloud_to_process(new PointCloudT);
  background_cloud->header = cloud->header;
  for (unsigned int i = 0; i < frames; i++)
  {
    *cloud_to_process = *cloud;

    // Remove low confidence points:
    removeLowConfidencePoints(confidence_image, sr_conf_threshold, cloud_to_process);

    // Voxel grid filtering:
    PointCloudT::Ptr cloud_filtered(new PointCloudT);
    pcl::VoxelGrid<PointT> voxel_grid_filter_object;
    voxel_grid_filter_object.setInputCloud(cloud_to_process);
    voxel_grid_filter_object.setLeafSize (voxel_size, voxel_size, voxel_size);
    voxel_grid_filter_object.filter (*cloud_filtered);

    *background_cloud += *cloud_filtered;
    ros::spinOnce();
    rate.sleep();
  }

  // Voxel grid filtering:
  PointCloudT::Ptr cloud_filtered(new PointCloudT);
  pcl::VoxelGrid<PointT> voxel_grid_filter_object;
  voxel_grid_filter_object.setInputCloud(background_cloud);
  voxel_grid_filter_object.setLeafSize (voxel_size, voxel_size, voxel_size);
  voxel_grid_filter_object.filter (*cloud_filtered);

  background_cloud = cloud_filtered;

  // Background saving:
  pcl::io::savePCDFileASCII ("/tmp/background_" + frame_id.substr(1, frame_id.length()-1) + ".pcd", *background_cloud);

  std::cout << "done." << std::endl << std::endl;
}

hmc5883l::hmc5883l(i2cfile* i2c_ptr, ros::NodeHandle* nh_ptr, ros::Rate rate, hmc5883l_callBackFunc dataReadyCallBack = 0) {
	ROS_INFO("HMC5883L : Initializing");
	char str[80];
	i2c = i2c_ptr;
	sem_init(&lock, 0, 1);
	ctl_reg_a = (0x03 << HMC5883L_MA0) | (0x06 << HMC5883L_DO0) | (0x00 << HMC5883L_MS0);
	i2c->write_byte(addr, HMC5883L_CONF_REG_A, ctl_reg_a);

	mode_reg = (0x00 << HMC5883L_MD0);
	i2c->write_byte(addr, HMC5883L_MODE_REG, mode_reg);

	setScale(hmc5883l_scale_130);

	nh = nh_ptr;
	timer = nh->createTimer(rate, &hmc5883l::timerCallback, this);
	if (dataReadyCallBack)
		dataCallback = dataReadyCallBack;

	sprintf(str, "Sampling @ %2.2f Hz", (float) 1.0f / rate.expectedCycleTime().toSec());
	ROS_INFO("HMC5883L : \t%s", str);
	ROS_INFO("HMC5883L : Initialization done");
}

micromag::micromag(i2cfile* i2c_ptr, ros::NodeHandle* nh_ptr, ros::Rate rate,
                   micromag_callBackFunc dataReadyCallBack = 0, int windowSize = 32) {
	ROS_INFO("micromag : Initializing");
	char str[80];
	i2c = i2c_ptr;

	sem_init(&lock, 0, 1);

	double freq = 1 / rate.expectedCycleTime().toSec();

	setWindow(windowSize);

	nh = nh_ptr;
	timer = nh->createTimer(rate, &micromag::timerCallback, this);
	if (dataReadyCallBack)
		dataCallback = dataReadyCallBack;

	sprintf(str, "Sampling @ %2.2f Hz", freq);
	ROS_INFO("micromag : \t%s", str);
	ROS_INFO("micromag : Initialization done");

	// Probe device... Return 0 on failure..
}

    //================================================================
    // Higher level motion to close gripper until contact is found
    // Pass in the rate at which contact is closed at and the 
    // distance the gripper moves rate is in Hz, 
    // move_gripper_distance in meters
    // Velocity gripper moves is found by how much moved by what rate
    //================================================================
    void findContact(ros::Rate rate, double move_gripper_distance)
    {
      int pressure_min = 0; 
      int pressure_max = 0;
      bool contact_found = false;
      int no_motion_counter = 0;
      int previous_pressure_max = 0;

      // Close until minimum pressure is found - however stop if
      // any finger has too much pressure
      while (pressure_min < LightPressureContact && ros::ok()
             && pressure_max < 600  && no_motion_counter < 250)
      {
        // Check pressure min and max
        simple_gripper->closeByAmount(move_gripper_distance);
        pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
        
        // Checks if pressure has been "stuck" 
        if (abs(previous_pressure_max-pressure_max) < 1)
          no_motion_counter++;

        // Set distance for object width 
        if (!contact_found && pressure_min > 10){
          gripper_initial_contact_position = simple_gripper->getGripperLastPosition();
          contact_found = true;
        }

        previous_pressure_max = pressure_max;

        //ROS_INFO("Pressure Min is: [%d]", pressure_min);
       // ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
    }

    //================================================================
    // Function to move the arm a little in a direction it needs to
    // move
    //================================================================
    void centerArmIncremental(ros::Rate rate, double move_gripper_distance)
    {

      // Find position of arm
      arm_controller->getArmTransform();
      double x = arm_controller->getTransform('x');
      double y = arm_controller->getTransform('y');
      double z = arm_controller->getTransform('z');

      int pressure_min = 0; 
      int pressure_max = 0;
      bool fingerSet = false;
      int num_run = 0;
      bool not_centered = true;
      int pressure_left = 0;
      int pressure_right = 0;
      int move_counter = 0;
      int lastFingerContact = 5;
      double distance_move_arm = 0.005;

      // Close until minimum pressure is found - however stop if
      // any finger has too much pressure
      while (num_run < 8 && not_centered 
             && pressure_max < 600 && ros::ok())
      { 

        simple_gripper->closeByAmount(move_gripper_distance);
       
        pressure_left = biotac_obs->pressure_normalized_[Left];
        pressure_right = biotac_obs->pressure_normalized_[Right];

        // Check pressure min and max
        pressure_min = min(pressure_left, pressure_right);
        pressure_max = max(pressure_left, pressure_right);
      
        // First touches object
        if (pressure_max > 5 && !fingerSet)
        {
          fingerSet = true;

          if (pressure_min > 5)
          {
            not_centered = false;
            fingerSet = false;
          }

          firstContact.position = simple_gripper->getGripperLastPosition();
          if (pressure_left > pressure_right)
          {
            firstContact.finger = Left;
          }
          else
          {
            firstContact.finger = Right;
          }
          
          // Checks if finger was not set yet
          if (lastFingerContact == 5)
          {
            lastFingerContact = firstContact.finger;
          }
          
          // Stop if finger contact changed
          if (lastFingerContact != firstContact.finger)
          {
           not_centered = false;
          }

        }

        if (fingerSet)
        {

          if (!not_centered)
          {
            distance_move_arm = 0.0025;
          }

          // Find position of arm
          arm_controller->getArmTransform();
          x = arm_controller->getTransform('x');
          y = arm_controller->getTransform('y');
          z = arm_controller->getTransform('z');
          
          // Open grippers 
          simple_gripper->open2Position(GripperMaxOpenPosition);
          
          // Move left case 
          if (firstContact.finger == Left)
          {
            arm_controller->move_arm_to(x,y+distance_move_arm,z,1);    
          } 
          else 
          {
            arm_controller->move_arm_to(x,y-distance_move_arm,z,1);
          }
//.........这里部分代码省略.........

    void runLocalController()
    {



        while(ros::ok())
        {
            rate->sleep();
            //If we are receiving data
            switch(currentState)
            {
            case STOPPED:
                //ROS_ERROR("STOPPED!");

                //Next state
                if(distanceToPerson > planner_activation_distance && hold == false)
                {

                    nextState = PLANNER;
                    //delete rate;
                    //rate = new ros::Rate(50);
                    geometry_msgs::Twist goal;
                    goal.angular.x = 0;
                    goal.angular.y = 0;
                    goal.angular.z = 0;
                    goal.linear.x = 0;
                    goal.linear.y = 0;
                    goal.linear.z = 0;
                    cmdPub.publish(goal);
                }
                else if(distanceToPerson < planner_activation_distance && distanceToPerson > minimum_distance && hold == false)
                {
                    nextState = LOCALCONTROLLER;
                    //delete rate;
                    //rate = new ros::Rate(50);
                    geometry_msgs::Twist goal;
                    goal.angular.x = 0;
                    goal.angular.y = 0;
                    goal.angular.z = 0;
                    goal.linear.x = 0;
                    goal.linear.y = 0;
                    goal.linear.z = 0;
                    cmdPub.publish(goal);
                }
                /*else
                {
                    nextState = STOPPED;
                    geometry_msgs::Twist goal;
                    goal.angular.x = 0;
                    goal.angular.y = 0;
                    goal.angular.z = 0;
                    goal.linear.x = 0;
                    goal.linear.y = 0;
                    goal.linear.z = 0;
                    cmdPub.publish(goal);

                }*/

                break;
            case PLANNER:
                //ROS_ERROR("PLANNER");

                if(distanceToPerson >= minimum_planner_distance && hold == false)
                {
                    nextState = PLANNER;
                    //Get transforms

                    cv::Point3d personPoint;
                    personPoint.x = personInRobotBaseFrame.point.x;
                    personPoint.y = personInRobotBaseFrame.point.y;
                    personPoint.z = personInRobotBaseFrame.point.z;

                    segwayController::proportionalController(personPoint, cmdPub, kv, kalfa);

                }
                else if( distanceToPerson < minimum_planner_distance && distanceToPerson >= minimum_distance && hold == false)
                {
                    nextState = LOCALCONTROLLER;
                    //delete rate;
                    //rate = new ros::Rate(50);

                    geometry_msgs::Twist goal;
                    goal.angular.x = 0;
                    goal.angular.y = 0;
                    goal.angular.z = 0;
                    goal.linear.x = 0;
                    goal.linear.y = 0;
                    goal.linear.z = 0;
                    cmdPub.publish(goal);
                }

                else if(distanceToPerson < minimum_distance || hold == true)
                {
                    nextState = STOPPED;
                    geometry_msgs::Twist goal;
                    goal.angular.x = 0;
                    goal.angular.y = 0;
                    goal.angular.z = 0;
                    goal.linear.x = 0;
                    goal.linear.y = 0;
//.........这里部分代码省略.........

void spinOnce(ros::Rate& loopRate) {
  loopRate.sleep(); //Maintain the loop rate
  ros::spinOnce();   //Check for new messages
}

void ObjRecInterface::recognize_objects_thread()
{
  ros::Rate max_rate(100.0);

  // Working structures
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_full(new pcl::PointCloud<pcl::PointXYZRGB>());
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
  boost::shared_ptr<pcl::VoxelGrid<pcl::PointXYZRGB> > voxel_grid(new pcl::VoxelGrid<pcl::PointXYZRGB>());

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::PointIndices::Ptr outliers (new pcl::PointIndices);

  // Create point clouds for foreground and background points
  vtkSmartPointer<vtkPoints> foreground_points;
  foreground_points.TakeReference(vtkPoints::New(VTK_DOUBLE));

  std::list<boost::shared_ptr<PointSetShape> > detected_models;

  while(ros::ok() && !time_to_stop_)
  {
    // Don't hog the cpu
    max_rate.sleep();

    cloud_full->clear();
    cloud->clear();

    ROS_DEBUG_STREAM("ObjRec: Aggregating point clouds... ");
    {
      // Scope for syncrhonization

      // Continue if the cloud is empty
      static ros::Rate warn_rate(1.0);
      if(clouds_.empty()) {
        ROS_WARN("Pointcloud buffer is empty!");
        warn_rate.sleep();
        continue;
      }

      // Lock the buffer mutex
      boost::mutex::scoped_lock buffer_lock(buffer_mutex_);

      ROS_DEBUG_STREAM("ObjRec: Computing objects from "
          <<clouds_.size()<<" point clounds "
          <<"between "<<(ros::Time::now() - pcl_conversions::fromPCL(clouds_.back()->header).stamp)
          <<" to "<<(ros::Time::now() - pcl_conversions::fromPCL(clouds_.front()->header).stamp)<<" seconds after they were acquired.");

      // Copy references to the stored clouds
      cloud_full->header = clouds_.front()->header;

      for(std::list<boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > >::const_iterator it = clouds_.begin();
          it != clouds_.end();
          ++it)
      {
        *cloud_full += *(*it);
      }
    }

    // Recongize objects
    bool objects_recognized = this->recognize_objects(
        cloud_full,
        cloud,
        voxel_grid,
        coefficients, inliers, outliers,
        foreground_points,
        detected_models,
        true,
        true);

    // No objects recognized
    if(!objects_recognized) {
      continue;
    }

    // Construct recognized objects message
    objrec_msgs::RecognizedObjects objects_msg;
    objects_msg.header.stamp = pcl_conversions::fromPCL(cloud->header).stamp;
    objects_msg.header.frame_id = "/world";//cloud->header;

    for(std::list<boost::shared_ptr<PointSetShape> >::iterator it = detected_models.begin();
        it != detected_models.end();
        ++it)
    {
      boost::shared_ptr<PointSetShape> detected_model = *it;

      // Construct and populate a message
      objrec_msgs::PointSetShape pss_msg;
      pss_msg.label = detected_model->getUserData()->getLabel();
      pss_msg.confidence = detected_model->getConfidence();
      array_to_pose(detected_model->getRigidTransform(), pss_msg.pose);

      // Transform into the world frame TODO: make this frame a parameter
      geometry_msgs::PoseStamped pose_stamped_in, pose_stamped_out;
      pose_stamped_in.header = pcl_conversions::fromPCL(cloud->header);
      pose_stamped_in.pose = pss_msg.pose;

      try {
        listener_.transformPose("/world",pose_stamped_in,pose_stamped_out);
        pss_msg.pose = pose_stamped_out.pose;
      }
//.........这里部分代码省略.........

void receive(ros::Rate & loop_rate) {
  for (unsigned i = 0u; i < 10u; i++){
    ros::spinOnce();
    loop_rate.sleep();
  }
}