107 }
else if (s ==
"sphere") {
109 }
else if (s ==
"block") {
111 }
else if (s ==
"cylinder") {
113 }
else if (s ==
"link") {
115 }
else if (s ==
"cone") {
131 for (
int i = 0; ; i++) {
151 for (
int j = 0; ; j++) {
177 case 3: q.setX(d);
break;
178 case 4: q.setY(d);
break;
179 case 5: q.setZ(d);
break;
180 case 6: q.setW(d);
break;
196 std::stringstream result;
214 result <<
"cylinder";
245 for (
int i = 0; i < param_count; i++) {
250 for (
int i = 0; i < tf_count; i++) {
252 result <<
' ' <<
frame_[i];
271 _x = _y = _z = _q =
true;
324 if (pos >= str.size()) {
329 if ((c !=
' ') && (c !=
'\t')) {
335 if (pos >= str.size()) {
340 if ((c !=
' ') && (c !=
'\t')) {
350 const char symbol)
const {
352 if (pos >= str.size()) {
356 if (str[pos] == symbol) {
371 if (pos >= str.size()) {
376 if ((c ==
' ') || (c ==
'\t') || (c ==
'#') || (c ==
':') ||
386 return str.substr(start, pos - start);
394 int pos = (int)str.find(
'#');
399 return str.substr(pos + 1);
405 std::stringstream ss;
413 double &value)
const {
415 std::stringstream ss;
420 }
catch (std::runtime_error &e) {
433 tf_lookup_time_ = 0.2;
444 filters_.push_back(f);
459 std::vector<std::string> result;
460 for (
int i = 0; i < filters_.size(); i++) {
461 result.push_back(filters_[i].
toString());
469 const std::string base_frame,
const ros::Time time,
470 std::vector<cPcdFilterPaFilter> &result)
const {
475 std::vector<std::pair<std::string, tf::Transform > > frames;
479 for (
int i_filter = 0; i_filter < filters_.size(); i_filter++) {
483 switch (filters_[i_filter].
type_) {
503 bool skip_filter =
false;
505 for (
int i_tf = 0; i_tf < tf_count; i_tf++) {
506 std::string frame_temp = filters_[i_filter].frame_[i_tf];
510 for (; i_frames < frames.size(); i_frames++) {
511 if (frames[i_frames].first == frame_temp) {
512 tf_temp[i_tf] = frames[i_frames].second;
514 if (tf_temp[i_tf].getBasis()[0][0] < -2) {
522 if (i_frames >= frames.size()) {
524 std::string error_msg;
526 bool check_transform =
true;
529 if (tf_lookup_time_ > 0) {
534 base_frame, time, max_duration,
536 check_transform =
false;
544 if ((! skip_filter) && check_transform) {
546 base_frame, time, &error_msg)) {
558 error_msg = ex.what();
568 tf_temp[i_tf].
getBasis()[0][0] = -10;
569 frames.push_back(std::pair<std::string, tf::Transform>
570 (frame_temp, tf_temp[i_tf]));
573 tf_temp[i_tf] = stf_temp;
574 frames.push_back(std::pair<std::string, tf::Transform>
575 (frame_temp, tf_temp[i_tf]));
586 tf_temp[i_tf] = filters_[i_filter].offset_[i_tf] * tf_temp[i_tf];
593 result.push_back(filter_temp);
597 switch (filters_[i_filter].type_) {
601 filters_[i_filter].parameter_[0] / 2;
605 filter_temp.
parameter_[0] = filters_[i_filter].parameter_[0] *
606 filters_[i_filter].parameter_[0];
610 for (
int i = 0; i < 3; i++) {
612 filters_[i_filter].parameter_[i] / 2;
617 filter_temp.
parameter_[0] = filters_[i_filter].parameter_[0] *
618 filters_[i_filter].parameter_[0];
620 filters_[i_filter].parameter_[1] / 2;
626 double d = update_transform(tf_temp[0], tf_temp[1],
634 filters_[i_filter].parameter_[0] *
635 filters_[i_filter].parameter_[0];
637 filters_[i_filter].parameter_[1];
644 if (update_transform(tf_temp[0], tf_temp[1], tf_temp[0], 0.5) < 0) {
649 filter_temp.
parameter_[0] = filters_[i_filter].parameter_[0];
650 filter_temp.
parameter_[1] = filters_[i_filter].parameter_[1] * filters_[i_filter].parameter_[1];
654 filter_temp.
inverse_ = filters_[i_filter].inverse_;
659 for (
int y = 0;
y < 3;
y++) {
660 for (
int x = 0;
x < 3;
x++) {
665 result.push_back(filter_temp);
674 return last_filter_.toString();
679 return filters_.size();
715 result = tf_shift * start;
std::string toString(void) const
returning string equivalent of filter
cPcdFilterPaRosFilter & operator[](int i)
void reset(void)
reseting this filter
std::string _floatToStr(const double &value) const
helper function for converting a double
std::string getLast(void) const
bool inverse_
inverted filter
TFSIMD_FORCE_INLINE void setX(tfScalar x)
TFSIMD_FORCE_INLINE bool fuzzyZero() const
eFiltertype type_
filter type
TFSIMD_FORCE_INLINE void setY(tfScalar y)
std::string frame_[COUNT_FRAME]
frame id(s)
bool fromString(const std::string &filter)
setting filter from string
cPcdFilterPaRosFilter & operator=(const cPcdFilterPaRosFilter &other)
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
bool isValid(void) const
returning if filter is valid
std::string _getValue(const std::string &str, int &pos) const
helper function for returning the next value
TFSIMD_FORCE_INLINE const tfScalar & y() const
bool _StrToFloat(const std::string &str, double &value) const
helper function for converting a double
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
bool inverse_
inverted filter
TFSIMD_FORCE_INLINE const tfScalar & z() const
static const Quaternion & getIdentity()
TFSIMD_FORCE_INLINE Vector3 normalized() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
bool _skipWhitespace(const std::string &str, int &pos) const
helper function for skipping whitespace
TFSIMD_FORCE_INLINE const tfScalar & x() const
std::string comment_
comment to filter
eFiltertype type_
filter type
Quaternion inverse() const
TFSIMD_FORCE_INLINE Vector3 & normalize()
std::string _getComment(const std::string &str) const
helper function for returning the comment
bool add(std::string filter)
bool update(const tf::TransformListener &tf_listener, const std::string base_frame, const ros::Time time, std::vector< cPcdFilterPaFilter > &result) const
TFSIMD_FORCE_INLINE tfScalar tfAsin(tfScalar x)
bool _checkSymbol(const std::string &str, int &pos, const char symbol) const
helper function for checking a certain symbol
tf::Transform offset_[COUNT_FRAME]
bool required_
required filter
double update_transform(const tf::Transform &start, const tf::Transform &goal, tf::Transform &result, const double relative_pos=0.0) const
std::vector< std::string > get(void) const
TFSIMD_FORCE_INLINE tfScalar length() const
double parameter_[COUNT_PARAMETER]
single parameter of filter