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:)
