00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #ifndef _plane_model_road_segmentation_CPP_
00028 #define _plane_model_road_segmentation_CPP_
00029
00030 #include <stdio.h>
00031 #include <plane_model_road_segmentation/plane_model_road_segmentation.h>
00032 #include <plane_model_road_segmentation/coefficients.h>
00033 #include "plane_model_road_segmentation_head.h"
00034
00035 #define PFLN printf("FUNCTION=%s LINE=%d\n",__FUNCTION__,__LINE__);
00036
00043
00044 tf::TransformListener *listener_center_bumper_ptr;
00045 plane_model_road<pcl::PointXYZRGB> pms;
00046 ros::Publisher pub_coeffs;
00047
00048 void topic_callback2(const sensor_msgs::PointCloud2ConstPtr& msg)
00049 {
00050 static int counter=0; counter++;
00051 ROS_INFO("Message received number:%d",counter);
00052
00053
00054 pcl::PointCloud<pcl::PointXYZRGB> cloud_in;
00055 pcl::fromROSMsg(*msg,cloud_in);
00056
00057 tf::Transform road_tf=pms.find_and_publish_road_frame(cloud_in);
00058
00059 plane_model_road_segmentation::coefficients coefficients;
00060 coefficients.A=pms.coef->values[0];
00061 coefficients.B=pms.coef->values[1];
00062 coefficients.C=pms.coef->values[2];
00063 coefficients.D=pms.coef->values[3];
00064 pub_coeffs.publish(coefficients);
00065 }
00066
00067
00068 int main(int argc, char **argv)
00069 {
00070 ROS_INFO("Starting plane_model_road_segmentation nodelet...");
00071 ros::init(argc,argv,"plane_segmentation_nodelet");
00072
00073 ros::NodeHandle n;
00074
00075
00076 tf::TransformListener listener_center_bumper;
00077 tf::TransformListener listener_atc_ground;
00078 listener_center_bumper_ptr=&listener_center_bumper;
00079 pms.listener_atc_ground_ptr=&listener_atc_ground;
00080
00081
00082 string pointcloud_subscribed;
00083 if (!n.hasParam("pointcloud_subscribed"))
00084 {
00085 ROS_ERROR("Must set properly a input pointcloud (pointcloud_subscribed).");
00086 return -1;
00087 }
00088 else
00089 n.getParam("pointcloud_subscribed", pointcloud_subscribed);
00090 ros::Subscriber sub = n.subscribe(pointcloud_subscribed, 1, topic_callback2);
00091
00092
00093 tf::TransformBroadcaster br;
00094 pms.broadcast_ptr=&br;
00095
00096
00097 string coefficients_topic_name;
00098 if (!n.hasParam("coefficients_topic_name"))
00099 {
00100 ROS_ERROR("Must set properly a coefficients_topic_name (coefficients_topic_name).");
00101 return -1;
00102 }
00103 else
00104 n.getParam("coefficients_topic_name", coefficients_topic_name);
00105 pub_coeffs=n.advertise<plane_model_road_segmentation::coefficients>(coefficients_topic_name,1);
00106
00107
00108 if (!n.hasParam("road_frame"))
00109 ROS_INFO("publishing frame as /environment/road_frame");
00110 else
00111 n.getParam("road_frame",pms.pub_frame_name);
00112
00113 if (!n.hasParam("reference_ground_frame"))
00114 ROS_WARN("You didn't set properly your ground reference frame. /atc/vehicle/ground is being used.");
00115 else
00116 n.getParam("reference_ground_frame",pms.reference_ground_frame);
00117
00118 ros::spin();
00119
00120 return 1;
00121 }
00122 #endif