0
$\begingroup$

I'm trying to implement PlanSys2 and Behavior trees. My package is heavily inspired by ros2_planning_system_examples plansys2_bt_example package. I want to simulate a warehouse robot with automatic planning. The package is launched by launching plansys2_bringup from ros2_planning_system and some behavior tree nodes.

Launch file snippet

plansys2_cmd = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(os.path.join(
        get_package_share_directory('plansys2_bringup'),
        'launch',
        'plansys2_bringup_launch_monolithic.py')),
    launch_arguments={
        'model_file': os.path.join(this_directory, 'pddl', 'domain.pddl'),
        'namespace': namespace
    }.items()
)

# set actions
# pass params and BT node to the action node
navigate_cmd = Node(
    package='plansys2_bt_actions',
    executable='bt_action_node',
    name='navigate',
    namespace=namespace,
    output='screen',
    parameters=[
        os.path.join(this_directory, 'config', 'params.yaml'),
        {
            'action_name': 'navigate',
            'publisher_port': 1668,
            'server_port': 1669,
            'bt_xml_file': os.path.join(this_directory, 'bt_xml', 'navigate.xml')
        }
    ]
)

The params for navigate (behavior tree node) from params.yaml is

navigate:
  ros__parameters:
    plugins:
    - plansys2_navigate_bt_node
    waypoints: ["charging_station", "unload_zone", "reol_1", "reol_2", "reol_3", "reol_4"]
    waypoint_coordinates:
      charging_station: [-2.0, -0.5, 0.0]
      unload_zone: [2.0, 0.5, 0.0]
      reol_1: [0.5, 2.0, 0.0]
      reol_2: [-0.5, 2.0, 0.0]
      reol_3: [0.5, -2.0, 0.0]
      reol_4: [-0.5, -2.0, 0.0]

where I set the params waypoints and waypoints_coordinates. The first part of BT node navigate's constructor is

Navigate::Navigate(const std::string &xml_tag, const std::string &action_name,
               const BT::NodeConfiguration &configuration):
               plansys2::BtActionNode<nav2_msgs::action::NavigateToPose>(xml_tag,
                                                                         action_name,                                                                   
                                                                       configuration)
{
  rclcpp::Node::SharedPtr ros_node;
  BT::TreeNode::config().blackboard->get("node", ros_node);

  try{
    std::cout << "Trying to declare parameter 'waypoints'..." << std::endl;
    ros_node->declare_parameter<std::vector<std::string>>("waypoints");
    std::cout << "Declared parameter 'waypoints'." << std::endl;
    
  } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &e){
      // pass
  }

  ...

where I've added print statements for debugging. The package is running until

ros_node->declare_parameter<std::vector<std::string>>("waypoints");

where the package returns

[bt_action_node-2] Trying to declare parameter 'waypoints'...
[bt_action_node-2] [ERROR] [1708975502.584947181] [navigate]: Caught exception in 
callback for transition 13
[bt_action_node-2] [ERROR] [1708975502.584966297] [navigate]: Original error: 
Statically typed parameter 'waypoints' must be initialized.
[bt_action_node-2] [WARN] [1708975502.584980393] [navigate]: Error occurred while 
doing error handling.

when running a ROS 2 node task_controller_node that set instances and predicates for a PDDL solver.

I thought this had to do with waypoints parameter not being correclty declared/initialized/set. Running ros2 param list /navigate do indeed show that there is no parameters waypoints or waypoint_coordiantes. However, all other parameters, like action_name and plugins is available. I tried to set the parameters direclty in the navigate BT node but I get the same error. I have no clue why some parameters are loaded and others are not. Is there anything I missed when declaring/initializing parameters or could it be a problem elsewhere in the code?

Any help would be appreciated! Thanks:)

$\endgroup$

1 Answer 1

1
$\begingroup$

Here the PlanSys2 author :)

I think this was a bug, but now it is fixed, as far as I remember. The node name didn't match the node name in the params file.

I hope it helps

$\endgroup$
1
  • $\begingroup$ Hi and welcome to SE.Robotics! $\endgroup$ Apr 4 at 14:15

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.