g2o jazzy compatibility

Author: renan028

teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h

@@ -129,7 +129,8 @@ class EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose,
     double dd_error_2 = deltaS[1]*sin1;
     double dd_dev = penaltyBoundFromBelowDerivative(dd_error_1+dd_error_2, 0,0);
 
-    double dev_nh_abs = g2o::sign( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - 
+
+    double dev_nh_abs = sign( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] -
 	      ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
 
     // conf1

teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h

@@ -136,8 +136,8 @@ class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose>
     double aux1 = -fabs(aux0) / dist;
     double dev_norm_x = deltaS[0]*aux1;
     double dev_norm_y = deltaS[1]*aux1;
-    
-    double aux2 = cos(angdiff) * g2o::sign(aux0);
+
+    double aux2 = cos(angdiff) * sign(aux0);
     double aux3 = aux2 / dist_squared;
     double dev_proj_x = aux3 * deltaS[1] * dist;
     double dev_proj_y = -aux3 * deltaS[0] * dist;

teb_local_planner/include/teb_local_planner/misc.h

@@ -100,6 +100,43 @@ inline double fast_sigmoid(double x)
   return x / (1 + fabs(x));
 }
 
+/**
+ * sign function. (g2o::sign)
+ * @return the sign of x. +1 for x > 0, -1 for x < 0, 0 for x == 0
+ */
+template <typename T>
+inline int sign(T x) {
+  if (x > 0)
+    return 1;
+  else if (x < 0)
+    return -1;
+  else
+    return 0;
+}
+
+/**
+ * average two angles
+ */
+inline double average_angle(double theta1, double theta2) {
+  double x, y;
+
+  x = std::cos(theta1) + std::cos(theta2);
+  y = std::sin(theta1) + std::sin(theta2);
+  if (x == 0 && y == 0)
+    return 0;
+  else
+    return std::atan2(y, x);
+}
+
+/**
+ * normalize the angle
+ */
+inline double normalize_theta(double theta) {
+  const double result = fmod(theta + M_PI, 2.0 * M_PI);
+  if (result <= 0.0) return result + M_PI;
+  return result - M_PI;
+}
+
 /**
  * @brief Calculate Euclidean distance between two 2D point datatypes
  * @param point1 object containing fields x and y

teb_local_planner/include/teb_local_planner/pose_se2.h

@@ -39,8 +39,6 @@
 #ifndef POSE_SE2_H_
 #define POSE_SE2_H_
 
-#include <g2o/stuff/misc.h>
-
 #include <Eigen/Core>
 #include "teb_local_planner/misc.h"
 #include <geometry_msgs/msg/pose.hpp>
@@ -254,7 +252,7 @@ class PoseSE2
   void scale(double factor)
   {
     _position *= factor;
-    _theta = g2o::normalize_theta( _theta*factor );
+    _theta = normalize_theta( _theta*factor );
   }
 
   /**
@@ -266,7 +264,7 @@ class PoseSE2
   {
     _position.coeffRef(0) += pose_as_array[0];
     _position.coeffRef(1) += pose_as_array[1];
-    _theta = g2o::normalize_theta( _theta + pose_as_array[2] );
+    _theta = normalize_theta( _theta + pose_as_array[2] );
   }
 
   /**
@@ -279,7 +277,7 @@ class PoseSE2
   void averageInPlace(const PoseSE2& pose1, const PoseSE2& pose2)
   {
     _position = (pose1._position + pose2._position)/2;
-    _theta = g2o::average_angle(pose1._theta, pose2._theta);
+    _theta = average_angle(pose1._theta, pose2._theta);
   }
 
   /**
@@ -292,7 +290,7 @@ class PoseSE2
     */ 
   static PoseSE2 average(const PoseSE2& pose1, const PoseSE2& pose2)
   {
-    return PoseSE2( (pose1._position + pose2._position)/2 , g2o::average_angle(pose1._theta, pose2._theta) );
+    return PoseSE2( (pose1._position + pose2._position)/2 , average_angle(pose1._theta, pose2._theta) );
   }
 
   /**
@@ -310,7 +308,7 @@ class PoseSE2
     _position.x() = new_x;
     _position.y() = new_y;
     if (adjust_theta)
-        _theta = g2o::normalize_theta(_theta+angle);
+        _theta = normalize_theta(_theta+angle);
   }
 
   ///@}
@@ -341,7 +339,7 @@ class PoseSE2
   PoseSE2& operator+=(const PoseSE2& rhs)
   {
     _position += rhs._position;
-    _theta = g2o::normalize_theta(_theta + rhs._theta);
+    _theta = normalize_theta(_theta + rhs._theta);
     return *this;
   }
 
@@ -362,7 +360,7 @@ class PoseSE2
   PoseSE2& operator-=(const PoseSE2& rhs)
   {
     _position -= rhs._position;
-    _theta = g2o::normalize_theta(_theta - rhs._theta);
+    _theta = normalize_theta(_theta - rhs._theta);
     return *this;
   }
 

teb_local_planner/src/optimal_planner.cpp

@@ -1113,7 +1113,7 @@ void TebOptimalPlanner::extractVelocity(const PoseSE2& pose1, const PoseSE2& pos
     Eigen::Vector2d conf1dir( cos(pose1.theta()), sin(pose1.theta()) );
     // translational velocity
     double dir = deltaS.dot(conf1dir);
-    vx = (double) g2o::sign(dir) * deltaS.norm()/dt;
+    vx = (double) sign(dir) * deltaS.norm()/dt;
     vy = 0;
   }
   else // holonomic robot

teb_local_planner/src/recovery_behaviors.cpp

@@ -41,7 +41,7 @@
 #include <limits>
 #include <functional>
 #include <numeric>
-#include <g2o/stuff/misc.h>
+#include "teb_local_planner/misc.h"
 
 namespace teb_local_planner
 {
@@ -99,7 +99,7 @@ bool FailureDetector::detect(double v_eps, double omega_eps)
     {
         v_mean += buffer_[i].v;
         omega_mean += buffer_[i].omega;
-        if ( i>0 && g2o::sign(buffer_[i].omega) != g2o::sign(buffer_[i-1].omega) )
+        if ( i>0 && sign(buffer_[i].omega) != sign(buffer_[i-1].omega) )
             ++omega_zero_crossings;
     }
     v_mean /= n;

teb_local_planner/src/teb_local_planner_ros.cpp

@@ -939,7 +939,7 @@ double TebLocalPlannerROS::convertTransRotVelToSteeringAngle(double v, double om
   double radius = v/omega;
 
   if (fabs(radius) < min_turning_radius)
-    radius = double(g2o::sign(radius)) * min_turning_radius; 
+    radius = double(sign(radius)) * min_turning_radius;
 
   return std::atan(wheelbase / radius);
 }