Go to the documentation of this file.00001 #include "pointcloud_to_eigenvectors_alg_node.h"
00002 #include <tf/transform_broadcaster.h>
00003
00004 PointcloudToEigenvectorsAlgNode::PointcloudToEigenvectorsAlgNode(void) :
00005 algorithm_base::IriBaseAlgorithm<PointcloudToEigenvectorsAlgorithm>()
00006 {
00007
00008
00009
00010
00011
00012
00013
00014
00015 this->getEigenVectors_server_ = this->public_node_handle_.advertiseService("getEigenVectors", &PointcloudToEigenvectorsAlgNode::getEigenVectorsCallback, this);
00016
00017
00018
00019
00020
00021
00022 }
00023
00024 PointcloudToEigenvectorsAlgNode::~PointcloudToEigenvectorsAlgNode(void)
00025 {
00026
00027 }
00028
00029 void PointcloudToEigenvectorsAlgNode::mainNodeThread(void)
00030 {
00031
00032
00033
00034
00035
00036
00037
00038 }
00039
00040
00041
00042
00043 bool PointcloudToEigenvectorsAlgNode::getEigenVectorsCallback(iri_perception_msgs::PclToMarker::Request &req, iri_perception_msgs::PclToMarker::Response &res)
00044 {
00045 ROS_INFO("PointcloudToEigenvectorsAlgNode::getEigenVectorsCallback: New Request Received!");
00046
00047
00048 this->alg_.lock();
00049
00050
00051 sensor_msgs::PointCloud2::ConstPtr point_cloud_ConstPtr;
00052 point_cloud_ConstPtr = boost::make_shared<sensor_msgs::PointCloud2>(req.pointcloud);
00053
00054 if(!alg_.get_eigenvectors(point_cloud_ConstPtr))
00055 {
00056 ROS_INFO("PointcloudToPlaneAlgNode::getPlaneCallback: ERROR: alg is not on run mode yet.");
00057 }
00058
00059 res.marker = alg_.getMarker();
00060
00061 static tf::TransformBroadcaster br;
00062 tf::Transform transform;
00063 transform.setOrigin( tf::Vector3(alg_.getPlaneCentroid().x(), alg_.getPlaneCentroid().y(), alg_.getPlaneCentroid().z()) );
00064 transform.setRotation( tf::Quaternion(alg_.getPlanePose().x(), alg_.getPlanePose().y(), alg_.getPlanePose().z(),alg_.getPlanePose().w()) );
00065 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), req.pointcloud.header.frame_id, "centroid"));
00066
00067
00068 this->alg_.unlock();
00069
00070
00071 return true;
00072 }
00073
00074
00075
00076
00077
00078 void PointcloudToEigenvectorsAlgNode::node_config_update(Config &config, uint32_t level)
00079 {
00080 this->alg_.lock();
00081
00082 this->alg_.unlock();
00083 }
00084
00085 void PointcloudToEigenvectorsAlgNode::addNodeDiagnostics(void)
00086 {
00087 }
00088
00089
00090 int main(int argc,char *argv[])
00091 {
00092 return algorithm_base::main<PointcloudToEigenvectorsAlgNode>(argc, argv, "pointcloud_to_eigenvectors_alg_node");
00093 }