Added missing transform of start and target pose in planner implementations

cvp_mesh_planner/src/cvp_mesh_planner.cpp

@@ -65,16 +65,27 @@ 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
+  geometry_msgs::msg::PoseStamped start_in_map, goal_in_map;
+
+  try {
+    // try to extract start and goal in map frame
+    start_in_map = mesh_map_->transformToMapFrame(start);
+    goal_in_map = mesh_map_->transformToMapFrame(goal);
+  } catch (tf2::TransformException& ex) {
+    RCLCPP_ERROR_STREAM(
+      node_->get_logger(), name_ <<
+        ": Could not transform start or goal pose to map frame '" << map_frame_ << "': " << ex.what());
+    return mbf_msgs::action::GetPath::Result::TF_ERROR;
+  }
+
+  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 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);
-
   const uint32_t outcome = waveFrontPropagation(goal_vec, start_vec, path, message);
 
   RCLCPP_DEBUG_STREAM(node_->get_logger(), "finished wave front propagation.");
@@ -105,9 +116,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,26 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st
                             double tolerance, std::vector<geometry_msgs::msg::PoseStamped>& plan, double& cost,
                             std::string& message)
 {
-  const auto mesh = mesh_map_->mesh();
+  // This planner requires start and goal pose to be in map frame
+  geometry_msgs::msg::PoseStamped start_in_map, goal_in_map;
+  try {
+    // try to extract start and goal in map frame
+    start_in_map = mesh_map_->transformToMapFrame(start);
+    goal_in_map = mesh_map_->transformToMapFrame(goal);
+  } catch (tf2::TransformException& ex) {
+    RCLCPP_ERROR_STREAM(
+      node_->get_logger(), name_ <<
+        ": Could not transform start or goal pose to map frame '" << map_frame_ << "': " << ex.what());
+    return mbf_msgs::action::GetPath::Result::TF_ERROR;
+  }
+
+  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 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);
+  RCLCPP_INFO(node_->get_logger(), "start dijkstra mesh planner.");
 
   // 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;
   }
@@ -496,6 +504,9 @@ class MeshMap
   //! The name of the mesh part that is used for navigation stored to the working file
   std::string mesh_working_part;
 
+  //! TF2 transform timeout duration (fetched from MBF)
+  double tf_timeout;
+
   //! dynamic params callback handle
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr config_callback;
 

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;
 
@@ -92,6 +91,7 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node)
   , node(node)
   , first_config(true)
   , map_loaded(false)
+  , tf_timeout(1.0) // default 1s
 {
   auto edge_cost_factor_desc = rcl_interfaces::msg::ParameterDescriptor{};
   edge_cost_factor_desc.name = MESH_MAP_NAMESPACE + ".edge_cost_factor";
@@ -119,6 +119,7 @@ MeshMap::MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node)
   mesh_working_file = node->declare_parameter(MESH_MAP_NAMESPACE + ".mesh_working_file", "");
   mesh_working_part = node->declare_parameter(MESH_MAP_NAMESPACE + ".mesh_working_part", "");
   global_frame = node->declare_parameter(MESH_MAP_NAMESPACE + ".global_frame", "map");
+  node->get_parameter("tf_timeout", tf_timeout); // declared in mbf abstract_navigation_server
 
   const bool enable_layer_timer = node->declare_parameter(MESH_MAP_NAMESPACE + ".enable_layer_timer", false);
   if (enable_layer_timer)
@@ -1307,6 +1308,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, rclcpp::Duration::from_seconds(tf_timeout));
+
+  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);