Skip to content

Fix status message dropped when received before goal response callback (Fixes #2782) #3275

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
Aaravanand00 wants to merge 1 commit into from
Closed

Fix status message dropped when received before goal response callback (Fixes #2782) #3275

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 18 additions & 1 deletion rclcpp_action/include/rclcpp_action/client.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#ifndef RCLCPP_ACTION__CLIENT_HPP_
#define RCLCPP_ACTION__CLIENT_HPP_

#include <algorithm>
#include <chrono>
#include <functional>
#include <future>
Expand All @@ -29,12 +30,15 @@
#include "rcl/event_callback.h"

#include "rclcpp/exceptions.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/node_interfaces/node_graph_interface.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/waitable.hpp"

#include "rosidl_runtime_c/action_type_support_struct.h"
#include "rosidl_typesupport_cpp/action_type_support.hpp"
Expand Down Expand Up @@ -194,6 +198,10 @@ class Client : public ClientBase
using GoalResponse = typename ActionT::Impl::SendGoalService::Response;
auto goal_response = std::static_pointer_cast<GoalResponse>(response);
if (!goal_response->accepted) {
{
std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
pending_statuses_.erase(goal_request->goal_id.uuid);
}
promise->set_value(nullptr);
if (options.goal_response_callback) {
options.goal_response_callback(nullptr);
Expand Down Expand Up @@ -222,6 +230,11 @@ class Client : public ClientBase
new GoalHandle(goal_info, options.feedback_callback, options.result_callback));
{
std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
auto pending_it = pending_statuses_.find(goal_info.goal_id.uuid);
if (pending_it != pending_statuses_.end()) {
goal_handle->set_status(pending_it->second);
pending_statuses_.erase(pending_it);
}
goal_handles_[goal_handle->get_goal_id()] = goal_handle;
}
promise->set_value(goal_handle);
Expand Down Expand Up @@ -510,9 +523,11 @@ class Client : public ClientBase
for (const GoalStatus & status : status_message->status_list) {
const GoalUUID & goal_id = status.goal_info.goal_id.uuid;
if (goal_handles_.count(goal_id) == 0) {
pending_statuses_[goal_id] = status.status;
RCLCPP_DEBUG(
this->get_logger(),
"Received status for unknown goal. Ignoring...");
"Received status for pending goal prior to response callback. Stored status %d.",
static_cast<int>(status.status));
continue;
}
typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock();
Expand All @@ -522,6 +537,7 @@ class Client : public ClientBase
this->get_logger(),
"Dropping weak reference to goal handle during status callback");
goal_handles_.erase(goal_id);
pending_statuses_.erase(goal_id);
continue;
}
goal_handle->set_status(status.status);
Expand Down Expand Up @@ -622,6 +638,7 @@ class Client : public ClientBase
}

std::map<GoalUUID, typename GoalHandle::WeakPtr> goal_handles_;
std::map<GoalUUID, int8_t> pending_statuses_;
std::recursive_mutex goal_handles_mutex_;
};
} // namespace rclcpp_action
Expand Down
64 changes: 56 additions & 8 deletions rclcpp_action/test/test_client.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,17 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <algorithm>
#include <array>
#include <atomic>
#include <chrono>
#include <future>
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <thread>
#include <utility>
#include <thread>

#include "gtest/gtest.h"

Expand All @@ -35,17 +32,14 @@
#include "rcl_action/action_client.h"
#include "rcl_action/wait.h"

#include "rclcpp/callback_group.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/executors.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/service.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/utilities.hpp"

#include "test_msgs/action/fibonacci.hpp"
#include "test_msgs/msg/empty.hpp"
Expand Down Expand Up @@ -707,6 +701,60 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait
}
}

TEST_F(TestClientAgainstServer, status_message_before_goal_response)
{
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));

enable_pending_handling_goal();

bool goal_response_received = false;
auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
send_goal_ops.goal_response_callback =
[&goal_response_received](typename ActionGoalHandle::SharedPtr goal_handle)
{
if (goal_handle) {
goal_response_received = true;
}
};

ActionGoal goal;
goal.order = 4;
auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);

// Spin server to process the goal request service call
server_executor.spin_some();

// Publish status message with STATUS_EXECUTING before completing the service response handling on the client side
ASSERT_EQ(1u, goals.size());
auto goal_request = goals.begin()->second.first;
auto goal_response = goals.begin()->second.second;

ActionStatusMessage status_message;
rclcpp_action::GoalStatus goal_status;
goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid;
goal_status.goal_info.stamp = goal_response->stamp;
goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING;
status_message.status_list.push_back(goal_status);
status_publisher->publish(status_message);

server_executor.spin_some();

// Allow server goal handling to proceed
disable_pending_handling_goal();

// Dual spin until goal handle future completes
dual_spin_until_future_complete(future_goal_handle);

auto goal_handle = future_goal_handle.get();
EXPECT_TRUE(goal_response_received);
ASSERT_NE(nullptr, goal_handle);
EXPECT_TRUE(
goal_handle->get_status() == rclcpp_action::GoalStatus::STATUS_ACCEPTED ||
goal_handle->get_status() == rclcpp_action::GoalStatus::STATUS_EXECUTING);
}


TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_result)
{
auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
Expand Down