pointcloud_to_octomap_alg_node.cpp
Go to the documentation of this file.
00001 #include "pointcloud_to_octomap_alg_node.h"
00002 #include <octomap/math/Vector3.h>
00003 
00004 PointcloudToOctomapAlgNode::PointcloudToOctomapAlgNode(void) :
00005   algorithm_base::IriBaseAlgorithm<PointcloudToOctomapAlgorithm>()
00006 {
00007   //init class attributes if necessary
00008   //this->loop_rate_ = 2;//in [Hz]
00009 
00010   // [init publishers]
00011   
00012   // [init subscribers]
00013   
00014   // [init services]
00015   this->store_octomap_server_ = this->public_node_handle_.advertiseService("store_octomap", &PointcloudToOctomapAlgNode::store_octomapCallback, this);
00016   
00017   // [init clients]
00018   
00019   // [init action servers]
00020   
00021   // [init action clients]
00022 }
00023 
00024 PointcloudToOctomapAlgNode::~PointcloudToOctomapAlgNode(void)
00025 {
00026   // [free dynamic memory]
00027 }
00028 
00029 void PointcloudToOctomapAlgNode::mainNodeThread(void)
00030 {
00031   // [fill msg structures]
00032   
00033   // [fill srv structure and make request to the server]
00034   
00035   // [fill action structure and make request to the action server]
00036 
00037   // [publish messages]
00038 }
00039 
00040 /*  [subscriber callbacks] */
00041 
00042 /*  [service callbacks] */
00043 bool PointcloudToOctomapAlgNode::store_octomapCallback(iri_perception_msgs::StorePointCloud2::Request &req, iri_perception_msgs::StorePointCloud2::Response &res) 
00044 { 
00045   ROS_INFO("PointcloudToOctomapAlgNode::store_octomapCallback: New Request Received!"); 
00046 
00047   //use appropiate mutex to shared variables if necessary 
00048   alg_.lock(); 
00049   store_octomap_mutex_.enter(); 
00050 
00051   // point_cloud 
00052   sensor_msgs::PointCloud2::ConstPtr point_cloud;
00053   point_cloud = boost::make_shared<sensor_msgs::PointCloud2>(req.point_cloud);
00054 
00055   // sensor_origin
00056   octomap::point3d sensor_origin(0.0f, 0.0f, 0.0f);
00057   // frame_origin
00058   octomath::Vector3 trans(req.sensor_pose.pose.position.x, req.sensor_pose.pose.position.y, req.sensor_pose.pose.position.z); 
00059   octomath::Quaternion rot(req.sensor_pose.pose.orientation.w, req.sensor_pose.pose.orientation.x, req.sensor_pose.pose.orientation.y, req.sensor_pose.pose.orientation.z); 
00060   octomap::pose6d frame_origin(trans, rot);
00061 
00062   if(!alg_.isRunning(req.file_name.data, point_cloud, sensor_origin, frame_origin)) 
00063   { 
00064     ROS_INFO("PointcloudToOctomapAlgNode::store_octomapCallback: ERROR: alg is not on run mode yet."); 
00065   } 
00066 
00067   //unlock previously blocked shared variables 
00068   alg_.unlock(); 
00069   store_octomap_mutex_.exit(); 
00070 
00071   return true; 
00072 }
00073 
00074 /*  [action callbacks] */
00075 
00076 /*  [action requests] */
00077 
00078 void PointcloudToOctomapAlgNode::node_config_update(Config &config, uint32_t level)
00079 {
00080   this->alg_.lock();
00081 
00082   this->alg_.unlock();
00083 }
00084 
00085 void PointcloudToOctomapAlgNode::addNodeDiagnostics(void)
00086 {
00087 }
00088 
00089 /* main function */
00090 int main(int argc,char *argv[])
00091 {
00092   return algorithm_base::main<PointcloudToOctomapAlgNode>(argc, argv, "pointcloud_to_octomap_alg_node");
00093 }


iri_pointcloud_to_octomap
Author(s): Sergi Foix
autogenerated on Fri Dec 6 2013 21:26:05