BT navigator not recognizing a basic ActionNode

[jesusramondovale]

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • ROS2 humble (binaries)
• Version or commit hash:
  • Unrelevant
• DDS implementation:
  • Fast-RTPS (unrelevant) )

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]

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]

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]

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.