36 #ifndef _texturize_polygon_primitives_CPP_
37 #define _texturize_polygon_primitives_CPP_
42 #define _USE_THREADS_ 0
48 cam_roof_fc.
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC3);
50 catch (cv_bridge::Exception& e)
52 ROS_ERROR(
"cv_bridge exception: %s", e.what());
65 cam_roof_fl.
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC3);
67 catch (cv_bridge::Exception& e)
69 ROS_ERROR(
"cv_bridge exception: %s", e.what());
82 cam_roof_fr.
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC3);
84 catch (cv_bridge::Exception& e)
86 ROS_ERROR(
"cv_bridge exception: %s", e.what());
99 cam_roof_rc.
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC3);
101 catch (cv_bridge::Exception& e)
103 ROS_ERROR(
"cv_bridge exception: %s", e.what());
118 catch (cv_bridge::Exception& e)
120 ROS_ERROR(
"cv_bridge exception: %s", e.what());
133 ros::Time t = ros::Time::now();
134 ROS_INFO(
"Building local mesh for camera %s to plg %s",cam_name.c_str(), plg->
data.
misc.
name);
141 catch (tf::TransformException ex){ROS_ERROR(
"%s",ex.what());
return -1;}
145 sprintf(str,
"{%s, %s, %0.1f}", plg->
data.
misc.
name,cam_name.c_str(), dur.toSec());
146 std::string projection_name = str;
152 ros::Duration d = ros::Time::now() - t;
153 ROS_INFO(
"Successfully build of local mesh (%ld faces) for camera projection %s to plg %s (%f seconds)",plg->
cp[v].mesh.number_of_faces(), cam_name.c_str(),plg->
data.
misc.
name, d.toSec());
158 ROS_INFO(
"Could not build local mesh for camera %s to plg %s",cam_name.c_str(),plg->
data.
misc.
name);
164 ROS_INFO(
"Did not receive any new image from camera %s. Cannot create local mesh",cam_name.c_str());
177 ROS_INFO(
"\n__________________________________\nStart processing thread plg %s\n__________________________________\n", plg->
data.
misc.
name);
178 pthread_mutex_lock(&plg->
mutex);
194 pthread_mutex_unlock(&plg->
mutex);
195 ROS_INFO(
"\n__________________________________\nFinished processing thread plg %s\n__________________________________\n", plg->
data.
misc.
name);
208 ros::Duration(0.1).sleep();
210 ROS_INFO(
"Received a new polygon primitive msg. Polygon name %s", input.name.c_str());
215 if (input.name!=
"p1")
222 std::map<std::string, c_polygon_primitive_with_texture>::iterator it;
223 it =
pmap.find(input.name);
228 if (it !=
pmap.end())
230 ROS_INFO(
"have a key with code %s . Will addapt polygon", input.name.c_str());
232 it->second.readapt_to_new_plane((polygon_primitive_msg::polygon_primitive*)&input);
245 ROS_INFO(
"have no key with code %s . Will add polygon to pmap", input.name.c_str());
249 new_plg_from_msg.import_from_polygon_primitive_msg((polygon_primitive_msg::polygon_primitive*) &input);
252 std::pair< std::map<std::string, c_polygon_primitive_with_texture>::iterator,
bool> ret;
253 ret =
pmap.insert(std::pair<std::string, c_polygon_primitive_with_texture>(input.name, new_plg_from_msg));
254 if (ret.second==
false)
257 ROS_ERROR(
"Error, could not insert polygon to map");
261 plg = &ret.first->second;
278 int main(
int argc,
char **argv)
280 ros::init(argc, argv,
"texturize_polygon_primitives");
286 cv::startWindowThread();
288 tf::TransformListener listener;
291 image_transport::ImageTransport it(n);
293 while(n.ok()){ros::spinOnce();
if (
cam_roof_fc.
msg_received)
break; ROS_INFO(
"Waiting for first topic /cam_roof_fc");r1.sleep();}
308 ros::Publisher markerarray_pub = n.advertise<visualization_msgs::MarkerArray>(
"/PolygonMarkers_with_texture", 5);
311 ROS_INFO(
"Waiting for topic /polygon_primitive");
314 ros::Time t=ros::Time::now();
317 ros::Duration d = ros::Time::now()-t;
324 std::vector<visualization_msgs::Marker> marker_vec;
325 visualization_msgs::MarkerArray marker_array_msg;
326 std::map<std::string, c_polygon_primitive_with_texture>::iterator it;
328 ROS_INFO(
"Sending textured polygon primitives to RVIZ");
329 for (it=
pmap.begin(); it!=
pmap.end();++it)
331 visualization_msgs::MarkerArray marker_vec;
336 if (pthread_mutex_trylock(&it->second.mutex))
339 ROS_INFO(
"Sending tpp %s", it->second.data.misc.name);
340 it->second.create_textures_vizualization_msg(&marker_vec,
false);
342 p_markerarray_pub->publish(marker_vec);
344 for (
size_t i=0; i<it->second.cp.size();i++)
346 cv::namedWindow(it->second.cp[i].projection_name);
347 cv::imshow(it->second.cp[i].projection_name,it->second.cp[i].image_gui);
353 pthread_mutex_unlock(&it->second.mutex);
357 ROS_WARN(
"Could not send tpp %s. Thread locked", it->second.data.misc.name);
_EXTERN_ ros::Publisher * p_markerarray_pub
_EXTERN_ t_cam cam_roof_rc
int add_camera_projection_known_camera(cv::Mat *m, ros::Time t, tf::StampedTransform tf, std::string cam_name, std::string projection_name, cv::Scalar color)
std::vector< class_camera_projection > cp
void handler_cam_roof_fr(const sensor_msgs::ImageConstPtr &msg)
_EXTERN_ t_cam cam_roof_fl
_EXTERN_ t_cam cam_roof_fc
int main(int argc, char **argv)
The main function for the extraction of polygon primitives.
_EXTERN_ tf::TransformListener * p_listener
void handler_cam_roof_fc_6mm(const sensor_msgs::ImageConstPtr &msg)
_EXTERN_ t_cam cam_roof_fr
class_colormap * colormap
_EXTERN_ ros::Time mission_start
_EXTERN_ t_cam cam_roof_fc_6mm
struct t_polygon_primitive_data::@2 misc
void handler_cam_roof_fl(const sensor_msgs::ImageConstPtr &msg)
int add_camera_projection_to_polygon(std::string cam_name, t_cam *cam, c_polygon_primitive_with_texture *plg)
void polygon_primitive_received_handler(const polygon_primitive_msg::polygon_primitive &input)
Handles the receiving of a new polygonal primitive message.
t_polygon_primitive_data data
_EXTERN_ ros::NodeHandle * p_node
cv_bridge::CvImagePtr cv_ptr
Header of the texturize polygon primitives binary.
int build_global_mesh(ros::Publisher *p_marker_pub)
void * process_polygon_primitive(void *ptr)
_EXTERN_ std::map< std::string, c_polygon_primitive_with_texture > pmap
void handler_cam_roof_fc(const sensor_msgs::ImageConstPtr &msg)
void handler_cam_roof_rc(const sensor_msgs::ImageConstPtr &msg)