27 #ifndef _plane_model_segmentation_H_
28 #define _plane_model_segmentation_H_
31 #include <sensor_msgs/PointCloud2.h>
34 #include <tf/transform_broadcaster.h>
35 #include <tf/transform_listener.h>
38 #include <pcl/point_cloud.h>
39 #include <pcl/point_types.h>
40 #include <pcl/conversions.h>
41 #include <pcl/filters/voxel_grid.h>
42 #include <pcl_ros/transforms.h>
43 #include <pcl/segmentation/extract_polygonal_prism_data.h>
44 #include <pcl/filters/extract_indices.h>
45 #include <pcl/segmentation/sac_segmentation.h>
46 #include <pcl/sample_consensus/method_types.h>
47 #include <pcl/sample_consensus/model_types.h>
48 #include <pcl/filters/project_inliers.h>
49 #include <pcl_conversions/pcl_conversions.h>
87 bool find_road_frame(pcl::PointCloud<T> cloud_in);
95 bool find_road_frame(
const sensor_msgs::PointCloud2ConstPtr& msg);
108 tf::Transform find_and_publish_road_frame(pcl::PointCloud<T> cloud_in);
116 tf::Transform find_and_publish_road_frame(
const sensor_msgs::PointCloud2ConstPtr& msg);
132 pcl::PointCloud<T> get_plane_coefficients(pcl::PointCloud<T> cloud_in,
int iterations,
float threshold, pcl::ModelCoefficients::Ptr coefficients);
145 pcl::ModelCoefficients::Ptr
coef;
154 point_origin.x=2; point_origin.y=0; point_origin.z=0;
155 point_x.x=4; point_x.y=0; point_x.z=0;
156 point_y.x=4; point_y.y=0; point_y.z=0;
157 pub_frame_name=
"/environment/road_frame";
158 reference_ground_frame=
"/atc/vehicle/ground";
169 pcl::PointCloud<T> outliers;
182 T project_point_to_plane(
const T ptin,
const pcl::ModelCoefficients::Ptr coeff);
192 T transform_pcl_point(std::string frame1, std::string frame2, T pt_origin);
203 if (distance<=threshold)
206 cout <<
"PMS_DEBUG: ROAD FRAME FOUND" << endl;
222 vector<double> find_RP_angle(T origin_projected, T pt_x_projected, T pt_y_projected);
232 tf::Transform get_transform(T origin_projected, T pt_x_projected, T pt_y_projected);
std::string pub_frame_name
tf::TransformBroadcaster * broadcast_ptr
tf::TransformListener * listener_atc_ground_ptr
std::string reference_ground_frame
Function implementation for class plane_model_road< T >
pcl::ModelCoefficients::Ptr coef
bool check_distance(double distance, double threshold)
check if distance matches with threshold If returned true, you probably get the road plane ...