commit
3619cd880d
6 changed files with 23 additions and 18 deletions
|
@ -270,7 +270,7 @@ protected:
|
|||
}));
|
||||
}
|
||||
// Use the number of subscriptions to allocate memory in the handles
|
||||
size_t number_of_subscriptions = subs.size();
|
||||
unsigned long number_of_subscriptions = subs.size();
|
||||
rmw_subscriptions_t subscriber_handles;
|
||||
subscriber_handles.subscriber_count = number_of_subscriptions;
|
||||
// TODO(wjwwood): Avoid redundant malloc's
|
||||
|
@ -291,7 +291,7 @@ protected:
|
|||
}
|
||||
|
||||
// Use the number of services to allocate memory in the handles
|
||||
size_t number_of_services = services.size();
|
||||
unsigned long number_of_services = services.size();
|
||||
rmw_services_t service_handles;
|
||||
service_handles.service_count = number_of_services;
|
||||
// TODO(esteve): Avoid redundant malloc's
|
||||
|
@ -312,7 +312,7 @@ protected:
|
|||
}
|
||||
|
||||
// Use the number of clients to allocate memory in the handles
|
||||
size_t number_of_clients = clients.size();
|
||||
unsigned long number_of_clients = clients.size();
|
||||
rmw_clients_t client_handles;
|
||||
client_handles.client_count = number_of_clients;
|
||||
// TODO: Avoid redundant malloc's
|
||||
|
@ -335,7 +335,8 @@ protected:
|
|||
// Use the number of guard conditions to allocate memory in the handles
|
||||
// Add 2 to the number for the ctrl-c guard cond and the executor's
|
||||
size_t start_of_timer_guard_conds = 2;
|
||||
size_t number_of_guard_conds = timers.size() + start_of_timer_guard_conds;
|
||||
unsigned long number_of_guard_conds =
|
||||
timers.size() + start_of_timer_guard_conds;
|
||||
rmw_guard_conditions_t guard_condition_handles;
|
||||
guard_condition_handles.guard_condition_count = number_of_guard_conds;
|
||||
// TODO(wjwwood): Avoid redundant malloc's
|
||||
|
|
|
@ -58,11 +58,11 @@ public:
|
|||
std::vector<std::thread> threads;
|
||||
{
|
||||
std::lock_guard<std::mutex> wait_lock(wait_mutex_);
|
||||
size_t thread_id = 1;
|
||||
size_t thread_id_ = 1; // Use a _ suffix to avoid shadowing `rclcpp::thread_id`
|
||||
for (size_t i = number_of_threads_; i > 0; --i)
|
||||
{
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id++);
|
||||
auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id_++);
|
||||
threads.emplace_back(func);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -135,7 +135,8 @@ private:
|
|||
} /* namespace node */
|
||||
} /* namespace rclcpp */
|
||||
|
||||
#define RCLCPP_REGISTER_NODE(Class) rclcpp::node::Node::SharedPtr \
|
||||
#define RCLCPP_REGISTER_NODE(Class) RMW_EXPORT \
|
||||
rclcpp::node::Node::SharedPtr \
|
||||
create_node() \
|
||||
{ \
|
||||
return rclcpp::node::Node::SharedPtr(new Class(rclcpp::contexts::default_context::DefaultContext::make_shared())); \
|
||||
|
|
|
@ -27,24 +27,26 @@
|
|||
namespace rclcpp
|
||||
{
|
||||
|
||||
constexpr std::chrono::seconds operator "" _s(unsigned long long s)
|
||||
const std::chrono::seconds operator "" _s(unsigned long long s)
|
||||
{
|
||||
return std::chrono::seconds(s);
|
||||
}
|
||||
constexpr std::chrono::duration<long double> operator "" _s(long double s)
|
||||
const std::chrono::nanoseconds operator "" _s(long double s)
|
||||
{
|
||||
return std::chrono::duration<long double>(s);
|
||||
return std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::duration<long double>(s));
|
||||
}
|
||||
|
||||
constexpr std::chrono::nanoseconds
|
||||
const std::chrono::nanoseconds
|
||||
operator "" _ns(unsigned long long ns)
|
||||
{
|
||||
return std::chrono::nanoseconds(ns);
|
||||
}
|
||||
constexpr std::chrono::duration<long double, std::nano>
|
||||
const std::chrono::nanoseconds
|
||||
operator "" _ns(long double ns)
|
||||
{
|
||||
return std::chrono::duration<long double, std::nano>(ns);
|
||||
return std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::duration<long double, std::nano>(ns));
|
||||
}
|
||||
|
||||
using rclcpp::node::Node;
|
||||
|
|
|
@ -27,6 +27,7 @@
|
|||
#include <mutex>
|
||||
#include <thread>
|
||||
|
||||
#include <rmw/macros.h>
|
||||
#include <rmw/rmw.h>
|
||||
|
||||
// Determine if sigaction is available
|
||||
|
@ -89,7 +90,7 @@ namespace
|
|||
namespace rclcpp
|
||||
{
|
||||
|
||||
__thread size_t thread_id = 0;
|
||||
RMW_THREAD_LOCAL size_t thread_id = 0;
|
||||
|
||||
namespace utilities
|
||||
{
|
||||
|
@ -115,6 +116,7 @@ init(int argc, char *argv[])
|
|||
throw std::runtime_error(
|
||||
std::string("Failed to set SIGINT signal handler: (" +
|
||||
std::to_string(errno) + ")") +
|
||||
// TODO(wjwwood): use strerror_r on POSIX and strerror_s on Windows.
|
||||
std::strerror(errno));
|
||||
}
|
||||
}
|
||||
|
@ -131,13 +133,12 @@ get_global_sigint_guard_condition()
|
|||
return ::g_sigint_guard_cond_handle;
|
||||
}
|
||||
|
||||
template<class Rep, class Period>
|
||||
bool
|
||||
sleep_for(const std::chrono::duration<Rep, Period>& sleep_duration)
|
||||
sleep_for(const std::chrono::nanoseconds& nanoseconds)
|
||||
{
|
||||
// TODO: determine if posix's nanosleep(2) is more efficient here
|
||||
std::unique_lock<std::mutex> lock(::g_interrupt_mutex);
|
||||
auto cvs = ::g_interrupt_condition_variable.wait_for(lock, sleep_duration);
|
||||
auto cvs = ::g_interrupt_condition_variable.wait_for(lock, nanoseconds);
|
||||
return cvs == std::cv_status::no_timeout;
|
||||
}
|
||||
|
||||
|
|
|
@ -16,7 +16,7 @@
|
|||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
// This forward declaration is implemented by the RCLCPP_REGISTER_NODE macro
|
||||
rclcpp::Node::SharedPtr create_node();
|
||||
RMW_IMPORT rclcpp::Node::SharedPtr create_node();
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue