Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef PLANNER_CSPACE_PLANNER_3D_JUMP_DETECTOR_H
00031 #define PLANNER_CSPACE_PLANNER_3D_JUMP_DETECTOR_H
00032
00033 #include <ros/ros.h>
00034
00035 #include <tf2/utils.h>
00036 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00037 #include <tf2_ros/transform_listener.h>
00038
00039 #include <string>
00040
00041 class JumpDetector
00042 {
00043 protected:
00044 tf2_ros::Buffer& tfbuf_;
00045 tf2_ros::TransformListener tfl_;
00046 tf2::Transform base_trans_prev_;
00047 tf2::Transform map_trans_prev_;
00048
00049 std::string jump_detect_frame_;
00050 std::string map_frame_;
00051
00052 double pos_jump_;
00053 double yaw_jump_;
00054
00055 bool init_;
00056
00057 public:
00058 explicit JumpDetector(tf2_ros::Buffer& tfbuf)
00059 : tfbuf_(tfbuf)
00060 , tfl_(tfbuf_)
00061 , base_trans_prev_(tf2::Quaternion(0, 0, 0, 1))
00062 , map_trans_prev_(tf2::Quaternion(0, 0, 0, 1))
00063 , init_(false)
00064 {
00065 }
00066 void setMapFrame(const std::string& frame_id)
00067 {
00068 map_frame_ = frame_id;
00069 }
00070 void setBaseFrame(const std::string& frame_id)
00071 {
00072 jump_detect_frame_ = frame_id;
00073 }
00074 void setThresholds(const double pos_jump, const double yaw_jump)
00075 {
00076 pos_jump_ = pos_jump;
00077 yaw_jump_ = yaw_jump;
00078 }
00079 bool detectJump()
00080 {
00081 tf2::Stamped<tf2::Transform> base_trans;
00082 tf2::Stamped<tf2::Transform> map_trans;
00083 try
00084 {
00085 geometry_msgs::TransformStamped base_trans_tmp =
00086 tfbuf_.lookupTransform(jump_detect_frame_, "base_link", ros::Time());
00087 geometry_msgs::TransformStamped map_trans_tmp =
00088 tfbuf_.lookupTransform(map_frame_, "base_link", ros::Time());
00089 tf2::fromMsg(base_trans_tmp, base_trans);
00090 tf2::fromMsg(map_trans_tmp, map_trans);
00091 }
00092 catch (tf2::TransformException& e)
00093 {
00094 return false;
00095 }
00096 const auto diff =
00097 map_trans.inverse() * map_trans_prev_ * (base_trans_prev_.inverse() * base_trans);
00098 base_trans_prev_ = base_trans;
00099 map_trans_prev_ = map_trans;
00100
00101 if (!init_)
00102 {
00103 init_ = true;
00104 return false;
00105 }
00106
00107 const auto pos_diff = diff.getOrigin().length();
00108 const auto yaw_diff = tf2::getYaw(diff.getRotation());
00109
00110 if (pos_diff > pos_jump_ || fabs(yaw_diff) > yaw_jump_)
00111 {
00112 ROS_ERROR("Position jumped (%0.3f/%0.3f, %0.3f/%0.3f); clearing history",
00113 pos_diff, pos_jump_, yaw_diff, yaw_jump_);
00114 return true;
00115 }
00116 return false;
00117 }
00118 };
00119
00120 #endif // PLANNER_CSPACE_PLANNER_3D_JUMP_DETECTOR_H