Go to the documentation of this file.00001 #include "pointcloud_to_pcd_alg_node.h"
00002
00003 PointcloudToPcdAlgNode::PointcloudToPcdAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<PointcloudToPcdAlgorithm>()
00005 {
00006
00007
00008
00009
00010
00011
00012
00013
00014 store_pointcloud_server_ = public_node_handle_.advertiseService("store_pointcloud", &PointcloudToPcdAlgNode::store_pointcloudCallback, this);
00015
00016
00017
00018
00019
00020
00021 }
00022
00023 PointcloudToPcdAlgNode::~PointcloudToPcdAlgNode(void)
00024 {
00025
00026 }
00027
00028 void PointcloudToPcdAlgNode::mainNodeThread(void)
00029 {
00030
00031
00032
00033
00034
00035
00036
00037 }
00038
00039
00040
00041
00042 bool PointcloudToPcdAlgNode::store_pointcloudCallback(iri_perception_msgs::StorePointCloud2::Request &req, iri_perception_msgs::StorePointCloud2::Response &res)
00043 {
00044 ROS_INFO("PointcloudToPcdAlgNode::store_pointcloudCallback: New Request Received!");
00045
00046
00047 alg_.lock();
00048 store_pointcloud_mutex_.enter();
00049
00050
00051 sensor_msgs::PointCloud2::ConstPtr point_cloud;
00052 point_cloud = boost::make_shared<sensor_msgs::PointCloud2>(req.point_cloud);
00053
00054 Eigen::Vector4f origin = Eigen::Vector4f(req.sensor_pose.pose.position.x, req.sensor_pose.pose.position.y, req.sensor_pose.pose.position.z, 1.0);
00055 Eigen::Quaternionf orientation = Eigen::Quaternionf (req.sensor_pose.pose.orientation.w, req.sensor_pose.pose.orientation.x, req.sensor_pose.pose.orientation.y, req.sensor_pose.pose.orientation.z);
00056 bool binary_mode = false;
00057
00058 if(!alg_.isRunning(req.file_name.data, point_cloud, origin, orientation, binary_mode ))
00059 {
00060 ROS_INFO("PointcloudToPcdAlgNode::store_pointcloudCallback: ERROR: alg is not on run mode yet.");
00061 }
00062
00063
00064 alg_.unlock();
00065 store_pointcloud_mutex_.exit();
00066
00067 return true;
00068 }
00069
00070
00071
00072
00073
00074 void PointcloudToPcdAlgNode::node_config_update(Config &config, uint32_t level)
00075 {
00076 this->alg_.lock();
00077
00078 this->alg_.unlock();
00079 }
00080
00081 void PointcloudToPcdAlgNode::addNodeDiagnostics(void)
00082 {
00083 }
00084
00085
00086 int main(int argc,char *argv[])
00087 {
00088 return algorithm_base::main<PointcloudToPcdAlgNode>(argc, argv, "pointcloud_to_pcd_alg_node");
00089 }