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;
}