-
Notifications
You must be signed in to change notification settings - Fork 417
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.
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
Refactor to use rcl #207
Refactor to use rcl #207
Changes from all commits
ea81bde
bf227d6
73bc7dd
93ba85d
0d16715
e0717f2
87adf80
ffb5da6
84cfe94
bb4cc98
85bd43b
61f9f34
69bb176
9bfbb8e
8473102
d65ae9b
cb8c54c
486e187
ea54212
06d66a4
b0fba6c
6bd0477
f3b1cc8
1965af9
ac5161e
da190eb
6992ed8
a7dcef5
0af71eb
2572a32
0a63018
321a17f
1ac062f
ad0f2c2
11a33fc
d37e00e
153cf13
355af64
88e2cd8
9d2661b
2472194
6c2296a
5fbd54b
ac1f22d
1c715fe
7b9e270
6739c79
c1fc16e
a67fe80
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -28,6 +28,11 @@ macro(get_rclcpp_information rmw_implementation var_prefix) | |
# so that the variables can be used by various functions / macros | ||
set(${var_prefix}_FOUND TRUE) | ||
|
||
# Get rcl using the existing macro | ||
if(NOT "${target_suffix} " STREQUAL " ") | ||
get_rcl_information("${rmw_implementation}" "rcl${target_suffix}") | ||
endif() | ||
|
||
# include directories | ||
normalize_path(${var_prefix}_INCLUDE_DIRS | ||
"${rclcpp_DIR}/../../../include") | ||
|
@@ -63,8 +68,7 @@ macro(get_rclcpp_information rmw_implementation var_prefix) | |
# dependencies | ||
set(_exported_dependencies | ||
"rcl_interfaces" | ||
"rmw" | ||
"${rmw_implementation}" | ||
"rcl${target_suffix}" | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Should There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I can add it back in, not sure why I removed it. |
||
"rosidl_generator_cpp") | ||
set(${var_prefix}_DEFINITIONS) | ||
foreach(_dep ${_exported_dependencies}) | ||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -23,10 +23,15 @@ | |
#include <tuple> | ||
#include <utility> | ||
|
||
#include "rcl/client.h" | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. nitpick: alpha-ordering |
||
#include "rcl/error_handling.h" | ||
|
||
#include "rclcpp/function_traits.hpp" | ||
#include "rclcpp/macros.hpp" | ||
#include "rclcpp/type_support_decl.hpp" | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. alpha |
||
#include "rclcpp/utilities.hpp" | ||
#include "rclcpp/visibility_control.hpp" | ||
|
||
#include "rmw/error_handling.h" | ||
#include "rmw/rmw.h" | ||
|
||
|
@@ -42,32 +47,31 @@ class ClientBase | |
|
||
RCLCPP_PUBLIC | ||
ClientBase( | ||
std::shared_ptr<rmw_node_t> node_handle, | ||
rmw_client_t * client_handle, | ||
std::shared_ptr<rcl_node_t> node_handle, | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Why did you remove the destructor declaration? I think it might be important that it is declared There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I'm not sure. Previously the child templated class There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. When I add it back I get:
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. It needs to be implemented or made pure virtual. I'd say give it an empty implementation in the |
||
const std::string & service_name); | ||
|
||
RCLCPP_PUBLIC | ||
virtual ~ClientBase(); | ||
~ClientBase(); | ||
|
||
RCLCPP_PUBLIC | ||
const std::string & | ||
get_service_name() const; | ||
|
||
RCLCPP_PUBLIC | ||
const rmw_client_t * | ||
const rcl_client_t * | ||
get_client_handle() const; | ||
|
||
virtual std::shared_ptr<void> create_response() = 0; | ||
virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0; | ||
virtual void handle_response( | ||
std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0; | ||
|
||
private: | ||
protected: | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Any rational here? making members protected is possibly the right thing to do, but it's not clear why you decided to do this. There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I decided to push some member variables that are not templated but need to be accessible by the child class into the parent class. |
||
RCLCPP_DISABLE_COPY(ClientBase); | ||
|
||
std::shared_ptr<rmw_node_t> node_handle_; | ||
std::shared_ptr<rcl_node_t> node_handle_; | ||
|
||
rmw_client_t * client_handle_; | ||
rcl_client_t client_handle_ = rcl_get_zero_initialized_client(); | ||
std::string service_name_; | ||
}; | ||
|
||
|
@@ -93,11 +97,32 @@ class Client : public ClientBase | |
RCLCPP_SMART_PTR_DEFINITIONS(Client); | ||
|
||
Client( | ||
std::shared_ptr<rmw_node_t> node_handle, | ||
rmw_client_t * client_handle, | ||
const std::string & service_name) | ||
: ClientBase(node_handle, client_handle, service_name) | ||
{} | ||
std::shared_ptr<rcl_node_t> node_handle, | ||
const std::string & service_name, | ||
rcl_client_options_t & client_options) | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. You changed the order of the arguments here, does this not need changes in the examples and stuff like that? There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. nvm, this is not used directly by the user. |
||
: ClientBase(node_handle, service_name) | ||
{ | ||
using rosidl_generator_cpp::get_service_type_support_handle; | ||
auto service_type_support_handle = | ||
get_service_type_support_handle<ServiceT>(); | ||
if (rcl_client_init(&client_handle_, this->node_handle_.get(), | ||
service_type_support_handle, service_name.c_str(), &client_options) != RCL_RET_OK) | ||
{ | ||
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here) | ||
throw std::runtime_error( | ||
std::string("could not create client: ") + | ||
rcl_get_error_string_safe()); | ||
// *INDENT-ON* | ||
} | ||
} | ||
|
||
virtual ~Client() | ||
{ | ||
if (rcl_client_fini(&client_handle_, node_handle_.get()) != RCL_RET_OK) { | ||
fprintf(stderr, | ||
"Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); | ||
} | ||
} | ||
|
||
std::shared_ptr<void> create_response() | ||
{ | ||
|
@@ -149,10 +174,10 @@ class Client : public ClientBase | |
{ | ||
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)) { | ||
if (RCL_RET_OK != rcl_send_request(get_client_handle(), request.get(), &sequence_number)) { | ||
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here) | ||
throw std::runtime_error( | ||
std::string("failed to send request: ") + rmw_get_error_string_safe()); | ||
std::string("failed to send request: ") + rcl_get_error_string_safe()); | ||
// *INDENT-ON* | ||
} | ||
|
||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -25,6 +25,9 @@ | |
#include <string> | ||
#include <vector> | ||
|
||
#include "rcl/guard_condition.h" | ||
#include "rcl/wait.h" | ||
|
||
#include "rclcpp/any_executable.hpp" | ||
#include "rclcpp/macros.hpp" | ||
#include "rclcpp/memory_strategies.hpp" | ||
|
@@ -193,10 +196,12 @@ class Executor | |
} | ||
|
||
auto end_time = std::chrono::steady_clock::now(); | ||
if (timeout > std::chrono::nanoseconds::zero()) { | ||
end_time += timeout; | ||
std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>( | ||
timeout); | ||
if (timeout_ns > std::chrono::nanoseconds::zero()) { | ||
end_time += timeout_ns; | ||
} | ||
auto timeout_left = timeout; | ||
std::chrono::nanoseconds timeout_left = timeout_ns; | ||
|
||
while (rclcpp::utilities::ok()) { | ||
// Do one item of work. | ||
|
@@ -207,7 +212,7 @@ class Executor | |
return FutureReturnCode::SUCCESS; | ||
} | ||
// If the original timeout is < 0, then this is blocking, never TIMEOUT. | ||
if (timeout < std::chrono::nanoseconds::zero()) { | ||
if (timeout_ns < std::chrono::nanoseconds::zero()) { | ||
continue; | ||
} | ||
// Otherwise check if we still have time to wait, return TIMEOUT if not. | ||
|
@@ -216,8 +221,7 @@ class Executor | |
return FutureReturnCode::TIMEOUT; | ||
} | ||
// Subtract the elapsed time from the original timeout. | ||
using duration_type = std::chrono::duration<int64_t, TimeT>; | ||
timeout_left = std::chrono::duration_cast<duration_type>(end_time - now); | ||
timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now); | ||
} | ||
|
||
// The future did not complete before ok() returned false, return INTERRUPTED. | ||
|
@@ -291,10 +295,6 @@ class Executor | |
void | ||
get_next_timer(AnyExecutable::SharedPtr any_exec); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Why was this removed? There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. nvm, I guess it's probably handled by |
||
|
||
RCLCPP_PUBLIC | ||
std::chrono::nanoseconds | ||
get_earliest_timer(); | ||
|
||
RCLCPP_PUBLIC | ||
AnyExecutable::SharedPtr | ||
get_next_ready_executable(); | ||
|
@@ -307,10 +307,10 @@ class Executor | |
std::atomic_bool spinning; | ||
|
||
/// Guard condition for signaling the rmw layer to wake up for special events. | ||
rmw_guard_condition_t * interrupt_guard_condition_; | ||
rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); | ||
|
||
/// Waitset for managing entities that the rmw layer waits on. | ||
rmw_waitset_t * waitset_; | ||
rcl_wait_set_t waitset_ = rcl_get_zero_initialized_wait_set(); | ||
|
||
/// The memory strategy: an interface for handling user-defined memory allocation strategies. | ||
memory_strategy::MemoryStrategy::SharedPtr memory_strategy_; | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Is
rclcpp
still usingrmw
in places? Can/should references tormw
be removed from therclcpp
build files?