Fix compilation

Author: LuisLechugaRuiz

src/task_behavior_tree/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.8)
+project(task_behavior_tree)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_behavior_tree REQUIRED)
+find_package(task_msgs REQUIRED)
+
+include_directories(
+  include
+)
+
+set(library_name ${PROJECT_NAME})
+
+set(dependencies
+  geometry_msgs
+  nav2_behavior_tree
+  task_msgs
+)
+
+add_library(task_initialize_navigation_action_bt_node SHARED plugins/src/action/initialize_navigation.cpp)
+list(APPEND plugin_libs task_initialize_navigation_action_bt_node)
+
+foreach(bt_plugin ${plugin_libs})
+  ament_target_dependencies(${bt_plugin} ${dependencies})
+  target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
+endforeach()
+
+install(TARGETS ${plugin_libs}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(
+  include
+)
+
+ament_export_libraries(
+  ${plugin_libs}
+)
+
+ament_export_dependencies(${dependencies})
+
+ament_package()

src/task_behavior_tree/plugins/include/actions/initialize_navigation.hpp renamed to src/task_behavior_tree/include/action/initialize_navigation.hpp

@@ -1,18 +1,19 @@
-
 #ifndef BEHAVIOR_TREE__PLUGINS__ACTION__INITIALIZE_NAVIGATION_ACTION_HPP_
 #define BEHAVIOR_TREE__PLUGINS__ACTION__INITIALIZE_NAVIGATION_ACTION_HPP_
 
 #include <string>
 
+#include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_behavior_tree/bt_action_node.hpp"
+#include "task_msgs/action/initialize_navigation.hpp"
 
 namespace behavior_tree {
 namespace action {
 
 /**
  * @brief A nav2_behavior_tree::BtActionNode class that wraps behavior_tree::action::InitializeNavigation
  */
-class InitializeNavigation : public BtActionNode<task_msgs::action::InitializeNavigation>
+class InitializeNavigation : public nav2_behavior_tree::BtActionNode<task_msgs::action::InitializeNavigation>
 {
 public:
   /**

src/task_behavior_tree/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>task_behavior_tree</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">luis</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav2_behavior_tree</build_depend>
+  <build_depend>task_msgs</build_depend>
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav2_behavior_tree</exec_depend>
+  <exec_depend>task_msgs</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/task_behavior_tree/plugins/src/actions/initialize_navigation.cpp renamed to src/task_behavior_tree/plugins/src/action/initialize_navigation.cpp

@@ -1,7 +1,7 @@
 #include <memory>
 #include <string>
 
-#include "actions/initialize_navigation.hpp"
+#include "action/initialize_navigation.hpp"
 
 namespace behavior_tree {
 namespace action {
@@ -40,10 +40,10 @@ BT_REGISTER_NODES(factory)
   BT::NodeBuilder builder =
     [](const std::string & name, const BT::NodeConfiguration & config)
     {
-      return std::make_unique<behavior_tree::InitializeNavigation>(
+      return std::make_unique<behavior_tree::action::InitializeNavigation>(
         name, "initialize_navigation", config);
     };
 
-  factory.registerBuilder<behavior_tree::InitializeNavigation>(
+  factory.registerBuilder<behavior_tree::action::InitializeNavigation>(
     "InitializeNavigation", builder);
 }

src/task_monitor/CMakeLists.txt

@@ -5,11 +5,16 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+find_package(ament_cmake REQUIRED)
 find_package(types REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(task_behavior_tree REQUIRED)
+find_package(task_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 include_directories(
   include
@@ -33,8 +38,12 @@ set(dependencies
   types
   geometry_msgs
   rclcpp
+  nav2_util
+  task_behavior_tree
+  task_msgs
   tf2
   tf2_geometry_msgs
+  tf2_ros
 )
 
 ament_target_dependencies(${library_name}

src/task_monitor/include/task_monitor.hpp

@@ -1,6 +1,9 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "nav2_util/simple_action_server.hpp"
+#include "task_msgs/action/initialize_navigation.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
 namespace brain {
 
@@ -16,40 +19,39 @@ class TaskMonitor : public rclcpp::Node {
     ~TaskMonitor();
 
   private:
-    const double pose_distance_tolerance_;
-    const double pose_distance_angle_;
-    const double timeout_s_;
-    const geometry_msgs::msg::PoseWithCovarianceStamped initial_pose_;
-
-    rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
-    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr robot_pose_sub_;
-
-    // Action server that implements the InitializeNavigation action
-    using task_msgs::action::InitializeNavigation InitializeNavigationAction;
-    using nav2_util::SimpleActionServer<InitializeNavigationAction> InitializeNavigationActionServer;
-    std::unique_ptr<nav2_util::SimpleActionServer<task_msgs::action::InitializeNavigation>> initialize_navigation_action_server_;
-
     /**
      * @brief Get the initial pose from a param or initialize to {0.0, 0.0, 0.0, 0.0}
      * @return geometry_msgs::msg::PoseWithCovarianceStamped with the initial pose
      */
     geometry_msgs::msg::PoseWithCovarianceStamped GetInitialPose();
     /**
      * @brief Set the initial pose as initial_pose
-     * @return bool with the result of the action
      */
-    bool SetInitialPose();
+    void SetInitialPose();
     /**
      * @brief Get the robot pose from AMCL
      */
-    void AmclPoseCallback (const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+    void AmclPoseCallback (const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
     /**
      * @brief Check if the robot is close to the desired pose
      * @param desired_pose the pose where we want the robot at
      * @return bool with the comparation result
      */
     bool IsRobotAtPose(geometry_msgs::msg::PoseWithCovarianceStamped desired_pose);
 
+    double pose_distance_tolerance_;
+    double pose_angle_tolerance_;
+    double timeout_s_;
+    geometry_msgs::msg::PoseWithCovarianceStamped initial_pose_;
+    geometry_msgs::msg::PoseWithCovarianceStamped robot_pose_;
+
+    rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
+    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr robot_pose_sub_;
+
+    // Action server that implements the InitializeNavigation action
+    using InitializeNavigationAction = task_msgs::action::InitializeNavigation;
+    using InitializeNavigationActionServer = nav2_util::SimpleActionServer<InitializeNavigationAction>;
+    std::unique_ptr<InitializeNavigationActionServer> initialize_navigation_action_server_;
 };
 
 } // namespace brain

src/task_monitor/package.xml

@@ -12,14 +12,22 @@
   <build_depend>rclcpp</build_depend>
   <build_depend>types</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav2_util</build_depend>
+  <build_depend>task_behavior_tree</build_depend>
+  <build_depend>task_msgs</build_depend>
   <build_depend>tf2</build_depend>
   <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>tf2_ros</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>types</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav2_util</exec_depend>
+  <exec_depend>task_behavior_tree</exec_depend>
+  <exec_depend>task_msgs</exec_depend>
   <exec_depend>tf2</exec_depend>
-  <build_depend>tf2_geometry_msgs</build_depend>
+  <exec_depend>tf2_geometry_msgs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/task_monitor/src/main.cpp

@@ -7,7 +7,6 @@ int main(int argc, char * argv[])
   // Need to wait some time for the publisher to stablish connection, bug ?
   std::chrono::seconds sleep_time_sec(1);
   rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(sleep_time_sec));
-  task_monitor_node->Start();
   rclcpp::spin_some(task_monitor_node->get_node_base_interface());
   RCLCPP_INFO(task_monitor_node->get_logger(), "Spin");
   rclcpp::shutdown();

src/task_monitor/src/task_monitor.cpp

@@ -2,6 +2,11 @@
 
 #include "nav2_util/simple_action_server.hpp"
 #include "pose_3d.hpp"
+#include "tf2/LinearMath/Matrix3x3.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2/convert.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 
 #define LOG(level, ...) RCLCPP_##level(this->get_logger(), __VA_ARGS__)
 
@@ -26,11 +31,11 @@ TaskMonitor::TaskMonitor() : Node("task_monitor") {
   // Subscriber to get the robot pose from amcl node
   robot_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
-    std::bind(&TaskMonitor::AmclPoseCallback(), this, std::placeholders::_1));
+    std::bind(&TaskMonitor::AmclPoseCallback, this, std::placeholders::_1));
 
   // Action server to initialize navigation properly
   initialize_navigation_action_server_ = std::make_unique<InitializeNavigationActionServer>(
-    rclcpp_node_, "initialize_navigation", std::bind(&TaskMonitor::SetInitialPose, this));
+    this->create_sub_node("action_sub_node"), "initialize_navigation", std::bind(&TaskMonitor::SetInitialPose, this));
 
   initial_pose_ = GetInitialPose();
 }
