Can't get action feedback data in BTaction node based on bt_action_node in Galactic

[shoufei403]

Bug report

Required Info:

• Operating System:
• ROS2 Version:
  • Galactic binary
• Version or commit hash:
  • 1.0.8
• DDS implementation:
  • Fast-RTPS

Steps to reproduce issue

//from nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
void send_new_goal()
  {
    goal_result_available_ = false;
    auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
    send_goal_options.goal_response_callback =
      [this](typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle) {
        if (!goal_handle) {
          RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
        } else {
          RCLCPP_INFO(node_->get_logger(), "Goal accepted by server, waiting for result");
        }
      };
    send_goal_options.feedback_callback =
      [this](typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
        const std::shared_ptr<const typename ActionT::Feedback> feedback) {
 
        //TODO: Want to get feedback data in BTaction node

        (void)(feedback);
        // feedback_ptr_ = feedback->get();

      };
    send_goal_options.result_callback =
      [this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
        if (future_goal_handle_) {
          RCLCPP_DEBUG(
            node_->get_logger(),
            "Goal result for %s available, but it hasn't received the goal response yet. "
            "It's probably a goal result for the last goal request", action_name_.c_str());
          return;
        }

        // TODO(#1652): a work around until rcl_action interface is updated
        // if goal ids are not matched, the older goal call this callback so ignore the result
        // if matched, it must be processed (including aborted)
        if (this->goal_handle_->get_goal_id() == result.goal_id) {
          goal_result_available_ = true;
          result_ = result;
        }
      };

    future_goal_handle_ = std::make_shared<
      std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>(
      action_client_->async_send_goal(goal_, send_goal_options));
    time_goal_sent_ = node_->now();
  }

Feature request

Can get BTaction node feedback data

Feature description

I try to add a feedback callback. But the shared pointer of feedback can't be assigned for another member pointer or shared pointer.
I wonder if there is a way to reach that.

[SteveMacenski]

I don’t understand the issue- what’s the specific behavior that isn’t working?

[shoufei403]

Thanks for your responding.
We can take controller server as an example. It has function of following a path. In the meantime, it will respond data to action client. You can see the following snippet from nav2_controller.cpp .

  std::shared_ptr<Action::Feedback> feedback = std::make_shared<Action::Feedback>();
  feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y);

  // Find the closest pose to current pose on global path
  nav_msgs::msg::Path & current_path = current_path_;
  auto find_closest_pose_idx =
    [&pose, &current_path]() {
      size_t closest_pose_idx = 0;
      double curr_min_dist = std::numeric_limits<double>::max();
      for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
        double curr_dist = nav2_util::geometry_utils::euclidean_distance(
          pose, current_path.poses[curr_idx]);
        if (curr_dist < curr_min_dist) {
          curr_min_dist = curr_dist;
          closest_pose_idx = curr_idx;
        }
      }
      return closest_pose_idx;
    };

  feedback->distance_to_goal =
    nav2_util::geometry_utils::calculate_path_length(current_path_, find_closest_pose_idx());
  action_server_->publish_feedback(feedback);

the action client is implemented in related BTaction node (in follow_path_action.cpp).
The feedback callback is not implemented, so we can't get the feedback data.

[SteveMacenski]

Spin the executor and it would then process callbacks, please review ROS 2 execution model flow. Your issues have nothing to do with bt action server but with your use / knowledge in ROS 2 action servers. Please reference the appropriate documentation.

These are changes that are not reflected in Nav2 and aren't attached to a particular bug / issue / contribution so closing this ticket since its not actionable for the project. This looks like its proprietary development outside of this project, which we do not provide support in the issue tracker.

[shoufei403]

Actually, the feedback function does not exist in action client BT node, so we can't reach feedback data.