Set orientation to go back along trajectory line.

aicp_core/src/utils/common.cpp

@@ -25,14 +25,7 @@ Eigen::Isometry3d fromMatrix4fToIsometry3d(Eigen::Matrix4f matrix){
 double angleBetweenVectors2d(Eigen::Vector2d& v1,
                              Eigen::Vector2d& v2){
 
-    double numerator = (v1.transpose() * v2);
-    std::cout << "numerator: " << numerator << std::endl;
-    double denominator = (v1.norm() * v2.norm());
-    std::cout << "denominator: " << denominator << std::endl;
-    double result = acos(numerator / denominator) * 180.0 / M_PI;
-    std::cout << "result: " << result << std::endl;
-
-    return result;
+    return atan2(v1(0)*v2(1) - v1(1)*v2(0), v1(0)*v2(0) + v1(1)*v2(1)) * 180.0 / M_PI;
 }
 
 //extract integers from a string.

aicp_ros/src/talker_ros.cpp

@@ -42,29 +42,22 @@ void ROSTalker::publishFootstepPlan(PathPoses& path,
 
 void ROSTalker::reversePath(PathPoses& path){
 
-//    Eigen::Vector3d v1;
-//    v1 << 1, 0, 0;
-//    Eigen::Vector3d v2;
-//    v2 << 1, 0, 0;
-
-//    Eigen::Matrix3d rotx;
-//    rotx = Eigen::AngleAxisd(0*M_PI/180, Eigen::Vector3d::UnitX())
-//         * Eigen::AngleAxisd(0*M_PI/180, Eigen::Vector3d::UnitY())
-//         * Eigen::AngleAxisd(30*M_PI/180, Eigen::Vector3d::UnitZ());
-//    v2 = rotx * v2;
-
-//    double res = angleBetweenVectors2d(v1, v2);
-//    std::cout << "res: " << res << std::endl;
     std::reverse(path.begin(),path.end());
 
     for (size_t i = 0; i < path.size(); ++i){
-        double angle = 0.0;
+        double angle = 180.0;
+
+        // Compute orientation along trajectory line
         if (i != path.size()-1)
         {
+            // angle between x-axis of current frame
             Eigen::Vector2d v1;
             v1 = path[i].matrix().block<2,1>(0,0);
+            // and trajectory line
             Eigen::Vector2d v2;
-            v2 = path[i+1].matrix().block<2,1>(0,0) - v1;
+            v2 << path[i+1].translation()(0, 0) - path[i].translation()(0, 0),
+                  path[i+1].translation()(1, 0) - path[i].translation()(1, 0);
+
             angle = angleBetweenVectors2d(v1, v2);
         }