@@ -56,25 +61,29 @@ geometry_msgs::msg::PoseWithCovarianceStamped TaskMonitor::GetInitialPose() {
   return initial_pose_with_covariance_msg;
 }
 
-void TaskMonitor::AmclPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
-  robot_pose_ = msg->pose;
+void TaskMonitor::AmclPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
+  if (msg) {
+    robot_pose_ = *msg;
+  }
 }
 
 bool TaskMonitor::IsRobotAtPose(geometry_msgs::msg::PoseWithCovarianceStamped desired_pose) {
   // Get the relative tf from the actual robot pose to the desired pose
-  tf2::Transform robot_pose_tf = tf2::fromMsg(robot_pose_);
-  tf2::Transform desired_pose_tf = tf2::fromMsg(desired_pose);
+  tf2::Stamped<tf2::Transform> robot_pose_tf, desired_pose_tf;
+  tf2::fromMsg(robot_pose_, robot_pose_tf);
+  tf2::fromMsg(desired_pose, desired_pose_tf);
   tf2::Transform relative_tf = robot_pose_tf.inverseTimes(desired_pose_tf);
 
   // Check that location coordinates are lower than the distance tolerance
   auto relative_translation = relative_tf.getOrigin();
-  for (auto& coordinate : relative_translation) {
+  std::vector<double> relative_translation_vector{relative_translation.getX(), relative_translation.getY(), relative_translation.getZ()};
+  for (auto &coordinate : relative_translation_vector) {
     if (coordinate > pose_distance_tolerance_)
       return false;
   }
 
   // Check that the RPY angles are lower than the angle tolerance
-  tf2::Matrix3x3 relative_orientation_matrix(relative_tf.getOrientation());
+  tf2::Matrix3x3 relative_orientation_matrix(relative_tf.getRotation());
   std::vector<double> relative_orientation_vector;
   relative_orientation_vector.reserve(3);
   relative_orientation_matrix.getRPY(relative_orientation_vector[0], relative_orientation_vector[1], relative_orientation_vector[2]);
@@ -88,13 +97,13 @@ bool TaskMonitor::IsRobotAtPose(geometry_msgs::msg::PoseWithCovarianceStamped de
 
 void TaskMonitor::SetInitialPose() {
   LOG(INFO, "Initialize navigation service called");
-  auto start_time = rclcpp::Time::now();
+  auto start_time = this->now();
   initial_pose_ = initialize_navigation_action_server_->get_current_goal()->pose;
   timeout_s_ = initialize_navigation_action_server_->get_current_goal()->timeout_s;
   initial_pose_pub_->publish(initial_pose_);
 
   // Check that the robot is at the desired pose
-  while ((rclcpp::Time::now() - start_time).seconds() < timeout_s_) {
+  while ((this->now() - start_time).seconds() < timeout_s_) {
     if (IsRobotAtPose(initial_pose_)) {
       LOG(INFO, "Robot is at initial pose");
       initialize_navigation_action_server_->succeeded_current();

src/task_msgs/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.8)
+project(task_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(action_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "action/InitializeNavigation.action"
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()