37 #include <visualization_msgs/MarkerArray.h>
46 #include <laser_geometry/laser_geometry.h>
48 #include <pcl/point_types.h>
49 #include <pcl/io/pcd_io.h>
50 #include <pcl/kdtree/kdtree_flann.h>
51 #include <pcl/surface/mls.h>
52 #include <pcl_conversions/pcl_conversions.h>
54 #include <Eigen/Dense>
55 #include <Eigen/Cholesky>
63 #include <boost/thread/thread.hpp>
65 using Eigen::Vector4d;
93 void update(visualization_msgs::Marker& marker);
155 std_msgs::ColorRGBA
makeColor(
double r,
double g,
double b,
double a);
166 geometry_msgs::Point
makePose(
double x,
double y,
double z);
173 void dataHandler(
const sensor_msgs::PointCloud2& msg);
194 void createObjects(vector<PointPtr>&data,vector<MeasurementPtr>&clusters,
double clustering_distance);
std_msgs::ColorRGBA makeColor(int id)
Create a std_msgs color from a hsv color map.
Dynamic markers support class.
vector< visualization_msgs::Marker > markers
Internal storing vector of markers.
vector< visualization_msgs::Marker > createTargetMarkers(vector< TargetPtr > &targets)
Create markers from targets.
double euclideanDistance(Point &p1, Point &p2)
Calculate Euclidean distance between Point class types.
void dataHandler(const sensor_msgs::PointCloud2 &msg)
Handler for the incoming data.
vector< visualization_msgs::Marker > createClustersMarkers(vector< MeasurementPtr > &clusters)
Create markers from clusters.
gvplugin_library_t gvplugin_xgtk_LTX_library
Xgtk external graphviz library/plugin (only plugin, there's no need to create a library since it can ...
void update(visualization_msgs::Marker &marker)
Update a internal marker.
void gvconfig_plugin_install_from_library(GVC_t *, char *, gvplugin_library_t *)
External function used to install a gv plugin from a library.
void createObjects(vector< PointPtr > &data, vector< MeasurementPtr > &clusters, double clustering_distance)
Clustering function.
vector< visualization_msgs::Marker > getOutgoingMarkers(void)
Obtain the list of outgoing markers.
K-best Murtys' algorithm header and auxiliary classes declaration.
void decrement(void)
Mark existing markers for deletion.
GVGraph class declaration.
geometry_msgs::Point makePose(double x, double y, double z)
Create a geometry_msgs::Point from values.
void * graphicalThread(void *data)
Graphical thread.
void clean(void)
Remove markers that should not be transmitted.
Hypotheses cluster definition.
void pointCloud2ToVector(const sensor_msgs::PointCloud2 &cloud, vector< PointPtr > &data)
Convert the point cloud data into a laser scan type structure.