Planner server doesn't receive correct goals after deactivation->activation cycle

[SteveMacenski]

Bug report

Required Info:

• Operating System: Ubuntu
• ROS2 Version: Master
• Version or commit hash: Master
• DDS implementation: Fast-RTPS

Steps to reproduce issue

Bring up stack and in a separate terminal:

ros2 service call /lifecycle_manager_navigation/manage_nodes nav2_msgs/srv/ManageLifecycleNodes command:\ 3\
ros2 service call /lifecycle_manager_navigation/manage_nodes nav2_msgs/srv/ManageLifecycleNodes command:\ 0\

Then go to a pose

Expected behavior

Robot goes to new pose

Actual behavior

Robot goes to either last pose every time, or if there is no last pose, goes to 0,0

[SteveMacenski]

BTNavigator gets the correct goal and prints at L330 the actual goal then sets on blackboard the goal parameter to goal->pose. Then in compute_path_to_pose BT node, when it gets the goal, its always the last value / zeros. I wonder if the BT navigator or simple action server isn't deactivating correctly.

[SteveMacenski]

Messing with compute path to pose:

void ComputePathToPoseAction::on_tick()
{
  getInput("goal", goal_.pose);
  std::cout << "ComputePathToPoseAction " << goal_.pose.pose.position.x <<  " " << goal_.pose.pose.position.y << std::endl;
      geometry_msgs::msg::PoseStamped goal_pose;
      config().blackboard->get("goal", goal_pose);

  std::cout << "ComputePathToPoseAction2 " << goal_pose.pose.position.x <<  " " << goal_pose.pose.position.y << std::endl;
  getInput("planner_id", goal_.planner_id);
}

The get() fails with "Any::cast failed because it is empty" so it appears to be empty, but if you get from the blackboard in the BT navigator you can get it. So either the new BT doesn't have the new blackboard or there's some async issue here. It doesn't seem that its getting to the blackboard correctly for the behavior tree nodes to use when reset and reconfigured / activated. That's interesting.