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
00008
00009
00010
00011
00012
00013
00014
00015 this->store_octomap_server_ = this->public_node_handle_.advertiseService("store_octomap", &PointcloudToOctomapAlgNode::store_octomapCallback, this);
00016
00017
00018
00019
00020
00021
00022 }
00023
00024 PointcloudToOctomapAlgNode::~PointcloudToOctomapAlgNode(void)
00025 {
00026
00027 }
00028
00029 void PointcloudToOctomapAlgNode::mainNodeThread(void)
00030 {
00031
00032
00033
00034
00035
00036
00037
00038 }
00039
00040
00041
00042
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
00048 alg_.lock();
00049 store_octomap_mutex_.enter();
00050
00051
00052 sensor_msgs::PointCloud2::ConstPtr point_cloud;
00053 point_cloud = boost::make_shared<sensor_msgs::PointCloud2>(req.point_cloud);
00054
00055
00056 octomap::point3d sensor_origin(0.0f, 0.0f, 0.0f);
00057
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
00068 alg_.unlock();
00069 store_octomap_mutex_.exit();
00070
00071 return true;
00072 }
00073
00074
00075
00076
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
00090 int main(int argc,char *argv[])
00091 {
00092 return algorithm_base::main<PointcloudToOctomapAlgNode>(argc, argv, "pointcloud_to_octomap_alg_node");
00093 }