void occlude_pcd(pcl::PointCloud<pcl::PointXYZ>::Ptr & cld_ptr,
int dim, double threshA, double threshB)
{
for(pcl::PointCloud<pcl::PointXYZ>::iterator it = cld_ptr->begin();
it != cld_ptr->end();)
{
if((it->data[dim] < threshA) || (it->data[dim] > threshB))
it = cld_ptr->erase(it);
else
++it;
}
}
void Node::crop_cloud(pcl::PointCloud<pcl::PointXYZ> &pcl_cloud, int laser, ros::Time time)
{
//CROP CLOUD
pcl::PointCloud<pcl::PointXYZ>::iterator i;
for (i = pcl_cloud.begin(); i != pcl_cloud.end();)
{
bool remove_point = 0;
if (i->z < 0 || i->z > max_z)
{
remove_point = 1;
}
if (sqrt(pow(i->x,2) + pow(i->y,2)) > max_radius)
{
remove_point = 1;
}
tf::StampedTransform transform;
listener_.lookupTransform ("/world", "/laser1", time, transform);
if (sqrt(pow(transform.getOrigin().x() - i->x,2) + pow(transform.getOrigin().y() - i->y,2)) > 0.25 && laser == 1)
{
remove_point = 1;
}
if (remove_point == 1)
{
i = pcl_cloud.erase(i);
}
else
{
i++;
}
}
//END CROP CLOUD
}
// int covered_no=0;
void GetOdom(const nav_msgs::Odometry::ConstPtr &msg)
{
nav_msgs::Odometry curr_pose = *msg;
geometry_msgs::Point ps;
ps.x = curr_pose.pose.pose.position.x;
ps.y = curr_pose.pose.pose.position.y;
ps.z = curr_pose.pose.pose.position.z;
int i=0;
while(i<(waypoint_msg.height*waypoint_msg.width))
{
float error=sqrt(pow((waypoint_msg.points.at(i).x-ps.x),2)+pow((waypoint_msg.at(i).y-ps.y),2)+ pow((waypoint_msg.at(i).z-ps.z),2));
if (error<0.5)
{
waypoint_msg.erase(waypoint_msg.begin()+i);
}
i++;
}
i=0;
while(i<(dummy_msg.height*dummy_msg.width))
{
float error=sqrt(pow((dummy_msg.points.at(i).x-ps.x),2)+pow((dummy_msg.at(i).y-ps.y),2)+ pow((dummy_msg.at(i).z-ps.z),2));
if (error<0.5)
{
if (i==dummy_msg.width-1)
{
covered_ids.push_back(1);
cout<<"id::"<<1<<endl;
}
else
{
covered_ids.push_back(csv_values[i][6]);
cout<<"id::"<<csv_values[i][6]<<endl;
}
}
i++;
}
cout<<"no of waypoints to be covered:"<<(waypoint_marker_msg.points.size()/2)<<endl;
std_msgs::ColorRGBA color_msg_local,redcolor;
geometry_msgs::Point point_local;
waypoint_marker_msg.points.resize(waypoint_msg.height*waypoint_msg.width);
waypoint_marker_msg.colors.resize(waypoint_msg.height*waypoint_msg.width);
for (int i=0;i<(waypoint_msg.height*waypoint_msg.width);i++)
{
waypoint_marker_msg.points.at(i).x = waypoint_msg.points.at(i).x;
waypoint_marker_msg.points.at(i).y = waypoint_msg.points.at(i).y;
waypoint_marker_msg.points.at(i).z = waypoint_msg.points.at(i).z;
waypoint_marker_msg.colors.at(i).r=0;
waypoint_marker_msg.colors.at(i).g=0;
waypoint_marker_msg.colors.at(i).b=1;
waypoint_marker_msg.colors.at(i).a=1;
}
//assigning each triangle of mesh to triangle list marker
redcolor.r=1;redcolor.b=0;redcolor.g=0;redcolor.a=1;
for(int i=0;i<covered_ids.size();i++)
{ int x=int(covered_ids.at(i)-2)*3;
if(x>=0)
{
marker.add_point(covered_regions,mesh_marker.points.at(x),redcolor);
marker.add_point(covered_regions,mesh_marker.points.at(x+1),redcolor);
marker.add_point(covered_regions,mesh_marker.points.at(x+2),redcolor);
}
}
covered_ids.clear();
pub_waypoint_marker.publish(waypoint_marker_msg);
pub_waypoint.publish(waypoint_msg);
pub_mesh_marker.publish(mesh_marker);
pub_covered_regions.publish(covered_regions);
}