Fix: last pose of plan was not transformed to the map frame

Author: JustusBraun

cvp_mesh_planner/src/cvp_mesh_planner.cpp

@@ -107,9 +107,10 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start,
       plan.push_back(pose);
     }
 
+    // Add the goal pose with correct orientation to the plan
     geometry_msgs::msg::PoseStamped pose;
     pose.header = header;
-    pose.pose = goal.pose;
+    pose.pose = goal_in_map.pose;
     plan.push_back(pose);
   }