Added missing transform of start and target pose in planner implementations

cvp_mesh_planner/src/cvp_mesh_planner.cpp

@@ -65,15 +65,17 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start,
                             std::vector<geometry_msgs::msg::PoseStamped>& plan, double& cost,
                             std::string& message)
 {
+  // This planner requires start and goal pose to be in map frame
+  const geometry_msgs::msg::PoseStamped start_in_map = mesh_map_->transformToMapFrame(start);
+  const geometry_msgs::msg::PoseStamped goal_in_map = mesh_map_->transformToMapFrame(goal);
+
   const auto mesh = mesh_map_->mesh();
   std::list<std::pair<mesh_map::Vector, lvr2::FaceHandle>> path;
 
-  // mesh_map->combineVertexCosts(); // TODO should be outside the planner
-
   RCLCPP_DEBUG_STREAM(node_->get_logger(), "start wave front propagation.");
 
-  mesh_map::Vector goal_vec = mesh_map::toVector(goal.pose.position);
-  mesh_map::Vector start_vec = mesh_map::toVector(start.pose.position);
+  mesh_map::Vector start_vec = mesh_map::toVector(start_in_map.pose.position);
+  mesh_map::Vector goal_vec = mesh_map::toVector(goal_in_map.pose.position);
 
   const uint32_t outcome = waveFrontPropagation(goal_vec, start_vec, path, message);
 
@@ -105,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);
   }
 

dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp

@@ -56,13 +56,17 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st
                             double tolerance, std::vector<geometry_msgs::msg::PoseStamped>& plan, double& cost,
                             std::string& message)
 {
+  // This planner requires start and goal pose to be in map frame
+  const geometry_msgs::msg::PoseStamped start_in_map = mesh_map_->transformToMapFrame(start);
+  const geometry_msgs::msg::PoseStamped goal_in_map = mesh_map_->transformToMapFrame(goal);
+
   const auto mesh = mesh_map_->mesh();
 
   std::list<lvr2::VertexHandle> path;
   RCLCPP_INFO(node_->get_logger(), "start dijkstra mesh planner.");
 
-  mesh_map::Vector goal_vec = mesh_map::toVector(goal.pose.position);
-  mesh_map::Vector start_vec = mesh_map::toVector(start.pose.position);
+  mesh_map::Vector start_vec = mesh_map::toVector(start_in_map.pose.position);
+  mesh_map::Vector goal_vec = mesh_map::toVector(goal_in_map.pose.position);
 
   // call dijkstra with the goal pose as seed / start vertex
   uint32_t outcome = dijkstra(goal_vec, start_vec, path);

mesh_map/include/mesh_map/mesh_map.h

@@ -42,7 +42,6 @@
 #include <mutex>
 #include <tuple>
 #include <unordered_map>
-#include <geometry_msgs/msg/point.hpp>
 #include <memory>
 
 #include <mesh_map/abstract_layer.h>
@@ -57,6 +56,9 @@
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 #include <std_srvs/srv/trigger.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
 
 #include <lvr2/algorithm/raycasting/RaycasterBase.hpp>
 #include <lvr2/geometry/BaseVector.hpp>
@@ -254,6 +256,12 @@ class MeshMap
    */
   bool resetLayers();
 
+  /**
+   * @brief Transforms the given transform to the global frame of the MeshMap
+   */
+  geometry_msgs::msg::PoseStamped transformToMapFrame(
+    const geometry_msgs::msg::PoseStamped& input_pose) const;
+
   /**
    * @brief Returns the stored vector map
    */
@@ -289,7 +297,7 @@ class MeshMap
   /**
    * @brief Returns the map frame / coordinate system id
    */
-  const std::string& mapFrame()
+  const std::string& mapFrame() const
   {
     return global_frame;
   }

mesh_map/src/mesh_map.cpp

@@ -41,43 +41,42 @@
 #include <boost/uuid/uuid_io.hpp>
 #include <optional>
 #include <memory>
-
 #include <functional>
-#include <geometry_msgs/msg/point_stamped.hpp>
-#include <geometry_msgs/msg/vector3.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
+#include <mutex>
+#include <filesystem>
+#include <assimp/Importer.hpp>
+#include <assimp/postprocess.h>
+#include <assimp/scene.h>
 
 #include <lvr2/geometry/Normal.hpp>
 #include <lvr2/algorithm/GeometryAlgorithms.hpp>
 #include <lvr2/algorithm/NormalAlgorithms.hpp>
 #include <lvr2/io/deprecated/hdf5/MeshIO.hpp>
 #include <lvr2/types/MeshBuffer.hpp>
-
 // Mesh structure for fast surface traversal
 #include <lvr2/geometry/PMPMesh.hpp>
-
 // Raycaster implementations
 #ifdef LVR2_USE_EMBREE
   #include <lvr2/algorithm/raycasting/EmbreeRaycaster.hpp>
 #else
   #include <lvr2/algorithm/raycasting/BVHRaycaster.hpp>
 #endif
 
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_ros/buffer.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
 #include <mesh_map/abstract_layer.h>
 #include <mesh_map/mesh_map.h>
 #include <mesh_map/util.h>
 #include <mesh_map/timer.h>
 #include <mesh_msgs/msg/mesh_geometry_stamped.hpp>
 #include <mesh_msgs_conversions/conversions.h>
-#include <mutex>
-#include <rclcpp/rclcpp.hpp>
-#include <visualization_msgs/msg/marker.hpp>
-
-#include <filesystem>
-
-#include <assimp/Importer.hpp>
-#include <assimp/postprocess.h>
-#include <assimp/scene.h>
 
 namespace fs = std::filesystem;
 
@@ -1307,6 +1306,24 @@ bool MeshMap::resetLayers()
   return true;  // TODO implement
 }
 
+geometry_msgs::msg::PoseStamped MeshMap::transformToMapFrame(
+    const geometry_msgs::msg::PoseStamped& input_pose) const
+{
+  if(input_pose.header.frame_id == mapFrame())
+  {
+    return input_pose;
+  }
+
+  // we shouldn't catch/ignore the error that comes from here, as it indicates a misconfiguration
+  const geometry_msgs::msg::TransformStamped Tim = tf_buffer.lookupTransform(
+      mapFrame(), input_pose.header.frame_id, input_pose.header.stamp);
+
+  geometry_msgs::msg::PoseStamped output_pose;
+  tf2::doTransform(input_pose, output_pose, Tim);
+
+  return output_pose;
+}
+
 void MeshMap::publishVertexCosts(const lvr2::VertexMap<float>& costs, const std::string& name, const rclcpp::Time& map_stamp)
 {
   layer_manager_.publish_cost_layer(costs, name, map_stamp);