Fawkes API
Fawkes Development Version
|
21 #include "roll_calibration.h"
23 #include <config/netconf.h>
24 #include <pcl/common/geometry.h>
25 #include <pcl/filters/filter.h>
26 #include <pcl/filters/passthrough.h>
27 #include <pcl/registration/icp.h>
28 #include <tf/transform_listener.h>
29 #include <tf/transformer.h>
62 printf(
"Starting to calibrate roll angle.\n");
73 printf(
"Left-right difference is %f.\n", lrd);
76 printf(
"Updating roll from %f to %f.\n", old_roll, new_roll);
80 printf(
"Roll calibration finished.\n");
98 error <<
"Not enough laser points on the left, got " << left_cloud->size() <<
", need "
104 error <<
"Not enough laser points on the right, got " << right_cloud->size() <<
", need "
108 printf(
"Using %zu points on the left, %zu points on the right\n",
110 right_cloud->size());
122 return old_roll + 0.5 * mean_error;
133 std::vector<int> indices;
134 input->is_dense =
false;
135 pcl::removeNaNFromPointCloud(*input, *filtered, indices);
virtual void calibrate()
The actual calibration.
const static uint max_iterations_
The number of iterations to run before aborting the calibration.
PointCloudPtr laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
PointCloudPtr filter_left_cloud(PointCloudPtr input)
Remove all points that are left of the robot.
PointCloudPtr filter_calibration_cloud(PointCloudPtr input)
Filter the input cloud to be useful for roll calibration.
LaserInterface * laser_
The laser that provides the input data.
PointCloudPtr filter_right_cloud(PointCloudPtr input)
Remove all points that are right of the robot.
void read()
Read from BlackBoard into local copy.
const std::string config_path_
The config path to use for reading and updating config values.
PointCloudPtr filter_cloud_in_rear(PointCloudPtr input)
Remove points in the rear of the robot.
void transform_pointcloud(const std::string &target_frame, PointCloudPtr cloud)
Transform the points in a pointcloud.
float get_lr_mean_diff()
Get the difference of the mean of z of the left and right pointclouds.
fawkes::NetworkConfiguration * config_
The network config to use for reading and updating config values.
float get_mean_z(PointCloudPtr cloud)
Compute the mean z value of all points in the given pointcloud.
constexpr static float threshold
The threshold of the left-right difference to stop calibration.
const static long sleep_time_
Time in micro seconds to sleep between iterations.
virtual void set_float(const char *path, float f)
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
RollCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
const static size_t min_points
The number of points required in a pointcloud to use it as input data.
float get_new_roll(float mean_error, float old_roll)
Compute a new roll angle based on the mean error and the old roll.
Exception that is thrown if there are not enough laser points to do a matching.
virtual float get_float(const char *path)