search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.52k stars 1.28k forks source link

BT navigator not recognizing a basic ActionNode #4250

Closed jesusramondovale closed 6 months ago

jesusramondovale commented 6 months ago

Required Info:

Steps to reproduce issue

I created a dummy ActionNode, which compiled with no problems, but when launching nav2 bringup, BT-Navigator doesn´t recognize my custom ActionNode tag on the BT XML 

#include "mss_bt_nodes/call_portenta_control_action.hpp"

namespace mss_bt_nodes {

    CallPortentaControlAction::CallPortentaControlAction(
        const std::string & xml_tag_name,
        const std::string & action_name,
        const BT::NodeConfiguration & conf) 
      : BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf) {}

    void CallPortentaControlAction::on_tick()
    {
        std::cout << "PortentaControl called!" << std::endl;
    }

    BT::NodeStatus CallPortentaControlAction::on_success()
    {
      return BT::NodeStatus::SUCCESS;
    }

    BT::NodeStatus CallPortentaControlAction::on_aborted()
    {
        return BT::NodeStatus::FAILURE;
    }

    BT::NodeStatus CallPortentaControlAction::on_cancelled()
    {
        return BT::NodeStatus::FAILURE;
    }

    void CallPortentaControlAction::halt()
    {
         BtActionNode::halt();
    }

}

#include "behaviortree_cpp_v3/bt_factory.h"

BT_REGISTER_NODES(factory)
{
  BT::NodeBuilder builder =
    [](const std::string & name, const BT::NodeConfiguration & config)
    {
      return std::make_unique<mss_bt_nodes::CallPortentaControlAction>(name, "call_portenta_control", config);
    };

  factory.registerBuilder<mss_bt_nodes::CallPortentaControlAction>("CallPortentaControl", builder);
}
#ifndef MSS_BT_NODES_CALL_PORTENTA_CONTROL_ACTION_HPP_
#define MSS_BT_NODES_CALL_PORTENTA_CONTROL_ACTION_HPP_

#include <string>
#include <memory>
#include <vector>

#include <behaviortree_cpp_v3/behavior_tree.h>
#include <behaviortree_cpp_v3/bt_factory.h>

#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/bool.hpp"
#include "nav2_msgs/action/wait.hpp"
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "nav2_behavior_tree/bt_action_node.hpp"

namespace mss_bt_nodes {

    class CallPortentaControlAction
         : public nav2_behavior_tree::BtActionNode<
            nav2_msgs::action::Wait>
        {

        public:

            CallPortentaControlAction(
                const std::string& xml_tag_name, 
                const std::string & action_name,
                const BT::NodeConfiguration& conf);

            void on_tick() override;

            BT::NodeStatus on_success() override;

            BT::NodeStatus on_aborted() override;

            BT::NodeStatus on_cancelled() override;

            void halt() override;

            static BT::PortsList providedPorts()
            {
                return {};
            }
    };

} // namespace mss_bt_nodes

#endif // MSS_BT_NODES_CALL_PORTENTA_CONTROL_ACTION_HPP_

plugins/mss_bt_nodes_plugins.xml: 

<library path="call_portenta_control_action_bt_node">
  <class name="CallPortentaControl" type="mss_bt_nodes::CallPortentaControlAction"/>
</library>

CMakeLists.txt: 

