Go to the documentation of this file.00001 #include "filter_jump_edge_alg_node.h"
00002
00003 FilterJumpEdgeAlgNode::FilterJumpEdgeAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<FilterJumpEdgeAlgorithm>(),
00005 ang_thr_(9.f),
00006 sx_(182.40f),
00007 sy_(182.91f),
00008 u0_(100.58f),
00009 v0_(94.79f),
00010 skw_(0.0f)
00011 {
00012
00013
00014
00015
00016
00017
00018 residual_pc_publisher_ = public_node_handle_.advertise<sensor_msgs::PointCloud2>("residual_pc", 1);
00019 filtered_pc_publisher_ = public_node_handle_.advertise<sensor_msgs::PointCloud2>("filtered_pc", 1);
00020 residual_img_publisher_ = public_node_handle_.advertise<sensor_msgs::Image>("image/output", 1);
00021
00022
00023 camera_info_subscriber_ = public_node_handle_.subscribe("camera_info", 1, &FilterJumpEdgeAlgNode::camera_info_callback, this);
00024 pcl_input_subscriber_ = public_node_handle_.subscribe("input", 1, &FilterJumpEdgeAlgNode::pcl_input_callback, this);
00025
00026
00027 apply_jump_edge_filter_server_ = public_node_handle_.advertiseService("apply_jump_edge_filter", &FilterJumpEdgeAlgNode::apply_jump_edge_filterCallback, this);
00028
00029
00030
00031
00032
00033
00034
00035 ROS_INFO("starting jump_edge_filter_node with apex_thr = %f", ang_thr_);
00036
00037 }
00038
00039 FilterJumpEdgeAlgNode::~FilterJumpEdgeAlgNode(void)
00040 {
00041
00042 }
00043
00044 void FilterJumpEdgeAlgNode::mainNodeThread(void)
00045 {
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059 }
00060
00061
00062 void FilterJumpEdgeAlgNode::camera_info_callback(const sensor_msgs::CameraInfo::ConstPtr& msg)
00063 {
00064
00065
00066
00067 alg_.lock();
00068 camera_info_mutex_.enter();
00069
00070 sx_ = msg->K[0];
00071 sy_ = msg->K[4];
00072 u0_ = msg->K[2];
00073 v0_ = msg->K[5];
00074 skw_ = msg->K[1];
00075
00076 alg_.set_sx(msg->K[0]);
00077 alg_.set_sy(msg->K[4]);
00078 alg_.set_u0(msg->K[2]);
00079 alg_.set_v0(msg->K[5]);
00080 alg_.set_skw(msg->K[1]);
00081
00082
00083 alg_.unlock();
00084 camera_info_mutex_.exit();
00085 }
00086
00087 void FilterJumpEdgeAlgNode::pcl_input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00088 {
00089
00090
00091
00092
00093 alg_.lock();
00094 pcl_input_mutex_.enter();
00095
00096 if(!alg_.apply_jump_edge_filter(msg))
00097 {
00098 ROS_INFO("FilterJumpEdgeAlgNode::pcl_input_callback: ERROR: alg is not on run mode yet.");
00099 }
00100
00101 filtered_pc_publisher_.publish(*alg_.get_filtered_pc());
00102 residual_pc_publisher_.publish(*alg_.get_residual_pc());
00103 residual_img_publisher_.publish(*alg_.get_residual_img());
00104
00105
00106 alg_.unlock();
00107 pcl_input_mutex_.exit();
00108
00109
00110
00111 }
00112
00113
00114 bool FilterJumpEdgeAlgNode::apply_jump_edge_filterCallback(iri_filter_jump_edge::ApplyPointCloudFilter::Request &req, iri_filter_jump_edge::ApplyPointCloudFilter::Response &res)
00115 {
00116 ROS_INFO("FilterJumpEdgeAlgNode::apply_jump_edge_filterCallback: New Request Received!");
00117
00118
00119 alg_.lock();
00120 apply_jump_edge_filter_mutex_.enter();
00121
00122 sensor_msgs::PointCloud2::ConstPtr point_cloud_ConstPtr;
00123 point_cloud_ConstPtr = boost::make_shared<sensor_msgs::PointCloud2>(req.input_pc);
00124
00125 if(!alg_.apply_jump_edge_filter(point_cloud_ConstPtr))
00126 {
00127 ROS_INFO("FilterJumpEdgeAlgNode::apply_jump_edge_filterCallback: ERROR: alg is not on run mode yet.");
00128 }
00129
00130 res.filtered_pc = *alg_.get_filtered_pc();
00131 res.residual_pc = *alg_.get_residual_pc();
00132 res.residual_img = *alg_.get_residual_img();
00133
00134 filtered_pc_publisher_.publish(*alg_.get_filtered_pc());
00135 residual_pc_publisher_.publish(*alg_.get_residual_pc());
00136 residual_img_publisher_.publish(*alg_.get_residual_img());
00137
00138
00139 alg_.unlock();
00140 apply_jump_edge_filter_mutex_.exit();
00141
00142 return true;
00143 }
00144
00145
00146
00147
00148
00149 void FilterJumpEdgeAlgNode::node_config_update(Config &config, uint32_t level)
00150 {
00151 alg_.lock();
00152 ang_thr_ = config.ang_thr;
00153 alg_.set_ang_thr(config.ang_thr);
00154 alg_.unlock();
00155 }
00156
00157 void FilterJumpEdgeAlgNode::addNodeDiagnostics(void)
00158 {
00159 }
00160
00161
00162 int main(int argc,char *argv[])
00163 {
00164 return algorithm_base::main<FilterJumpEdgeAlgNode>(argc, argv, "filter_jump_edge_alg_node");
00165 }