27 #ifndef _plane_model_road_segmentation_CPP_
28 #define _plane_model_road_segmentation_CPP_
32 #include <plane_model_road_segmentation/coefficients.h>
35 #define PFLN printf("FUNCTION=%s LINE=%d\n",__FUNCTION__,__LINE__);
50 static int counter=0; counter++;
51 ROS_INFO(
"Message received number:%d",counter);
54 pcl::PointCloud<pcl::PointXYZRGB> cloud_in;
55 pcl::fromROSMsg(*msg,cloud_in);
59 plane_model_road_segmentation::coefficients coefficients;
60 coefficients.A=pms.
coef->values[0];
61 coefficients.B=pms.
coef->values[1];
62 coefficients.C=pms.
coef->values[2];
63 coefficients.D=pms.
coef->values[3];
68 int main(
int argc,
char **argv)
70 ROS_INFO(
"Starting plane_model_road_segmentation nodelet...");
71 ros::init(argc,argv,
"plane_segmentation_nodelet");
76 tf::TransformListener listener_center_bumper;
77 tf::TransformListener listener_atc_ground;
82 string pointcloud_subscribed;
83 if (!n.hasParam(
"pointcloud_subscribed"))
85 ROS_ERROR(
"Must set properly a input pointcloud (pointcloud_subscribed).");
89 n.getParam(
"pointcloud_subscribed", pointcloud_subscribed);
90 ros::Subscriber sub = n.subscribe(pointcloud_subscribed, 1,
topic_callback2);
93 tf::TransformBroadcaster br;
97 string coefficients_topic_name;
98 if (!n.hasParam(
"coefficients_topic_name"))
100 ROS_ERROR(
"Must set properly a coefficients_topic_name (coefficients_topic_name).");
104 n.getParam(
"coefficients_topic_name", coefficients_topic_name);
105 pub_coeffs=n.advertise<plane_model_road_segmentation::coefficients>(coefficients_topic_name,1);
108 if (!n.hasParam(
"road_frame"))
109 ROS_INFO(
"publishing frame as /environment/road_frame");
113 if (!n.hasParam(
"reference_ground_frame"))
114 ROS_WARN(
"You didn't set properly your ground reference frame. /atc/vehicle/ground is being used.");
tf::TransformListener * listener_center_bumper_ptr
std::string pub_frame_name
tf::TransformBroadcaster * broadcast_ptr
void topic_callback2(const sensor_msgs::PointCloud2ConstPtr &msg)
A class to determine a planar model for road detection.
tf::TransformListener * listener_atc_ground_ptr
std::string reference_ground_frame
ros::Publisher pub_coeffs
Plane model road segmentation global includes.
pcl::ModelCoefficients::Ptr coef
plane_model_road< pcl::PointXYZRGB > pms
tf::Transform find_and_publish_road_frame(pcl::PointCloud< T > cloud_in)
get and publish the road frame T his f*unction does all the work for you. here are the main steps: ST...
int main(int argc, char **argv)