#include <perception.h>
Public Member Functions | |
DockPerception (ros::NodeHandle &nh) | |
bool | getPose (geometry_msgs::PoseStamped &pose, std::string frame="") |
Get the pose of the dock. More... | |
bool | start (const geometry_msgs::PoseStamped &pose) |
Start dock detection. More... | |
bool | stop () |
Stop tracking the dock. More... | |
Private Member Functions | |
void | callback (const sensor_msgs::LaserScanConstPtr &scan) |
Callback to process laser scans. More... | |
DockCandidatePtr | extract (laser_processor::SampleSet *cluster) |
Extract a DockCandidate from a cluster, filling in the lengths and slopes of each line found using ransac. More... | |
double | fit (const DockCandidatePtr &candidate, geometry_msgs::Pose &pose) |
Try to fit a dock to candidate. More... | |
Static Private Member Functions | |
static bool | isValid (const tf::Quaternion &q) |
Method to check if the quaternion is valid. More... | |
Private Attributes | |
bool | debug_ |
ros::Publisher | debug_points_ |
geometry_msgs::PoseStamped | dock_ |
boost::mutex | dock_mutex_ |
LinearPoseFilter2DPtr | dock_pose_filter_ |
ros::Time | dock_stamp_ |
bool | found_dock_ |
std::vector< geometry_msgs::Point > | front_cloud_ |
std::vector< geometry_msgs::Point > | ideal_cloud_ |
tf::TransformListener | listener_ |
double | max_alignment_error_ |
bool | running_ |
ros::Subscriber | scan_sub_ |
std::string | tracking_frame_ |
Low pass filter object for filtering dock poses. More... | |
Definition at line 38 of file perception.h.
|
explicit |
Definition at line 40 of file perception.cpp.
|
private |
Callback to process laser scans.
Definition at line 174 of file perception.cpp.
|
private |
Extract a DockCandidate from a cluster, filling in the lengths and slopes of each line found using ransac.
cluster | The pointer to the cluster to extract from. |
Definition at line 369 of file perception.cpp.
|
private |
Try to fit a dock to candidate.
candidate | The candidate to fit to. |
pose | The fitted pose, if successful. |
Definition at line 417 of file perception.cpp.
bool DockPerception::getPose | ( | geometry_msgs::PoseStamped & | pose, |
std::string | frame = "" |
||
) |
Get the pose of the dock.
Definition at line 120 of file perception.cpp.
|
staticprivate |
Method to check if the quaternion is valid.
q | Quaternion to check. |
Definition at line 553 of file perception.cpp.
bool DockPerception::start | ( | const geometry_msgs::PoseStamped & | pose | ) |
Start dock detection.
pose | The initial estimate of dock pose |
Definition at line 105 of file perception.cpp.
bool DockPerception::stop | ( | ) |
Stop tracking the dock.
Definition at line 114 of file perception.cpp.
|
private |
Definition at line 85 of file perception.h.
|
private |
Definition at line 103 of file perception.h.
|
private |
Definition at line 92 of file perception.h.
|
private |
Definition at line 94 of file perception.h.
|
private |
Definition at line 87 of file perception.h.
|
private |
Definition at line 98 of file perception.h.
|
private |
Definition at line 96 of file perception.h.
|
private |
Definition at line 108 of file perception.h.
|
private |
Definition at line 106 of file perception.h.
|
private |
Definition at line 82 of file perception.h.
|
private |
Definition at line 100 of file perception.h.
|
private |
Definition at line 84 of file perception.h.
|
private |
Definition at line 81 of file perception.h.
|
private |
Low pass filter object for filtering dock poses.
Definition at line 90 of file perception.h.