cmake_minimum_required(VERSION 3.8)
project(mss_bt_nodes)
set(CMAKE_CXX_STANDARD 17)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(std_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(behaviortree_cpp_v3 REQUIRED)
find_package(geometry_msgs REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

add_library(call_portenta_control_action_bt_node SHARED  src/call_portenta_control_action.cpp)
list(APPEND plugins call_portenta_control_action_bt_node)

include_directories(include plugins)

install(FILES plugins/mss_bt_nodes_plugins.xml
  DESTINATION share/${PROJECT_NAME}
)

install(TARGETS call_portenta_control_action_bt_node
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

install(DIRECTORY include/
  DESTINATION include/
)

ament_export_include_directories(include)

ament_export_dependencies(std_msgs std_msgs rclcpp behaviortree_cpp_v3 rclcpp_action geometry_msgs)

ament_target_dependencies(call_portenta_control_action_bt_node
  rclcpp
  rclcpp_action
  std_msgs
  nav2_msgs
  geometry_msgs
  behaviortree_cpp_v3
)

ament_package()

nav2_params (bt_navigator) : 

bt_navigator:
  ros__parameters:
    use_sim_time: false
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_compute_path_through_poses_action_bt_node
      - nav2_smooth_path_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_assisted_teleop_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_drive_on_heading_bt_node
      - nav2_clear_costmap_service_bt_node
      - nav2_is_stuck_condition_bt_node
      - nav2_goal_reached_condition_bt_node
      - nav2_goal_updated_condition_bt_node
      - nav2_globally_updated_goal_condition_bt_node
      - nav2_is_path_valid_condition_bt_node
      - nav2_initial_pose_received_condition_bt_node
      - nav2_reinitialize_global_localization_service_bt_node
      - nav2_rate_controller_bt_node
      - nav2_distance_controller_bt_node
      - nav2_speed_controller_bt_node
      - nav2_truncate_path_action_bt_node
      - nav2_truncate_path_local_action_bt_node
      - nav2_goal_updater_node_bt_node
      - nav2_recovery_node_bt_node
      - nav2_pipeline_sequence_bt_node
      - nav2_round_robin_node_bt_node
      - nav2_transform_available_condition_bt_node
      - nav2_time_expired_condition_bt_node
      - nav2_path_expiring_timer_condition
      - nav2_distance_traveled_condition_bt_node
      - nav2_single_trigger_bt_node
      - nav2_goal_updated_controller_bt_node
      - nav2_is_battery_low_condition_bt_node
      - nav2_navigate_through_poses_action_bt_node
      - nav2_navigate_to_pose_action_bt_node
      - nav2_remove_passed_goals_action_bt_node
      - nav2_planner_selector_bt_node
      - nav2_controller_selector_bt_node
      - nav2_goal_checker_selector_bt_node
      - nav2_controller_cancel_bt_node
      - nav2_path_longer_on_approach_bt_node
      - nav2_wait_cancel_bt_node
      - nav2_spin_cancel_bt_node
      - nav2_back_up_cancel_bt_node
      - nav2_assisted_teleop_cancel_bt_node
      - nav2_drive_on_heading_cancel_bt_node
      - nav2_is_battery_charging_condition_bt_node
      - call_portenta_control_action_bt_node

Actual behavior

Exception when loading BT: Error at line 31: -> Node not recognized: CallPortentaControl
[component_container_isolated-5] [ERROR] [1712674147.857141533] [bt_navigator]: Error loading XML file: /opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
SteveMacenski commented 6 months ago

I don’t immediately see your issue, but you can find a full example in opennav_coverage https://github.com/open-navigation/opennav_coverage of custom external BT nodes for an external server. Hopefully you can find your mistake reviewing that!

jesusramondovale commented 6 months ago

Thanks for your response! I got some questions: Should it work just launching bringup.launch (ros2 launch nav2_bringup bringup.launch)?

Must I create also a action server like your example? I think I mustn't as I'm using the Wait action from nav2?

Actually I almost copied that exact example but I think there's a problem registering the node or compiling the library.

I know that Nav2 is finding and loading the library "correctly" because if I misspell it on nav2_params.yaml, it says that library isn't found, so it must be with the coding/building step. Using nm -C to "decode" the library.so generated, it contents 3700+ lines of "code"

Maybe am I missing some step related to the plugin.xml file? 

<library path="call_portenta_control_action_bt_node">
  <class name="CallPortentaControl" type="mss_bt_nodes::CallPortentaControlAction"/>
</library>

This is the complete code scheme: 

sim_ws/pkg/ BTNodes/
                     |_____________ mss_bt_nodes
                                               |_____________ include/
                                                                       |_____________ call_portenta_control.hpp
                                               |_____________ plugins/                                                                         
                                                                     |_____________mss_bt_nodes/mss_bt_nodes.xml
                                               |_____________ src/
                                                                       |_____________  call_portenta_control.cpp
                                               |_____________ CMakeLists.txt
                                                |_____________ package.xml

Could you tell me where can I find fully extended documentation about doing this? Cause the nav2 doc is very basic.

SteveMacenski commented 6 months ago

Sorry, please ask on robot stack exchange or look into the examples yourself in more detail. We don't have the bandwidth to debug every user's errors / misunderstandings. We have plenty of examples and references available so there's no shortage of recipes to look into to find your error. This isn't really a Nav2 issue but a BT.CPP problem understanding of how to build and work with custom plugins, it seems to me.