niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

potential fix for issue 192

Browse source
This commit is contained in:
Jackie Kay 2016-02-03 14:37:33 -08:00 • committed by Dirk Thomas
parent 6fbf3f8c5f
commit 4a04fe8b4a
5 changed files with 21 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

4
rclcpp/include/rclcpp/callback_group.hpp
View file

@ -66,7 +66,7 @@ public:
get_service_ptrs() const;
RCLCPP_PUBLIC
const std::vector<rclcpp::client::ClientBase::SharedPtr> &
const std::vector<rclcpp::client::ClientBase::WeakPtr> &
get_client_ptrs() const;
RCLCPP_PUBLIC
@ -100,7 +100,7 @@ private:
std::vector<rclcpp::subscription::SubscriptionBase::WeakPtr> subscription_ptrs_;
std::vector<rclcpp::timer::TimerBase::WeakPtr> timer_ptrs_;
std::vector<rclcpp::service::ServiceBase::SharedPtr> service_ptrs_;
std::vector<rclcpp::client::ClientBase::SharedPtr> client_ptrs_;
std::vector<rclcpp::client::ClientBase::WeakPtr> client_ptrs_;
std::atomic_bool can_be_taken_from_;
};

14
rclcpp/include/rclcpp/client.hpp
View file

@ -60,7 +60,7 @@ public:
virtual std::shared_ptr<void> create_response() = 0;
virtual std::shared_ptr<void> create_request_header() = 0;
virtual void handle_response(
std::shared_ptr<void> & request_header, std::shared_ptr<void> & response) = 0;
std::shared_ptr<void> request_header, std::shared_ptr<void> response) = 0;
private:
RCLCPP_DISABLE_COPY(ClientBase);
@ -111,13 +111,17 @@ public:
return std::shared_ptr<void>(new rmw_request_id_t);
}
void handle_response(std::shared_ptr<void> & request_header, std::shared_ptr<void> & response)
void handle_response(std::shared_ptr<void> request_header, std::shared_ptr<void> response)
{
std::lock_guard<std::mutex> lock(pending_requests_mutex_);
auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
int64_t sequence_number = typed_request_header->sequence_number;
// TODO(esteve) this must check if the sequence_number is valid otherwise the
// call_promise will be null
// TODO(esteve) this should throw instead since it is not expected to happen in the first place
if (this->pending_requests_.count(sequence_number) == 0) {
fprintf(stderr, "Received invalid sequence number. Ignoring...\n");
return;
}
auto tuple = this->pending_requests_[sequence_number];
auto call_promise = std::get<0>(tuple);
auto callback = std::get<1>(tuple);
@ -143,6 +147,7 @@ public:
>
SharedFuture async_send_request(SharedRequest request, CallbackT && cb)
{
std::lock_guard<std::mutex> lock(pending_requests_mutex_);
int64_t sequence_number;
if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
@ -187,6 +192,7 @@ private:
RCLCPP_DISABLE_COPY(Client);
std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
std::mutex pending_requests_mutex_;
};
} // namespace client

3
rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
View file

@ -151,7 +151,8 @@ public:
services_.push_back(service);
}
}
for (auto & client : group->get_client_ptrs()) {
for (auto & weak_client : group->get_client_ptrs()) {
auto client = weak_client.lock();
if (client) {
clients_.push_back(client);
}

2
rclcpp/src/rclcpp/callback_group.cpp
View file

@ -41,7 +41,7 @@ CallbackGroup::get_service_ptrs() const
return service_ptrs_;
}
const std::vector<rclcpp::client::ClientBase::SharedPtr> &
const std::vector<rclcpp::client::ClientBase::WeakPtr> &
CallbackGroup::get_client_ptrs() const
{
return client_ptrs_;

10
rclcpp/src/rclcpp/memory_strategy.cpp
View file

@ -84,8 +84,9 @@ MemoryStrategy::get_client_by_handle(void * client_handle, const WeakNodeVector
if (!group) {
continue;
}
for (auto & client : group->get_client_ptrs()) {
if (client->get_client_handle()->data == client_handle) {
for (auto & weak_client : group->get_client_ptrs()) {
auto client = weak_client.lock();
if (client && client->get_client_handle()->data == client_handle) {
return client;
}
}
@ -182,8 +183,9 @@ MemoryStrategy::get_group_by_client(
if (!group) {
continue;
}
for (auto & cli : group->get_client_ptrs()) {
if (cli == client) {
for (auto & weak_client : group->get_client_ptrs()) {
auto cli = weak_client.lock();
if (cli && cli == client) {
return group;
}
}