vector<pair<double,double> > PoissonDiskSampling::Generate(){
point first_point(rand() % m_width, rand() % m_height);
m_process.push_back(first_point);
m_sample.push_back(make_pair(first_point.x, first_point.y));
int first_point_x = first_point.x/m_cell_size;
int first_point_y = first_point.y/m_cell_size;
m_grid[first_point_x][first_point_y] = new point(first_point);
while( !m_process.empty() ){
int new_point_index = rand() % m_process.size();
point new_point = m_process[new_point_index];
m_process.erase(m_process.begin() + new_point_index);
for(int i = 0; i < m_point_count; i++){
point new_point_around = generatePointAround(new_point);
if(inRectangle(new_point_around) && !inNeighbourhood(new_point_around)){
// cout << "Nuevo punto: (" << new_point_around.x << ", " << new_point_around.y << ")" << endl;
m_process.push_back(new_point_around);
m_sample.push_back(make_pair(new_point_around.x, new_point_around.y));
int new_point_x = new_point_around.x/m_cell_size;
int new_point_y = new_point_around.y/m_cell_size;
m_grid[new_point_x][new_point_y] = new point(new_point_around);
}
}
}
return m_sample;
}
void ScavTaskWhiteBoard::callback_human_detected(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
Pose a1 = Pose(-30.88, 0.06);
Pose b1 = Pose(-29.48, 0.06);
Pose c1 = Pose(-30.89, -3.16);
Pose a2 = Pose(-8.25, -5.99);
Pose b2 = Pose(-6.83, -6.05);
Pose c2 = Pose(-8.21, -11.26);
Pose pose = Pose(msg->pose.position.x, msg->pose.position.y);
if (inRectangle(pose, a1, b1, c1) or inRectangle(pose, a2, b2, c2)) {
ROS_INFO("People detected near a white board, saving picture...");
cv_bridge::CvImageConstPtr cv_ptr;
cv_ptr = cv_bridge::toCvShare(wb_image, sensor_msgs::image_encodings::BGR8);
boost::posix_time::ptime curr_time = boost::posix_time::second_clock::local_time();
// std::string time_str = boost::posix_time::to_simple_string(curr_time);
std::string time_str = boost::posix_time::to_iso_extended_string(curr_time);
if (false == boost::filesystem::is_directory(directory)) {
boost::filesystem::path tmp_path(directory);
boost::filesystem::create_directory(tmp_path);
}
wb_path_to_image = directory + "white_board_" + time_str + ".PNG";
cv::imwrite(wb_path_to_image, cv_ptr->image);
search_planner->setTargetDetection(true); // change status to terminate the motion thread
ROS_INFO_STREAM("Finished saving picture: " << wb_path_to_image);
task_completed = true;
}
}
