23 #include "projection.h" 25 #include <core/exception.h> 26 #include <sys/types.h> 27 #include <utils/math/angle.h> 59 std::string target_frame,
66 unsigned int in_data_size,
67 std::vector<LaserDataFilter::Buffer *> &in)
70 target_frame_(target_frame),
71 not_from_x_(not_from_x),
73 not_from_y_(not_from_y),
75 only_from_z_(only_from_z),
79 for (
unsigned int i = 0; i < 360; ++i) {
80 sin_angles360[i] = sinf(
deg2rad(i));
81 cos_angles360[i] = cosf(
deg2rad(i));
83 for (
unsigned int i = 0; i < 720; ++i) {
84 sin_angles720[i] = sinf(
deg2rad((
float)i / 2.));
85 cos_angles720[i] = cosf(
deg2rad((
float)i / 2.));
91 LaserProjectionDataFilter::~LaserProjectionDataFilter()
103 LaserProjectionDataFilter::set_output(
float *outbuf, fawkes::tf::Point &p)
105 if (((p.x() >= not_from_x_) && (p.x() <= not_to_x_) && (p.y() >= not_from_y_)
106 && (p.y() <= not_to_y_))
107 || (p.z() < only_from_z_) || (p.z() > only_to_z_)) {
114 float phi = atan2f(p.y(), p.x());
120 if (outbuf[j] == 0.) {
121 outbuf[j] = (float)p.length();
123 outbuf[j] = std::min(outbuf[j], (
float)p.length());
130 const unsigned int vecsize = std::min(
in.size(),
out.size());
131 for (
unsigned int a = 0; a < vecsize; ++a) {
132 out[a]->frame = target_frame_;
133 out[a]->timestamp->set_time(
in[a]->timestamp);
134 float *inbuf =
in[a]->values;
135 float *outbuf =
out[a]->values;
144 for (
unsigned int i = 0; i < 360; ++i) {
147 p.setValue((btScalar)inbuf[i] * cos_angles360[i],
148 (btScalar)inbuf[i] * sin_angles360[i],
152 set_output(outbuf, p);
155 for (
unsigned int i = 0; i < 720; ++i) {
159 p.setValue((btScalar)inbuf[i] * cos_angles720[i],
160 (btScalar)inbuf[i] * sin_angles720[i],
164 set_output(outbuf, p);
172 p.setValue((btScalar)inbuf[i] * cos(a), (btScalar)inbuf[i] * sin(a), 0.);
175 set_output(outbuf, p);
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
std::vector< Buffer * > out
Vector of output arrays.
Fawkes library namespace.
A class for handling time.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
LaserProjectionDataFilter(const std::string &filter_name, fawkes::tf::Transformer *tf, std::string target_frame, float not_from_x, float not_to_x, float not_from_y, float not_to_y, float only_from_z, float only_to_z, unsigned int in_data_size, std::vector< LaserDataFilter::Buffer * > &in)
Constructor.
unsigned int out_data_size
Number of entries in output arrays.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
void filter()
Filter the incoming data.
std::vector< Buffer * > in
Vector of input arrays.
unsigned int in_data_size
Number of entries in input arrays.