ROS2 include ros_pkg attribute support (#351)

* ROS2 include pkg support

* ros2 build fixed

Co-authored-by: Benjamin Linne <benjamin.linne.civ@army.mil>

Author: benjinne

CMakeLists.txt

@@ -83,8 +83,7 @@ find_package(ament_cmake QUIET)
 
 if ( ament_cmake_FOUND )
 
-    # Not adding -DUSING_ROS since xml_parsing.cpp hasn't been ported to ROS2
-
+    add_definitions( -DUSING_ROS2 )
     message(STATUS "------------------------------------------")
     message(STATUS "BehaviourTree is being built using AMENT.")
     message(STATUS "------------------------------------------")
@@ -116,35 +115,6 @@ elseif(BUILD_UNIT_TESTS)
 endif()
 
 
-#############################################################
-if(ament_cmake_FOUND)
-    set( BEHAVIOR_TREE_LIB_DESTINATION   lib )
-    set( BEHAVIOR_TREE_INC_DESTINATION   include )
-    set( BEHAVIOR_TREE_BIN_DESTINATION   bin )
-
-    ament_export_include_directories(include)
-    ament_export_libraries(${BEHAVIOR_TREE_LIBRARY})
-    ament_package()
-elseif(catkin_FOUND)
-    set( BEHAVIOR_TREE_LIB_DESTINATION   ${CATKIN_PACKAGE_LIB_DESTINATION} )
-    set( BEHAVIOR_TREE_INC_DESTINATION   ${CATKIN_GLOBAL_INCLUDE_DESTINATION} )
-    set( BEHAVIOR_TREE_BIN_DESTINATION   ${CATKIN_GLOBAL_BIN_DESTINATION} )
-else()
-    set( BEHAVIOR_TREE_LIB_DESTINATION   lib )
-    set( BEHAVIOR_TREE_INC_DESTINATION   include )
-    set( BEHAVIOR_TREE_BIN_DESTINATION   bin )
-
-    set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${BEHAVIOR_TREE_BIN_DESTINATION}" )
-    set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${BEHAVIOR_TREE_LIB_DESTINATION}" )
-    set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${BEHAVIOR_TREE_BIN_DESTINATION}" )
-endif()
-
-message( STATUS "BEHAVIOR_TREE_LIB_DESTINATION:   ${BEHAVIOR_TREE_LIB_DESTINATION} " )
-message( STATUS "BEHAVIOR_TREE_BIN_DESTINATION:   ${BEHAVIOR_TREE_BIN_DESTINATION} " )
-message( STATUS "CMAKE_RUNTIME_OUTPUT_DIRECTORY:  ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} " )
-message( STATUS "CMAKE_LIBRARY_OUTPUT_DIRECTORY:  ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} " )
-message( STATUS "CMAKE_ARCHIVE_OUTPUT_DIRECTORY:  ${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} " )
-
 #############################################################
 # LIBRARY
 
@@ -249,6 +219,38 @@ else()
         -Wall -Wextra -Werror=return-type)
 endif()
 
+#############################################################
+if(ament_cmake_FOUND)
+    find_package(ament_index_cpp REQUIRED)
+    ament_target_dependencies(${BEHAVIOR_TREE_LIBRARY} PUBLIC ament_index_cpp)
+
+    set( BEHAVIOR_TREE_LIB_DESTINATION   lib )
+    set( BEHAVIOR_TREE_INC_DESTINATION   include )
+    set( BEHAVIOR_TREE_BIN_DESTINATION   bin )
+
+    ament_export_include_directories(include)
+    ament_export_libraries(${BEHAVIOR_TREE_LIBRARY})
+    ament_package()
+elseif(catkin_FOUND)
+    set( BEHAVIOR_TREE_LIB_DESTINATION   ${CATKIN_PACKAGE_LIB_DESTINATION} )
+    set( BEHAVIOR_TREE_INC_DESTINATION   ${CATKIN_GLOBAL_INCLUDE_DESTINATION} )
+    set( BEHAVIOR_TREE_BIN_DESTINATION   ${CATKIN_GLOBAL_BIN_DESTINATION} )
+else()
+    set( BEHAVIOR_TREE_LIB_DESTINATION   lib )
+    set( BEHAVIOR_TREE_INC_DESTINATION   include )
+    set( BEHAVIOR_TREE_BIN_DESTINATION   bin )
+
+    set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${BEHAVIOR_TREE_BIN_DESTINATION}" )
+    set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${BEHAVIOR_TREE_LIB_DESTINATION}" )
+    set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${BEHAVIOR_TREE_BIN_DESTINATION}" )
+endif()
+
+message( STATUS "BEHAVIOR_TREE_LIB_DESTINATION:   ${BEHAVIOR_TREE_LIB_DESTINATION} " )
+message( STATUS "BEHAVIOR_TREE_BIN_DESTINATION:   ${BEHAVIOR_TREE_BIN_DESTINATION} " )
+message( STATUS "CMAKE_RUNTIME_OUTPUT_DIRECTORY:  ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} " )
+message( STATUS "CMAKE_LIBRARY_OUTPUT_DIRECTORY:  ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} " )
+message( STATUS "CMAKE_ARCHIVE_OUTPUT_DIRECTORY:  ${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} " )
+
 ######################################################
 # Samples
 if (BUILD_SAMPLES)

src/xml_parsing.cpp

@@ -30,6 +30,10 @@
 #include <ros/package.h>
 #endif
 
+#ifdef USING_ROS2
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#endif
+
 #include "behaviortree_cpp_v3/blackboard.h"
 #include "behaviortree_cpp_v3/utils/demangle_util.h"
 
@@ -137,23 +141,28 @@ void XMLParser::Pimpl::loadDocImpl(BT_TinyXML2::XMLDocument* doc)
     {
 
         filesystem::path file_path( include_node->Attribute("path") );
+        const char* ros_pkg_relative_path = include_node->Attribute("ros_pkg");
+        std::string ros_pkg_path;
 
-        if( include_node->Attribute("ros_pkg") )
+        if( ros_pkg_relative_path )
         {
-#ifdef USING_ROS
             if( file_path.is_absolute() )
             {
-                std::cout << "WARNING: <include path=\"...\"> containes an absolute path.\n"
+                std::cout << "WARNING: <include path=\"...\"> contains an absolute path.\n"
                           << "Attribute [ros_pkg] will be ignored."<< std::endl;
             }
-            else {
-                auto ros_pkg_path = ros::package::getPath(  include_node->Attribute("ros_pkg") );
-                file_path = filesystem::path( ros_pkg_path ) / file_path;
-            }
+            else 
+            {
+#ifdef USING_ROS
+            ros_pkg_path = ros::package::getPath(ros_pkg_relative_path);
+#elif defined USING_ROS2
+            ros_pkg_path = ament_index_cpp::get_package_share_directory(ros_pkg_relative_path);
 #else
             throw RuntimeError("Using attribute [ros_pkg] in <include>, but this library was compiled "
                                "without ROS support. Recompile the BehaviorTree.CPP using catkin");
 #endif
+            file_path = filesystem::path( ros_pkg_path ) / file_path;
+            }
         }
 
         if( !file_path.is_absolute() )