00001 #include "pointcloud_to_mesh_alg_node.h" 00002 00003 PointCloudToMeshAlgNode::PointCloudToMeshAlgNode(void) : 00004 algorithm_base::IriBaseAlgorithm<PointCloudToMeshAlgorithm>() 00005 { 00006 //init class attributes if necessary 00007 //this->loop_rate_ = 2;//in [Hz] 00008 00009 // [init publishers] 00010 00011 // [init subscribers] 00012 00013 // [init services] 00014 this->get_mesh_server_ = this->public_node_handle_.advertiseService("get_mesh", &PointCloudToMeshAlgNode::get_meshCallback, this); 00015 00016 // [init clients] 00017 00018 // [init action servers] 00019 00020 // [init action clients] 00021 } 00022 00023 PointCloudToMeshAlgNode::~PointCloudToMeshAlgNode(void) 00024 { 00025 // [free dynamic memory] 00026 } 00027 00028 void PointCloudToMeshAlgNode::mainNodeThread(void) 00029 { 00030 // [fill msg structures] 00031 00032 // [fill srv structure and make request to the server] 00033 00034 // [fill action structure and make request to the action server] 00035 00036 // [publish messages] 00037 } 00038 00039 /* [subscriber callbacks] */ 00040 00041 /* [service callbacks] */ 00042 bool PointCloudToMeshAlgNode::get_meshCallback(iri_perception_msgs::ProcessPointCloud2::Request &req, iri_perception_msgs::ProcessPointCloud2::Response &res) 00043 { 00044 ROS_INFO("PointCloudToMeshAlgNode::get_meshCallback: New Request Received!"); 00045 00046 //use appropiate mutex to shared variables if necessary 00047 this->alg_.lock(); 00048 this->get_mesh_mutex_.enter(); 00049 00050 sensor_msgs::PointCloud2::ConstPtr point_cloud_ConstPtr; 00051 point_cloud_ConstPtr = boost::make_shared<sensor_msgs::PointCloud2>(req.input); 00052 00053 if(!alg_.compute_mesh(point_cloud_ConstPtr)) 00054 { 00055 ROS_INFO("PointCloudToMeshAlgNode::get_meshCallback: ERROR: alg is not on run mode yet."); 00056 } 00057 // TODO: There is not response right now 00058 // It would be great to return the mesh as a message or the url to the stored file. 00059 //res.mesh = *alg_.get_mesh(); 00060 00061 //unlock previously blocked shared variables 00062 this->alg_.unlock(); 00063 this->get_mesh_mutex_.exit(); 00064 00065 return true; 00066 } 00067 00068 /* [action callbacks] */ 00069 00070 /* [action requests] */ 00071 00072 void PointCloudToMeshAlgNode::node_config_update(Config &config, uint32_t level) 00073 { 00074 this->alg_.lock(); 00075 alg_.set_down_res(config.down_res); 00076 alg_.set_radius(config.radius); 00077 alg_.set_mu(config.mu); 00078 alg_.set_num_of_neigh(config.num_neigh); 00079 alg_.set_max_surface_angle(config.max_surf_angle); 00080 alg_.set_min_angle(config.min_ang); 00081 alg_.set_max_angle(config.max_ang); 00082 alg_.set_normal_consistency(config.norm_consistency); 00083 this->alg_.unlock(); 00084 } 00085 00086 void PointCloudToMeshAlgNode::addNodeDiagnostics(void) 00087 { 00088 } 00089 00090 /* main function */ 00091 int main(int argc,char *argv[]) 00092 { 00093 return algorithm_base::main<PointCloudToMeshAlgNode>(argc, argv, "pointcloud_to_mesh_alg_node"); 00094 }