From 46ab547a709cfb0d09482a6f5f1a1436a528c1e0 Mon Sep 17 00:00:00 2001
From: dennyboby123 <denny.boby@rapyuta-robotics.com>
Date: Tue, 3 Dec 2019 21:24:19 +0900
Subject: [PATCH] ported to new checkpoint msg

---
 .../mbf_abstract_nav/controller_action.h      |  1 +
 .../mbf_abstract_nav/move_base_action.h       |  1 +
 mbf_abstract_nav/src/controller_action.cpp    | 20 +++++++++++++++++--
 mbf_abstract_nav/src/move_base_action.cpp     | 16 +++++++++++++--
 4 files changed, 34 insertions(+), 4 deletions(-)

diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h b/mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h
index 63f6743a..3279af24 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h
@@ -46,6 +46,7 @@
 #include "mbf_abstract_nav/robot_information.h"
 #include <actionlib/server/action_server.h>
 #include <forklift_interfaces/NavigateAction.h>
+#include <tf2/LinearMath/Quaternion.h>
 
 namespace mbf_abstract_nav{
 
diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/move_base_action.h b/mbf_abstract_nav/include/mbf_abstract_nav/move_base_action.h
index e3469b5c..8d4f7e16 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/move_base_action.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/move_base_action.h
@@ -47,6 +47,7 @@
 #include <mbf_msgs/GetPathAction.h>
 #include <forklift_interfaces/NavigateAction.h>
 #include <mbf_msgs/RecoveryAction.h>
+#include <tf2/LinearMath/Quaternion.h>
 
 #include "mbf_abstract_nav/MoveBaseFlexConfig.h"
 #include "mbf_abstract_nav/robot_information.h"
diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp
index d31c1788..d3b4ffa3 100644
--- a/mbf_abstract_nav/src/controller_action.cpp
+++ b/mbf_abstract_nav/src/controller_action.cpp
@@ -79,7 +79,15 @@ void ControllerAction::start(
       std::vector<geometry_msgs::PoseStamped> goal_path;
       for(std::size_t it = 0; it<goal.path.checkpoints.size(); it++)
       {
-        goal_path.push_back(goal.path.checkpoints[it].pose);
+        geometry_msgs::PoseStamped checkpoint_posestamped;
+        checkpoint_posestamped.header.frame_id = "map";
+        checkpoint_posestamped.pose.position.x = goal.path.checkpoints[it].pose.x;
+        checkpoint_posestamped.pose.position.y = goal.path.checkpoints[it].pose.y;
+        tf2::Quaternion q;
+        q.setRPY( 0, 0, goal.path.checkpoints[it].pose.theta );
+        q.normalize();
+        checkpoint_posestamped.pose.orientation = tf2::toMsg(q);
+        goal_path.push_back(checkpoint_posestamped);
       }
       execution_ptr->setNewPlan(goal_path);
       // Update also goal pose, so the feedback remains consistent
@@ -130,7 +138,15 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution
   std::vector<geometry_msgs::PoseStamped> goal_path;
   for (int it = 0; it < goal.path.checkpoints.size(); it++)
   {
-    goal_path.push_back(goal.path.checkpoints[it].pose);
+    geometry_msgs::PoseStamped checkpoint_posestamped;
+    checkpoint_posestamped.header.frame_id = "map";
+    checkpoint_posestamped.pose.position.x = goal.path.checkpoints[it].pose.x;
+    checkpoint_posestamped.pose.position.y = goal.path.checkpoints[it].pose.y;
+    tf2::Quaternion q;
+    q.setRPY( 0, 0, goal.path.checkpoints[it].pose.theta );
+    q.normalize();
+    checkpoint_posestamped.pose.orientation = tf2::toMsg(q);
+    goal_path.push_back(checkpoint_posestamped);
   }
   
   const std::vector<geometry_msgs::PoseStamped> &plan = goal_path;
diff --git a/mbf_abstract_nav/src/move_base_action.cpp b/mbf_abstract_nav/src/move_base_action.cpp
index f70d0441..431a5685 100644
--- a/mbf_abstract_nav/src/move_base_action.cpp
+++ b/mbf_abstract_nav/src/move_base_action.cpp
@@ -263,7 +263,13 @@ void MoveBaseAction::actionGetPathDone(
       navigate_goal_.path.header = result.path.header; //TODO handle conversion from nav_msgs/Path to NavigatePath/Path
       for(std::size_t it=0; it<result.path.poses.size(); it++)
       {
-        navigate_goal_.path.checkpoints[it].pose = result.path.poses[it];
+        navigate_goal_.path.checkpoints[it].pose.x = result.path.poses[it].pose.position.x;
+        navigate_goal_.path.checkpoints[it].pose.y = result.path.poses[it].pose.position.y;
+        tf2::Quaternion qp(result.path.poses[it].pose.orientation.x, result.path.poses[it].pose.orientation.y, result.path.poses[it].pose.orientation.z, result.path.poses[it].pose.orientation.w);
+        tf2::Matrix3x3 mp(qp);
+        double roll, pitch, yaw;
+        mp.getRPY(roll, pitch, yaw);
+        navigate_goal_.path.checkpoints[it].pose.theta = yaw;
       }
       ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
           << "move_base\" sends the path to \""
@@ -579,7 +585,13 @@ void MoveBaseAction::actionGetPathReplanningDone(
     navigate_goal_.path.header = result->path.header; //TODO handle conversion from nav_msgs/Path to NavigatePath/Path
     for(std::size_t it=0; it<result->path.poses.size(); it++)
     {
-      navigate_goal_.path.checkpoints[it].pose = result->path.poses[it];
+      navigate_goal_.path.checkpoints[it].pose.x = result->path.poses[it].pose.position.x;
+      navigate_goal_.path.checkpoints[it].pose.y = result->path.poses[it].pose.position.y;
+      tf2::Quaternion qp(result->path.poses[it].pose.orientation.x, result->path.poses[it].pose.orientation.y, result->path.poses[it].pose.orientation.z,result->path.poses[it].pose.orientation.w);
+      tf2::Matrix3x3 mp(qp);
+      double roll, pitch, yaw;
+      mp.getRPY(roll, pitch, yaw);
+      navigate_goal_.path.checkpoints[it].pose.theta = yaw;
     }
     forklift_interfaces::NavigateGoal goal(navigate_goal_);
     action_client_navigate_.sendGoal(