0
$\begingroup$

Hey I'm trying to write a behavior tree plugin following this tutorial

https://docs.nav2.org/plugin_tutorials/docs/writing_new_bt_plugin.html

I'm on Ubuntu20.04 using Ros2 galactic and v3 for behavior_tree (going to v4 had not solved the problem)

So I'm launching a bt_navigator node for autonomous navigation with couple of plugins and a behavior_tree for managing the robot behavior. But I want to include a custom action inside that tree.

The custom xml tag I use is called CustomRecXNode, and it should call an action client

It is supposed to be triggered here in my tree in the middle of other bt_navigation's tasks.

    <Control ID="RoundRobin" name="RecoveryActions">
            <Sequence name="ClearingActions">
              <Action ID="ClearEntireCostmap" name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap" />
            </Sequence>
            <Action ID="Wait" wait_duration="3"/>
            <CustomRecXNode order="10"/>
            <Action ID="BackUp" backup_dist="2.0" backup_speed="0.4" />
          </Control>

But when I launch my bt_navigator node it does not find my plugin.

[bt_navigator-3] [INFO] [1718168941.935772843] [bt_navigator]: Configuring
[bt_navigator-3] [ERROR] [1718168941.960941808] []: Caught exception in callback for transition 10
[bt_navigator-3] [ERROR] [1718168941.960966865] []: Original error: Could not load library: libcustom_recx_bt_node.so: cannot open shared object file: No such file or directory
[bt_navigator-3] [WARN] [1718168941.961002492] []: Error occurred while doing error handling.
[bt_navigator-3] [FATAL] [1718168941.961012991] [bt_navigator]: Lifecycle node bt_navigator does not have error state implemented
[lifecycle_manager-8] [ERROR] [1718168941.961570466] [lifecycle_manager_navigation]: Failed to change state for node: bt_navigator
[lifecycle_manager-8] [ERROR] [1718168941.961608117] [lifecycle_manager_navigation]: Failed to bring up all requested nodes. Aborting bringup.



Yet, I did what the tutorial said and I look at how they do it on nav2_behavior_tree https://github.com/ros-navigation/navigation2/blob/main/nav2_behavior_tree/plugins/action/follow_path_action.cpp (follow_path_action for exemple)

This is my custom_recx_node.cpp and I'm registering the tag

/**
 * @file custom_recx_bt_node.cpp
 * @author Emonot--De Carolis Evrard
 * @brief Custom behavior tree node implementation for calling recx action and return success
 * @version 1.0
 * @date 2024-05-07
 */
#include <memory>
#include <string>
#include "custom_recx_bt_node.hpp"

namespace nav2_behavior_tree
{

  CustomRecXNode::CustomRecXNode(
    const std::string& xml_tag_xml,
    const std::string& action_name,  
    const BT::NodeConfiguration& config)
    : BtActionNode<kisui_msgs::action::TestAction>(xml_tag_xml, action_name, config), client_(std::make_shared<RecXClient>())
    {
    }

  void CustomRecXNode::on_tick() 
    {
        auto order = getInput<int>("order");
        if (!order) {
            throw BT::RuntimeError("missing required input [order]: ", order.error());
        }

        client_->callAction(order.value());
    }
}  
BT_REGISTER_NODES(factory) 
{
    BT::NodeBuilder builder =
    [](const std::string & name, const BT::NodeConfiguration & config)
    {
      return std::make_unique<nav2_behavior_tree::CustomRecXNode>(name, "CustomRecXNode", config);
    };
    factory.registerBuilder<nav2_behavior_tree::CustomRecXNode>("CustomRecXNode", builder);
}

and the hpp :

/**
 * @file custom_recx_bt_node.hpp
 * @brief Custom behavior tree node header for calling recx action and return success
 * @version 1.0
 * @date 2024-05-07
 */

#ifndef CUSTOM_RECX_BT_NODE_HPP_
#define CUSTOM_RECX_BT_NODE_HPP_


#include <string>
#include <nav2_behavior_tree/bt_action_node.hpp>
#include <behaviortree_cpp_v3/action_node.h>
#include <behaviortree_cpp_v3/bt_factory.h>
#include "recx_client.hpp"


