Go to the documentation of this file.00001 #include "apply_tof_calibration_alg_node.h"
00002
00003 ApplyTofCalibrationAlgNode::ApplyTofCalibrationAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<ApplyTofCalibrationAlgorithm>()
00005 {
00006
00007
00008
00009
00010 calibrated_pcl2_publisher_ = public_node_handle_.advertise<sensor_msgs::PointCloud2>("output", 1);
00011
00012
00013 uncalibrated_pcl2_subscriber_ = public_node_handle_.subscribe("input", 1, &ApplyTofCalibrationAlgNode::uncalibrated_pcl2_callback, this);
00014
00015
00016 get_pointcloud_server_ = public_node_handle_.advertiseService("get_pointcloud", &ApplyTofCalibrationAlgNode::get_pointcloudCallback, this);
00017
00018
00019
00020
00021
00022
00023
00024 }
00025
00026 ApplyTofCalibrationAlgNode::~ApplyTofCalibrationAlgNode(void)
00027 {
00028
00029
00030 }
00031
00032 void ApplyTofCalibrationAlgNode::mainNodeThread(void)
00033 {
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043 }
00044
00045
00046 void ApplyTofCalibrationAlgNode::uncalibrated_pcl2_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00047 {
00048
00049
00050
00051 alg_.lock();
00052 uncalibrated_pcl2_mutex_.enter();
00053
00054 if (!alg_.apply_tof_calibration_to_pcl2(msg))
00055 {
00056 ROS_INFO("ApplyTofCalibrationAlgNode::uncalibrated_pcl2_callback: ERROR: alg is not on run mode yet.");
00057 }
00058
00059 calibrated_pcl2_publisher_.publish(*alg_.get_calibrated_pcl());
00060
00061
00062
00063
00064 alg_.unlock();
00065 uncalibrated_pcl2_mutex_.exit();
00066
00067 }
00068
00069
00070 bool ApplyTofCalibrationAlgNode::get_pointcloudCallback(iri_perception_msgs::CapturePointCloud2::Request &req, iri_perception_msgs::CapturePointCloud2::Response &res)
00071 {
00072 ROS_INFO("ApplyTofCalibrationAlgNode::get_pointcloudCallback: New Request Received!");
00073
00074
00075 this->alg_.lock();
00076 this->get_pointcloud_mutex_.enter();
00077
00078
00079
00080
00081
00082
00083 res.pointcloud = *alg_.get_calibrated_pcl();
00084
00085
00086 this->alg_.unlock();
00087 this->get_pointcloud_mutex_.exit();
00088
00089 return true;
00090 }
00091
00092
00093
00094
00095
00096 void ApplyTofCalibrationAlgNode::node_config_update(Config &config, uint32_t level)
00097 {
00098 this->alg_.lock();
00099
00100 this->alg_.unlock();
00101 }
00102
00103 void ApplyTofCalibrationAlgNode::addNodeDiagnostics(void)
00104 {
00105 }
00106
00107
00108 int main(int argc,char *argv[])
00109 {
00110 return algorithm_base::main<ApplyTofCalibrationAlgNode>(argc, argv, "apply_tof_calibration_alg_node");
00111 }