21 #include "pitch_calibration.h"
23 #include <config/netconf.h>
56 printf(
"Starting pitch angle calibration.\n");
63 printf(
"Rear cloud has %zu points.\n", rear_cloud->points.size());
71 printf(
"Mean z is %f.\n", mean_z);
74 printf(
"Updating pitch from %f to %f.\n", old_pitch, new_pitch);
78 printf(
"Pitch calibration finished.\n");
Exception that is thrown if there are not enough laser points to do a matching.
Abstract base class for laser calibration.
LaserInterface * laser_
The laser that provides the input data.
float get_mean_z(PointCloudPtr cloud)
Compute the mean z value of all points in the given pointcloud.
const std::string config_path_
The config path to use for reading and updating config values.
void transform_pointcloud(const std::string &target_frame, PointCloudPtr cloud)
Transform the points in a pointcloud.
PointCloudPtr laser_to_pointcloud(const LaserInterface &laser)
Convert the laser data into a pointcloud.
PointCloudPtr filter_cloud_in_rear(PointCloudPtr input)
Remove points in the rear of the robot.
fawkes::NetworkConfiguration * config_
The network config to use for reading and updating config values.
static const long sleep_time_
Time in micro seconds to sleep between iterations.
constexpr static float threshold
The threshold of the mean of z to stop calibration.
PitchCalibration(LaserInterface *laser, fawkes::tf::Transformer *tf_transformer, fawkes::NetworkConfiguration *config, std::string config_path)
Constructor.
virtual void calibrate()
The actual calibration.
float get_new_pitch(float z, float old_pitch)
Compute the new pitch based on the old pitch and the mean z.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
void read()
Read from BlackBoard into local copy.
Laser360Interface Fawkes BlackBoard Interface.
Remote configuration via Fawkes net.
virtual void set_float(const char *path, float f)
Set new value in configuration of type float.
virtual float get_float(const char *path)
Get value from configuration which is of type float.
Fawkes library namespace.