#include <laser_height_estimation.h>
Public Member Functions | |
LaserHeightEstimation (ros::NodeHandle nh, ros::NodeHandle nh_private) | |
virtual | ~LaserHeightEstimation () |
Private Member Functions | |
void | getStats (const std::vector< double > values, double &ave, double &stdev) |
void | histogramSegmentation (const std::vector< double > &input, std::vector< double > &output) |
void | imuCallback (const sensor_msgs::ImuPtr &imu_msg) |
void | kMeansSegmentation (const std::vector< double > &input, std::vector< double > &output) |
bool | kMeansSegmentation (const std::vector< double > &input, std::vector< bool > &mask, double &mean_a, double &mean_b, int &count_a, int &count_b) |
void | scanCallback (const sensor_msgs::LaserScanPtr &scan_msg) |
bool | setBaseToLaserTf (const sensor_msgs::LaserScanPtr &scan_msg) |
Private Attributes | |
std::string | base_frame_ |
tf::Transform | base_to_footprint_ |
tf::Transform | base_to_laser_ |
double | bin_size_ |
bool | first_time_ |
double | floor_height_ |
ros::Publisher | floor_publisher_ |
std::string | footprint_frame_ |
ros::Publisher | height_to_base_publisher_ |
ros::Publisher | height_to_footprint_publisher_ |
ros::Subscriber | imu_subscriber_ |
bool | initialized_ |
ros::Time | last_update_time_ |
sensor_msgs::Imu | latest_imu_msg_ |
double | max_height_jump_ |
double | max_stdev_ |
int | min_values_ |
boost::mutex | mutex_ |
ros::NodeHandle | nh_ |
ros::NodeHandle | nh_private_ |
double | prev_height_ |
ros::Subscriber | scan_subscriber_ |
tf::TransformListener | tf_listener_ |
bool | use_imu_ |
bool | use_segmentation_ |
std::string | world_frame_ |
tf::Transform | world_to_base_ |
Definition at line 53 of file laser_height_estimation.h.
mav::LaserHeightEstimation::LaserHeightEstimation | ( | ros::NodeHandle | nh, |
ros::NodeHandle | nh_private | ||
) |
Definition at line 42 of file laser_height_estimation.cpp.
mav::LaserHeightEstimation::~LaserHeightEstimation | ( | ) | [virtual] |
Definition at line 98 of file laser_height_estimation.cpp.
void mav::LaserHeightEstimation::getStats | ( | const std::vector< double > | values, |
double & | ave, | ||
double & | stdev | ||
) | [private] |
Definition at line 307 of file laser_height_estimation.cpp.
void mav::LaserHeightEstimation::histogramSegmentation | ( | const std::vector< double > & | input, |
std::vector< double > & | output | ||
) | [private] |
Definition at line 323 of file laser_height_estimation.cpp.
void mav::LaserHeightEstimation::imuCallback | ( | const sensor_msgs::ImuPtr & | imu_msg | ) | [private] |
Definition at line 103 of file laser_height_estimation.cpp.
void mav::LaserHeightEstimation::kMeansSegmentation | ( | const std::vector< double > & | input, |
std::vector< double > & | output | ||
) | [private] |
Definition at line 376 of file laser_height_estimation.cpp.
bool mav::LaserHeightEstimation::kMeansSegmentation | ( | const std::vector< double > & | input, |
std::vector< bool > & | mask, | ||
double & | mean_a, | ||
double & | mean_b, | ||
int & | count_a, | ||
int & | count_b | ||
) | [private] |
Definition at line 418 of file laser_height_estimation.cpp.
void mav::LaserHeightEstimation::scanCallback | ( | const sensor_msgs::LaserScanPtr & | scan_msg | ) | [private] |
Definition at line 114 of file laser_height_estimation.cpp.
bool mav::LaserHeightEstimation::setBaseToLaserTf | ( | const sensor_msgs::LaserScanPtr & | scan_msg | ) | [private] |
Definition at line 260 of file laser_height_estimation.cpp.
std::string mav::LaserHeightEstimation::base_frame_ [private] |
Definition at line 93 of file laser_height_estimation.h.
Definition at line 83 of file laser_height_estimation.h.
Definition at line 82 of file laser_height_estimation.h.
double mav::LaserHeightEstimation::bin_size_ [private] |
Definition at line 100 of file laser_height_estimation.h.
bool mav::LaserHeightEstimation::first_time_ [private] |
Definition at line 78 of file laser_height_estimation.h.
double mav::LaserHeightEstimation::floor_height_ [private] |
Definition at line 79 of file laser_height_estimation.h.
Definition at line 68 of file laser_height_estimation.h.
std::string mav::LaserHeightEstimation::footprint_frame_ [private] |
Definition at line 94 of file laser_height_estimation.h.
Definition at line 69 of file laser_height_estimation.h.
Definition at line 70 of file laser_height_estimation.h.
Definition at line 66 of file laser_height_estimation.h.
bool mav::LaserHeightEstimation::initialized_ [private] |
Definition at line 77 of file laser_height_estimation.h.
Definition at line 88 of file laser_height_estimation.h.
sensor_msgs::Imu mav::LaserHeightEstimation::latest_imu_msg_ [private] |
Definition at line 86 of file laser_height_estimation.h.
double mav::LaserHeightEstimation::max_height_jump_ [private] |
Definition at line 97 of file laser_height_estimation.h.
double mav::LaserHeightEstimation::max_stdev_ [private] |
Definition at line 96 of file laser_height_estimation.h.
int mav::LaserHeightEstimation::min_values_ [private] |
Definition at line 95 of file laser_height_estimation.h.
boost::mutex mav::LaserHeightEstimation::mutex_ [private] |
Definition at line 75 of file laser_height_estimation.h.
Definition at line 64 of file laser_height_estimation.h.
Definition at line 65 of file laser_height_estimation.h.
double mav::LaserHeightEstimation::prev_height_ [private] |
Definition at line 80 of file laser_height_estimation.h.
Definition at line 67 of file laser_height_estimation.h.
Definition at line 71 of file laser_height_estimation.h.
bool mav::LaserHeightEstimation::use_imu_ [private] |
Definition at line 98 of file laser_height_estimation.h.
bool mav::LaserHeightEstimation::use_segmentation_ [private] |
Definition at line 99 of file laser_height_estimation.h.
std::string mav::LaserHeightEstimation::world_frame_ [private] |
Definition at line 92 of file laser_height_estimation.h.
Definition at line 84 of file laser_height_estimation.h.