jump_detector.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef PLANNER_CSPACE_JUMP_DETECTOR_H
31 #define PLANNER_CSPACE_JUMP_DETECTOR_H
32 
33 #include <cmath>
34 #include <string>
35 
36 #include <ros/ros.h>
37 
38 #include <tf2/utils.h>
41 
42 namespace planner_cspace
43 {
44 namespace planner_3d
45 {
47 {
48 protected:
52 
53  std::string jump_detect_frame_;
54  std::string map_frame_;
55 
56  double pos_jump_;
57  double yaw_jump_;
58 
59  bool init_;
60 
61 public:
62  explicit JumpDetector(tf2_ros::Buffer& tfbuf)
63  : tfbuf_(tfbuf)
64  , base_trans_prev_(tf2::Quaternion(0, 0, 0, 1))
65  , map_trans_prev_(tf2::Quaternion(0, 0, 0, 1))
66  , init_(false)
67  {
68  }
69  void setMapFrame(const std::string& frame_id)
70  {
71  map_frame_ = frame_id;
72  }
73  void setBaseFrame(const std::string& frame_id)
74  {
75  jump_detect_frame_ = frame_id;
76  }
77  void setThresholds(const double pos_jump, const double yaw_jump)
78  {
79  pos_jump_ = pos_jump;
80  yaw_jump_ = yaw_jump;
81  }
82  bool detectJump()
83  {
86  try
87  {
88  geometry_msgs::TransformStamped base_trans_tmp =
89  tfbuf_.lookupTransform(jump_detect_frame_, "base_link", ros::Time());
90  geometry_msgs::TransformStamped map_trans_tmp =
91  tfbuf_.lookupTransform(map_frame_, "base_link", ros::Time());
92  tf2::fromMsg(base_trans_tmp, base_trans);
93  tf2::fromMsg(map_trans_tmp, map_trans);
94  }
95  catch (tf2::TransformException& e)
96  {
97  return false;
98  }
99  const auto diff =
100  map_trans.inverse() * map_trans_prev_ * (base_trans_prev_.inverse() * base_trans);
101  base_trans_prev_ = base_trans;
102  map_trans_prev_ = map_trans;
103 
104  if (!init_)
105  {
106  init_ = true;
107  return false;
108  }
109 
110  const auto pos_diff = diff.getOrigin().length();
111  const auto yaw_diff = tf2::getYaw(diff.getRotation());
112 
113  if (pos_diff > pos_jump_ || std::abs(yaw_diff) > yaw_jump_)
114  {
115  ROS_ERROR("Position jumped (%0.3f/%0.3f, %0.3f/%0.3f); clearing history",
116  pos_diff, pos_jump_, yaw_diff, yaw_jump_);
117  return true;
118  }
119  return false;
120  }
121 };
122 } // namespace planner_3d
123 } // namespace planner_cspace
124 
125 #endif // PLANNER_CSPACE_JUMP_DETECTOR_H
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void setBaseFrame(const std::string &frame_id)
Definition: jump_detector.h:73
JumpDetector(tf2_ros::Buffer &tfbuf)
Definition: jump_detector.h:62
void fromMsg(const A &, B &b)
Transform inverse() const
void setMapFrame(const std::string &frame_id)
Definition: jump_detector.h:69
void setThresholds(const double pos_jump, const double yaw_jump)
Definition: jump_detector.h:77
double getYaw(const A &a)
#define ROS_ERROR(...)


planner_cspace
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:42