Use geometry_msgs::msg::Pose as Pose2D is now in vision_msgs

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>

Author: fmrico

plansys2_bt_actions/CMakeLists.txt

@@ -69,7 +69,7 @@ if(BUILD_TESTING)
   find_package(plansys2_msgs REQUIRED)
   find_package(test_msgs REQUIRED)
   find_package(geometry_msgs REQUIRED)
-
+  find_package(tf2_geometry_msgs)
   add_subdirectory(test)
 endif()
 

plansys2_bt_actions/test/behavior_tree/Move.cpp

@@ -19,7 +19,8 @@
 
 #include "Move.hpp"
 
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 #include "behaviortree_cpp/behavior_tree.h"
 
@@ -50,10 +51,10 @@ Move::Move(
 
       std::vector<double> coords;
       if (node->get_parameter_or("waypoint_coords." + wp, coords, {})) {
-        geometry_msgs::msg::Pose2D pose;
-        pose.x = coords[0];
-        pose.y = coords[1];
-        pose.theta = coords[2];
+        geometry_msgs::msg::Pose pose;
+        pose.position.x = coords[0];
+        pose.position.y = coords[1];
+        pose.orientation = tf2::toMsg(tf2::Quaternion({0.0, 0.0, 1.0}, coords[2]));
 
         waypoints_[wp] = pose;
       } else {
@@ -70,7 +71,7 @@ Move::on_tick()
     std::string goal;
     getInput<std::string>("goal", goal);
 
-    geometry_msgs::msg::Pose2D pose2nav;
+    geometry_msgs::msg::Pose pose2nav;
     if (waypoints_.find(goal) != waypoints_.end()) {
       pose2nav = waypoints_[goal];
     } else {

plansys2_bt_actions/test/behavior_tree/Move.hpp

@@ -18,7 +18,7 @@
 #include <string>
 #include <map>
 
-#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 #include "test_msgs/action/fibonacci.hpp"
 
 #include "plansys2_bt_actions/BTActionNode.hpp"
@@ -49,7 +49,7 @@ class Move : public plansys2::BtActionNode<test_msgs::action::Fibonacci>
 
 private:
   int goal_reached_;
-  std::map<std::string, geometry_msgs::msg::Pose2D> waypoints_;
+  std::map<std::string, geometry_msgs::msg::Pose> waypoints_;
 };
 
 }  // namespace plansys2_bt_tests

plansys2_bt_actions/test/unit/CMakeLists.txt

@@ -23,6 +23,7 @@ foreach(bt_plugin ${plugin_libs})
   target_link_libraries(${bt_plugin}
     behaviortree_cpp::behaviortree_cpp
     ${geometry_msgs_TARGETS}
+    tf2_geometry_msgs::tf2_geometry_msgs
     rclcpp::rclcpp
     rclcpp_action::rclcpp_action
     rclcpp_lifecycle::rclcpp_lifecycle