#include <soft_obstacle_detection.h>
Public Member Functions | |
SoftObstacleDetection () | |
~SoftObstacleDetection () | |
Private Member Functions | |
bool | checkFrequencyMatching (const std::vector< double > &edges, const std::vector< double > ¢ers, double lambda, double min_segments, double max_segments) const |
bool | checkSegmentsMatching (const std::vector< double > &edges, const std::vector< double > ¢ers, double veil_segment_size, double min_segments, double max_segments) const |
void | dynRecParamCallback (SoftObstacleDetectionConfig &config, uint32_t level) |
void | edgeDetection (const cv::Mat &signal, std::vector< double > &edges, std::vector< double > ¢ers) const |
double | evalModel (const std::vector< double > &edges, const std::vector< double > ¢ers, double lambda) const |
void | getLine (const cv::Mat &img, const cv::Vec4i &line, cv::Mat &out) const |
uchar | getMaxAtLine (const cv::Mat &img, const cv::Point &p1, const cv::Point &p2) const |
void | houghTransform (const cv::Mat &img, std::vector< cv::Vec4i > &lines) const |
void | laserScanCallback (const sensor_msgs::LaserScanConstPtr &scan) |
void | lineToPointCloud (const cv::Vec4i &line, const cv::Point2i &scan_center, pcl::PointCloud< pcl::PointXYZ > &out) const |
void | transformScanToImage (const sensor_msgs::LaserScanConstPtr scan, cv::Mat &img, cv::Point2i &scan_center) const |
void | update (const ros::TimerEvent &event) |
Private Attributes | |
int | border_size |
dynamic_reconfigure::Server < SoftObstacleDetectionConfig > | dyn_rec_server_ |
ros::Subscriber | laser_scan_sub_ |
sensor_msgs::LaserScanConstPtr | last_scan |
double | max_curtain_length_sq_ |
double | max_frequency_ |
double | max_frequency_mse_ |
double | max_segment_dist_var_ |
double | max_segment_size_mse_ |
double | max_segment_size_var_ |
int | max_segments_ |
double | min_frequency_ |
unsigned int | min_hole_size_ |
int | min_segments_ |
std::string | percept_class_id_ |
double | size_dist_ratio_ |
tf::TransformListener | tf_listener |
double | unit_scale |
ros::Timer | update_timer |
ros::Publisher | veil_percept_pose_pub_ |
ros::Publisher | veil_percept_pub_ |
ros::Publisher | veil_point_cloud_pub_ |
double | veil_segment_size_ |
Definition at line 25 of file soft_obstacle_detection.h.
Definition at line 17 of file soft_obstacle_detection.cpp.
Definition at line 38 of file soft_obstacle_detection.cpp.
bool SoftObstacleDetection::checkFrequencyMatching | ( | const std::vector< double > & | edges, |
const std::vector< double > & | centers, | ||
double | lambda, | ||
double | min_segments, | ||
double | max_segments | ||
) | const [private] |
Definition at line 309 of file soft_obstacle_detection.cpp.
bool SoftObstacleDetection::checkSegmentsMatching | ( | const std::vector< double > & | edges, |
const std::vector< double > & | centers, | ||
double | veil_segment_size, | ||
double | min_segments, | ||
double | max_segments | ||
) | const [private] |
Definition at line 217 of file soft_obstacle_detection.cpp.
void SoftObstacleDetection::dynRecParamCallback | ( | SoftObstacleDetectionConfig & | config, |
uint32_t | level | ||
) | [private] |
Definition at line 605 of file soft_obstacle_detection.cpp.
void SoftObstacleDetection::edgeDetection | ( | const cv::Mat & | signal, |
std::vector< double > & | edges, | ||
std::vector< double > & | centers | ||
) | const [private] |
Definition at line 157 of file soft_obstacle_detection.cpp.
double SoftObstacleDetection::evalModel | ( | const std::vector< double > & | edges, |
const std::vector< double > & | centers, | ||
double | lambda | ||
) | const [private] |
Definition at line 277 of file soft_obstacle_detection.cpp.
void SoftObstacleDetection::getLine | ( | const cv::Mat & | img, |
const cv::Vec4i & | line, | ||
cv::Mat & | out | ||
) | const [private] |
Definition at line 132 of file soft_obstacle_detection.cpp.
uchar SoftObstacleDetection::getMaxAtLine | ( | const cv::Mat & | img, |
const cv::Point & | p1, | ||
const cv::Point & | p2 | ||
) | const [private] |
Definition at line 116 of file soft_obstacle_detection.cpp.
void SoftObstacleDetection::houghTransform | ( | const cv::Mat & | img, |
std::vector< cv::Vec4i > & | lines | ||
) | const [private] |
Definition at line 95 of file soft_obstacle_detection.cpp.
void SoftObstacleDetection::laserScanCallback | ( | const sensor_msgs::LaserScanConstPtr & | scan | ) | [private] |
Definition at line 600 of file soft_obstacle_detection.cpp.
void SoftObstacleDetection::lineToPointCloud | ( | const cv::Vec4i & | line, |
const cv::Point2i & | scan_center, | ||
pcl::PointCloud< pcl::PointXYZ > & | out | ||
) | const [private] |
Definition at line 430 of file soft_obstacle_detection.cpp.
void SoftObstacleDetection::transformScanToImage | ( | const sensor_msgs::LaserScanConstPtr | scan, |
cv::Mat & | img, | ||
cv::Point2i & | scan_center | ||
) | const [private] |
Definition at line 42 of file soft_obstacle_detection.cpp.
void SoftObstacleDetection::update | ( | const ros::TimerEvent & | event | ) | [private] |
Definition at line 476 of file soft_obstacle_detection.cpp.
int SoftObstacleDetection::border_size [private] |
Definition at line 56 of file soft_obstacle_detection.h.
dynamic_reconfigure::Server<SoftObstacleDetectionConfig> SoftObstacleDetection::dyn_rec_server_ [private] |
Definition at line 68 of file soft_obstacle_detection.h.
Definition at line 61 of file soft_obstacle_detection.h.
sensor_msgs::LaserScanConstPtr SoftObstacleDetection::last_scan [private] |
Definition at line 53 of file soft_obstacle_detection.h.
double SoftObstacleDetection::max_curtain_length_sq_ [private] |
Definition at line 72 of file soft_obstacle_detection.h.
double SoftObstacleDetection::max_frequency_ [private] |
Definition at line 74 of file soft_obstacle_detection.h.
double SoftObstacleDetection::max_frequency_mse_ [private] |
Definition at line 82 of file soft_obstacle_detection.h.
double SoftObstacleDetection::max_segment_dist_var_ [private] |
Definition at line 80 of file soft_obstacle_detection.h.
double SoftObstacleDetection::max_segment_size_mse_ [private] |
Definition at line 78 of file soft_obstacle_detection.h.
double SoftObstacleDetection::max_segment_size_var_ [private] |
Definition at line 79 of file soft_obstacle_detection.h.
int SoftObstacleDetection::max_segments_ [private] |
Definition at line 77 of file soft_obstacle_detection.h.
double SoftObstacleDetection::min_frequency_ [private] |
Definition at line 73 of file soft_obstacle_detection.h.
unsigned int SoftObstacleDetection::min_hole_size_ [private] |
Definition at line 71 of file soft_obstacle_detection.h.
int SoftObstacleDetection::min_segments_ [private] |
Definition at line 76 of file soft_obstacle_detection.h.
std::string SoftObstacleDetection::percept_class_id_ [private] |
Definition at line 83 of file soft_obstacle_detection.h.
double SoftObstacleDetection::size_dist_ratio_ [private] |
Definition at line 81 of file soft_obstacle_detection.h.
Definition at line 58 of file soft_obstacle_detection.h.
double SoftObstacleDetection::unit_scale [private] |
Definition at line 55 of file soft_obstacle_detection.h.
Definition at line 51 of file soft_obstacle_detection.h.
Definition at line 65 of file soft_obstacle_detection.h.
Definition at line 64 of file soft_obstacle_detection.h.
Definition at line 66 of file soft_obstacle_detection.h.
double SoftObstacleDetection::veil_segment_size_ [private] |
Definition at line 75 of file soft_obstacle_detection.h.