Go to the documentation of this file.00001 #include "from_topic_to_pointcloud_alg_node.h"
00002
00003 FromTopicToPointcloudAlgNode::FromTopicToPointcloudAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<FromTopicToPointcloudAlgorithm>(), ack_(false)
00005 {
00006
00007
00008
00009
00010
00011
00012
00013
00014 this->getPointCloud2_server_ = this->public_node_handle_.advertiseService("getPointCloud2", &FromTopicToPointcloudAlgNode::getPointCloud2Callback, this);
00015
00016
00017
00018
00019
00020
00021 }
00022
00023 FromTopicToPointcloudAlgNode::~FromTopicToPointcloudAlgNode(void)
00024 {
00025
00026 }
00027
00028 void FromTopicToPointcloudAlgNode::mainNodeThread(void)
00029 {
00030
00031
00032
00033
00034
00035
00036
00037 }
00038
00039
00040 void FromTopicToPointcloudAlgNode::topic_name_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00041 {
00042
00043
00044
00045
00046 this->topic_name_mutex_.enter();
00047
00048 pointcloud_ = *msg;
00049 ack_ = true;
00050
00051
00052
00053 this->topic_name_mutex_.exit();
00054 }
00055
00056
00057 bool FromTopicToPointcloudAlgNode::getPointCloud2Callback(iri_perception_msgs::GetPointCloud2::Request &req, iri_perception_msgs::GetPointCloud2::Response &res)
00058 {
00059 ROS_INFO("FromTopicToPointcloudAlgNode::getPointCloud2Callback: New Request Received!");
00060
00061 topic_name_subscriber_ = this->public_node_handle_.subscribe(req.topic.data, 1, &FromTopicToPointcloudAlgNode::topic_name_callback, this);
00062 usleep(50000);
00063
00064
00065
00066 this->getPointCloud2_mutex_.enter();
00067
00068 if (!ack_){
00069 this->getPointCloud2_mutex_.exit();
00070
00071 return false;
00072 }
00073
00074 res.pointcloud = pointcloud_;
00075 ack_ = false;
00076 topic_name_subscriber_.shutdown();
00077
00078
00079
00080 this->getPointCloud2_mutex_.exit();
00081
00082 return true;
00083 }
00084
00085
00086
00087
00088
00089 void FromTopicToPointcloudAlgNode::node_config_update(Config &config, uint32_t level)
00090 {
00091 this->alg_.lock();
00092
00093 this->alg_.unlock();
00094 }
00095
00096 void FromTopicToPointcloudAlgNode::addNodeDiagnostics(void)
00097 {
00098 }
00099
00100
00101 int main(int argc,char *argv[])
00102 {
00103 return algorithm_base::main<FromTopicToPointcloudAlgNode>(argc, argv, "from_topic_to_pointcloud_alg_node");
00104 }