C++ ros::Publisher类代码示例

void VisualizationPublisherCCD::visualizationduringmotion(){

    	R_point *temp_point;
    	int temp_length;

      robotpath.points.clear();
      toolpath.points.clear();
//      mine.points.clear();
//      coilpath.points.clear();
      visitedpath.points.clear();


      int metric=M->metric;
			if(CCD->getPathLength()>0){
				temp_length=CCD->getPathLength();
				temp_point=CCD->GetRealPathRobot();
				geometry_msgs::Point p; 	
				for(int pathLength=0; pathLength<temp_length;pathLength++){
			        	p.x = temp_point[pathLength].x/metric;
    					p.y = temp_point[pathLength].y/metric;
    					p.z = 0;

    					robotpath.points.push_back(p);
				}
			//publish path			
			robotpath_pub.publish(robotpath);
		
			}
			if(CCD->getPathLength()>0){
				temp_length=CCD->getPathLength();
				temp_point=CCD->GetRealPathTool();
				geometry_msgs::Point p; 	
				for(int pathLength=0; pathLength<temp_length;pathLength++){
			        	p.x = temp_point[pathLength].x/metric;
    					p.y = temp_point[pathLength].y/metric;
    					p.z = 0;

    					toolpath.points.push_back(p);
				}
			//publish path			
			toolpath_pub.publish(toolpath);
		
			}

			if(CCD->getPathLength()>0){
			geometry_msgs::Point p; 	
			for (int i=0; i<CCD->MapSizeX; i++){
				for (int j=0; j<CCD->MapSizeY; j++){
					if ((CCD->map[i][j].presao>0)){
					  p.x=(GM->Map_Home.x+i*GM->Map_Cell_Size+0.5*GM->Map_Cell_Size)/metric;
            p.y=(GM->Map_Home.y+j*GM->Map_Cell_Size+0.5*GM->Map_Cell_Size)/metric;
					  visitedpath.points.push_back(p);
					}
				}
			}
			visitedpath_pub.publish(visitedpath);
			}

#if 0
			if(CCD->coilpeak){
				geometry_msgs::Point p; 	
			        	p.x = CCD->tocka.x;
    					p.y = CCD->tocka.y;
    					p.z = 0;

    					coilpath.points.push_back(p);
			        	p.x = CCD->tockaR.x;
    					p.y = CCD->tockaR.y;
    					p.z = 0;

    					coilpath.points.push_back(p);
			//publish path			
			coil_pub.publish(coilpath);
		
			}
			if (0)
			{
				geometry_msgs::Point p; 	
			        	p.x = 3.;
    					p.y = 0;
    					p.z = 0;
    					mine.points.push_back(p);
			        	p.x = 2.;
    					p.y = 0;
    					p.z = 0;
    					mine.points.push_back(p);
			        	p.x = -3.;
    					p.y = 0;
    					p.z = 0;
    					mine.points.push_back(p);
			        	p.x = -2.;
    					p.y = 0;
    					p.z = 0;
    					mine.points.push_back(p);
			        	p.x = 0;
    					p.y = -3.;
    					p.z = 0;
    					mine.points.push_back(p);
			        	p.x = 0;
    					p.y = 2.;
//.........这里部分代码省略.........

void
TBK_Node::keyboardLoop()
{
    char keyboard_input;
    double max_tv = max_speed;
    double max_rv = max_turn;
    bool dirty=false;

    int speed=0;
    int turn=0;

    // get the console in raw mode
    tcgetattr(kfd, &cooked);
    memcpy(&raw, &cooked, sizeof(struct termios));
    raw.c_lflag &=~ (ICANON | ECHO);
    raw.c_cc[VEOL] = 1;
    raw.c_cc[VEOF] = 2;
    tcsetattr(kfd, TCSANOW, &raw);

    puts("Reading from keyboard");
    puts("---------------------------");
    puts("q/z : increase/decrease max angular and linear speeds by 10%");
    puts("w/x : increase/decrease max linear speed by 10%");
    puts("e/c : increase/decrease max angular speed by 10%");
    puts("---------------------------");
    puts("Moving around:");
    puts("   u    i    o");
    puts("   j    k    l");
    puts("   m    ,    .");
    puts("1/2 kick left/right");
    puts("3/4 get up from forward/backwards falll");
    puts("arrow keys move head");
    puts("---------------------------");
    puts("6 stand up action");
    puts("7 sit down action");
    puts("9 start walking gait");
    puts("0 stop walking gait" );
    puts("---------------------------");
    puts("anything else : stop");
    puts("---------------------------");

    struct pollfd ufd;
    ufd.fd = kfd;
    ufd.events = POLLIN;
    for(;;)
    {
        boost::this_thread::interruption_point();

        // get the next event from the keyboard
        int num;
        if((num = poll(&ufd, 1, 250)) < 0)
        {
            perror("poll():");
            return;
        }
        else if(num > 0)
        {
            if(read(kfd, &keyboard_input, 1) < 0)
            {
                perror("read():");
                return;
            }
        }
        else
            continue;

        switch ( keyboard_input ) {
        case KEYCODE_I:
            ROS_INFO("i Go straight without rotation");
            speed = 1;
            turn = 0;
            dirty = true;
            break;
        case KEYCODE_J:
            ROS_INFO("j Turn left at a fixed position");
            speed = 0;
            turn = 1;
            dirty = true;
            break;
        case KEYCODE_COMMA:
            ROS_INFO(", Go backward without rotation");
            speed = -1;
            turn = 0;
            dirty = true;
            break;
        case KEYCODE_O:
            ROS_INFO("O Move forward with right turn");
            speed = 1;
            turn = -1;
            dirty = true;
            break;
        case KEYCODE_L:
            ROS_INFO("l Turn right at a fixed position");
            speed = 0;
            turn = -1;
            dirty = true;
            break;
        case KEYCODE_U:
            ROS_INFO("U Move forward with left turn");
            speed = 1;
//.........这里部分代码省略.........

    // Proccess the point clouds, called when subscriber gets msg
    void cloudCallback( const sensor_msgs::PointCloud2ConstPtr& msg )
    {
        ROS_INFO_STREAM("Recieved callback");
        // Basic point cloud conversions ---------------------------------------------------------------
        // Convert from ROS to PCL
        pcl::PointCloud<pcl::PointXYZRGB> cloud;
        pcl::fromROSMsg(*msg, cloud);

        // Make new point cloud that is in our working frame
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);

        // Transform to whatever frame we're working in, probably the arm's base frame, ie "base_link"
        tf_listener_.waitForTransform(std::string(arm_link), cloud.header.frame_id,
                                      cloud.header.stamp, ros::Duration(1.0));
        if(!pcl_ros::transformPointCloud(std::string(arm_link), cloud, *cloud_transformed, tf_listener_))
        {
            ROS_ERROR("Error converting to desired frame");
            return;
        }

        // Limit to things we think are roughly at the table height ------------------------------------
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filteredZ(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::PassThrough<pcl::PointXYZRGB> pass;
        pass.setInputCloud(cloud_transformed);
        pass.setFilterFieldName("z");
        pass.setFilterLimits(min_cam_dist-.05,max_cam_dist+0.05);
        pass.filter(*cloud_filteredZ);


        // Limit to things in front of the robot ---------------------------------------------------
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
        pass.setInputCloud(cloud_filteredZ);
        pass.setFilterFieldName("x");
        pass.setFilterLimits(.1,.5);
        pass.filter(*cloud_filtered);


        // Check if any points remain
        if( cloud_filtered->points.size() == 0 )
        {
            ROS_ERROR("0 points left");
            return;
        }
        else
        {
            ROS_INFO("[block detection] Filtered, %d points left", (int) cloud_filtered->points.size());
        }

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

        int nr_points = cloud_filtered->points.size();

        // Segment cloud until there are less than 30% of points left? not sure why this is necessary
        pcl::SACSegmentation<pcl::PointXYZRGB> seg;
        seg.setOptimizeCoefficients(true);
        seg.setModelType(pcl::SACMODEL_PLANE);
        seg.setMethodType(pcl::SAC_RANSAC); // robustness estimator - RANSAC is simple
        seg.setMaxIterations(200);
        seg.setDistanceThreshold(0.005); // determines how close a point must be to the model in order to be considered an inlier
        while(cloud_filtered->points.size() > 0.3 * nr_points)
        {
            // Segment the largest planar component from the remaining cloud (find the table)
            seg.setInputCloud(cloud_filtered);
            seg.segment(*inliers, *coefficients);

            if(inliers->indices.size() == 0)
            {
                ROS_ERROR("[block detection] Could not estimate a planar model for the given dataset.");
                return;
            }

            // Debug output - DTC
            // Show the contents of the inlier set, together with the estimated plane parameters, in ax+by+cz+d=0 form (general equation of a plane)
            std::cerr << "Model coefficients: " << coefficients->values[0] << " "
                      << coefficients->values[1] << " "
                      << coefficients->values[2] << " "
                      << coefficients->values[3] << std::endl;
        }//end while

        // DTC: Removed to make compatible with PCL 1.5
        // Creating the KdTree object for the search method of the extraction
        //pcl::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
        //tree->setInputCloud(cloud_filtered);
        // Publish point cloud data
        depth_pub_.publish(cloud_filtered);

    }//end cloudCallBack

void TeleopUAVController::keyboardLoop(){
  char c;
  bool dirty=false;

  // get the console in raw mode
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);

  intro_ = true;

  puts("Reading from keyboard");
  puts("---------------------------");
  puts("***************************");
  puts("Press Characters Once");
  puts("***************************");
  puts("");
  puts("Use 'WASD' to translate");
  puts("Use 'T' to take off");
  puts("Use 'L' to land");
  puts("Use 'H' to hover");
  puts("Use 'QE' to yaw");
  puts("Press 'Shift' to run");
  puts("");
  puts("---------------------------");

  for(;;)
  {
    // get the next event from the keyboard
    if(read(kfd, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }

    cmd.linear.z = cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;

    switch(c)
    {
      // Walking
    case KEYCODE_T:
      cmd.linear.z = walk_vel;
      ROS_INFO_STREAM("TAKE OFF!!");
      dirty = true;
      break;
    case KEYCODE_L:
      cmd.linear.z = - walk_vel;
      ROS_INFO_STREAM("LAND!!");
      dirty = true;
      break;
    case KEYCODE_H:
    	ROS_INFO_STREAM("** HOVERING **");
      dirty = true;
      break;
    case KEYCODE_W:
      cmd.linear.x = walk_vel;
	ROS_INFO_STREAM("FORWARD");
      dirty = true;
      break;
    case KEYCODE_S:
      cmd.linear.x = - walk_vel;
      ROS_INFO_STREAM("BACKWARD");
      dirty = true;
      break;
    case KEYCODE_A:
      cmd.linear.y = walk_vel;
      ROS_INFO_STREAM("LEFT <-");
      dirty = true;
      break;
    case KEYCODE_D:
      cmd.linear.y = - walk_vel;
      ROS_INFO_STREAM("RIGHT ->");
      dirty = true;
      break;
    case KEYCODE_Q:
      cmd.angular.z = yaw_rate;
      dirty = true;
      break;
    case KEYCODE_E:
      cmd.angular.z = - yaw_rate;
      dirty = true;
      break;

      // Running 
    case KEYCODE_W_CAP:
      cmd.linear.x = run_vel;

      dirty = true;
      break;
    case KEYCODE_S_CAP:
      cmd.linear.x = - run_vel;
      dirty = true;
      break;
    case KEYCODE_A_CAP:
      cmd.linear.y = run_vel;
      dirty = true;
//.........这里部分代码省略.........

void red_light(bool status)
{
    std_msgs::Bool msg;
    msg.data = status;
    _redlight.publish(msg);
}

void speak(string val)
{
    std_msgs::String msg;
    msg.data = val;
    _speak.publish(msg);
}

static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
  pcl::PointXYZI sampled_p;
  pcl::PointCloud<pcl::PointXYZI> scan;

  pcl::fromROSMsg(*input, scan);

  if(measurement_range != MAX_MEASUREMENT_RANGE){
    scan = removePointsByRange(scan, 0, measurement_range);
  }

  pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());
  filtered_scan_ptr->header = scan.header;

  int points_num = scan.size();

  double w_total = 0.0;
  double w_step = 0.0;
  int m = 0;
  double c = 0.0;

  filter_start = std::chrono::system_clock::now();

  for (pcl::PointCloud<pcl::PointXYZI>::const_iterator item = scan.begin(); item != scan.end(); item++)
  {
    w_total += item->x * item->x + item->y * item->y + item->z * item->z;
  }
  w_step = w_total / sample_num;

  pcl::PointCloud<pcl::PointXYZI>::const_iterator item = scan.begin();
  for (m = 0; m < sample_num; m++)
  {
    while (m * w_step > c)
    {
      item++;
      c += item->x * item->x + item->y * item->y + item->z * item->z;
    }
    sampled_p.x = item->x;
    sampled_p.y = item->y;
    sampled_p.z = item->z;
    sampled_p.intensity = item->intensity;
    filtered_scan_ptr->points.push_back(sampled_p);
  }

  sensor_msgs::PointCloud2 filtered_msg;
  pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);

  filter_end = std::chrono::system_clock::now();

  filtered_msg.header = input->header;
  filtered_points_pub.publish(filtered_msg);

  points_downsampler_info_msg.header = input->header;
  points_downsampler_info_msg.filter_name = "distance_filter";
  points_downsampler_info_msg.measurement_range = measurement_range;
  points_downsampler_info_msg.original_points_size = points_num;
  points_downsampler_info_msg.filtered_points_size = std::min((int)filtered_scan_ptr->size(), points_num);
  points_downsampler_info_msg.original_ring_size = 0;
  points_downsampler_info_msg.original_ring_size = 0;
  points_downsampler_info_msg.exe_time = std::chrono::duration_cast<std::chrono::microseconds>(filter_end - filter_start).count() / 1000.0;
  points_downsampler_info_pub.publish(points_downsampler_info_msg);

  if(_output_log == true){
	  if(!ofs){
		  std::cerr << "Could not open " << filename << "." << std::endl;
		  exit(1);
	  }
	  ofs << points_downsampler_info_msg.header.seq << ","
		  << points_downsampler_info_msg.header.stamp << ","
		  << points_downsampler_info_msg.header.frame_id << ","
		  << points_downsampler_info_msg.filter_name << ","
		  << points_downsampler_info_msg.original_points_size << ","
		  << points_downsampler_info_msg.filtered_points_size << ","
		  << points_downsampler_info_msg.original_ring_size << ","
		  << points_downsampler_info_msg.filtered_ring_size << ","
		  << points_downsampler_info_msg.exe_time << ","
		  << std::endl;
  }

}

void
AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
  last_laser_received_ts_ = ros::Time::now();
  if( map_ == NULL ) {
    return;
  }
  boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
  int laser_index = -1;

  // Do we have the base->base_laser Tx yet?
  if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
  {
    ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan->header.frame_id.c_str());
    lasers_.push_back(new AMCLLaser(*laser_));
    lasers_update_.push_back(true);
    laser_index = frame_to_laser_.size();

    tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
                                             tf::Vector3(0,0,0)),
                                 ros::Time(), laser_scan->header.frame_id);
    tf::Stamped<tf::Pose> laser_pose;
    try
    {
      this->tf_->transformPose(base_frame_id_, ident, laser_pose);
    }
    catch(tf::TransformException& e)
    {
      ROS_ERROR("Couldn't transform from %s to %s, "
                "even though the message notifier is in use",
                laser_scan->header.frame_id.c_str(),
                base_frame_id_.c_str());
      return;
    }

    pf_vector_t laser_pose_v;
    laser_pose_v.v[0] = laser_pose.getOrigin().x();
    laser_pose_v.v[1] = laser_pose.getOrigin().y();
    // laser mounting angle gets computed later -> set to 0 here!
    laser_pose_v.v[2] = 0;
    lasers_[laser_index]->SetLaserPose(laser_pose_v);
    ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
              laser_pose_v.v[0],
              laser_pose_v.v[1],
              laser_pose_v.v[2]);

    frame_to_laser_[laser_scan->header.frame_id] = laser_index;
  } else {
    // we have the laser pose, retrieve laser index
    laser_index = frame_to_laser_[laser_scan->header.frame_id];
  }

  // Where was the robot when this scan was taken?
  pf_vector_t pose;
  if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
                  laser_scan->header.stamp, base_frame_id_))
  {
    ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
    return;
  }


  pf_vector_t delta = pf_vector_zero();

  if(pf_init_)
  {
    // Compute change in pose
    //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
    delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
    delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
    delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);

    // See if we should update the filter
    bool update = fabs(delta.v[0]) > d_thresh_ ||
                  fabs(delta.v[1]) > d_thresh_ ||
                  fabs(delta.v[2]) > a_thresh_;
    update = update || m_force_update;
    m_force_update=false;

    // Set the laser update flags
    if(update)
      for(unsigned int i=0; i < lasers_update_.size(); i++)
        lasers_update_[i] = true;
  }

  bool force_publication = false;
  if(!pf_init_)
  {
    // Pose at last filter update
    pf_odom_pose_ = pose;

    // Filter is now initialized
    pf_init_ = true;

    // Should update sensor data
    for(unsigned int i=0; i < lasers_update_.size(); i++)
      lasers_update_[i] = true;

    force_publication = true;

//.........这里部分代码省略.........

 void tell_want_to_dance()
 {
     int sentence_num = rand() % NUM_OF_SENTENCES;
     etts_msg.text = sentences_to_dance[sentence_num];
     etts_say_text.publish(etts_msg);
 }

 ///////////////////////////////////////////////////////////////////
 ///Syncronize objects  only for clouds
 //////////////////////////////////////////////////////////////////
 void Callback(const sensor_msgs::PointCloud2ConstPtr& cloudI)
 {
  
   
   ROS_INFO("cloud timestamps %i.%i ", cloudI->header.stamp.sec,cloudI->header.stamp.nsec);
      
   //////////////////////////////////////////////////////////////////
   ///for range images
   /////////////////////////////////////////////////////////////////
   
   Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
   
   pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::LASER_FRAME;
   
   float noiseLevel=0.00;
   
   float minRange = 0.0f;
   
   int borderSize = 1;
   
   
   /////////////////////////////////////////////////////////////////////
   /// point cloud 
   ///////////////////////////////////////////////////////////////////
   PointCloud<PointXYZ>::Ptr cloud_ptr (new PointCloud<PointXYZ>);
   
   
   if ((cloudI->width * cloudI->height) == 0)
     return ;
   
   ROS_INFO ("Received %d data points in frame %s with the following fields: %s",
	     
	     (int)cloudI->width * cloudI->height,
	     
	     cloudI->header.frame_id.c_str (),
	     
	     pcl::getFieldsList (*cloudI).c_str ());
   
   
   
   
   pcl::fromROSMsg(*cloudI, *cloud_ptr);

     
   ///segment data only the part in front of
   SegmentData(cloud_ptr,cloud_filtered,cloud_nonPlanes,cloud_Plane,cloud_projected);
    
   ///////////////////////////////////////////////
   ///image range images
   //////////////////////////////////////////////
   range_image->createFromPointCloud (*cloud_filtered, pcl::deg2rad(0.2f),
				     pcl::deg2rad (360.f), pcl::deg2rad (180.0f),
				     sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
   
   /////////////////////////////////////////////////////////////////////////////////////////////
   ///planar range image TESTING
   //////////////////////////////////////////////////////////////////////////////////////////
   
   Eigen::Affine3f trans,P_affine;//does not include K
   

     trans (0,0)= 0.0f; trans (0,1)= 0.0f; trans (0,2)=-1.0f; trans(0,3)=0.0f;       

     trans (1,0)= 0.0f; trans (1,1)= 1.0f; trans (1,2)=0.0f; trans (1,3)=0.0f;

     trans (2,0)= 1.0f; trans (2,1)= 0.0f; trans (2,2)=0.0f; trans (2,3)=0.0f;

     trans (3,0)= 0.0f; trans (3,1)= 0.0f; trans (3,2)=0.0f; trans (3,3)=1.0f;
     
     
     P_affine (0,0)= 0.0187f; P_affine (0,1)= -0.8024f; P_affine (0,2)= -0.5965f; P_affine(0,3)=-0.7813f;       

     P_affine (1,0)= -0.9998f; P_affine (1,1)= -0.0168f; P_affine (1,2)=-0.0088f; P_affine (1,3)=0.1818f;

     P_affine (2,0)= -0.0030f; P_affine (2,1)= 0.5965f; P_affine (2,2)=-0.8026f; P_affine (2,3)=0.7670f;

     P_affine (3,0)= 0.0f; P_affine (3,1)= 0.0f; P_affine (3,2)=0.0f; P_affine (3,3)=1.0f;
     
     
//    0.0187   -0.8024   -0.5965   -0.7813
//    -0.9998   -0.0168   -0.0088    0.1818
//    -0.0030    0.5965   -0.8026    0.7670

   for(unsigned int i=0; i<cloud_filtered->size();i++)
   {
      PointXYZ p1,p2;
      p1=cloud_filtered->points[i];
      
      p2.x=-p1.z;
      p2.y=p1.y;
      p2.z=p1.x;
      
      cloud_trans->push_back(p2);
   }
     
  // pcl::transformPointCloud (*cloud_filtered, *cloud_filtered, trans);
   Eigen::Affine3f pose(Eigen::Affine3f::Identity());
//.........这里部分代码省略.........

void PlaneFinder::pcCallback(const sensor_msgs::PointCloud2::ConstPtr & pc)
{


	pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2);
	pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered1 (new pcl::PointCloud<pcl::PointXYZRGB>());
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZRGB>());
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered3 (new pcl::PointCloud<pcl::PointXYZRGB>());
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>());
	pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ());

	sensor_msgs::PointCloud2 pc2;

	double height = -0.5;


	// Transformation into PCL type PointCloud2
	pcl_conversions::toPCL(*(pc), *(pcl_pc));

	//////////////////
	// Voxel filter //
	//////////////////
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud (pcl_pc);
	sor.setLeafSize (0.01f, 0.01f, 0.01f);
	sor.filter (*cloud_filtered);

	// Transformation into ROS type
	//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
	//pcl_conversions::moveFromPCL(*(cloud_filtered), pc2);

	//debug2_pub.publish(pc2);


	// Transformation into PCL type PointCloud<pcl::PointXYZRGB>
	pcl::fromPCLPointCloud2(*(cloud_filtered), *(cloud_filtered1));

	if(pcl_ros::transformPointCloud("map", *(cloud_filtered1), *(cloud_filtered2), tf_)) 
	{

		////////////////////////
		// PassThrough filter //
		////////////////////////
		pcl::PassThrough<pcl::PointXYZRGB> pass;
		pass.setInputCloud (cloud_filtered2);
		pass.setFilterFieldName ("x");
		pass.setFilterLimits (-0.003, 0.9);
		//pass.setFilterLimitsNegative (true);
		pass.filter (*cloud_filtered2);

		pass.setInputCloud (cloud_filtered2);
		pass.setFilterFieldName ("y");
		pass.setFilterLimits (-0.5, 0.5);
		//pass.setFilterLimitsNegative (true);
		pass.filter (*cloud_filtered2);

		/////////////////////////
		// Planar segmentation //
		/////////////////////////
		pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
		pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
		// Create the segmentation object
		pcl::SACSegmentation<pcl::PointXYZRGB> seg;
		// Optional
		seg.setOptimizeCoefficients (true);
		// Mandatory
		seg.setModelType (pcl::SACMODEL_PLANE);
		seg.setMethodType (pcl::SAC_RANSAC);
		//seg.setMaxIterations (1000);
		seg.setDistanceThreshold (0.01);

		// Create the filtering object
		pcl::ExtractIndices<pcl::PointXYZRGB> extract;

		int i = 0, nr_points = (int) cloud_filtered2->points.size ();
		// While 50% of the original cloud is still there
		while (cloud_filtered2->points.size () > 0.5 * nr_points)
		{
			// Segment the largest planar component from the remaining cloud
			seg.setInputCloud (cloud_filtered2);
			seg.segment (*inliers, *coefficients);
			if (inliers->indices.size () == 0)
			{
				std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
				break;
			}

			if( (fabs(coefficients->values[0]) < 0.02) && 
					(fabs(coefficients->values[1]) < 0.02) && 
					(fabs(coefficients->values[2]) > 0.9) )
			{

				std::cerr << "Model coefficients: " << coefficients->values[0] << " " 
					<< coefficients->values[1] << " "
					<< coefficients->values[2] << " " 
					<< coefficients->values[3] << std::endl;

				height = coefficients->values[3];

//.........这里部分代码省略.........

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
//cloud definition  
   pcl::PointCloud<pcl::PointXYZ> cloud;
   pcl::PointCloud<pcl::PointXYZ> cloud2;
   pcl::PointCloud<pcl::PointXYZ> cloud3;
   pcl::ExtractIndices<pcl::PointXYZ> extract;
   pcl::PointCloud<pcl::PointXYZ> obj_cloud1;
   pcl::PointCloud<pcl::PointXYZ> cloud_objetos;

//sensor definition 
   sensor_msgs::PointCloud2 Plano_suelo;
   sensor_msgs::PointCloud2 cloud_sobrante; 
   sensor_msgs::PointCloud2 Objeto_cercano;
   sensor_msgs::PointCloud2 Objetos_detectados;

  Fitrar_Voxel(input, 0.008);

 if (cloud_voxel.height*cloud_voxel.width>9000){ 
    if (error==true){   
	  std::cerr << "DETECTANDO PUNTOS " << cloud_voxel.height*cloud_voxel.width<< std::endl;
	  error=false;
    }
  
  pcl::fromROSMsg (cloud_voxel, cloud);
 
  // segmentacion plana
  pcl::ModelCoefficients coefficients;
  pcl::PointIndices inliers; 
  float anguloK_S;

 while(true){ 
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  seg.setOptimizeCoefficients (true); 
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);
  seg.setMaxIterations (200); 
  seg.setInputCloud (cloud.makeShared ());
  seg.segment (inliers, coefficients); 
 
  //calculo del angulo de la kinect (angulo formado por la kinect y el suelo)
  anguloK_S=acos(-coefficients.values[1]/sqrt(pow(coefficients.values[2],2)+pow(-coefficients.values[0],2)+pow(-coefficients.values[1],2)))-PI; 
  if (anguloK_S*180/PI>-75){
    //std::cerr << "Angulo kinect/suelo: " << anguloK_S*180/PI << std::endl;
    break;
  }
  else{
    //std::cerr << "Angulo kinect/suelo: " << anguloK_S*180/PI <<  "Recalculo del plan "  << std::endl;
    extract.setInputCloud (cloud.makeShared ());
    extract.setIndices (boost::make_shared<pcl::PointIndices> (inliers));
    extract.setNegative (true);
    extract.filter (cloud);  
      	
  }
 }
	  /*std::cerr << "Model coefficients: " << coefficients.values[0] << " " 
		                              << coefficients.values[1] << " "
		                              << coefficients.values[2] << " " 
		                              << coefficients.values[3] << std::endl;
	  std::cerr << "Model inliers: " << inliers.indices.size () << std::endl;*/
    
    pcl::copyPointCloud(cloud, inliers, cloud2);  
    cloud3=cloud;

    extract.setInputCloud (cloud.makeShared ());
    extract.setIndices (boost::make_shared<pcl::PointIndices> (inliers));
    extract.setNegative (true);
    extract.filter (cloud3);      	

  // Creating the KdTree object for the search method of the extraction
  //Segmentacion por agrupacion
  pcl::search::KdTree<pcl::PointXYZ> tree;
  tree.setInputCloud (cloud3.makeShared ());

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.02); // 2cm
  ec.setMinClusterSize (80);
  ec.setMaxClusterSize (1500);
  ec.setSearchMethod (boost::make_shared<pcl::search::KdTree<pcl::PointXYZ> > (tree));
  ec.setInputCloud (cloud3.makeShared ());
  ec.extract (cluster_indices);

//std::cerr << "Numero de objecto detectado " << cluster_indices.size() << std::endl;


// Examine the clusters and find the nearest centroid
    float min_d = 10000;
    int memorize_indice=0;
    Eigen::Vector4f close;
    cloud_objetos.header=cloud3.header;
    for (unsigned int i = 0; i < cluster_indices.size(); ++i){
      pcl::PointCloud<pcl::PointXYZ> obj_cloud;
      pcl::copyPointCloud(cloud3, cluster_indices[i], obj_cloud); 

      cloud_objetos+=obj_cloud;

      Eigen::Vector4f xyz_centroid;
      pcl::compute3DCentroid(obj_cloud, xyz_centroid);

//.........这里部分代码省略.........

void callback(const sensor_msgs::PointCloud2::ConstPtr& cloud)
{
    ros::Time whole_start = ros::Time::now();

    ros::Time declare_types_start = ros::Time::now();

    // filter
    pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid;
    pcl::PassThrough<sensor_msgs::PointCloud2> pass;
    pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
    pcl::ExtractIndices<pcl::PointXYZ> extract_indices2;

    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZ> seg;

    // The plane and sphere coefficients
    pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
    pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());

    // The plane and sphere inliers
    pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
    pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());

    // The point clouds
    sensor_msgs::PointCloud2::Ptr passthrough_filtered (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr plane_seg_output_cloud (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr sphere_RANSAC_output_cloud (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr rest_whole_cloud (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr whole_pc (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr ball_candidate_output_cloud (new sensor_msgs::PointCloud2);

    // The PointCloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_output (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr remove_plane_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_RANSAC_output (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr remove_false_ball_candidate (new pcl::PointCloud<pcl::PointXYZ>);

    std::vector<int> inliers;

    ros::Time declare_types_end = ros::Time::now();

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /*
     * Pass through Filtering
     */
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    ros::Time pass_start = ros::Time::now();

    pass.setInputCloud (cloud);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0, 2.5);
    pass.filter (*passthrough_filtered);

    passthrough_pub.publish(passthrough_filtered);

    ros::Time pass_end = ros::Time::now();

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /*
     * for plane features pcl::SACSegmentation
     */
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    ros::Time plane_seg_start = ros::Time::now();

    // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
    pcl::fromROSMsg (*passthrough_filtered, *plane_seg_cloud);

    // Optional
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setAxis(Eigen::Vector3f (0, -1, 0));       // best plane should be perpendicular to z-axis
    seg.setMaxIterations (40);
    seg.setDistanceThreshold (0.05);
    seg.setInputCloud (plane_seg_cloud);
    seg.segment (*inliers_plane, *coefficients_plane);

    extract_indices.setInputCloud(plane_seg_cloud);
    extract_indices.setIndices(inliers_plane);
    extract_indices.setNegative(false);
    extract_indices.filter(*plane_seg_output);


    pcl::toROSMsg (*plane_seg_output, *plane_seg_output_cloud);
    plane_pub.publish(plane_seg_output_cloud);

    ros::Time plane_seg_end = ros::Time::now();

//.........这里部分代码省略.........

    void lines_callback(const art_lrf::Lines::ConstPtr& msg_lines) {

        if ((msg_lines->theta_index.size() != 0) &&
                (msg_lines->est_rho.size() != 0) && (msg_lines->endpoints.size() !=
                        0)) {


            scan2.reset(new vector<line> ());
            line temp;
            geometry_msgs::Point32 temp2;
            geometry_msgs::Point32 temp_theta_index;
            vector<int> temp3;

            for (int i = 0; i < msg_lines->theta_index.size(); i++) {
                temp.theta_index = msg_lines->theta_index[i];
                temp.est_rho = msg_lines->est_rho[i];

                for (int j = 0; j < msg_lines->endpoints[i].points.size(); j++) {
                    temp2 = msg_lines->endpoints[i].points[j];
                    temp3.push_back(temp2.x);
                    temp3.push_back(temp2.y);
                    temp.endpoints.push_back(temp3);
                }


                scan2->push_back(temp);
                temp.endpoints.clear();
            }

            if (firstRun == true) {
                firstRun = false;
                scan1 = scan2;
                previous_translation << 0,0;
            }
            else
            {

                compare_scans(scan1, scan2, previous_est_rot, est_rot,
                              previous_translation, est_translation );

                if( pow(pow(est_translation[0],2) + pow(est_translation[1],2),0.5) >
                        WAYPOINT_THRESHOLD)
                {
                    measurement new_base;
                    for(int i=0; i<scan1->size(); i++)
                    {
                        new_base.lines.push_back(scan1->at(i));
                    }
                    new_base.x = base_translation(0) + est_translation(0);
                    new_base.y = base_translation(1) + est_translation(1);
                    new_base.yaw = base_rot;

                    // float min_dist = 2*REMATCH_CANDIDATE_THRESH; //start with
                    //	      arbitrarily high value
                    // int min_index = 0;
                    // for( int i=0; i<base_scans.size(); i++)
                    // {
                    //  float current_dist = base_scans[i].dist_from(new_base);
                    //  if( current_dist < min_dist )
                    //  {
                    //      min_index = i;
                    //      min_dist = current_dist;
                    //  }
                    // }
                    // if(min_dist < REMATCH_CANDIDATE_THRESH)
                    // {
                    //  boost::shared_ptr<vector<line> > temp_boost_scan(new vector<line>);
                    //  for(int i=0; i<base_scans[min_index].lines.size(); i++)
                    //  {
                    //      temp_boost_scan->push_back(base_scans[min_index].lines[i]);
                    //  }

                    //  int expected_rot_difference = new_base.yaw -
                    //base_scans[min_index].yaw;
                    //  int actual_rot_difference;
                    //  Vector2f expected_translation_difference;
                    //  expected_translation_difference << new_base.x -
                    //base_scans[min_index].x, new_base.y - base_scans[min_index].y;
                    //  Vector2f actual_translation_difference;

                    //  compare_scans(temp_boost_scan, scan2,
                    //                new_base.yaw - base_scans[min_index].yaw,
                    //actual_rot_difference,
                    //                expected_translation_difference,
                    //actual_translation_difference );
                    //  float translation_error =
                    // pow(pow(expected_translation_difference(0) -
                    // 	  actual_translation_difference(0), 2) +
                    //     pow(expected_translation_difference(1) -
                    // 	  actual_translation_difference(1),2), 0.5);
                    //  if( (translation_error < REMATCH_VALIDATION_THRESH) &&
                    //(abs(expected_rot_difference - actual_rot_difference) < 7))
                    //  {
                    //      //overwrite the new_base values with the difference
                    //between the waypoint and the current scan
                    //      new_base.x = base_scans[min_index].x +
                    //actual_translation_difference[0];
                    //      new_base.y = base_scans[min_index].y +
                    //actual_translation_difference[1];
                    //;	//      new_base.yaw = base_scans[min_index].yaw + actual_rot_difference;
//.........这里部分代码省略.........

// In this function, the Voronoi cell is calculated, integrated and the new goal point is calculated and published.
void SimpleDeployment::publish()
{
    if ( got_map_ && got_me_ ) {
        double factor = map_.info.resolution / resolution_; // zoom factor for openCV visualization

        ROS_DEBUG("SimpleDeployment: Map received, determining Voronoi cells and publishing goal");

        removeOldAgents();

        // Create variables for x-values, y-values and the maximum and minimum of these, needed for VoronoiDiagramGenerator
        float xvalues[agent_catalog_.size()];
        float yvalues[agent_catalog_.size()];
        double xmin = 0.0, xmax = 0.0, ymin = 0.0, ymax = 0.0;
        cv::Point seedPt = cv::Point(1,1);

        // Create empty image with the size of the map to draw points and voronoi diagram in
        cv::Mat vorImg = cv::Mat::zeros(map_.info.height*factor,map_.info.width*factor,CV_8UC1);

        for ( uint i = 0; i < agent_catalog_.size(); i++ ) {
            geometry_msgs::Pose pose = agent_catalog_[i].getPose();

            // Keep track of x and y values
            xvalues[i] = pose.position.x;
            yvalues[i] = pose.position.y;

            // Keep track of min and max x
            if ( pose.position.x < xmin ) {
                xmin = pose.position.x;
            } else if ( pose.position.x > xmax ) {
                xmax = pose.position.x;
            }

            // Keep track of min and max y
            if ( pose.position.y < ymin ) {
                ymin = pose.position.y;
            } else if ( pose.position.y > ymax ) {
                ymax = pose.position.y;
            }

            // Store point as seed point if it represents this agent
            if ( agent_catalog_[i].getName() == this_agent_.getName() ){
                // Scale positions in metric system column and row numbers in image
                int c = ( pose.position.x - map_.info.origin.position.x ) * factor / map_.info.resolution;
                int r = map_.info.height * factor - ( pose.position.y - map_.info.origin.position.y ) * factor / map_.info.resolution;
                cv::Point pt =  cv::Point(c,r);

                seedPt = pt;
            }
            // Draw point on image
//            cv::circle( vorImg, pt, 3, WHITE, -1, 8);
        }

        ROS_DEBUG("SimpleDeployment: creating a VDG object and generating Voronoi diagram");
        // Construct a VoronoiDiagramGenerator (VoronoiDiagramGenerator.h)
        VoronoiDiagramGenerator VDG;

        xmin = map_.info.origin.position.x; xmax = map_.info.width  * map_.info.resolution + map_.info.origin.position.x;
        ymin = map_.info.origin.position.y; ymax = map_.info.height * map_.info.resolution + map_.info.origin.position.y;

        // Generate the Voronoi diagram using the collected x and y values, the number of points, and the min and max x and y values
        VDG.generateVoronoi(xvalues,yvalues,agent_catalog_.size(),float(xmin),float(xmax),float(ymin),float(ymax));

        float x1,y1,x2,y2;

        ROS_DEBUG("SimpleDeployment: collecting line segments from the VDG object");
        // Collect the generated line segments from the VDG object
        while(VDG.getNext(x1,y1,x2,y2))
        {
            // Scale the line segment end-point coordinates to column and row numbers in image
            int c1 = ( x1 - map_.info.origin.position.x ) * factor / map_.info.resolution;
            int r1 = vorImg.rows - ( y1 - map_.info.origin.position.y ) * factor / map_.info.resolution;
            int c2 = ( x2 - map_.info.origin.position.x ) * factor / map_.info.resolution;
            int r2 = vorImg.rows - ( y2 - map_.info.origin.position.y ) * factor / map_.info.resolution;

            // Draw line segment
            cv::Point pt1 = cv::Point(c1,r1),
                      pt2 = cv::Point(c2,r2);
            cv::line(vorImg,pt1,pt2,WHITE);
        }

        ROS_DEBUG("SimpleDeployment: drawing map occupancygrid and resizing to voronoi image size");
        // Create cv image from map data and resize it to the same size as voronoi image
        cv::Mat mapImg = drawMap();
        cv::Mat viewImg(vorImg.size(),CV_8UC1);
        cv::resize(mapImg, viewImg, vorImg.size(), 0.0, 0.0, cv::INTER_NEAREST );

        // Add images together to make the total image
        cv::Mat totalImg(vorImg.size(),CV_8UC1);
        cv::bitwise_or(viewImg,vorImg,totalImg);

        cv::Mat celImg = cv::Mat::zeros(vorImg.rows+2, vorImg.cols+2, CV_8UC1);
        cv::Scalar newVal = cv::Scalar(1), upDiff = cv::Scalar(100), loDiff = cv::Scalar(256);
        cv::Rect rect;

        cv::floodFill(totalImg,celImg,seedPt,newVal,&rect,loDiff,upDiff,4 + (255 << 8) + cv::FLOODFILL_MASK_ONLY);

        // Compute the center of mass of the Voronoi cell
        cv::Moments m = moments(celImg, false);
        cv::Point centroid(m.m10/m.m00, m.m01/m.m00);
//.........这里部分代码省略.........