int main(int argc, char** argv) { marker.line_list(linelist); ros::init (argc, argv, "uncovered_regions"); ros::NodeHandle nh; //publishers pub_waypoint = nh.advertise<pcl::PointCloud<pcl::PointXYZ>> ("waypoints", 1); pub_waypoint_marker = nh.advertise<visualization_msgs::Marker> ("all_nodes", 10); pub_mesh_marker= nh.advertise<visualization_msgs::Marker> ("mesh_marker", 10); pub_linelist = nh.advertise<visualization_msgs::Marker> ("line_list",10); pub_covered_regions=nh.advertise<visualization_msgs::Marker>("covered_regions",10); //subscribers ros::Subscriber odometry_sub = nh.subscribe<nav_msgs::Odometry>("quad_sim_sysid_node/pose", 1, GetOdom); //reading stl file Filereader file; string path=ros::package::getPath("koptplanner")+"/src/mesh.txt"; file.read(path,mesh_coor); marker.triangle_list(mesh_marker); marker.triangle_list(covered_regions); std_msgs::ColorRGBA mesh_color; geometry_msgs::Point mesh_point; //define mesh color mesh_color.r = 0 ; mesh_color.g = 0.5 ; mesh_color.b = 0.5 ; mesh_color.a = 0.5; //assigning each triangle of mesh to triangle list marker for(int i=0;i<mesh_coor.size();i++) { mesh_point.x = mesh_coor[i][0]; mesh_point.y = mesh_coor[i][1]; mesh_point.z = mesh_coor[i][2]; mesh_marker.points.push_back(mesh_point); // marker.add_point(mesh_marker,mesh_point,mesh_color); } mesh_marker.color=mesh_color; path=ros::package::getPath("boiler_gazebo")+"/src/latestPath.csv"; file.read(path,csv_values); // display results cout.precision(2); cout.setf(ios::fixed,ios::floatfield); path=ros::package::getPath("koptplanner")+"/src/tour.txt"; file.read(path,tour); // display results cout.precision(2); cout.setf(ios::fixed,ios::floatfield); //waypoint marker msg basically points marker.points(waypoint_marker_msg); //scaling down the size waypoint_marker_msg.scale.x = 0.1; waypoint_marker_msg.scale.y = 0.1; waypoint_marker_msg.scale.z = 0.1; std_msgs::ColorRGBA color_msg_local; waypoint_marker_msg.points.resize(csv_values.size()); waypoint_marker_msg.colors.resize(csv_values.size()); waypoint_msg.header.frame_id = "world"; //point cloud of positions waypoint_msg.height = 1; waypoint_msg.width =csv_values.size(); for (int i=0;i<csv_values.size();i++) { for(int j=0;j<csv_values[i].size();j++) { cout<<csv_values[i][j]<<" "; } cout<<endl; waypoint_msg.points.push_back(pcl::PointXYZ(csv_values[i][0],csv_values[i][1],csv_values[i][2])); } geometry_msgs::Point point_local; mesh_color.r=0.5; for(int i=0;i<csv_values.size();i++) { int l=csv_values[i][6]-2; if(l>=0) { point_local.x=csv_values[i][0];point_local.y=csv_values[i][1];point_local.z=csv_values[i][2]; marker.add_point(linelist,point_local,mesh_color); point_local.x=(mesh_marker.points.at(l*3).x+mesh_marker.points.at(l*3+1).x+mesh_marker.points.at(l*3+2).x)/3; point_local.y=(mesh_marker.points.at(l*3).y+mesh_marker.points.at(l*3+1).y+mesh_marker.points.at(l*3+2).y)/3; point_local.z=(mesh_marker.points.at(l*3).z+mesh_marker.points.at(l*3+1).z+mesh_marker.points.at(l*3+2).z)/3; marker.add_point(linelist,point_local,mesh_color); } } //dummy_msg because by erasing the waypoint we will not get correct id of covered triangle dummy_msg.header.frame_id = "world"; //point cloud of positions dummy_msg.height = 1; dummy_msg.width =csv_values.size(); for (int i=0;i<csv_values.size();i++) { dummy_msg.points.push_back(pcl::PointXYZ(csv_values[i][0],csv_values[i][1],csv_values[i][2])); } ros::Rate loop_rate(10.0); while(ros::ok()) { pub_linelist.publish(linelist); ros::spinOnce(); loop_rate.sleep(); } }