Class PlaneFinder

Inheritance Relationships

Base Type

Derived Type

Class Documentation

class PlaneFinder : public robot_calibration::FeatureFinder

Finds the largest plane in a point cloud.

Subclassed by robot_calibration::RobotFinder

Public Functions

PlaneFinder()
virtual ~PlaneFinder() = default
virtual bool init(const std::string &name, std::shared_ptr<tf2_ros::Buffer> buffer, rclcpp::Node::SharedPtr node)

Initialize the feature finder.

Parameters:

  • name – The name of this finder.

  • node – The node to use when loading feature finder configuration data.

Returns:

True/False if the feature finder was able to be initialized

virtual bool find(robot_calibration_msgs::msg::CalibrationData *msg)

Once the robot has been moved into the proper position and settled, this function will be called. It should add any new observations to the msg passed in.

Parameters:

msg – The message to which observations should be added.

Returns:

True if feature finder succeeded in finding the features and adding them to the observation list. False otherwise.

Protected Functions

virtual void cameraCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud)

ROS callback - updates cloud_ and resets waiting_ to false.

virtual void removeInvalidPoints(sensor_msgs::msg::PointCloud2 &cloud, double min_x, double max_x, double min_y, double max_y, double min_z, double max_z)

Remove invalid points from a cloud.

Invalid points include:

  • points with NAN values

  • points with infinite values

  • points with z-distance of 0

  • points outside our min/max x,y,z parameters

Parameters:

cloud – The point cloud to remove invalid points from

virtual sensor_msgs::msg::PointCloud2 extractPlane(sensor_msgs::msg::PointCloud2 &cloud)

Extract a plane from the point cloud.

cloud The cloud to extract plane from - non-plane points will remain

Returns:

New point cloud comprising only the points in the plane

virtual void extractObservation(const std::string &sensor_name, const sensor_msgs::msg::PointCloud2 &cloud, robot_calibration_msgs::msg::CalibrationData *msg, rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher)

Extract points from a cloud into a calibration message.

Parameters:

  • sensor_name – Used to fill in observation “sensor_name”

  • cloud – Point cloud from which to extract observations

  • msg – CalibrationData to fill with observation

  • publisher – Optional pointer to publish the observations as a cloud

virtual bool waitForCloud()

Wait until a new cloud has arrived.

Protected Attributes

rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscriber_
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_
rclcpp::Clock::SharedPtr clock_
bool waiting_
sensor_msgs::msg::PointCloud2 cloud_
DepthCameraInfoManager depth_camera_manager_
std::string plane_sensor_name_
int points_max_
double initial_sampling_distance_
double plane_tolerance_
double min_x_
double max_x_
double min_y_
double max_y_
double min_z_
double max_z_
Eigen::Vector3d desired_normal_
double cos_normal_angle_
std::string transform_frame_
int ransac_iterations_
int ransac_points_
bool output_debug_