namespace nav2_behavior_tree
{

class CustomRecXNode : public BtActionNode<kisui_msgs::action::TestAction>{
public:
    CustomRecXNode(
        const std::string& xml_tag_xml,
        const std::string& action_name,  
        const BT::NodeConfiguration& config); 
    
    void on_tick() override;

    void on_wait_for_result() override;
    
    static BT::PortsList providedPorts()
    {
        return providedBasicPorts(
        { 
            BT::InputPort<int>("order") 
        });
    }

private:
    std::shared_ptr<RecXClient> client_;
};

}//namespace

#endif // CUSTOM_RECX_BT_NODE_HPP_

I put it in the CMakeFile.txt

find_package(nav2_msgs REQUIRED)
find_package(nav2_bt_navigator REQUIRED)
find_package(nav2_behavior_tree REQUIRED)
install(
  TARGETS stuff_i_cant_show
          custom_recx_bt_node
          recx_server
  DESTINATION lib/${PROJECT_NAME})
add_library(custom_recx_bt_node SHARED src/custom_recx_bt_node.cpp)
ament_target_dependencies(custom_recx_bt_node ${COMMON_DEPENDENCIES})
target_link_libraries(custom_recx_bt_node ${rclcpp_LIBRARIES} ${rclcpp_action_LIBRARIES})

and add to the plugin of my parameters

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_footprint
    odom_topic: /local_odom
    transform_tolerance: 0.2
    goal_reached_tol: 2.0
    default_nav_to_pose_bt_xml: replace/with/path/to/bt.xml #don't be triggered by this it finds the behavior_tree, it's just handle somewhere else in the code
    goal_blackboard_id: goal
    goals_blackboard_id: goals
    path_blackboard_id: path
    navigators: ['navigate_to_pose', 'navigate_through_poses']
    navigate_to_pose:
      plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"    
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    -... many other nav2_plugin
    - custom_recx_bt_node

This is the piece of code that launch the node :

Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[configured_params],
            remappings=remappings),

What name should you put in the plugins ? where this libcustom_recx_bt_node.so should be generated ? is bt_navigator looking at the wrong place ?

Thanks for your help.

$\endgroup$
1
  • $\begingroup$ Could you make sure the correct bt is loaded? It should be a parameter set for the bt_navigator-node. So you just launch it all and do ros2 param list bt_navigator and see what behavior tree is loaded. My experience says most of the time it's just not loaded correctly, but the default bt is loaded instead. $\endgroup$
    – Scoeerg
    Commented Jun 12 at 11:47

1 Answer 1

0
$\begingroup$

Try adding the same to export ament_export_libraries() at the end of CMake

ament_export_libraries(custom_recx_bt_node)
$\endgroup$
4
  • $\begingroup$ Yes I add but did not really solve the problem, I managed to delete the error by writting by exporting the library path by typing this in the terminal export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:~/my_ws/install/my_package/lib But my xml tag is still a problem as my robot does not move and now I don't have any error message. when I remove the custom tag bt_navigator function normally $\endgroup$ Commented Jun 12 at 8:06
  • $\begingroup$ github.com/ros-navigation/navigation2/blob/main/… use this as a reference and try to write your CMake. This has worked for me in the past $\endgroup$
    – Sriram
    Commented Jun 12 at 9:41
  • $\begingroup$ Thanks I correct this error by adding this in the cmake install(TARGETS custom_recx_bt_node ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) install(DIRECTORY include/ DESTINATION include/ ) When I launch my bt_navigator node, it says activating but then nothing happens unless I remove my XML tag from the behavior tree. It still doesn't work but shows no error. [lifecycle_manager-8] [INFO] [1718766602.381426689] [lifecycle_manager_navigation]: Activating bt_navigator [bt_navigator-5] [INFO] [1718766602.381994634] [bt_navigator]: Activating $\endgroup$ Commented Jun 19 at 3:15
  • $\begingroup$ This might have something to do with the behaviour that you are running. Also I noticed you're calling the action at every tick. The behaviour tree constantly ticks the node if it returns running, the rate is defined in nav2 params file, as bt_loop_duration, if you want to call the action once and check status, call the action from the constructor. $\endgroup$
    – Sriram
    Commented Jun 19 at 4:23

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.