示例#1
0
void SurfaceReconstruction::msgCallback(const sensor_msgs::PointCloud2::ConstPtr& data)
{
	if (data.get()->data.size() && this->noCloudReceived)
	{
		pcl::fromROSMsg(*data, this->points);
		ROS_INFO(" surface_reconstruction: Point cloud received for reconstruction ...");
		this->noCloudReceived = false;
	}
}
    //only called on obstacle detected
    //Relies on empty point clouds is still being published!
    void pcCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
    {
      //if (activated == true)
      //{
      static uint8_t obstacle_state = 0;
      static ros::Time last_time = ros::Time::now();
      ros::Duration duration(3.0);
      ros::Time cur_time = ros::Time::now();

      //ROS_INFO("cur_time %.2f and last_time %.2f and duration %.2f", cur_time.toSec(), last_time.toSec(), duration.toSec());
      if ((cur_time - last_time) > duration)
      {
        ROS_INFO("Resetting obstacle state");
        obstacle_state = 0;
      }

      //ROS_INFO("Got point cloud detection result..");
      pcl::PointCloud<pcl::PointXYZ>::Ptr input (new pcl::PointCloud<pcl::PointXYZ>);
      pcl::PointCloud<pcl::PointXYZ>::Ptr output (new pcl::PointCloud<pcl::PointXYZ>);

      pcl::fromROSMsg(*msg.get(), *input.get());

      // transform
      pcl_ros::transformPointCloud("/base_link", *input, *output, tf_listener);

      // mutex lock cloud_bb
      bb_mutex.lock();

      // calculate bounding box
      getBoundingBox(*output, cloud_bb);
      float bb_zmax = cloud_bb.z_max;
      if (bb_zmax > 0)
      {
        publishMarker(cloud_bb);
      }

      bb_mutex.unlock();

      if (bb_zmax > HUMAN_MIN)
      {
        human_detected = true;
        obstacle_mutex.lock();
        obstacle_detected = true;
        obstacle_mutex.unlock();
        if (obstacle_state != 1)
        {
          ROS_INFO("Obstacle detected (%.2fm): human", bb_zmax);
          obstacle_state = 1;
        }
      }
      else if (bb_zmax > OBS_MIN) // higher than the ground
      {
        human_detected = false;
        obstacle_mutex.lock();
        obstacle_detected = true;
        obstacle_mutex.unlock();
        if (obstacle_state != 2)
        {
          ROS_INFO("Obstacle detected (%.2fm): static object", bb_zmax);
          obstacle_state = 2;
        }
      }//probably no obstacle
      else
      {
        obstacle_mutex.lock();
        obstacle_detected = false;
        obstacle_mutex.unlock();
      }
      last_time = cur_time;
    }