diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 52dc2be0..37db70aa 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -21,11 +21,11 @@ jobs: run: sudo apt-get install libfl-dev - uses: actions/checkout@v2 - name: Setup ROS 2 - uses: ros-tooling/setup-ros@0.7.9 + uses: ros-tooling/setup-ros@0.7.13 with: required-ros-distributions: rolling - name: build and test - uses: ros-tooling/action-ros-ci@0.3.15 + uses: ros-tooling/action-ros-ci@0.4.3 with: package-name: br2_basics br2_bt_bumpgo br2_bt_patrolling br2_fsm_bumpgo_cpp br2_fsm_bumpgo_py br2_navigation br2_tf2_detector br2_tiago br2_tracking br2_tracking_msgs br2_vff_avoidance target-ros2-distro: rolling @@ -35,4 +35,4 @@ jobs: "test": { "parallel-workers" : 1 } - } \ No newline at end of file + } diff --git a/br2_basics/CMakeLists.txt b/br2_basics/CMakeLists.txt index ac40bbb9..74d8c407 100644 --- a/br2_basics/CMakeLists.txt +++ b/br2_basics/CMakeLists.txt @@ -1,35 +1,37 @@ cmake_minimum_required(VERSION 3.5) project(br2_basics) +set(CMAKE_CXX_STANDARD 23) + find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) set(dependencies - rclcpp - std_msgs + rclcpp::rclcpp + ${std_msgs_TARGETS} ) add_executable(publisher src/publisher.cpp) -ament_target_dependencies(publisher ${dependencies}) +target_link_libraries(publisher ${dependencies}) add_executable(publisher_class src/publisher_class.cpp) -ament_target_dependencies(publisher_class ${dependencies}) +target_link_libraries(publisher_class ${dependencies}) add_executable(subscriber_class src/subscriber_class.cpp) -ament_target_dependencies(subscriber_class ${dependencies}) +target_link_libraries(subscriber_class ${dependencies}) add_executable(executors src/executors.cpp) -ament_target_dependencies(executors ${dependencies}) +target_link_libraries(executors ${dependencies}) add_executable(logger src/logger.cpp) -ament_target_dependencies(logger ${dependencies}) +target_link_libraries(logger ${dependencies}) add_executable(logger_class src/logger_class.cpp) -ament_target_dependencies(logger_class ${dependencies}) +target_link_libraries(logger_class ${dependencies}) add_executable(param_reader src/param_reader.cpp) -ament_target_dependencies(param_reader ${dependencies}) +target_link_libraries(param_reader ${dependencies}) install(TARGETS publisher @@ -51,5 +53,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_export_dependencies(${dependencies}) ament_package() diff --git a/br2_bt_bumpgo/CMakeLists.txt b/br2_bt_bumpgo/CMakeLists.txt index b1c4aa3f..0c29ab7c 100644 --- a/br2_bt_bumpgo/CMakeLists.txt +++ b/br2_bt_bumpgo/CMakeLists.txt @@ -1,36 +1,35 @@ cmake_minimum_required(VERSION 3.5) project(br2_bt_bumpgo) +set(CMAKE_CXX_STANDARD 23) + set(CMAKE_CONFIG_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") list(APPEND CMAKE_MODULE_PATH "${CMAKE_CONFIG_PATH}") find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(behaviortree_cpp_v3 REQUIRED) +find_package(behaviortree_cpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(ament_index_cpp REQUIRED) -find_package(ZMQ) -if(ZMQ_FOUND) - message(STATUS "ZeroMQ found.") - add_definitions(-DZMQ_FOUND) -else() - message(WARNING "ZeroMQ NOT found. Not including PublisherZMQ.") -endif() - -set(CMAKE_CXX_STANDARD 17) set(dependencies + rclcpp::rclcpp + behaviortree_cpp::behaviortree_cpp + ament_index_cpp::ament_index_cpp + ${sensor_msgs_TARGETS} + ${geometry_msgs_TARGETS} +) + +set(export_dependencies rclcpp - behaviortree_cpp_v3 + behaviortree_cpp + ament_index_cpp sensor_msgs geometry_msgs - ament_index_cpp ) -include_directories(include ${ZMQ_INCLUDE_DIRS}) - add_library(br2_forward_bt_node SHARED src/br2_bt_bumpgo/Forward.cpp) add_library(br2_back_bt_node SHARED src/br2_bt_bumpgo/Back.cpp) add_library(br2_turn_bt_node SHARED src/br2_bt_bumpgo/Turn.cpp) @@ -44,24 +43,28 @@ list(APPEND plugin_libs ) foreach(bt_plugin ${plugin_libs}) - ament_target_dependencies(${bt_plugin} ${dependencies}) + target_include_directories(${bt_plugin} PUBLIC + $ + $ + ) + target_link_libraries(${bt_plugin} ${dependencies}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) endforeach() add_executable(bt_bumpgo src/bt_bumpgo_main.cpp) -ament_target_dependencies(bt_bumpgo ${dependencies}) -target_link_libraries(bt_bumpgo ${ZMQ_LIBRARIES}) +target_link_libraries(bt_bumpgo ${dependencies}) install(TARGETS ${plugin_libs} bt_bumpgo + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY behavior_tree_xml @@ -81,7 +84,9 @@ if(BUILD_TESTING) add_subdirectory(tests) endif() -ament_export_include_directories(include) -ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${bt_plugin}) +ament_export_dependencies(${export_dependencies}) +ament_export_targets(export_${PROJECT_NAME}) ament_package() diff --git a/br2_bt_bumpgo/behavior_tree_xml/bumpgo.xml b/br2_bt_bumpgo/behavior_tree_xml/bumpgo.xml index 1efe0046..48f07693 100644 --- a/br2_bt_bumpgo/behavior_tree_xml/bumpgo.xml +++ b/br2_bt_bumpgo/behavior_tree_xml/bumpgo.xml @@ -1,5 +1,5 @@ - + diff --git a/br2_bt_bumpgo/include/br2_bt_bumpgo/Back.hpp b/br2_bt_bumpgo/include/br2_bt_bumpgo/Back.hpp index 1b69fa6d..ab7d465e 100644 --- a/br2_bt_bumpgo/include/br2_bt_bumpgo/Back.hpp +++ b/br2_bt_bumpgo/include/br2_bt_bumpgo/Back.hpp @@ -17,8 +17,8 @@ #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" #include "geometry_msgs/msg/twist.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/br2_bt_bumpgo/include/br2_bt_bumpgo/Forward.hpp b/br2_bt_bumpgo/include/br2_bt_bumpgo/Forward.hpp index 27e82013..760d2cde 100644 --- a/br2_bt_bumpgo/include/br2_bt_bumpgo/Forward.hpp +++ b/br2_bt_bumpgo/include/br2_bt_bumpgo/Forward.hpp @@ -17,8 +17,8 @@ #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" #include "geometry_msgs/msg/twist.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/br2_bt_bumpgo/include/br2_bt_bumpgo/IsObstacle.hpp b/br2_bt_bumpgo/include/br2_bt_bumpgo/IsObstacle.hpp index 4242052f..c2efcfe4 100644 --- a/br2_bt_bumpgo/include/br2_bt_bumpgo/IsObstacle.hpp +++ b/br2_bt_bumpgo/include/br2_bt_bumpgo/IsObstacle.hpp @@ -17,8 +17,8 @@ #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" #include "sensor_msgs/msg/laser_scan.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/br2_bt_bumpgo/include/br2_bt_bumpgo/Turn.hpp b/br2_bt_bumpgo/include/br2_bt_bumpgo/Turn.hpp index d4b40a11..38a3d9ad 100644 --- a/br2_bt_bumpgo/include/br2_bt_bumpgo/Turn.hpp +++ b/br2_bt_bumpgo/include/br2_bt_bumpgo/Turn.hpp @@ -17,8 +17,8 @@ #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" #include "geometry_msgs/msg/twist.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/br2_bt_bumpgo/package.xml b/br2_bt_bumpgo/package.xml index 5a9d2bb6..e671bef5 100644 --- a/br2_bt_bumpgo/package.xml +++ b/br2_bt_bumpgo/package.xml @@ -10,7 +10,7 @@ ament_cmake rclcpp - behaviortree_cpp_v3 + behaviortree_cpp sensor_msgs geometry_msgs libzmq3-dev diff --git a/br2_bt_bumpgo/src/br2_bt_bumpgo/Back.cpp b/br2_bt_bumpgo/src/br2_bt_bumpgo/Back.cpp index 4f306aa4..43901a4b 100644 --- a/br2_bt_bumpgo/src/br2_bt_bumpgo/Back.cpp +++ b/br2_bt_bumpgo/src/br2_bt_bumpgo/Back.cpp @@ -17,7 +17,7 @@ #include "br2_bt_bumpgo/Back.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/twist.hpp" #include "rclcpp/rclcpp.hpp" @@ -32,7 +32,7 @@ Back::Back( const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf) { - config().blackboard->get("node", node_); + [[maybe_unused]] bool success = config().blackboard->get("node", node_); vel_pub_ = node_->create_publisher("/output_vel", 100); } @@ -64,7 +64,7 @@ Back::tick() } // namespace br2_bt_bumpgo -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("Back"); diff --git a/br2_bt_bumpgo/src/br2_bt_bumpgo/Forward.cpp b/br2_bt_bumpgo/src/br2_bt_bumpgo/Forward.cpp index 6ee181f2..b9470260 100644 --- a/br2_bt_bumpgo/src/br2_bt_bumpgo/Forward.cpp +++ b/br2_bt_bumpgo/src/br2_bt_bumpgo/Forward.cpp @@ -17,7 +17,7 @@ #include "br2_bt_bumpgo/Forward.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/twist.hpp" #include "rclcpp/rclcpp.hpp" @@ -32,7 +32,7 @@ Forward::Forward( const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf) { - config().blackboard->get("node", node_); + [[maybe_unused]] bool success = config().blackboard->get("node", node_); vel_pub_ = node_->create_publisher("/output_vel", 100); } @@ -49,7 +49,7 @@ Forward::tick() } // namespace br2_bt_bumpgo -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("Forward"); diff --git a/br2_bt_bumpgo/src/br2_bt_bumpgo/IsObstacle.cpp b/br2_bt_bumpgo/src/br2_bt_bumpgo/IsObstacle.cpp index b9bf2935..64fe6b9f 100644 --- a/br2_bt_bumpgo/src/br2_bt_bumpgo/IsObstacle.cpp +++ b/br2_bt_bumpgo/src/br2_bt_bumpgo/IsObstacle.cpp @@ -17,7 +17,7 @@ #include "br2_bt_bumpgo/IsObstacle.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" #include "sensor_msgs/msg/laser_scan.hpp" #include "rclcpp/rclcpp.hpp" @@ -33,7 +33,7 @@ IsObstacle::IsObstacle( const BT::NodeConfiguration & conf) : BT::ConditionNode(xml_tag_name, conf) { - config().blackboard->get("node", node_); + [[maybe_unused]] bool success = config().blackboard->get("node", node_); laser_sub_ = node_->create_subscription( "/input_scan", 100, std::bind(&IsObstacle::laser_callback, this, _1)); @@ -66,7 +66,7 @@ IsObstacle::tick() } // namespace br2_bt_bumpgo -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsObstacle"); diff --git a/br2_bt_bumpgo/src/br2_bt_bumpgo/Turn.cpp b/br2_bt_bumpgo/src/br2_bt_bumpgo/Turn.cpp index e54fa678..b334e2fe 100644 --- a/br2_bt_bumpgo/src/br2_bt_bumpgo/Turn.cpp +++ b/br2_bt_bumpgo/src/br2_bt_bumpgo/Turn.cpp @@ -17,7 +17,7 @@ #include "br2_bt_bumpgo/Turn.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/twist.hpp" #include "rclcpp/rclcpp.hpp" @@ -32,7 +32,7 @@ Turn::Turn( const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf) { - config().blackboard->get("node", node_); + [[maybe_unused]] bool success = config().blackboard->get("node", node_); vel_pub_ = node_->create_publisher("/output_vel", 100); } @@ -64,7 +64,7 @@ Turn::tick() } // namespace br2_bt_bumpgo -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("Turn"); diff --git a/br2_bt_bumpgo/src/bt_bumpgo_main.cpp b/br2_bt_bumpgo/src/bt_bumpgo_main.cpp index 9bb93d18..2d94deee 100644 --- a/br2_bt_bumpgo/src/bt_bumpgo_main.cpp +++ b/br2_bt_bumpgo/src/bt_bumpgo_main.cpp @@ -15,10 +15,9 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -46,13 +45,11 @@ int main(int argc, char * argv[]) blackboard->set("node", node); BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard); - auto publisher_zmq = std::make_shared(tree, 10, 1666, 1667); - rclcpp::Rate rate(10); bool finish = false; while (!finish && rclcpp::ok()) { - finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING; + finish = tree.tickOnce() != BT::NodeStatus::RUNNING; rclcpp::spin_some(node); rate.sleep(); diff --git a/br2_bt_bumpgo/tests/CMakeLists.txt b/br2_bt_bumpgo/tests/CMakeLists.txt index 80b377d7..97c3acfd 100644 --- a/br2_bt_bumpgo/tests/CMakeLists.txt +++ b/br2_bt_bumpgo/tests/CMakeLists.txt @@ -1,10 +1,9 @@ ament_add_gtest(bt_action_test bt_action_test.cpp) -ament_target_dependencies(bt_action_test ${dependencies}) +target_link_libraries(bt_action_test ${dependencies}) add_executable(bt_forward bt_forward_main.cpp) -ament_target_dependencies(bt_forward ${dependencies}) -target_link_libraries(bt_forward ${ZMQ_LIBRARIES}) +target_link_libraries(bt_forward ${dependencies}) install(TARGETS bt_forward diff --git a/br2_bt_bumpgo/tests/bt_action_test.cpp b/br2_bt_bumpgo/tests/bt_action_test.cpp index e3d3c132..8f5c2a37 100644 --- a/br2_bt_bumpgo/tests/bt_action_test.cpp +++ b/br2_bt_bumpgo/tests/bt_action_test.cpp @@ -18,9 +18,9 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -69,7 +69,7 @@ TEST(bt_action, turn_btn) std::string xml_bt = R"( - + @@ -82,7 +82,7 @@ TEST(bt_action, turn_btn) rclcpp::Rate rate(10); bool finish = false; while (!finish && rclcpp::ok()) { - finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + finish = tree.tickOnce() == BT::NodeStatus::SUCCESS; rclcpp::spin_some(node_sink); rate.sleep(); } @@ -108,7 +108,7 @@ TEST(bt_action, back_btn) std::string xml_bt = R"( - + @@ -121,7 +121,7 @@ TEST(bt_action, back_btn) rclcpp::Rate rate(10); bool finish = false; while (!finish && rclcpp::ok()) { - finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + finish = tree.tickOnce() == BT::NodeStatus::SUCCESS; rclcpp::spin_some(node_sink); rate.sleep(); } @@ -147,7 +147,7 @@ TEST(bt_action, forward_btn) std::string xml_bt = R"( - + @@ -161,7 +161,7 @@ TEST(bt_action, forward_btn) auto current_status = BT::NodeStatus::FAILURE; int counter = 0; while (counter++ < 30 && rclcpp::ok()) { - current_status = tree.rootNode()->executeTick(); + current_status = tree.tickOnce(); rclcpp::spin_some(node_sink); rate.sleep(); } @@ -188,7 +188,7 @@ TEST(bt_action, is_obstacle_btn) std::string xml_bt = R"( - + @@ -208,7 +208,7 @@ TEST(bt_action, is_obstacle_btn) rate.sleep(); } - BT::NodeStatus current_status = tree.rootNode()->executeTick(); + BT::NodeStatus current_status = tree.tickOnce(); ASSERT_EQ(current_status, BT::NodeStatus::FAILURE); scan.ranges[0] = 0.3; @@ -218,12 +218,12 @@ TEST(bt_action, is_obstacle_btn) rate.sleep(); } - current_status = tree.rootNode()->executeTick(); + current_status = tree.tickOnce(); ASSERT_EQ(current_status, BT::NodeStatus::SUCCESS); xml_bt = R"( - + @@ -237,7 +237,7 @@ TEST(bt_action, is_obstacle_btn) rate.sleep(); } - current_status = tree.rootNode()->executeTick(); + current_status = tree.tickOnce(); ASSERT_EQ(current_status, BT::NodeStatus::SUCCESS); scan.ranges[0] = 0.6; @@ -247,7 +247,7 @@ TEST(bt_action, is_obstacle_btn) rate.sleep(); } - current_status = tree.rootNode()->executeTick(); + current_status = tree.tickOnce(); ASSERT_EQ(current_status, BT::NodeStatus::FAILURE); } diff --git a/br2_bt_bumpgo/tests/bt_forward_main.cpp b/br2_bt_bumpgo/tests/bt_forward_main.cpp index 39258b1f..4d9bbeb2 100644 --- a/br2_bt_bumpgo/tests/bt_forward_main.cpp +++ b/br2_bt_bumpgo/tests/bt_forward_main.cpp @@ -15,10 +15,9 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -38,7 +37,7 @@ int main(int argc, char * argv[]) std::string xml_bt = R"( - + @@ -51,7 +50,7 @@ int main(int argc, char * argv[]) rclcpp::Rate rate(10); bool finish = false; while (!finish && rclcpp::ok()) { - finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING; + finish = tree.tickOnce() != BT::NodeStatus::RUNNING; rclcpp::spin_some(node); rate.sleep(); diff --git a/br2_bt_patrolling/CMakeLists.txt b/br2_bt_patrolling/CMakeLists.txt index fca66242..0b680465 100644 --- a/br2_bt_patrolling/CMakeLists.txt +++ b/br2_bt_patrolling/CMakeLists.txt @@ -10,28 +10,32 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(rclcpp_action REQUIRED) -find_package(behaviortree_cpp_v3 REQUIRED) +find_package(behaviortree_cpp REQUIRED) find_package(action_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_msgs REQUIRED) find_package(ament_index_cpp REQUIRED) -find_package(ZMQ) -if(ZMQ_FOUND) - message(STATUS "ZeroMQ found.") - add_definitions(-DZMQ_FOUND) -else() - message(WARNING "ZeroMQ NOT found. Not including PublisherZMQ.") -endif() - -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 23) set(dependencies + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + rclcpp_action::rclcpp_action + behaviortree_cpp::behaviortree_cpp + ament_index_cpp::ament_index_cpp + ${action_msgs_TARGETS} + ${lifecycle_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${nav2_msgs_TARGETS} +) + +set(export_dependencies rclcpp rclcpp_lifecycle rclcpp_action - behaviortree_cpp_v3 + behaviortree_cpp action_msgs lifecycle_msgs geometry_msgs @@ -39,8 +43,6 @@ set(dependencies ament_index_cpp ) -include_directories(include ${ZMQ_INCLUDE_DIRS}) - add_library(br2_recharge_bt_node SHARED src/br2_bt_patrolling/Recharge.cpp) add_library(br2_patrol_bt_node SHARED src/br2_bt_patrolling/Patrol.cpp) add_library(br2_move_bt_node SHARED src/br2_bt_patrolling/Move.cpp) @@ -57,24 +59,29 @@ list(APPEND plugin_libs ) foreach(bt_plugin ${plugin_libs}) - ament_target_dependencies(${bt_plugin} ${dependencies}) + target_include_directories(${bt_plugin} PUBLIC + $ + $ + ) + target_link_libraries(${bt_plugin} ${dependencies}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) endforeach() add_executable(patrolling_main src/patrolling_main.cpp) -ament_target_dependencies(patrolling_main ${dependencies}) -target_link_libraries(patrolling_main ${ZMQ_LIBRARIES}) +target_link_libraries(patrolling_main ${dependencies}) install(TARGETS ${plugin_libs} patrolling_main + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY include/ - DESTINATION include/ +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY behavior_tree_xml launch @@ -94,7 +101,9 @@ if(BUILD_TESTING) add_subdirectory(tests) endif() -ament_export_include_directories(include) -ament_export_dependencies(${dependencies}) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${bt_plugin}) +ament_export_dependencies(${export_dependencies}) +ament_export_targets(export_${PROJECT_NAME}) ament_package() diff --git a/br2_bt_patrolling/behavior_tree_xml/patrolling.xml b/br2_bt_patrolling/behavior_tree_xml/patrolling.xml index 4ac3850c..e4d0c329 100644 --- a/br2_bt_patrolling/behavior_tree_xml/patrolling.xml +++ b/br2_bt_patrolling/behavior_tree_xml/patrolling.xml @@ -1,5 +1,5 @@ - + @@ -14,7 +14,7 @@ - + diff --git a/br2_bt_patrolling/include/br2_bt_patrolling/BatteryChecker.hpp b/br2_bt_patrolling/include/br2_bt_patrolling/BatteryChecker.hpp index 694f1227..b0407381 100644 --- a/br2_bt_patrolling/include/br2_bt_patrolling/BatteryChecker.hpp +++ b/br2_bt_patrolling/include/br2_bt_patrolling/BatteryChecker.hpp @@ -18,8 +18,8 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" #include "geometry_msgs/msg/twist.hpp" diff --git a/br2_bt_patrolling/include/br2_bt_patrolling/GetWaypoint.hpp b/br2_bt_patrolling/include/br2_bt_patrolling/GetWaypoint.hpp index fc74ba1a..899b11bb 100644 --- a/br2_bt_patrolling/include/br2_bt_patrolling/GetWaypoint.hpp +++ b/br2_bt_patrolling/include/br2_bt_patrolling/GetWaypoint.hpp @@ -18,8 +18,8 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/br2_bt_patrolling/include/br2_bt_patrolling/Move.hpp b/br2_bt_patrolling/include/br2_bt_patrolling/Move.hpp index b4375e7d..ed0d569f 100644 --- a/br2_bt_patrolling/include/br2_bt_patrolling/Move.hpp +++ b/br2_bt_patrolling/include/br2_bt_patrolling/Move.hpp @@ -21,8 +21,8 @@ #include "nav2_msgs/action/navigate_to_pose.hpp" #include "br2_bt_patrolling/ctrl_support/BTActionNode.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" namespace br2_bt_patrolling { diff --git a/br2_bt_patrolling/include/br2_bt_patrolling/Patrol.hpp b/br2_bt_patrolling/include/br2_bt_patrolling/Patrol.hpp index 29f46437..0ca9b8be 100644 --- a/br2_bt_patrolling/include/br2_bt_patrolling/Patrol.hpp +++ b/br2_bt_patrolling/include/br2_bt_patrolling/Patrol.hpp @@ -17,8 +17,8 @@ #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" #include "geometry_msgs/msg/twist.hpp" diff --git a/br2_bt_patrolling/include/br2_bt_patrolling/Recharge.hpp b/br2_bt_patrolling/include/br2_bt_patrolling/Recharge.hpp index 48a9d9ce..f436829d 100644 --- a/br2_bt_patrolling/include/br2_bt_patrolling/Recharge.hpp +++ b/br2_bt_patrolling/include/br2_bt_patrolling/Recharge.hpp @@ -17,8 +17,8 @@ #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" namespace br2_bt_patrolling { diff --git a/br2_bt_patrolling/include/br2_bt_patrolling/TrackObjects.hpp b/br2_bt_patrolling/include/br2_bt_patrolling/TrackObjects.hpp index e3e30830..05860cd9 100644 --- a/br2_bt_patrolling/include/br2_bt_patrolling/TrackObjects.hpp +++ b/br2_bt_patrolling/include/br2_bt_patrolling/TrackObjects.hpp @@ -21,8 +21,8 @@ #include "nav2_msgs/action/navigate_to_pose.hpp" #include "br2_bt_patrolling/ctrl_support/BTLifecycleCtrlNode.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" namespace br2_bt_patrolling { diff --git a/br2_bt_patrolling/include/br2_bt_patrolling/ctrl_support/BTActionNode.hpp b/br2_bt_patrolling/include/br2_bt_patrolling/ctrl_support/BTActionNode.hpp index 82b3c29a..8a8795b1 100644 --- a/br2_bt_patrolling/include/br2_bt_patrolling/ctrl_support/BTActionNode.hpp +++ b/br2_bt_patrolling/include/br2_bt_patrolling/ctrl_support/BTActionNode.hpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -194,7 +194,7 @@ class BtActionNode : public BT::ActionNodeBase } } - setStatus(BT::NodeStatus::IDLE); + resetStatus(); } protected: diff --git a/br2_bt_patrolling/include/br2_bt_patrolling/ctrl_support/BTLifecycleCtrlNode.hpp b/br2_bt_patrolling/include/br2_bt_patrolling/ctrl_support/BTLifecycleCtrlNode.hpp index 730b4cad..53d40968 100644 --- a/br2_bt_patrolling/include/br2_bt_patrolling/ctrl_support/BTLifecycleCtrlNode.hpp +++ b/br2_bt_patrolling/include/br2_bt_patrolling/ctrl_support/BTLifecycleCtrlNode.hpp @@ -22,7 +22,7 @@ #include "lifecycle_msgs/srv/get_state.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" namespace br2_bt_patrolling @@ -98,7 +98,7 @@ class BtLifecycleCtrlNode : public BT::ActionNodeBase if (ctrl_node_state_ == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { set_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); } - setStatus(BT::NodeStatus::IDLE); + resetStatus(); } // Get the state of the controlled node diff --git a/br2_bt_patrolling/package.xml b/br2_bt_patrolling/package.xml index 7a4c3ec9..384a5617 100644 --- a/br2_bt_patrolling/package.xml +++ b/br2_bt_patrolling/package.xml @@ -12,7 +12,7 @@ rclcpp rclcpp_lifecycle rclcpp_action - behaviortree_cpp_v3 + behaviortree_cpp action_msgs geometry_msgs lifecycle_msgs diff --git a/br2_bt_patrolling/src/br2_bt_patrolling/BatteryChecker.cpp b/br2_bt_patrolling/src/br2_bt_patrolling/BatteryChecker.cpp index 10befbcb..049bf803 100644 --- a/br2_bt_patrolling/src/br2_bt_patrolling/BatteryChecker.cpp +++ b/br2_bt_patrolling/src/br2_bt_patrolling/BatteryChecker.cpp @@ -18,7 +18,7 @@ #include "br2_bt_patrolling/BatteryChecker.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/twist.hpp" @@ -35,7 +35,7 @@ BatteryChecker::BatteryChecker( const BT::NodeConfiguration & conf) : BT::ConditionNode(xml_tag_name, conf) { - config().blackboard->get("node", node_); + [[maybe_unused]] bool success = config().blackboard->get("node", node_); vel_sub_ = node_->create_subscription( "/output_vel", 100, std::bind(&BatteryChecker::vel_callback, this, _1)); @@ -87,7 +87,7 @@ BatteryChecker::tick() } // namespace br2_bt_patrolling -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("BatteryChecker"); diff --git a/br2_bt_patrolling/src/br2_bt_patrolling/GetWaypoint.cpp b/br2_bt_patrolling/src/br2_bt_patrolling/GetWaypoint.cpp index 0fde6d8d..2d759eef 100644 --- a/br2_bt_patrolling/src/br2_bt_patrolling/GetWaypoint.cpp +++ b/br2_bt_patrolling/src/br2_bt_patrolling/GetWaypoint.cpp @@ -18,7 +18,7 @@ #include "br2_bt_patrolling/GetWaypoint.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -34,9 +34,6 @@ GetWaypoint::GetWaypoint( const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf) { - rclcpp::Node::SharedPtr node; - config().blackboard->get("node", node); - geometry_msgs::msg::PoseStamped wp; wp.header.frame_id = "map"; wp.pose.orientation.w = 1.0; @@ -85,7 +82,7 @@ GetWaypoint::tick() } // namespace br2_bt_patrolling -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GetWaypoint"); diff --git a/br2_bt_patrolling/src/br2_bt_patrolling/Move.cpp b/br2_bt_patrolling/src/br2_bt_patrolling/Move.cpp index e3d5074a..590a299a 100644 --- a/br2_bt_patrolling/src/br2_bt_patrolling/Move.cpp +++ b/br2_bt_patrolling/src/br2_bt_patrolling/Move.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" namespace br2_bt_patrolling { @@ -56,7 +56,7 @@ Move::on_success() } // namespace br2_bt_patrolling -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/br2_bt_patrolling/src/br2_bt_patrolling/Patrol.cpp b/br2_bt_patrolling/src/br2_bt_patrolling/Patrol.cpp index 6b3086dc..d402632e 100644 --- a/br2_bt_patrolling/src/br2_bt_patrolling/Patrol.cpp +++ b/br2_bt_patrolling/src/br2_bt_patrolling/Patrol.cpp @@ -17,7 +17,7 @@ #include "br2_bt_patrolling/Patrol.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/twist.hpp" @@ -33,7 +33,7 @@ Patrol::Patrol( const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf) { - config().blackboard->get("node", node_); + [[maybe_unused]] bool success = config().blackboard->get("node", node_); vel_pub_ = node_->create_publisher("/output_vel", 100); } @@ -66,7 +66,7 @@ Patrol::tick() } // namespace br2_bt_patrolling -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("Patrol"); diff --git a/br2_bt_patrolling/src/br2_bt_patrolling/Recharge.cpp b/br2_bt_patrolling/src/br2_bt_patrolling/Recharge.cpp index e3267a13..44090e9d 100644 --- a/br2_bt_patrolling/src/br2_bt_patrolling/Recharge.cpp +++ b/br2_bt_patrolling/src/br2_bt_patrolling/Recharge.cpp @@ -18,7 +18,7 @@ #include "br2_bt_patrolling/Recharge.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" namespace br2_bt_patrolling { @@ -51,7 +51,7 @@ Recharge::tick() } // namespace br2_bt_patrolling -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("Recharge"); diff --git a/br2_bt_patrolling/src/br2_bt_patrolling/TrackObjects.cpp b/br2_bt_patrolling/src/br2_bt_patrolling/TrackObjects.cpp index 6fc4600f..c96e64a1 100644 --- a/br2_bt_patrolling/src/br2_bt_patrolling/TrackObjects.cpp +++ b/br2_bt_patrolling/src/br2_bt_patrolling/TrackObjects.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" namespace br2_bt_patrolling { @@ -37,7 +37,7 @@ TrackObjects::TrackObjects( } // namespace br2_bt_patrolling -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/br2_bt_patrolling/src/patrolling_main.cpp b/br2_bt_patrolling/src/patrolling_main.cpp index abac053a..b7fc1699 100644 --- a/br2_bt_patrolling/src/patrolling_main.cpp +++ b/br2_bt_patrolling/src/patrolling_main.cpp @@ -15,10 +15,9 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -48,13 +47,11 @@ int main(int argc, char * argv[]) blackboard->set("node", node); BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard); - auto publisher_zmq = std::make_shared(tree, 10, 2666, 2667); - rclcpp::Rate rate(10); bool finish = false; while (!finish && rclcpp::ok()) { - finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + finish = tree.tickOnce() == BT::NodeStatus::SUCCESS; rclcpp::spin_some(node); rate.sleep(); diff --git a/br2_bt_patrolling/tests/CMakeLists.txt b/br2_bt_patrolling/tests/CMakeLists.txt index 810b1cb2..f4375a4d 100644 --- a/br2_bt_patrolling/tests/CMakeLists.txt +++ b/br2_bt_patrolling/tests/CMakeLists.txt @@ -1,4 +1,3 @@ ament_add_gtest(bt_action_test bt_action_test.cpp) -ament_target_dependencies(bt_action_test ${dependencies}) -target_link_libraries(bt_action_test br2_track_objects_bt_node) +target_link_libraries(bt_action_test br2_track_objects_bt_node ${dependencies}) diff --git a/br2_bt_patrolling/tests/bt_action_test.cpp b/br2_bt_patrolling/tests/bt_action_test.cpp index aeb43c59..d93e021b 100644 --- a/br2_bt_patrolling/tests/bt_action_test.cpp +++ b/br2_bt_patrolling/tests/bt_action_test.cpp @@ -18,9 +18,9 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -157,7 +157,7 @@ TEST(bt_action, recharge_btn) std::string xml_bt = R"( - + @@ -171,7 +171,7 @@ TEST(bt_action, recharge_btn) bool finish = false; while (!finish && rclcpp::ok()) { - finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + finish = tree.tickOnce() == BT::NodeStatus::SUCCESS; rate.sleep(); } @@ -192,7 +192,7 @@ TEST(bt_action, patrol_btn) std::string xml_bt = R"( - + @@ -207,7 +207,7 @@ TEST(bt_action, patrol_btn) bool finish = false; int counter = 0; while (!finish && rclcpp::ok()) { - finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + finish = tree.tickOnce() == BT::NodeStatus::SUCCESS; rclcpp::spin_some(node_sink->get_node_base_interface()); rate.sleep(); } @@ -241,7 +241,7 @@ TEST(bt_action, move_btn) std::string xml_bt = R"( - + @@ -259,7 +259,7 @@ TEST(bt_action, move_btn) int counter = 0; while (!finish && rclcpp::ok()) { - finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + finish = tree.tickOnce() == BT::NodeStatus::SUCCESS; rate.sleep(); } @@ -280,7 +280,7 @@ TEST(bt_action, get_waypoint_btn) std::string xml_bt = R"( - + @@ -297,7 +297,7 @@ TEST(bt_action, get_waypoint_btn) bool finish = false; int counter = 0; while (!finish && rclcpp::ok()) { - finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + finish = tree.tickOnce() == BT::NodeStatus::SUCCESS; counter++; rate.sleep(); } @@ -318,7 +318,7 @@ TEST(bt_action, get_waypoint_btn) std::string xml_bt = R"( - + @@ -348,7 +348,7 @@ TEST(bt_action, get_waypoint_btn) bool finish = false; while (!finish && rclcpp::ok()) { - finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + finish = tree.tickOnce() == BT::NodeStatus::SUCCESS; rate.sleep(); } @@ -386,7 +386,7 @@ TEST(bt_action, battery_checker_btn) std::string xml_bt = R"( - + @@ -406,7 +406,7 @@ TEST(bt_action, battery_checker_btn) bool finish = false; int counter = 0; while (!finish && rclcpp::ok()) { - finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + finish = tree.tickOnce() == BT::NodeStatus::SUCCESS; vel_pub->publish(vel); @@ -547,7 +547,7 @@ TEST(bt_action, track_objects_btn_3) std::string xml_bt = R"( - + @@ -568,7 +568,7 @@ TEST(bt_action, track_objects_btn_3) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); while (rclcpp::ok() && (node->now() - start) < 1s) { - tree.rootNode()->executeTick() == BT::NodeStatus::RUNNING; + tree.tickOnce() == BT::NodeStatus::RUNNING; rclcpp::spin_some(node); rate.sleep(); @@ -618,9 +618,9 @@ TEST(bt_action, move_track_btn) std::string xml_bt = R"( - + - + @@ -643,7 +643,7 @@ TEST(bt_action, move_track_btn) auto start = node->now(); auto finish_tree = false; while (rclcpp::ok() && (node->now() - start) < 1s) { - finish_tree = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + finish_tree = tree.tickOnce() == BT::NodeStatus::SUCCESS; rclcpp::spin_some(node); rate.sleep(); @@ -655,7 +655,7 @@ TEST(bt_action, move_track_btn) lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); while (rclcpp::ok() && !finish_tree) { - finish_tree = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS; + finish_tree = tree.tickOnce() == BT::NodeStatus::SUCCESS; rclcpp::spin_some(node); rate.sleep(); diff --git a/br2_deep_ros/CMakeLists.txt b/br2_deep_ros/CMakeLists.txt new file mode 100644 index 00000000..98a299a5 --- /dev/null +++ b/br2_deep_ros/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.5) +project(br2_deep_ros) + + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(vision_msgs REQUIRED) +find_package(yaets REQUIRED) + +set(dependencies + rclcpp::rclcpp + yaets::yaets + ${std_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${vision_msgs_TARGETS} +) + +include_directories(include) + +add_executable(executors src/executors.cpp) +target_link_libraries(executors ${dependencies}) + +add_executable(executors_cbg src/executors_cbg.cpp) +target_link_libraries(executors_cbg ${dependencies}) + +add_executable(executors_cbg_reentrant src/executors_cbg_reentrant.cpp) +target_link_libraries(executors_cbg_reentrant ${dependencies}) + +add_executable(wakeup src/wakeup.cpp) +target_link_libraries(wakeup ${dependencies}) + +add_executable(wakeup_ros src/wakeup_ros.cpp) +target_link_libraries(wakeup_ros ${dependencies}) + +add_executable(strategy_1 src/strategy_1.cpp) +target_link_libraries(strategy_1 ${dependencies}) + +add_executable(strategy_2 src/strategy_2.cpp) +target_link_libraries(strategy_2 ${dependencies}) + +add_executable(strategy_3 src/strategy_3.cpp) +target_link_libraries(strategy_3 ${dependencies}) + +install(TARGETS + executors + executors_cbg + executors_cbg_reentrant + wakeup + wakeup_ros + strategy_1 + strategy_2 + strategy_3 + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/br2_deep_ros/package.xml b/br2_deep_ros/package.xml new file mode 100644 index 00000000..9e3bcefb --- /dev/null +++ b/br2_deep_ros/package.xml @@ -0,0 +1,24 @@ + + + + br2_deep_ros + 0.2.0 + Basic nodes for ROS2 introduction + fmrico + Apache License, Version 2.0 + + ament_cmake + + rclcpp + std_msgs + sensor_msgs + vision_msgs + yaets + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/br2_deep_ros/src/executors.cpp b/br2_deep_ros/src/executors.cpp new file mode 100644 index 00000000..5797ba49 --- /dev/null +++ b/br2_deep_ros/src/executors.cpp @@ -0,0 +1,115 @@ +// Copyright 2024 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "yaets/tracing.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +using namespace std::chrono_literals; +using std::placeholders::_1; + + +yaets::TraceSession session("session1.log"); + +class ProducerNode : public rclcpp::Node +{ +public: + ProducerNode() : Node("producer_node") + { + pub_1_ = create_publisher("topic_1", 100); + pub_2_ = create_publisher("topic_2", 100); + timer_ = create_wall_timer(1ms, std::bind(&ProducerNode::timer_callback, this)); + } + + void timer_callback() + { + message_.data += 1; + pub_1_->publish(message_); + message_.data += 1; + pub_2_->publish(message_); + } + +private: + rclcpp::Publisher::SharedPtr pub_1_, pub_2_; + rclcpp::TimerBase::SharedPtr timer_; + std_msgs::msg::Int32 message_; +}; + +class ConsumerNode : public rclcpp::Node +{ +public: + ConsumerNode() : Node("consumer_node") + { + sub_2_ = create_subscription( + "topic_2", 100, std::bind(&ConsumerNode::cb_2, this, _1)); + sub_1_ = create_subscription( + "topic_1", 100, std::bind(&ConsumerNode::cb_1, this, _1)); + + timer_ = create_wall_timer(10ms, std::bind(&ConsumerNode::timer_cb, this)); + } + + void cb_1(const std_msgs::msg::Int32::SharedPtr msg) + { + TRACE_EVENT(session); + + waste_time(500us); + } + + void cb_2(const std_msgs::msg::Int32::SharedPtr msg) + { + TRACE_EVENT(session); + + waste_time(500us); + } + + void timer_cb() + { + TRACE_EVENT(session); + + waste_time(5ms); + } + + void waste_time(const rclcpp::Duration & duration) + { + auto start = now(); + while (now() - start < duration); + } + +private: + rclcpp::Subscription::SharedPtr sub_1_; + rclcpp::Subscription::SharedPtr sub_2_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node_pub = std::make_shared(); + auto node_sub1 = std::make_shared(); + + rclcpp::executors::SingleThreadedExecutor executor; + // rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 8); + + executor.add_node(node_pub); + executor.add_node(node_sub1); + + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/br2_deep_ros/src/executors_cbg.cpp b/br2_deep_ros/src/executors_cbg.cpp new file mode 100644 index 00000000..b1d6c78b --- /dev/null +++ b/br2_deep_ros/src/executors_cbg.cpp @@ -0,0 +1,121 @@ +// Copyright 2024 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "yaets/tracing.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +using namespace std::chrono_literals; +using std::placeholders::_1; + + +yaets::TraceSession session("session1.log"); + +class ProducerNode : public rclcpp::Node +{ +public: + ProducerNode() : Node("producer_node") + { + pub_1_ = create_publisher("topic_1", 100); + pub_2_ = create_publisher("topic_2", 100); + timer_ = create_wall_timer(1ms, std::bind(&ProducerNode::timer_callback, this)); + } + + void timer_callback() + { + message_.data += 1; + pub_1_->publish(message_); + message_.data += 1; + pub_2_->publish(message_); + } + +private: + rclcpp::Publisher::SharedPtr pub_1_, pub_2_; + rclcpp::TimerBase::SharedPtr timer_; + std_msgs::msg::Int32 message_; +}; + +class ConsumerNode : public rclcpp::Node +{ +public: + ConsumerNode() : Node("consumer_node") + { + custom_cb_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + rclcpp::SubscriptionOptions options; + options.callback_group = custom_cb_; + + sub_2_ = create_subscription( + "topic_2", 100, std::bind(&ConsumerNode::cb_2, this, _1), options); + sub_1_ = create_subscription( + "topic_1", 100, std::bind(&ConsumerNode::cb_1, this, _1), options); + + timer_ = create_wall_timer(10ms, std::bind(&ConsumerNode::timer_cb, this)); + } + + void cb_1(const std_msgs::msg::Int32::SharedPtr msg) + { + TRACE_EVENT(session); + + waste_time(500us); + } + + void cb_2(const std_msgs::msg::Int32::SharedPtr msg) + { + TRACE_EVENT(session); + + waste_time(500us); + } + + void timer_cb() + { + TRACE_EVENT(session); + + waste_time(5ms); + } + + void waste_time(const rclcpp::Duration & duration) + { + auto start = now(); + while (now() - start < duration); + } + +private: + rclcpp::Subscription::SharedPtr sub_1_; + rclcpp::Subscription::SharedPtr sub_2_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::CallbackGroup::SharedPtr custom_cb_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node_pub = std::make_shared(); + auto node_sub1 = std::make_shared(); + + //rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 8); + + executor.add_node(node_pub); + executor.add_node(node_sub1); + + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/br2_deep_ros/src/executors_cbg_reentrant.cpp b/br2_deep_ros/src/executors_cbg_reentrant.cpp new file mode 100644 index 00000000..fd069730 --- /dev/null +++ b/br2_deep_ros/src/executors_cbg_reentrant.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "yaets/tracing.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +using namespace std::chrono_literals; +using std::placeholders::_1; + + +yaets::TraceSession session("session1.log"); + +class ProducerNode : public rclcpp::Node +{ +public: + ProducerNode() : Node("producer_node") + { + pub_1_ = create_publisher("topic_1", 100); + pub_2_ = create_publisher("topic_2", 100); + timer_ = create_wall_timer(1ms, std::bind(&ProducerNode::timer_callback, this)); + } + + void timer_callback() + { + message_.data += 1; + pub_1_->publish(message_); + message_.data += 1; + pub_2_->publish(message_); + } + +private: + rclcpp::Publisher::SharedPtr pub_1_, pub_2_; + rclcpp::TimerBase::SharedPtr timer_; + std_msgs::msg::Int32 message_; +}; + +class ConsumerNode : public rclcpp::Node +{ +public: + ConsumerNode() : Node("consumer_node") + { + custom_cb_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + rclcpp::SubscriptionOptions options; + options.callback_group = custom_cb_; + + sub_2_ = create_subscription("topic_2", 100, std::bind(&ConsumerNode::cb_2, this, _1), options); + sub_1_ = create_subscription("topic_1", 100, std::bind(&ConsumerNode::cb_1, this, _1), options); + + timer_ = create_wall_timer(10ms, std::bind(&ConsumerNode::timer_cb, this), custom_cb_); + } + + void cb_1(const std_msgs::msg::Int32::SharedPtr msg) + { + std::unique_lock lock(mutex_); + TRACE_EVENT(session); + + waste_time(500us); + } + + void cb_2(const std_msgs::msg::Int32::SharedPtr msg) + { + TRACE_EVENT(session); + + waste_time(500us); + } + + void timer_cb() + { + std::unique_lock lock(mutex_); + TRACE_EVENT(session); + + waste_time(5ms); + } + + void waste_time(const rclcpp::Duration & duration) + { + auto start = now(); + while (now() - start < duration); + } + +private: + rclcpp::Subscription::SharedPtr sub_1_; + rclcpp::Subscription::SharedPtr sub_2_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::CallbackGroup::SharedPtr custom_cb_; + std::mutex mutex_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node_pub = std::make_shared(); + auto node_sub1 = std::make_shared(); + + //rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 8); + + executor.add_node(node_pub); + executor.add_node(node_sub1); + + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/br2_deep_ros/src/strategy_1.cpp b/br2_deep_ros/src/strategy_1.cpp new file mode 100644 index 00000000..9f126679 --- /dev/null +++ b/br2_deep_ros/src/strategy_1.cpp @@ -0,0 +1,156 @@ +// Copyright 2024 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "yaets/tracing.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +using namespace std::chrono_literals; +using std::placeholders::_1; + + +yaets::TraceSession session("strategy_1.log"); + + +void waste_time(rclcpp::Node::SharedPtr node, const rclcpp::Duration & duration) +{ + auto start = node->now(); + while (node->now() - start < duration); +} + +class ProducerNode : public rclcpp::Node +{ +public: + ProducerNode() : Node("producer_node") + { + pub_ = create_publisher("int_topic", 100); + timer_ = create_wall_timer(10ms, std::bind(&ProducerNode::timer_callback, this)); + } + + void timer_callback() + { + TRACE_EVENT(session); + + waste_time(shared_from_this(), 200us); + + message_.data += 1; + pub_->publish(message_); + } + +private: + rclcpp::Publisher::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_; + std_msgs::msg::Int32 message_; +}; + +class ConsumerNode : public rclcpp::Node +{ +public: + ConsumerNode() : Node("consumer_node") + { + sub_ = create_subscription( + "int_topic", 100, std::bind(&ConsumerNode::cb, this, _1)); + + timer_ = create_wall_timer(10ms, std::bind(&ConsumerNode::timer_cb, this)); + } + + void cb(const std_msgs::msg::Int32::SharedPtr msg) + { + TRACE_EVENT(session); + + waste_time(shared_from_this(), 500us); + } + + + void timer_cb() + { + TRACE_EVENT(session); + + waste_time(shared_from_this(), 2ms); + } + +private: + rclcpp::Subscription::SharedPtr sub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +class LoggerNode : public rclcpp::Node +{ +public: + LoggerNode() : Node("logger_node") + { + sub_ = create_subscription( + "int_topic", 100, std::bind(&LoggerNode::cb, this, _1)); + + timer_ = create_wall_timer(10ms, std::bind(&LoggerNode::timer_cb, this)); + } + + void cb(const std_msgs::msg::Int32::SharedPtr msg) + { + TRACE_EVENT(session); + + waste_time(shared_from_this(), 500us); + } + + + void timer_cb() + { + TRACE_EVENT(session); + + waste_time(shared_from_this(), 2ms); + } + +private: + rclcpp::Subscription::SharedPtr sub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node_producer = std::make_shared(); + auto node_consumer = std::make_shared(); + auto node_logger = std::make_shared(); + + rclcpp::executors::SingleThreadedExecutor no_rt_executor; + rclcpp::executors::SingleThreadedExecutor rt_executor; + + no_rt_executor.add_node(node_producer); + no_rt_executor.add_node(node_logger); + rt_executor.add_node(node_consumer); + + auto rt_thread = std::thread( + [&]() { + sched_param sch; + sch.sched_priority = 90; + + if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) { + throw std::runtime_error{std::string("failed to set scheduler: ") + std::strerror(errno)}; + } + + rt_executor.spin(); + }); + + no_rt_executor.spin(); + + rt_thread.join(); + + rclcpp::shutdown(); + return 0; +} diff --git a/br2_deep_ros/src/strategy_2.cpp b/br2_deep_ros/src/strategy_2.cpp new file mode 100644 index 00000000..b725cb46 --- /dev/null +++ b/br2_deep_ros/src/strategy_2.cpp @@ -0,0 +1,171 @@ +// Copyright 2024 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "yaets/tracing.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +using namespace std::chrono_literals; +using std::placeholders::_1; + + +yaets::TraceSession session("strategy_2.log"); + + +void waste_time(rclcpp::Node::SharedPtr node, const rclcpp::Duration & duration) +{ + auto start = node->now(); + while (node->now() - start < duration); +} + +class ProducerNode : public rclcpp::Node +{ +public: + ProducerNode() : Node("producer_node") + { + pub_ = create_publisher("int_topic", 100); + timer_ = create_wall_timer(10ms, std::bind(&ProducerNode::timer_callback, this)); + } + + void timer_callback() + { + TRACE_EVENT(session); + + waste_time(shared_from_this(), 200us); + + message_.data += 1; + pub_->publish(message_); + } + +private: + rclcpp::Publisher::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_; + std_msgs::msg::Int32 message_; +}; + +class ConsumerNode : public rclcpp::Node +{ +public: + ConsumerNode() : Node("consumer_node") + { + rt_callback_group_ = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = rt_callback_group_; + sub_ = create_subscription( + "int_topic", 100, std::bind(&ConsumerNode::cb, this, _1), sub_options); + + timer_ = create_wall_timer(10ms, std::bind(&ConsumerNode::timer_cb, this)); + } + + void cb(const std_msgs::msg::Int32::SharedPtr msg) + { + TRACE_EVENT(session); + + waste_time(shared_from_this(), 500us); + } + + + void timer_cb() + { + TRACE_EVENT(session); + + waste_time(shared_from_this(), 2ms); + } + + rclcpp::CallbackGroup::SharedPtr get_rt_callback_group() + { + return rt_callback_group_; + } + +private: + rclcpp::Subscription::SharedPtr sub_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::CallbackGroup::SharedPtr rt_callback_group_; +}; + +class LoggerNode : public rclcpp::Node +{ +public: + LoggerNode() : Node("logger_node") + { + sub_ = create_subscription( + "int_topic", 100, std::bind(&LoggerNode::cb, this, _1)); + + timer_ = create_wall_timer(10ms, std::bind(&LoggerNode::timer_cb, this)); + } + + void cb(const std_msgs::msg::Int32::SharedPtr msg) + { + TRACE_EVENT(session); + + waste_time(shared_from_this(), 500us); + } + + + void timer_cb() + { + TRACE_EVENT(session); + + waste_time(shared_from_this(), 2ms); + } + +private: + rclcpp::Subscription::SharedPtr sub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node_producer = std::make_shared(); + auto node_consumer = std::make_shared(); + auto node_logger = std::make_shared(); + + rclcpp::executors::SingleThreadedExecutor no_rt_executor; + rclcpp::executors::SingleThreadedExecutor rt_executor; + + no_rt_executor.add_node(node_producer); + no_rt_executor.add_node(node_logger); + no_rt_executor.add_node(node_consumer); + + rt_executor.add_callback_group( + node_consumer->get_rt_callback_group(), + node_consumer->get_node_base_interface()); + + auto rt_thread = std::thread( + [&]() { + sched_param sch; + sch.sched_priority = 90; + + if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) { + throw std::runtime_error{std::string("failed to set scheduler: ") + std::strerror(errno)}; + } + + rt_executor.spin(); + }); + + no_rt_executor.spin(); + + rt_thread.join(); + + rclcpp::shutdown(); + return 0; +} diff --git a/br2_deep_ros/src/strategy_3.cpp b/br2_deep_ros/src/strategy_3.cpp new file mode 100644 index 00000000..e641e9a3 --- /dev/null +++ b/br2_deep_ros/src/strategy_3.cpp @@ -0,0 +1,241 @@ +// Copyright 2024 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "yaets/tracing.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "vision_msgs/msg/detection3_d.hpp" + +using namespace std::chrono_literals; +using std::placeholders::_1; + + +yaets::TraceSession session("strategy_3.log"); + + +void waste_time(rclcpp::Node::SharedPtr node, const rclcpp::Duration & duration) +{ + auto start = node->now(); + while (node->now() - start < duration); +} + + +class SensorDriverNode : public rclcpp::Node +{ +public: + SensorDriverNode() : Node("sensor_driver") + { + rt_callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + pub_ = create_publisher("image", 100); + timer_scan_ = create_wall_timer( + 10ms, std::bind(&SensorDriverNode::produce_data, this), rt_callback_group_); + timer_state_ = create_wall_timer(100ms, std::bind(&SensorDriverNode::print_state, this)); + } + + void produce_data() + { + SHARED_TRACE_START("brake_process"); + + waste_time(shared_from_this(), 200us); + + sensor_msgs::msg::Image image_msg; + pub_->publish(image_msg); + } + + void print_state() + { + waste_time(shared_from_this(), 1ms); + } + + rclcpp::CallbackGroup::SharedPtr get_rt_callback_group() + { + return rt_callback_group_; + } + +private: + rclcpp::Publisher::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_scan_, timer_state_; + rclcpp::CallbackGroup::SharedPtr rt_callback_group_; +}; + + +class ObstacleDetectorNode : public rclcpp::Node +{ +public: + ObstacleDetectorNode() : Node("obstacle_detector") + { + rt_callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = rt_callback_group_; + + sub_ = create_subscription( + "image", 100, + std::bind(&ObstacleDetectorNode::detect_obstacle, this, _1), + sub_options); + pub_ = create_publisher("obstacles", 100); + timer_state_ = create_wall_timer(100ms, std::bind(&ObstacleDetectorNode::print_state, this)); + } + + void detect_obstacle(const sensor_msgs::msg::Image::SharedPtr msg) + { + waste_time(shared_from_this(), 5ms); + + vision_msgs::msg::Detection3D detection_msg; + pub_->publish(detection_msg); + } + + + void print_state() + { + waste_time(shared_from_this(), 1ms); + } + + rclcpp::CallbackGroup::SharedPtr get_rt_callback_group() + { + return rt_callback_group_; + } + +private: + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher< vision_msgs::msg::Detection3D>::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_state_; + rclcpp::CallbackGroup::SharedPtr rt_callback_group_; +}; + + +class LoggerNode : public rclcpp::Node +{ +public: + LoggerNode() : Node("logger_node") + { + sub_ = create_subscription( + "image", 100, std::bind(&LoggerNode::cb, this, _1)); + + timer_state_ = create_wall_timer(10ms, std::bind(&LoggerNode::print_state, this)); + } + + void cb(const sensor_msgs::msg::Image::SharedPtr msg) + { + waste_time(shared_from_this(), 500us); + } + + + void print_state() + { + waste_time(shared_from_this(), 2ms); + } + +private: + rclcpp::Subscription::SharedPtr sub_; + rclcpp::TimerBase::SharedPtr timer_state_; +}; + + +class BrakeActuatorNode : public rclcpp::Node +{ +public: + BrakeActuatorNode() : Node("brake_actuator") + { + rt_callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = rt_callback_group_; + + sub_ = create_subscription( + "obstacles", 100, + std::bind(&BrakeActuatorNode::react_obstacle, this, _1), + sub_options); + timer_state_ = create_wall_timer(100ms, std::bind(&BrakeActuatorNode::print_state, this)); + } + + void react_obstacle(vision_msgs::msg::Detection3D::SharedPtr msg) + { + waste_time(shared_from_this(), 2ms); + SHARED_TRACE_END("brake_process"); + } + + + void print_state() + { + waste_time(shared_from_this(), 1ms); + } + + rclcpp::CallbackGroup::SharedPtr get_rt_callback_group() + { + return rt_callback_group_; + } + +private: + rclcpp::Subscription::SharedPtr sub_; + rclcpp::TimerBase::SharedPtr timer_state_; + rclcpp::CallbackGroup::SharedPtr rt_callback_group_; +}; + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + SHARED_TRACE_INIT(session, "brake_process"); + + auto node_sensor_driver = std::make_shared(); + auto node_obstacle_detector = std::make_shared(); + auto node_logger = std::make_shared(); + auto node_brake_actuator = std::make_shared(); + + rclcpp::executors::SingleThreadedExecutor no_rt_executor; + rclcpp::executors::MultiThreadedExecutor rt_executor(rclcpp::ExecutorOptions(), 3); + + no_rt_executor.add_node(node_sensor_driver); + no_rt_executor.add_node(node_obstacle_detector); + no_rt_executor.add_node(node_logger); + no_rt_executor.add_node(node_brake_actuator); + + rt_executor.add_callback_group( + node_sensor_driver->get_rt_callback_group(), + node_sensor_driver->get_node_base_interface()); + rt_executor.add_callback_group( + node_obstacle_detector->get_rt_callback_group(), + node_obstacle_detector->get_node_base_interface()); + rt_executor.add_callback_group( + node_brake_actuator->get_rt_callback_group(), + node_brake_actuator->get_node_base_interface()); + + auto rt_thread = std::thread( + [&]() { + // sched_param sch; + // sch.sched_priority = 90; + // + // if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) { + // throw std::runtime_error{std::string("failed to set scheduler: ") + std::strerror(errno)}; + // } + + rt_executor.spin(); + }); + + no_rt_executor.spin(); + + rt_thread.join(); + + rclcpp::shutdown(); + return 0; +} diff --git a/br2_deep_ros/src/wakeup.cpp b/br2_deep_ros/src/wakeup.cpp new file mode 100644 index 00000000..f651f8e1 --- /dev/null +++ b/br2_deep_ros/src/wakeup.cpp @@ -0,0 +1,38 @@ +// Copyright 2024 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "yaets/tracing.hpp" + +int main(int argc, char * argv[]) +{ + yaets::TraceSession session("wakeup.log"); + + sched_param sch; + sch.sched_priority = 80; + if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) { + throw std::runtime_error{std::string("failed to set scheduler: ") + std::strerror(errno)}; + } + + while(true) { + { + TRACE_EVENT(session); + } + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + + return 0; +} diff --git a/br2_deep_ros/src/wakeup_ros.cpp b/br2_deep_ros/src/wakeup_ros.cpp new file mode 100644 index 00000000..37cb90be --- /dev/null +++ b/br2_deep_ros/src/wakeup_ros.cpp @@ -0,0 +1,43 @@ +// Copyright 2024 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include "yaets/tracing.hpp" +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + yaets::TraceSession session("wakeup.log"); + + // sched_param sch; + // sch.sched_priority = 90; + // if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) { + // throw std::runtime_error{std::string("failed to set scheduler: ") + std::strerror(errno)}; + // } + + rclcpp::Rate loop_rate(20ms); + while (rclcpp::ok()) { + { + TRACE_EVENT(session); + } + loop_rate.sleep(); + } + + rclcpp::shutdown(); + + return 0; +} diff --git a/br2_fsm_bumpgo_cpp/CMakeLists.txt b/br2_fsm_bumpgo_cpp/CMakeLists.txt index 7e23764a..f537b2b7 100644 --- a/br2_fsm_bumpgo_cpp/CMakeLists.txt +++ b/br2_fsm_bumpgo_cpp/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5) project(br2_fsm_bumpgo_cpp) -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 23) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -9,18 +9,20 @@ find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) set(dependencies - rclcpp - sensor_msgs - geometry_msgs + rclcpp::rclcpp + ${sensor_msgs_TARGETS} + ${geometry_msgs_TARGETS} ) -include_directories(include) - add_executable(bumpgo src/br2_fsm_bumpgo_cpp/BumpGoNode.cpp src/bumpgo_main.cpp ) -ament_target_dependencies(bumpgo ${dependencies}) +target_include_directories(bumpgo PUBLIC + $ + $ +) +target_link_libraries(bumpgo ${dependencies}) install(TARGETS bumpgo diff --git a/br2_tf2_detector/CMakeLists.txt b/br2_tf2_detector/CMakeLists.txt index b340c00d..4c16d9b9 100644 --- a/br2_tf2_detector/CMakeLists.txt +++ b/br2_tf2_detector/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5) project(br2_tf2_detector) -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 23) # find dependencies find_package(ament_cmake REQUIRED) @@ -13,12 +13,12 @@ find_package(sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) set(dependencies - rclcpp - tf2_ros - geometry_msgs - tf2_geometry_msgs - sensor_msgs - visualization_msgs + rclcpp::rclcpp + tf2_ros::tf2_ros + ${geometry_msgs_TARGETS} + ${tf2_geometry_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${visualization_msgs_TARGETS} ) include_directories(include) @@ -28,15 +28,13 @@ add_library(${PROJECT_NAME} SHARED src/br2_tf2_detector/ObstacleMonitorNode.cpp src/br2_tf2_detector/ObstacleDetectorImprovedNode.cpp ) -ament_target_dependencies(${PROJECT_NAME} ${dependencies}) +target_link_libraries(${PROJECT_NAME} ${dependencies}) add_executable(detector src/detector_main.cpp) -ament_target_dependencies(detector ${dependencies}) -target_link_libraries(detector ${PROJECT_NAME}) +target_link_libraries(detector ${PROJECT_NAME} ${dependencies}) add_executable(detector_improved src/detector_improved_main.cpp) -ament_target_dependencies(detector_improved ${dependencies}) -target_link_libraries(detector_improved ${PROJECT_NAME}) +target_link_libraries(detector_improved ${PROJECT_NAME} ${dependencies}) install(TARGETS ${PROJECT_NAME} diff --git a/br2_tf2_detector/src/br2_tf2_detector/ObstacleDetectorImprovedNode.cpp b/br2_tf2_detector/src/br2_tf2_detector/ObstacleDetectorImprovedNode.cpp index 31ab8d83..fabf665b 100644 --- a/br2_tf2_detector/src/br2_tf2_detector/ObstacleDetectorImprovedNode.cpp +++ b/br2_tf2_detector/src/br2_tf2_detector/ObstacleDetectorImprovedNode.cpp @@ -12,16 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include - #include #include "br2_tf2_detector/ObstacleDetectorImprovedNode.hpp" +#include "tf2/transform_datatypes.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + #include "sensor_msgs/msg/laser_scan.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" - #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/br2_tf2_detector/src/br2_tf2_detector/ObstacleMonitorNode.cpp b/br2_tf2_detector/src/br2_tf2_detector/ObstacleMonitorNode.cpp index 7b85f99f..7e6024a6 100644 --- a/br2_tf2_detector/src/br2_tf2_detector/ObstacleMonitorNode.cpp +++ b/br2_tf2_detector/src/br2_tf2_detector/ObstacleMonitorNode.cpp @@ -12,9 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include - #include #include "br2_tf2_detector/ObstacleMonitorNode.hpp" @@ -22,6 +19,8 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/transform_datatypes.hpp" +#include "tf2/LinearMath/Quaternion.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/br2_tracking/CMakeLists.txt b/br2_tracking/CMakeLists.txt index d2ed5f72..140d2b68 100644 --- a/br2_tracking/CMakeLists.txt +++ b/br2_tracking/CMakeLists.txt @@ -1,8 +1,7 @@ cmake_minimum_required(VERSION 3.5) project(br2_tracking) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_BUILD_TYPE Debug) +set(CMAKE_CXX_STANDARD 23) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -18,6 +17,18 @@ find_package(cv_bridge REQUIRED) find_package(OpenCV REQUIRED) set(dependencies + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + image_transport::image_transport + cv_bridge::cv_bridge + ${br2_tracking_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${vision_msgs_TARGETS} + ${control_msgs_TARGETS} +) + +set(export_dependencies rclcpp rclcpp_lifecycle br2_tracking_msgs @@ -30,36 +41,37 @@ set(dependencies OpenCV ) -include_directories(include) - add_library(${PROJECT_NAME} SHARED src/br2_tracking/ObjectDetector.cpp src/br2_tracking/HeadController.cpp src/br2_tracking/PIDController.cpp ) -ament_target_dependencies(${PROJECT_NAME} ${dependencies}) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} ${dependencies} ${OpenCV_LIBS}) add_executable(object_detector src/object_detector_main.cpp) -ament_target_dependencies(object_detector ${dependencies}) -target_link_libraries(object_detector ${PROJECT_NAME}) +target_link_libraries(object_detector ${PROJECT_NAME} ${dependencies} ${OpenCV_LIBS}) add_executable(object_tracker src/object_tracker_main.cpp) -ament_target_dependencies(object_tracker ${dependencies}) -target_link_libraries(object_tracker ${PROJECT_NAME}) +target_link_libraries(object_tracker ${PROJECT_NAME} ${dependencies} ${OpenCV_LIBS}) install(TARGETS ${PROJECT_NAME} object_detector object_tracker + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} ) install( - DIRECTORY include - DESTINATION include + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) @@ -75,7 +87,8 @@ if(BUILD_TESTING) add_subdirectory(tests) endif() -ament_export_include_directories(include) +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(${PROJECT_NAME}) -ament_export_dependencies(${dependencies}) +ament_export_dependencies(${export_dependencies}) +ament_export_targets(export_${PROJECT_NAME}) ament_package() diff --git a/br2_tracking/tests/CMakeLists.txt b/br2_tracking/tests/CMakeLists.txt index 53358d85..6a1127a0 100644 --- a/br2_tracking/tests/CMakeLists.txt +++ b/br2_tracking/tests/CMakeLists.txt @@ -1,4 +1,3 @@ ament_add_gtest(pid_test pid_test.cpp) -ament_target_dependencies(pid_test ${dependencies}) -target_link_libraries(pid_test ${PROJECT_NAME}) +target_link_libraries(pid_test ${PROJECT_NAME} ${dependencies}) diff --git a/br2_vff_avoidance/CMakeLists.txt b/br2_vff_avoidance/CMakeLists.txt index fddb13fe..0f081a7e 100644 --- a/br2_vff_avoidance/CMakeLists.txt +++ b/br2_vff_avoidance/CMakeLists.txt @@ -1,8 +1,7 @@ cmake_minimum_required(VERSION 3.5) project(br2_vff_avoidance) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_BUILD_TYPE Debug) +set(CMAKE_CXX_STANDARD 23) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) diff --git a/third_parties.repos b/third_parties.repos index 5ed0b738..d5dbbf6b 100644 --- a/third_parties.repos +++ b/third_parties.repos @@ -67,7 +67,7 @@ repositories: type: git url: https://github.com/Tiago-Harmonic/pal_urdf_utils.git version: rolling - ThirdParty/Groot: + ThirdParty/yaets: type: git - url: https://github.com/BehaviorTree/Groot.git - version: master + url: https://github.com/fmrico/yaets.git + version: rolling