Go to the documentation of this file.00001 #include "zyonz_geometric_based_single_leaf_probing_alg_node.h"
00002 #include <tf/transform_broadcaster.h>
00003
00004 ZyonzGeometricBasedSingleLeafProbingAlgNode::ZyonzGeometricBasedSingleLeafProbingAlgNode(void) :
00005 algorithm_base::IriBaseAlgorithm<ZyonzGeometricBasedSingleLeafProbingAlgorithm>()
00006 {
00007
00008
00009
00010
00011 output_marker_publisher_ = public_node_handle_.advertise<visualization_msgs::Marker>("output_marker", 1);
00012 pose_publisher_ = public_node_handle_.advertise<geometry_msgs::PoseStamped>("probing_pose", 1);
00013
00014
00015
00016 get_leaf_probing_server_ = public_node_handle_.advertiseService("leaf_probing_srv", &ZyonzGeometricBasedSingleLeafProbingAlgNode::get_leaf_probingCallback, this);
00017
00018
00019
00020
00021
00022
00023 }
00024
00025 ZyonzGeometricBasedSingleLeafProbingAlgNode::~ZyonzGeometricBasedSingleLeafProbingAlgNode(void)
00026 {
00027
00028 }
00029
00030 void ZyonzGeometricBasedSingleLeafProbingAlgNode::mainNodeThread(void)
00031 {
00032
00033
00034
00035
00036
00037
00038
00039 }
00040
00041
00042
00043
00044 bool ZyonzGeometricBasedSingleLeafProbingAlgNode::get_leaf_probingCallback(zyonz_msgs::GetProbingSteps::Request &req, zyonz_msgs::GetProbingSteps::Response &res)
00045 {
00046 ROS_INFO("ZyonzGeometricBasedSingleLeafProbingAlgNode::get_leaf_probingCallback: New Request Received!");
00047
00048
00049 this->alg_.lock();
00050
00051
00052 sensor_msgs::PointCloud2::ConstPtr point_cloud_ConstPtr;
00053 point_cloud_ConstPtr = boost::make_shared<sensor_msgs::PointCloud2>(req.point_cloud);
00054
00055 if(!alg_.get_leaf_probing_point(point_cloud_ConstPtr))
00056 {
00057 ROS_INFO("ZyonzGeometricBasedSingleLeafProbingAlgNode::get_leaf_probingCallback: ERROR: alg is not on run mode yet.");
00058 }
00059
00060 res.steps.clear();
00061 res.steps.push_back(alg_.getProbingSteps());
00062
00063
00064 output_marker_publisher_.publish(alg_.getMarker());
00065 pose_publisher_.publish(alg_.getProbingSteps().probing);
00066
00067 static tf::TransformBroadcaster br;
00068 tf::Transform transform;
00069 transform.setOrigin( tf::Vector3(alg_.getLeafCentroid().x(), alg_.getLeafCentroid().y(), alg_.getLeafCentroid().z()) );
00070 transform.setRotation( tf::Quaternion(alg_.getLeafPose().x(), alg_.getLeafPose().y(), alg_.getLeafPose().z(),alg_.getLeafPose().w()) );
00071 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/camboard_frame", "centroid"));
00072
00073
00074 this->alg_.unlock();
00075
00076
00077 return true;
00078 }
00079
00080
00081
00082
00083
00084 void ZyonzGeometricBasedSingleLeafProbingAlgNode::node_config_update(Config &config, uint32_t level)
00085 {
00086 this->alg_.lock();
00087
00088 this->alg_.unlock();
00089 }
00090
00091 void ZyonzGeometricBasedSingleLeafProbingAlgNode::addNodeDiagnostics(void)
00092 {
00093 }
00094
00095
00096 int main(int argc,char *argv[])
00097 {
00098 return algorithm_base::main<ZyonzGeometricBasedSingleLeafProbingAlgNode>(argc, argv, "zyonz_geometric_based_single_leaf_probing_alg_node");
00099 }