Skip to content

BT navigator not recognizing a basic ActionNode #2

@jesusramondovale

Description

@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

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions