From 75fafe948a8767b5c9fc65943c108995fd848221 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 23 Nov 2015 19:15:50 -0800 Subject: [PATCH 01/71] new rcl headers for init, node, pub/sub, gc, wait --- rcl/include/rcl/allocator.h | 50 +++++ rcl/include/rcl/error_handling.h | 38 ++++ rcl/include/rcl/guard_condition.h | 140 ++++++++++++++ rcl/include/rcl/node.h | 168 +++++++++++++++++ rcl/include/rcl/publisher.h | 267 +++++++++++++++++++++++++++ rcl/include/rcl/rcl.h | 185 ++++++------------- rcl/include/rcl/subscription.h | 256 ++++++++++++++++++++++++++ rcl/include/rcl/time.h | 31 ++++ rcl/include/rcl/types.h | 49 ++--- rcl/include/rcl/wait.h | 294 ++++++++++++++++++++++++++++++ 10 files changed, 1316 insertions(+), 162 deletions(-) create mode 100644 rcl/include/rcl/allocator.h create mode 100644 rcl/include/rcl/error_handling.h create mode 100644 rcl/include/rcl/guard_condition.h create mode 100644 rcl/include/rcl/node.h create mode 100644 rcl/include/rcl/publisher.h create mode 100644 rcl/include/rcl/subscription.h create mode 100644 rcl/include/rcl/time.h create mode 100644 rcl/include/rcl/wait.h diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h new file mode 100644 index 0000000..44b509d --- /dev/null +++ b/rcl/include/rcl/allocator.h @@ -0,0 +1,50 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__ALLOCATOR_H_ +#define RCL__ALLOCATOR_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/types.h" + +/// Encapsulation of an allocator. +/* NULL for allocate implies that the system should use malloc and free. */ +typedef struct rcl_allocator_t { + /// Allocate memory, given a size and state structure. + /* An error should be indicated by returning null. */ + void * (*allocate)(size_t size, void * state); + /// Deallocate previously allocated memory, mimicking free(). + void (*deallocate)(void * pointer, void * state); + /// Reallocates if possible, otherwise it deallocates and allocates. + /* If unsupported then do deallocate and then allocate. */ + void * (*reallocate)(void * pointer, size_t size, void * state); + /// Implementation defined state storage. + /* This is passed as the second parameter to the (de)allocate functions. */ + void * state; +} rcl_allocator_t; + +/// Return a properly initialized rcl_allocator_t with default values. +/* This function does not allocate memory. */ +rcl_allocator_t +rcl_get_default_allocator(); + +#if __cplusplus +} +#endif + +#endif // RCL__ALLOCATOR_H_ diff --git a/rcl/include/rcl/error_handling.h b/rcl/include/rcl/error_handling.h new file mode 100644 index 0000000..1e81e95 --- /dev/null +++ b/rcl/include/rcl/error_handling.h @@ -0,0 +1,38 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__ERROR_HANDLING_H_ +#define RCL__ERROR_HANDLING_H_ + +#include "rmw/error_handling.h" + +/* The error handling in RCL is just an alias to the error handling in RMW. */ + +typedef rmw_error_state_t rcl_error_state_t; + +#define rcl_set_error_state rmw_set_error_state + +#define RCL_SET_ERROR_MSG(msg) RMW_SET_ERROR_MSG(msg) + +#define rcl_error_is_set rmw_error_is_set + +#define rcl_get_error_state rmw_get_error_state + +#define rcl_get_error_string rmw_get_error_string + +#define rcl_get_error_string_safe rmw_get_error_string_safe + +#define rcl_reset_error rmw_reset_error + +#endif // RCL__ERROR_HANDLING_H_ diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h new file mode 100644 index 0000000..b4a9b82 --- /dev/null +++ b/rcl/include/rcl/guard_condition.h @@ -0,0 +1,140 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__GUARD_CONDITION_H_ +#define RCL__GUARD_CONDITION_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/node.h" + +/// Internal rcl guard condition implementation struct. +struct rcl_guard_condition_impl_t; + +/// Handle for a rcl guard condition. +typedef struct rcl_guard_condition_t { + struct rcl_guard_condition_impl_t * impl; +} rcl_guard_condition_t; + +/// Options available for a rcl guard condition. +typedef struct rcl_guard_condition_options_t { + /// Custom allocator for the guard condition, used for incidental allocations. + /* For default behavior (malloc/free), use: rcl_get_default_allocator() */ + rcl_allocator_t allocator; +} rcl_guard_condition_options_t; + +/// Return a rcl_guard_condition_t struct with members set to NULL. +/* Should be called to get a null rcl_guard_condition_t before passing to + * rcl_initalize_guard_condition(). + * It's also possible to use calloc() instead of this if the rcl guard condition is + * being allocated on the heap. + */ +rcl_guard_condition_t +rcl_get_zero_initialized_guard_condition(); + +/// Initialize a rcl guard_condition. +/* After calling this function on a rcl_guard_condition_t, it can be passed to + * rcl_wait and can be triggered to wake rcl_wait up. + * + * The given rcl_node_t must be valid and the resulting rcl_guard_condition_t + * is only valid as long as the given rcl_node_t remains valid. + * + * Expected usage: + * + * #include + * + * rcl_node_t node = rcl_get_zero_initialized_node(); + * rcl_node_options_t node_ops = rcl_node_get_default_options(); + * rcl_ret_t ret = rcl_node_init(&node, "node_name", &node_ops); + * // ... error handling + * rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition(); + * rcl_guard_condition_options_t guard_condition_ops = rcl_guard_condition_get_default_options(); + * ret = rcl_guard_condition_init(&guard_condition, &node, ts, "chatter", &guard_condition_ops); + * // ... error handling, and on shutdown do deinitialization: + * ret = rcl_guard_condition_fini(&guard_condition, &node); + * // ... error handling for rcl_guard_condition_fini() + * ret = rcl_node_fini(&node); + * // ... error handling for rcl_deinitialize_node() + * + * This function is not thread-safe. + * + * \param[inout] guard_condition preallocated guard_condition structure + * \param[in] node valid rcl node handle + * \param[in] options the guard_condition's options + * \return RMW_RET_OK if guard_condition was initialized successfully, otherwise RMW_RET_ERROR + */ +rcl_ret_t +rcl_guard_condition_init( + rcl_guard_condition_t * guard_condition, + const rcl_node_t * node, + const rcl_guard_condition_options_t * options); + +/// Deinitialize a rcl_guard_condition_t. +/* After calling, calls to rcl_trigger_guard_condition will fail when using + * this guard condition. + * However, the given node handle is still valid. + * + * This function is not thread-safe. + * + * \param[inout] guard_condition handle to the guard_condition to be deinitialized + * \param[in] node handle to the node used to create the guard_condition + * \return RMW_RET_OK if guard_condition was deinitialized successfully, otherwise RMW_RET_ERROR + */ +rcl_ret_t +rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * node); + +/// Return the default guard_condition options in a rcl_guard_condition_options_t. +rcl_guard_condition_options_t +rcl_guard_condition_get_default_options(); + +/// Trigger a rcl guard condition. +/* This function can fail, and therefore return NULL, if the: + * - guard condition is NULL + * - guard condition is invalid (never called init, called fini, or invalid node) + * + * A guard condition can be triggered from any thread. + * + * This function is thread-safe with itself, but cannot be called concurrently + * with rcl_guard_condition_fini() on the same guard condition. + * + * \param[in] guard_condition handle to the guard_condition to be triggered + * \return RMW_RET_OK if the message was published, otherwise RMW_RET_ERROR + */ +rcl_ret_t +rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition); + +/// Return the rmw guard condition handle. +/* The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return NULL, if the: + * - guard_condition is NULL + * - guard_condition is invalid (never called init, called fini, or invalid node) + * + * The returned handle is only valid as long as the given guard_condition is valid. + * + * \TODO(wjwwood) should the return value of this be const? + * + * \param[in] guard_condition pointer to the rcl guard_condition + * \return rmw guard_condition handle if successful, otherwise NULL + */ +rmw_guard_condition_t * +rcl_guard_condition_get_rmw_guard_condition_handle(rcl_guard_condition_t * guard_condition); + +#if __cplusplus +} +#endif + +#endif // RCL__GUARD_CONDITION_H_ diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h new file mode 100644 index 0000000..8d8a128 --- /dev/null +++ b/rcl/include/rcl/node.h @@ -0,0 +1,168 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__NODE_H_ +#define RCL__NODE_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/allocator.h" +#include "rcl/types.h" + +struct rcl_node_impl_t; + +/// Handle for a ROS node. +/* This handle can be in one a few stats, which are referred to below: + * - uninitialized: memory allocated, but not initialized to anything + * - invalid: memory allocated but not usable + * - either zero initialized or shutdown (rcl_shutdown() or rcl_node_fini()) + * - valid: memory allocated and usable + */ +typedef struct rcl_node_t { + /// Private implementation pointer. + struct rcl_node_impl_t * impl; +} rcl_node_t; + +typedef struct rcl_node_options_t { + // bool anonymous_name; + // rmw_qos_profile_t parameter_qos; + /// If true, no parameter infrastructure will be setup. + bool no_parameters; + /// Custom allocator used for incidental allocations, e.g. node name string. + rcl_allocator_t allocator; +} rcl_node_options_t; + +/// Return a rcl_node_t struct with members initialized to NULL. +/* Should be called to get rcl_node_t before passing to rcl_node_init(). + * It's also possible to use calloc() instead of this if the rcl_node is being + * allocated on the heap. + */ +rcl_node_t +rcl_get_zero_initialized_node(); + +/// Initialize a ROS node. +/* Calling this on a rcl_node_t makes it a valid node handle until rcl_shutdown + * is called or until rcl_node_fini is called on it. + * + * After calling the ROS node object can be used to create other middleware + * primitives like publishers, services, parameters, and etc. + * + * The name of the node cannot coincide with another node of the same name. + * If a node of the same name is already in the domain, it will be shutdown. + * + * A node contains infrastructure for ROS parameters, which include advertising + * publishers and service servers. + * So this function will create those external parameter interfaces even if + * parameters are not used later. + * + * This function should be called on a rcl_node_t exactly once, and calling it + * multiple times is undefined behavior. + * + * Expected usage: + * + * rcl_node_t node = rcl_get_zero_initialized_node(); + * rcl_node_options_t * node_ops = rcl_node_get_default_options(); + * rcl_ret_t ret = rcl_node_init(&node, "node_name", node_ops); + * // ... error handling and then use the node, but finally deinitialize it: + * ret = rcl_node_fini(&node); + * // ... error handling for rcl_node_fini() + * + * This function is not thread-safe. + * + * \pre the node handle must be allocated, zero initialized, and invalid + * \post the node handle is valid and can be used to in other rcl_* functions + * + * \param[inout] node a preallocated node structure + * \param[in] name the name of the node + * \param[in] options the node options; pass null to use default options + * \return RMW_RET_OK if node was initialized successfully, otherwise RMW_RET_ERROR + */ +rcl_ret_t +rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options); + +/// Deinitialize a rcl_node_t. +/* Shuts down any automatically created infrastructure and deallocates memory. + * After calling, the rcl_node_t can be safely released. + * + * Any middleware primitives created by the user, e.g. publishers, services, etc., + * are invalid after deinitialization. + * + * This function is not thread-safe. + * + * \param[in] node handle to the node to be deinitialized + * \return RMW_RET_OK if node was deinitialized successfully, otherwise RMW_RET_ERROR + */ +rcl_ret_t +rcl_node_fini(rcl_node_t * node); + +/// Return the default node options in a rcl_node_options_t. +rcl_node_options_t +rcl_node_get_default_options(); + +/// Get the name of the node. +/* This function returns the node's internal name string. + * This function can fail, and therefore return NULL, if: + * - node is NULL + * - node has not been initialized (the implementation is invalid) + * + * The returned string is only valid as long as the given rcl_node_t is valid. + * The value of the string may change if the value in the rcl_node_t changes, + * and therefore copying the string is recommended if this is a concern. + * + * \param[in] node pointer to the node + * \return name string if successful, otherwise NULL + */ +const char * +rcl_node_get_name(const rcl_node_t * node); + +/// Return the rcl node options. +/* This function returns the node's internal options struct. + * This function can fail, and therefore return NULL, if: + * - node is NULL + * - node has not been initialized (the implementation is invalid) + * + * The returned struct is only valid as long as the given rcl_node_t is valid. + * The values in the struct may change if the options of the rcl_node_t changes, + * and therefore copying the struct is recommended if this is a concern. + * + * \param[in] node pointer to the node + * \return options struct if successful, otherwise NULL + */ +const rcl_node_options_t * +rcl_node_get_options(const rcl_node_t * node); + +/// Return the rmw node handle. +/* The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return NULL, if: + * - node is NULL + * - node has not been initialized (the implementation is invalid) + * + * The returned handle is only valid as long as the given node is valid. + * + * \TODO(wjwwood) should the return value of this be const? + * + * \param[in] node pointer to the rcl node + * \return rmw node handle if successful, otherwise NULL + */ +rmw_node_t * +rcl_node_get_rmw_node_handle(const rcl_node_t * node); + +#if __cplusplus +} +#endif + +#endif // RCL__NODE_H_ diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h new file mode 100644 index 0000000..0ce2548 --- /dev/null +++ b/rcl/include/rcl/publisher.h @@ -0,0 +1,267 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__PUBLISHER_H_ +#define RCL__PUBLISHER_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include "rosidl_generator_c/message_type_support.h" + +#include "rcl/node.h" + +/// Internal rcl publisher implementation struct. +struct rcl_publisher_impl_t; + +/// Handle for a rcl publisher. +typedef struct rcl_publisher_t { + struct rcl_publisher_impl_t * impl; +} rcl_publisher_t; + +/// Options available for a rcl publisher. +typedef struct rcl_publisher_options_t { + /// Middleware quality of service settings for the publisher. + rmw_qos_profile_t qos; + /// Custom allocator for the publisher, used for incidental allocations. + /* For default behavior (malloc/free), use: rcl_get_default_allocator() */ + rcl_allocator_t allocator; +} rcl_publisher_options_t; + +/// Return a rcl_publisher_t struct with members set to NULL. +/* Should be called to get a null rcl_publisher_t before passing to + * rcl_initalize_publisher(). + * It's also possible to use calloc() instead of this if the rcl_publisher is + * being allocated on the heap. + */ +rcl_publisher_t +rcl_get_zero_initialized_publisher(); + +/// Initialize a rcl publisher. +/* After calling this function on a rcl_publisher_t, it can be used to publish + * messages of the given type to the given topic using rcl_publish(). + * + * The given rcl_node_t must be valid and the resulting rcl_publisher_t is only + * valid as long as the given rcl_node_t remains valid. + * + * The rosidl_message_type_support_t is obtained on a per .msg type basis. + * When the user defines a ROS message, code is generated which provides the + * required rosidl_message_type_support_t object. + * This object can be obtained using a language appropriate mechanism. + * \TODO(wjwwood) probably should talk about this once and link to it instead + * For C this macro can be used (using std_msgs/String as an example): + * + * #include + * #include + * rosidl_message_type_support_t * string_ts = + * ROSIDL_GET_MESSAGE_TYPE_SUPPORT(std_msgs, String); + * + * For C++ a template function is used: + * + * #include + * #include + * rosidl_message_type_support_t * string_ts = + * rosidl_generator_cpp::get_message_type_support_handle(); + * + * The rosidl_message_type_support_t object contains message type specific + * information used to publish messages. + * + * \TODO(wjwwood) update this once we've come up with an official scheme. + * The topic name must be a non-empty string which follows the topic naming + * format. + * + * The options struct allows the user to set the quality of service settings as + * well as a custom allocator which is used when (de)initializing the publisher + * to allocate space for incidental things, e.g. the topic name string. + * + * Expected usage (for C messages): + * + * #include + * #include + * #include + * + * rcl_node_t node = rcl_get_zero_initialized_node(); + * rcl_node_options_t node_ops = rcl_node_get_default_options(); + * rcl_ret_t ret = rcl_node_init(&node, "node_name", &node_ops); + * // ... error handling + * rosidl_message_type_support_t * ts = ROSIDL_GET_MESSAGE_TYPE_SUPPORT(std_msgs, String); + * rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + * rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options(); + * ret = rcl_publisher_init(&publisher, &node, ts, "chatter", &publisher_ops); + * // ... error handling, and on shutdown do deinitialization: + * ret = rcl_publisher_fini(&publisher, &node); + * // ... error handling for rcl_publisher_fini() + * ret = rcl_node_fini(&node); + * // ... error handling for rcl_deinitialize_node() + * + * This function is not thread-safe. + * + * \param[inout] publisher preallocated publisher structure + * \param[in] node valid rcl node handle + * \param[in] type_support type support object for the topic's type + * \param[in] topic_name the name of the topic to publish on + * \param[in] options publisher options, including quality of service settings + * \return RMW_RET_OK if publisher was initialized successfully, otherwise RMW_RET_ERROR + */ +rcl_ret_t +rcl_publisher_init( + rcl_publisher_t * publisher, + const rcl_node_t * node, + const rosidl_message_type_support_t * type_support, + const char * topic_name, + const rcl_publisher_options_t * options); + +/// Deinitialize a rcl_publisher_t. +/* After calling, the node will no longer be advertising that it is publishing + * on this topic (assuming this is the only publisher on this topic). + * + * After calling, calls to rcl_publish will fail when using this publisher. + * However, the given node handle is still valid. + * + * This function is not thread-safe. + * + * \param[inout] publisher handle to the publisher to be deinitialized + * \param[in] node handle to the node used to create the publisher + * \return RMW_RET_OK if publisher was deinitialized successfully, otherwise RMW_RET_ERROR + */ +rcl_ret_t +rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node); + +/// Return the default publisher options in a rcl_publisher_options_t. +rcl_publisher_options_t +rcl_publisher_get_default_options(); + +// TODO(wjwwood): I have an idea on how to do a type-checking macro version of +// publish using C11's _Generic "generic selection", but it +// needs support from the generated code (look into it later) +// It might also use static assert, see: +// http://www.robertgamble.net/2012/01/c11-static-assertions.html +// #define RCL_PUBLISH(publisher, ros_message) ... + +/// Publish a ROS message on a topic using a publisher. +/* It is the job of the caller to ensure that the type of the ros_message + * parameter and the type associate with the publisher (via the type support) + * match. + * Passing a different type to publish produces undefined behavior and cannot + * be checked by this function and therefore no deliberate error will occur. + * + * TODO(wjwwood): update after researching what the blocking behavior is + * or optionally link to a document that describes blocking + * behavior is more detail. + * Calling rcl_publish is a potentially blocking call. + * When called rcl_publish will immediately do any publishing related work, + * including, but not limited to, converting the message into a different type, + * serializing the message, collecting publish statistics, etc. + * The last thing it will do is call the underlying middleware's publish + * function which may or may not block based on the quality of service settings + * given via the publisher options in rcl_publisher_init(). + * For example, if the reliability is set to reliable, then a publish may block + * until space in the publish queue is available, but if the reliability is set + * to best effort then it should not block. + * + * The ROS message given by the ros_message void pointer is always owned by the + * calling code, but should remain constant during publish. + * + * This function is thread safe so long as access to both the publisher and the + * ros_message is synchronized. + * That means that calling rcl_publish from multiple threads is allowed, but + * calling rcl_publish at the same time as non-thread safe publisher functions + * is not, e.g. calling rcl_publish and rcl_publisher_fini concurrently + * is not allowed. + * Before calling rcl_publish the message can change and after calling + * rcl_publish the message can change, but it cannot be changed during the + * publish call. + * The same ros_message, however, can be passed to multiple calls of + * rcl_publish simultaneously, even if the publishers differ. + * The ros_message is unmodified by rcl_publish. + * + * \param[in] publisher handle to the publisher which will do the publishing + * \param[in] ros_message type-erased pointer to the ROS message + * \return RMW_RET_OK if the message was published, otherwise RMW_RET_ERROR + */ +rcl_ret_t +rcl_publish(const rcl_publisher_t * publisher, const void * ros_message); + +/// Get the topic name for the publisher. +/* This function returns the publisher's internal topic name string. + * This function can fail, and therefore return NULL, if the: + * - publisher is NULL + * - publisher is invalid (never called init, called fini, or invalid node) + * + * The returned string is only valid as long as the rcl_publisher_t is valid. + * The value of the string may change if the topic name changes, and therefore + * copying the string is recommended if this is a concern. + * + * This function is not thread-safe, and copying the result is not thread-safe. + * \TODO(wjwwood): should we provide a thread-safe copy_topic_name? + * + * \param[in] publisher pointer to the publisher + * \return name string if successful, otherwise NULL + */ +const char * +rcl_publisher_get_topic_name(const rcl_publisher_t * publisher); + +/// Return the rcl publisher options. +/* This function returns the publisher's internal options struct. + * This function can fail, and therefore return NULL, if the: + * - publisher is NULL + * - publisher is invalid (never called init, called fini, or invalid node) + * + * The returned struct is only valid as long as the rcl_publisher_t is valid. + * The values in the struct may change if the options of the publisher change, + * and therefore copying the struct is recommended if this is a concern. + * + * This function is not thread-safe, and copying the result is not thread-safe. + * \TODO(wjwwood): should we provide a thread-safe copy_publisher_options? + * + * \param[in] publisher pointer to the publisher + * \return options struct if successful, otherwise NULL + */ +const rcl_publisher_options_t * +rcl_publisher_get_options(rcl_publisher_t * publisher); + +/// Return the rmw publisher handle. +/* The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return NULL, if the: + * - publisher is NULL + * - publisher is invalid (never called init, called fini, or invalid node) + * + * The returned handle is only valid as long as the given publisher is valid. + * + * \TODO(wjwwood) will the publisher ever need to remove and recreate the rmw + * handle under the hood? Perhaps not now, but if we add the + * ability to reconfigure, alias, or remap topics, and don't + * implement that into rmw, then we should consider the + * implications here. This argument applies to other parts of + * the interface, but I'll just mention it here. + * + * \TODO(wjwwood) should the return value of this be const? + * + * \param[in] publisher pointer to the rcl publisher + * \return rmw publisher handle if successful, otherwise NULL + */ +rmw_publisher_t * +rcl_publisher_get_rmw_publisher_handle(rcl_publisher_t * publisher); + +int main() { + +} + +#if __cplusplus +} +#endif + +#endif // RCL__PUBLISHER_H_ diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 5ee1fe9..9cc89f0 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -15,140 +15,71 @@ #ifndef RCL__RCL_H_ #define RCL__RCL_H_ -#include // For bool -#include // For size_t +#if __cplusplus +extern "C" +{ +#endif -// For rosidl_message_type_support_t -#include +#include "rcl/node.h" +#include "rcl/publisher.h" +#include "rcl/subscription.h" +#include "rcl/types.h" -#include "rcl/types.h" // For rcl_*_t types - -/// Global initialization for rcl; should be called once per process. +/// Global initialization of rcl. +/* Unless otherwise noted, this must be called before using any rcl functions. + * + * This function can only be run once after starting the program, and once + * after each call to rcl_shutdown. + * Repeated calls will fail with RCL_RET_ALREADY_INIT. + * This function is not thread safe. + * + * This function can be called any time after rcl_shutdown is called, but it + * cannot be called from within a callback being executed by an rcl executor. + * For example, you can call rcl_shutdown from within a timer callback, but + * you have to return from the callback, and therefore exit any in-progress + * call to a spin function, before calling rcl_init again. + * + * The argc and argv parameters can contain command line arguments for the + * program. + * rcl specific arguments will be parsed and removed, but other arguments will + * be ignored. + * + * \param[in] argc number of strings in argv + * \param[in] argv command line arguments; rcl specific arguments are removed + * \return RCL_RET_OK if initialization is successful, otherwise RCL_RET_ERROR + * or RCL_RET_ALREADY_INIT if rcl_init has already been called + */ rcl_ret_t rcl_init(int argc, char ** argv); -/// Creates a rcl_node_t; used to implement a ROS Node. -rcl_node_t * -rcl_create_node(const char * name); - -/// Destroys a rcl_node_t. +/// Signal global shutdown of rcl. +/* This function does not have to be called on exit, but does have to be called + * making a repeat call to rcl_init. + * + * This function can only be called once after each call to rcl_init. + * Repeated calls will fail with RCL_RET_NOT_INIT. + * This function is not thread safe. + * + * When this function is called: + * - Any rcl objects created since the last call to rcl_init are invalidated. + * - Calls to rcl_ok will return false. + * - Any executors waiting for work (within a call to spin) are interrupted. + * - No new work (executing callbacks) will be done in executors. + * - Currently running work in executors will be finished. + * + * \return RCL_RET_OK if shutdown is successful, otherwise RCL_RET_ERROR or + * RCL_RET_NOT_INIT if rcl_init has not yet been called + */ rcl_ret_t -rcl_destroy_node(rcl_node_t * node); +rcl_shutdown(); -/// Creates a rcl_publisher_t; used to implement a ROS Publisher. -rcl_publisher_t * -rcl_create_publisher( - const rcl_node_t * node, - const rosidl_message_type_support_t * type_support, - const char * topic_name, - size_t queue_size); +/// Return true until rcl_shutdown is called, then false. +/* This function is thread safe. */ +bool +rcl_ok(); -/// Destroys a rcl_publisher_t. -rcl_ret_t -rcl_destroy_publisher(rcl_publisher_t * publisher); - -/// Publishes a ROS message to a ROS Topic using a rcl_publisher_t. -rcl_ret_t -rcl_publish(const rcl_publisher_t * publisher, const void * ros_message); - -/// Creates a rcl_subscription_t; used to implement a ROS Subscription. -rcl_subscription_t * -rcl_create_subscription( - const rcl_node_t * node, - const rosidl_message_type_support_t * type_support, - const char * topic_name, - size_t queue_size, - bool ignore_local_publications); - -/// Destroys a rcl_subscription_t. -rcl_ret_t -rcl_destroy_subscription(rcl_subscription_t * subscription); - -/// Takes a ROS message from a given rcl_subscription_t if one is available. -rcl_ret_t -rcl_take(const rcl_subscription_t * subscriber, void * ros_message); - -/// Creates a rcl_guard_condition_t; is used by the implementation to -/// asynchronously interrupt rmw_wait. -rcl_guard_condition_t * -rcl_create_guard_condition(); - -/// Destroys a rcl_guard_condition_t. -rcl_ret_t -rcl_destroy_guard_condition(rcl_guard_condition_t * guard_condition); - -/// Triggers the condition, which will interrupt rmw_wait asynchronously. -rcl_ret_t -rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition); - -/// Creates a rcl_callback_group_t, which provides hints about how to execute -/// groups of callbacks to an executor. -rcl_callback_group_t * -rcl_create_callback_group(rcl_node_t * node); - -/// Destroys a rcl_callback_group_t. -rcl_ret_t -rcl_destroy_callback_group(rcl_callback_group_t * callback_group); - -/// Creates a rcl_subscription_info_t, which associates rcl_subscription_t's -/// to a callback group. -rcl_subscription_info_t * -rcl_create_subscription_info( - rcl_subscription_t * const subscription, - rcl_callback_group_t * const callback_group); - -/// Destroys a rcl_subscription_info_t. -rcl_ret_t -rcl_destroy_subscription_info(rcl_subscription_info_t * subscription_info); - -/// Creates a rcl_timer_info_t, which associates a ROS Timer's guard condition -/// with a callback group. -rcl_timer_info_t * -rcl_create_timer_info( - rcl_guard_condition_t * const guard_condition, - rcl_callback_group_t * const callback_group); - -/// Destroys a rcl_timer_info_t. -rcl_ret_t -rcl_destroy_timer_info(rcl_timer_info_t * timer_info); - -/// Creates a rcl_executor_helper_t, which is a collection of callable items. -rcl_executor_helper_t * -rcl_create_executor_helper(); - -/// Destroys a rcl_executor_helper_t. -rcl_ret_t -rcl_destroy_executor_helper(rcl_executor_helper_t * executor_helper); - -/// Add a rcl_subscription_info_t to the collection of callable items. -rcl_ret_t -rcl_add_subscription_info( - rcl_executor_helper_t * executor_helper, - rcl_subscription_info_t * subscription_info); - -/// Removes a rcl_subscription_t from the collection of callable items. -rcl_ret_t -rcl_remove_subscription_info( - rcl_executor_helper_t * executor_helper, - rcl_subscription_info_t * subscription_info); - -/// Add a rcl_timer_info_t to the collection of callable items. -rcl_ret_t -rcl_add_timer_info( - rcl_executor_helper_t * executor_helper, - rcl_timer_info_t * timer_info); - -/// Removes a rcl_subscription_t from the collection of callable items. -rcl_ret_t -rcl_remove_timer_info( - rcl_executor_helper_t * executor_helper, - rcl_timer_info_t * timer_info); - -/// Finds the next ready to be called item if one exists; optionally blocking. -rcl_ret_t -rcl_get_next_any_executable( - rcl_executor_helper_t * executor_helper, - rcl_any_executable_t * any_executable, - bool non_blocking); +#if __cplusplus +} +#endif #endif // RCL__RCL_H_ diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h new file mode 100644 index 0000000..b2dd86f --- /dev/null +++ b/rcl/include/rcl/subscription.h @@ -0,0 +1,256 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__SUBSCRIPTION_H_ +#define RCL__SUBSCRIPTION_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include "rosidl_generator_c/message_type_support.h" + +#include "rcl/node.h" + +/// Internal rcl implementation struct. +struct rcl_subscription_impl_t; + +/// Handle for a rcl subscription. +typedef struct rcl_subscription_t { + struct rcl_subscription_impl_t * impl; +} rcl_subscription_t; + +/// Options available for a rcl subscription. +typedef struct rcl_subscription_options_t { + /// Middleware quality of service settings for the subscription. + rmw_qos_profile_t qos; + /// If true, messages published from within the same node are ignored. + bool ignore_local_publications; + /// Custom allocator for the subscription, used for incidental allocations. + /* For default behavior (malloc/free), see: rcl_get_default_allocator() */ + rcl_allocator_t allocator; +} rcl_subscription_options_t; + +/// Return a rcl_subscription_t struct with members set to NULL. +/* Should be called to get a null rcl_subscription_t before passing to + * rcl_initalize_subscription(). + * It's also possible to use calloc() instead of this if the rcl_subscription_t + * is being allocated on the heap. + */ +rcl_subscription_t +rcl_get_zero_initialized_subscription(); + +/// Initialize a ROS subscription. +/* After calling this function on a rcl_subscription_t, it can be used to take + * messages of the given type to the given topic using rcl_take(). + * + * The given rcl_node_t must be valid and the resulting rcl_subscription_t is + * only valid as long as the given rcl_node_t remains valid. + * + * The rosidl_message_type_support_t is obtained on a per .msg type basis. + * When the user defines a ROS message, code is generated which provides the + * required rosidl_message_type_support_t object. + * This object can be obtained using a language appropriate mechanism. + * \TODO(wjwwood) probably should talk about this once and link to it instead + * For C this macro can be used (using std_msgs/String as an example): + * + * #include + * #include + * rosidl_message_type_support_t * string_ts = + * ROSIDL_GET_MESSAGE_TYPE_SUPPORT(std_msgs, String); + * + * For C++ a template function is used: + * + * #include + * #include + * rosidl_message_type_support_t * string_ts = + * rosidl_generator_cpp::get_message_type_support_handle(); + * + * The rosidl_message_type_support_t object contains message type specific + * information used to publish messages. + * + * \TODO(wjwwood) update this once we've come up with an official scheme. + * The topic name must be a non-empty string which follows the topic naming + * format. + * + * The options struct allows the user to set the quality of service settings as + * well as a custom allocator which is used when (de)initializing the + * subscription to allocate space for incidental things, e.g. the topic + * name string. + * + * Expected usage (for C messages): + * + * #include + * #include + * #include + * + * rcl_node_t node = rcl_get_zero_initialized_node(); + * rcl_node_options_t node_ops = rcl_node_get_default_options(); + * rcl_ret_t ret = rcl_node_init(&node, "node_name", &node_ops); + * // ... error handling + * rosidl_message_type_support_t * ts = ROSIDL_GET_MESSAGE_TYPE_SUPPORT(std_msgs, String); + * rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + * rcl_subscription_options_t subscription_ops = rcl_subscription_get_default_options(); + * ret = rcl_subscription_init(&subscription, &node, ts, "chatter", &subscription_ops); + * // ... error handling, and when finished deinitialization + * ret = rcl_subscription_fini(&subscription, &node); + * // ... error handling for rcl_subscription_fini() + * ret = rcl_node_fini(&node); + * // ... error handling for rcl_deinitialize_node() + * + * This function is not thread-safe. + * + * \param[out] subscription preallocated subscription structure + * \param[in] node valid rcl node handle + * \param[in] type_support type support object for the topic's type + * \param[in] topic_name the name of the topic + * \param[in] options subscription options, including quality of service settings + * \return RMW_RET_OK if subscription was initialized successfully, otherwise RMW_RET_ERROR + */ +rcl_ret_t +rcl_subscription_init( + rcl_subscription_t * subscription, + const rcl_node_t * node, + const rosidl_message_type_support_t * type_support, + const char * topic_name, + const rcl_subscription_options_t * options); + +/// Deinitialize a rcl_subscription_t. +/* After calling, the node will no longer be subscribed on this topic + * (assuming this is the only subscription on this topic in this node). + * + * After calling, calls to rcl_wait and rcl_take will fail when using this + * subscription. + * Additioanlly rcl_wait will be interrupted if currently blocking. + * However, the given node handle is still valid. + * + * This function is not thread-safe. + * + * \param[inout] subscription handle to the subscription to be deinitialized + * \param[in] node handle to the node used to create the subscription + * \return RMW_RET_OK if subscription was deinitialized successfully, otherwise RMW_RET_ERROR + */ +rcl_ret_t +rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node); + +/// Return the default subscription options in a rcl_subscription_options_t. +rcl_subscription_options_t +rcl_subscription_get_default_options(); + +/// Take a ROS message from a topic using a rcl subscription. +/* It is the job of the caller to ensure that the type of the ros_message + * parameter and the type associate with the subscription, via the type + * support, match. + * Passing a different type to rcl_take produces undefined behavior and cannot + * be checked by this function and therefore no deliberate error will occur. + * + * TODO(wjwwood) blocking of take? + * TODO(wjwwood) pre- and post-conditions for message ownership? + * TODO(wjwwood) is rcl_take thread-safe? + * + * The ros_message pointer should point to an already allocated ROS message + * struct of the correct type, into which the taken ROS message will be copied + * if one is available. + * If taken is false after calling, then the ROS message will be unmodified. + * + * If allocation is required when taking the message, e.g. if space needs to + * be allocated for a dynamically sized array in the target message, then the + * allocator given in the subscription options is used. + * + * The taken pointer should point to an already allocated boolean in which the + * result can be stored. + * + * The rmw message_info struct contains meta information about this particular + * message instance, like what the GUID of the publisher which published it + * originally or whether or not the message received from within the same + * process. + * The message_info parameter should be an already allocated rmw_message_info_t + * structure. + * Passing NULL for message_info will be ignored. + * + * \param[in] subscription the handle to the subscription from which to take + * \param[inout] ros_message type-erased ptr to a allocated ROS message + * \param[out] taken pointer to a bool, if set to false a message was not taken + * \param[out] message_info rmw struct which contains meta-data for the message + * \return RMW_RET_OK if the message was published, otherwise RMW_RET_ERROR + */ +rcl_ret_t +rcl_take( + const rcl_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info); + +/// Get the topic name for the subscription. +/* This function returns the subscription's internal topic name string. + * This function can fail, and therefore return NULL, if the: + * - subscription is NULL + * - subscription is invalid (never called init, called fini, or invalid node) + * + * The returned string is only valid as long as the subscription is valid. + * The value of the string may change if the topic name changes, and therefore + * copying the string is recommended if this is a concern. + * + * This function is not thread-safe, and copying the result is not thread-safe. + * \TODO(wjwwood): should we provide a thread-safe copy_topic_name? + * + * \param[in] subscription the pointer to the subscription + * \return name string if successful, otherwise NULL + */ +const char * +rcl_subscription_get_topic_name(const rcl_subscription_t * subscription); + +/// Return the rcl subscription options. +/* This function returns the subscription's internal options struct. + * This function can fail, and therefore return NULL, if the: + * - subscription is NULL + * - subscription is invalid (never called init, called fini, or invalid node) + * + * The returned struct is only valid as long as the subscription is valid. + * The values in the struct may change if the subscription's options change, + * and therefore copying the struct is recommended if this is a concern. + * + * This function is not thread-safe, and copying the result is not thread-safe. + * \TODO(wjwwood): should we provide a thread-safe copy_subscription_options? + * + * \param[in] subscription pointer to the subscription + * \return options struct if successful, otherwise NULL + */ +const rcl_subscription_options_t * +rcl_subscription_get_options(rcl_subscription_t * subscription); + +/// Return the rmw subscription handle. +/* The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return NULL, if the: + * - subscription is NULL + * - subscription is invalid (never called init, called fini, or invalid node) + * + * The returned handle is only valid as long as the rcl subscription is valid. + * + * \TODO(wjwwood) should the return value of this be const? + * + * This function is not thread-safe. + * + * \param[in] subscription pointer to the rcl subscription + * \return rmw subscription handle if successful, otherwise NULL + */ +rmw_subscription_t * +rcl_subscription_get_rmw_subscription_handle(rcl_subscription_t * subscription); + +#if __cplusplus +} +#endif + +#endif // RCL__SUBSCRIPTION_H_ diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h new file mode 100644 index 0000000..8596913 --- /dev/null +++ b/rcl/include/rcl/time.h @@ -0,0 +1,31 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__TIME_H_ +#define RCL__TIME_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include "rmw/rmw.h" + +typedef rmw_time_t rcl_time_t; + +#if __cplusplus +} +#endif + +#endif // RCL__TIME_H_ diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index d9135bb..cef999e 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -19,40 +19,19 @@ typedef rmw_ret_t rcl_ret_t; #define RCL_RET_OK RMW_RET_OK - -typedef rmw_node_t rcl_node_t; -typedef rmw_publisher_t rcl_publisher_t; -typedef rmw_subscription_t rcl_subscription_t; -typedef rmw_guard_condition_t rcl_guard_condition_t; -typedef rmw_subscriptions_t rcl_subscriptions_t; -typedef rmw_guard_conditions_t rcl_guard_conditions_t; - -typedef struct rcl_callback_group_t -{ - rcl_node_t ** node; -} rcl_callback_group_t; - -typedef struct rcl_subscription_info_t -{ - rcl_subscription_t ** subscription; - rcl_callback_group_t ** callback_group; -} rcl_subscription_info_t; - -typedef struct rcl_timer_info_t -{ - rcl_guard_condition_t ** guard_condition; - rcl_callback_group_t ** callback_group; -} rcl_timer_info_t; - -typedef struct rcl_executor_helper_t -{ - // TODO(wjwwood): fill with something -} rcl_executor_helper_t; - -typedef struct rcl_any_executable_t -{ - rcl_subscription_info_t ** subscription_info; - rcl_timer_info_t ** timer_info; -} rcl_any_executable_t; +#define RCL_RET_ERROR RMW_RET_ERROR +#define RCL_RET_TIMEOUT RMW_RET_TIMEOUT +// rcl specific ret codes start at 100 +#define RCL_RET_ALREADY_INIT 100 +#define RCL_RET_NOT_INIT 101 +// rcl node specific ret codes in 2XX +// rcl publisher specific ret codes in 3XX +// rcl subscription specific ret codes in 4XX +// rcl service client specific ret codes in 5XX +// rcl service server specific ret codes in 6XX +// rcl guard condition specific ret codes in 7XX +// rcl wait and wait set specific ret codes in 8XX +#define RCL_RET_WAIT_SET_EMPTY 800 +#define RCL_RET_WAIT_SET_FULL 801 #endif // RCL__TYPES_H_ diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h new file mode 100644 index 0000000..2d415dc --- /dev/null +++ b/rcl/include/rcl/wait.h @@ -0,0 +1,294 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__WAIT_H_ +#define RCL__WAIT_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include +#include + +#include "rcl/subscription.h" +#include "rcl/guard_condition.h" +#include "rcl/time.h" +#include "rcl/types.h" + +struct rcl_wait_set_impl_t; + +/// Container for subscription's, guard condition's, etc to be waited on. +typedef struct rcl_wait_set_t { + /// Storage for subscription pointers. + rcl_subscription_t ** subscriptions; + size_t size_of_subscriptions; + size_t __current_subscription_offset; + /// Storage for guard condition pointers. + rcl_guard_condition_t ** guard_conditions; + size_t size_of_guard_conditions; + size_t __current_guard_condition_offset; + /// Allocator for storage. + rcl_allocator_t allocator; + /// If set to true, actions like add_subscription will fail until cleared. + bool pruned; + /// Implementation specific storage. + rcl_wait_set_impl_t * impl; +} rcl_wait_set_t; + +/// Return a rcl_wait_set_t struct with members set to NULL. +rcl_wait_set_t +rcl_get_zero_initialized_wait_set(); + +/// Initialize a rcl wait set with space for items to be waited on. +/* This function allocates space for the subscriptions and other wait-able + * entities that can be stored in the wait set. + * It also sets the allocator to the given one and initializes the pruned + * member to be false. + * + * The wait_set struct should be allocated and initialized to NULL. + * If the wait_set is allocated but the memory is uninitialized the behavior is + * undefined. + * Calling this function on a wait set that has already been initialized will + * result in an error. + * A wait set can be reinitialized if rcl_wait_set_fini() was called on it. + * + * To use the default allocator use rcl_get_default_allocator(). + * + * Expected usage: + * + * #include + * + * rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + * rcl_ret_t ret = rcl_wait_set_init(&wait_set, 42, 42, rcl_get_default_allocator()); + * // ... error handling, and then after using call matching fini: + * ret = rcl_wait_set_fini(&wait_set); + * // ... error handling + * + * This function is thread-safe for different wait_set objects. + * Thread-safety of this function requires a thread-safe allocator if the + * allocator is shared with other parts of the system. + * + * \param[inout] wait_set the wait set struct to be initialized + * \param[in] number_of_subscriptions size of the subscriptions set + * \param[in] number_of_guard_conditions size of the guard conditions set + * \param[in] allocator the allocator to use when allocating space in the sets + * \return RCL_RET_OK if the wait set is initialized successfully, or + * RCL_RET_ALREADY_INIT if the wait set is not zero initialized, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +rcl_ret_t +rcl_wait_set_init( + rcl_wait_set_t * wait_set, + size_t number_of_subscriptions, + size_t number_of_guard_conditions, + rcl_allocator_t allocator); + +/// Finalize a rcl wait set. +/* Deallocates any memory in the wait set that was allocated in + * rcl_wait_set_init() using the allocator given in the initialization. + * + * Calling this function on a zero initialized wait set will do nothing and + * return RCL_RET_OK. + * Calling this function on uninitialized memory results in undefined behavior. + * After calling this function the wait set will once again be zero initialized + * and so calling this function or rcl_wait_set_init() immediately after will + * succeed. + * + * This function is not thread-safe. + * + * \param[inout] wait_set the wait set struct to be finalized. + * \return RCL_RET_OK if the finalization was successful, otherwise RCL_RET_ERROR + */ +rcl_ret_t +rcl_wait_set_fini(rcl_wait_set_t * wait_set); + +/// Stores a pointer to the given subscription in next empty spot in the set. +/* This function does not guarantee that the subscription is not already in the + * wait set. + * + * This function is not thread-safe. + * + * \param[inout] wait_set struct in which the subscription is to be stored + * \param[in] subscription the subscription to be added to the wait set + * \return RCL_RET_OK if added successfully, or + * RCL_RET_WAIT_SET_FULL if the subscription set is full, or + * RCL_RET_NOT_INIT if the wait set is zero initialized, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +rcl_ret_t +rcl_wait_set_add_subscription( + rcl_wait_set_t * wait_set, + const rcl_subscription_t * subscription); + +/// Removes (sets to NULL) the subscriptions in the wait set. +/* This function should be used after passing using rcl_wait, but before + * adding new subscriptions to the set. + * + * Calling this on an uninitialized (zero initialized) wait set will fail. + * + * This function is not thread-safe. + * + * \param[inout] wait_set struct to have its subscriptions cleared + * \return RCL_RET_OK if cleared successfully, or + * RCL_RET_NOT_INIT if the wait set is zero initialized, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +rcl_ret_t +rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); + +/// Reallocates space for the subscriptions in the wait set. +/* This function will deallocate and reallocate the memory for the + * subscriptions set. + * + * A size of 0 will just deallocate the memory and assign NULL to the array. + * + * Allocation and deallocation is done with the allocator given during the + * wait set's initialization. + * + * After calling this function all values in the set will be set to NULL, + * effectively the same as calling rcl_wait_set_clear_subscriptions(). + * + * If the requested size matches the current size, no allocation will be done. + * + * This can be called on an uninitialized (zero initialized) wait set. + * + * This function is not thread-safe. + * + * \param[inout] wait_set struct to have its subscriptions cleared + * \param[in] size a size for the new set + * \return RCL_RET_OK if resized successfully, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +rcl_ret_t +rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size); + +/// Stores a pointer to the guard condition in next empty spot in the set. +/* This function behaves exactly the same as for subscriptions. + * \see rcl_wait_set_add_subscription + */ +rcl_ret_t +rcl_wait_set_add_guard_condition( + rcl_wait_set_t * wait_set, + const rcl_guard_condition_t * guard_condition); + +/// Removes (sets to NULL) the guard conditions in the wait set. +/* This function behaves exactly the same as for subscriptions. + * \see rcl_wait_set_clear_subscriptions + */ +rcl_ret_t +rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set); + +/// Reallocates space for the subscriptions in the wait set. +/* This function behaves exactly the same as for subscriptions. + * \see rcl_wait_set_resize_subscriptions + */ +rcl_ret_t +rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); + +/// Block until the wait set is ready or until the timeout has been exceeded. +/* This function will collect the items in the rcl_wait_set_t and pass them + * to the underlying rmw_wait function. + * + * The items in the wait set will be either left untouched or set to NULL after + * this function returns. + * Items that are not NULL are ready, where ready means different things based + * on the type of the item. + * For subscriptions this means there are messages that can be taken. + * For guard conditions this means the guard condition was triggered. + * + * Expected usage: + * + * #include + * + * // rcl_init() called successfully before here... + * rcl_node_t node; // initialize this, see rcl_node_init() + * rcl_subscription_t sub1; // initialize this, see rcl_subscription_init() + * rcl_subscription_t sub2; // initialize this, see rcl_subscription_init() + * rcl_guard_condition_t gc1; // initialize this, see rcl_guard_condition_init() + * rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + * rcl_ret_t ret = rcl_wait_set_init(&wait_set, 2, 1, rcl_get_default_allocator()); + * // ... error handling + * do { + * ret = rcl_wait_set_clear_subscriptions(&wait_set); + * // ... error handling + * ret = rcl_wait_set_clear_guard_conditions(&wait_set); + * // ... error handling + * ret = rcl_wait_set_add_subscription(&wait_set, &sub1); + * // ... error handling + * ret = rcl_wait_set_add_subscription(&wait_set, &sub2); + * // ... error handling + * ret = rcl_wait_set_add_guard_condition(&wait_set, &gc1); + * // ... error handling + * rcl_time_t timeout = {1, 0}; // 1 second and 0 nanoseconds. + * ret = rcl_wait(&wait_set, &timeout); + * if (ret == RCL_RET_TIMEOUT) { + * continue; + * } + * for (int i = 0; i < wait_set.size_of_subscriptions; ++i) { + * if (wait_set.subscriptions[i]) { + * // The subscription is ready... + * } + * } + * for (int i = 0; i < wait_set.size_of_guard_conditions; ++i) { + * if (wait_set.guard_conditions[i]) { + * // The subscription is ready... + * } + * } + * } while(check_some_condition()); + * // ... fini node, and subscriptions and guard conditions... + * ret = rcl_wait_set_fini(&wait_set); + * // ... error handling + * + * The wait set struct must be allocated, initialized, and should have been + * cleared and then filled with items, e.g. subscriptions and guard conditions. + * Passing a wait set with no wait-able items in it will fail. + * NULL items in the sets are ignored, e.g. it is valid to have as input: + * - subscriptions[0] = valid pointer + * - subscriptions[1] = NULL + * - subscriptions[2] = valid pointer + * - size_of_subscriptions = 3 + * Passing an uninitialized (zero initialized) wait set struct will fail. + * Passing a wait set struct with uninitialized memory is undefined behavior. + * + * If the timeout pointer is NULL then this function will block indefinitely + * until something in the wait set is valid or it is interrupted. + * If the timeout contains 0 for seconds and nanoseconds this function will be + * non-blocking; checking what's ready but not waiting if nothing is ready yet. + * If the timeout contains a non-zero time then this function will return after + * that period of time has elapsed if something in the wait set does not become + * ready before then. + * The timeout's value is not changed. + * Passing a timeout struct with uninitialized memory is undefined behavior. + * + * This function is not thread-safe and cannot be called concurrently, even if + * the given wait sets are not the same and non-overlapping in contents. + * + * \param[inout] wait_set the set of things to be waited on and to be pruned if not ready + * \param[in] timeout the time duration to wait something in the wait set to be ready + * \return RCL_RET_OK something in the wait set became ready, or + * RCL_RET_TIMEOUT timeout expired before something was ready, or + * RCL_RET_NOT_INIT wait set is zero initialized, or + * RCL_RET_WAIT_SET_EMPTY wait set contains no items, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_wait(rcl_wait_set_t * wait_set, const rcl_time_t * timeout); + +#if __cplusplus +} +#endif + +#endif // RCL__WAIT_H_ From 9631cba15710f15b2dfa4127f5234fa72eed4589 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 24 Nov 2015 03:43:23 -0800 Subject: [PATCH 02/71] rename rcl_shutdown to rcl_fini --- rcl/include/rcl/node.h | 4 ++-- rcl/include/rcl/rcl.h | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 8d8a128..7c4dee0 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -29,7 +29,7 @@ struct rcl_node_impl_t; /* This handle can be in one a few stats, which are referred to below: * - uninitialized: memory allocated, but not initialized to anything * - invalid: memory allocated but not usable - * - either zero initialized or shutdown (rcl_shutdown() or rcl_node_fini()) + * - either zero initialized or shutdown (rcl_fini() or rcl_node_fini()) * - valid: memory allocated and usable */ typedef struct rcl_node_t { @@ -55,7 +55,7 @@ rcl_node_t rcl_get_zero_initialized_node(); /// Initialize a ROS node. -/* Calling this on a rcl_node_t makes it a valid node handle until rcl_shutdown +/* Calling this on a rcl_node_t makes it a valid node handle until rcl_fini * is called or until rcl_node_fini is called on it. * * After calling the ROS node object can be used to create other middleware diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 9cc89f0..3ce0478 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -29,13 +29,13 @@ extern "C" /* Unless otherwise noted, this must be called before using any rcl functions. * * This function can only be run once after starting the program, and once - * after each call to rcl_shutdown. + * after each call to rcl_fini. * Repeated calls will fail with RCL_RET_ALREADY_INIT. * This function is not thread safe. * - * This function can be called any time after rcl_shutdown is called, but it + * This function can be called any time after rcl_fini is called, but it * cannot be called from within a callback being executed by an rcl executor. - * For example, you can call rcl_shutdown from within a timer callback, but + * For example, you can call rcl_fini from within a timer callback, but * you have to return from the callback, and therefore exit any in-progress * call to a spin function, before calling rcl_init again. * @@ -71,9 +71,9 @@ rcl_init(int argc, char ** argv); * RCL_RET_NOT_INIT if rcl_init has not yet been called */ rcl_ret_t -rcl_shutdown(); +rcl_fini(); -/// Return true until rcl_shutdown is called, then false. +/// Return true until rcl_fini is called, then false. /* This function is thread safe. */ bool rcl_ok(); From d9e493a387401060862b4d219cb088dca3459f29 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 24 Nov 2015 03:44:21 -0800 Subject: [PATCH 03/71] pass allocator to rcl_init and clean up docs --- rcl/include/rcl/rcl.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 3ce0478..017a1ea 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -43,14 +43,17 @@ extern "C" * program. * rcl specific arguments will be parsed and removed, but other arguments will * be ignored. + * If argc is 0 and argv is NULL no parameters will be parsed. * * \param[in] argc number of strings in argv * \param[in] argv command line arguments; rcl specific arguments are removed - * \return RCL_RET_OK if initialization is successful, otherwise RCL_RET_ERROR - * or RCL_RET_ALREADY_INIT if rcl_init has already been called + * \param[in] allocator allocator to be used during rcl_init and rcl_fini + * \return RCL_RET_OK if initialization is successful, or + * RCL_RET_ALREADY_INIT if rcl_init has already been called, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t -rcl_init(int argc, char ** argv); +rcl_init(int argc, char ** argv, rcl_allocator_t allocator); /// Signal global shutdown of rcl. /* This function does not have to be called on exit, but does have to be called From bac3b210d7f8d7409e872762a4804229672dd639 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 24 Nov 2015 03:45:17 -0800 Subject: [PATCH 04/71] add the concept of an rcl instance id it changes with each successful call to rcl_init --- rcl/include/rcl/node.h | 18 ++++++++++++++++++ rcl/include/rcl/rcl.h | 5 +++++ 2 files changed, 23 insertions(+) diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 7c4dee0..9d5f706 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -161,6 +161,24 @@ rcl_node_get_options(const rcl_node_t * node); rmw_node_t * rcl_node_get_rmw_node_handle(const rcl_node_t * node); +/// Return the associated rcl instance id. +/* This id is stored when rcl_node_init is called and can be compared with the + * value returned by rcl_get_instance_id() to check if this node was created in + * the current rcl context (since the latest call to rcl_init(). + * + * This function can fail, and therefore return 0, if: + * - node is NULL + * - node has not been initialized (the implementation is invalid) + * + * This function will succeed, however, even if rcl_fini has been called since + * the node was created. + * + * \param[in] node pointer to the rcl node + * \return rcl instance id captured at node creation or 0 if there was an error + */ +uint64_t +rcl_node_get_rcl_instance_id(const rcl_node_t * node); + #if __cplusplus } #endif diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 017a1ea..485375e 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -76,6 +76,11 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator); rcl_ret_t rcl_fini(); +/// Returns an uint64_t number that is unique for the latest rcl_init call. +/* If called before rcl_init or after rcl_fini then 0 will be returned. */ +uint64_t +rcl_get_instance_id(); + /// Return true until rcl_fini is called, then false. /* This function is thread safe. */ bool From 2f029b560e217f3bcace6b04fab6e64b061292bc Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 24 Nov 2015 03:45:40 -0800 Subject: [PATCH 05/71] implement allocator, node, and rcl functions --- rcl/src/rcl/allocator.c | 37 +++++++++ rcl/src/rcl/common.c | 48 +++++++++++ rcl/src/rcl/common.h | 42 ++++++++++ rcl/src/rcl/node.c | 173 ++++++++++++++++++++++++++++++++++++++++ rcl/src/rcl/rcl.c | 108 +++++++++++++++++++++++++ 5 files changed, 408 insertions(+) create mode 100644 rcl/src/rcl/allocator.c create mode 100644 rcl/src/rcl/common.c create mode 100644 rcl/src/rcl/common.h create mode 100644 rcl/src/rcl/node.c create mode 100644 rcl/src/rcl/rcl.c diff --git a/rcl/src/rcl/allocator.c b/rcl/src/rcl/allocator.c new file mode 100644 index 0000000..b4caf3e --- /dev/null +++ b/rcl/src/rcl/allocator.c @@ -0,0 +1,37 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/allocator.h" + +static void * +__default_allocate(size_t size, void * state) +{ + (void)state; // unused + return malloc(size); +} + +static void +__default_deallocate(void * pointer, void * state) +{ + (void)state; // unused + return free(pointer); +} + +rcl_allocator_t +rcl_get_default_allocator() +{ + return {__default_allocate, __default_deallocate, NULL}; +} diff --git a/rcl/src/rcl/common.c b/rcl/src/rcl/common.c new file mode 100644 index 0000000..563921c --- /dev/null +++ b/rcl/src/rcl/common.c @@ -0,0 +1,48 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#endif + +#include "./common.h" + +#include + +#if defined(WIN32) +static char __env_buffer[1024]; +#endif + +rcl_ret_t +rcl_impl_getenv(const char * env_name, char ** env_value) +{ + env_value = NULL; +#if !defined(WIN32) + *env_value = getenv(env_name); +#else + size_t required_size; + error_t ret = getenv_s(&required_size, __env_buffer, sizeof(__env_buffer), env_name); + if (ret != 0) { + RCL_SET_ERROR_MSG("value in env variable too large to read in"); + return RCL_RET_ERROR; + } + env_value = __env_buffer; +#endif + return RCL_RET_OK; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h new file mode 100644 index 0000000..c62260b --- /dev/null +++ b/rcl/src/rcl/common.h @@ -0,0 +1,42 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__COMMON_H_ +#define RCL__COMMON_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/error_handling.h" +#include "rcl/types.h" + +#define RCL_CHECK_PARAMETER_FOR_NULL(parameter, error_return_type) \ + RCL_CHECK_FOR_NULL_WITH_MSG(parameter, #parameter " parameter is null", return error_return_type) + +#define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) if (!value) { \ + RCL_SET_ERROR_MSG(msg); \ + error_statement; \ +} + +// This value put in env_value is only valid until the next call to rcl_impl_getenv (on Windows). +rcl_ret_t +rcl_impl_getenv(const char * env_name, char ** env_value); + +#if __cplusplus +} +#endif + +#endif // RCL__COMMON_H_ diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c new file mode 100644 index 0000000..6d2d19c --- /dev/null +++ b/rcl/src/rcl/node.c @@ -0,0 +1,173 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#endif + +#include +#include +#include + +#include "rcl/node.h" +#include "rcl/rcl.h" +#include "rmw/rmw.h" + +#include "./common.h" + +typedef struct rcl_node_impl_t { + char * name; + rcl_node_options_t options; + rmw_node_t * rmw_node_handle; + uint64_t rcl_instance_id; +} rcl_node_impl_t; + +rcl_node_t +rcl_get_uninitialized_node() +{ + return {0}; +} + +rcl_ret_t +rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options) +{ + size_t domain_id = 0; + char * ros_domain_id; + rcl_ret_t ret; + RCL_CHECK_PARAMETER_FOR_NULL(name, RCL_RET_ERROR); + RCL_CHECK_PARAMETER_FOR_NULL(options, RCL_RET_ERROR); + RCL_CHECK_PARAMETER_FOR_NULL(node, RCL_RET_ERROR); + if (node->impl) { + RCL_SET_ERROR_MSG( + "either double call to rcl_node_init or rcl_get_uninitialized_node was not used"); + return RCL_RET_ERROR; + } + const rcl_allocator_t * allocator = &options->allocator; + // Allocate space for the implementation struct. + RCL_CHECK_FOR_NULL_WITH_MSG(allocator->allocate, "allocate not set", return RCL_RET_ERROR); + node->impl = (rcl_node_impl_t *)allocator->allocate(sizeof(rcl_node_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "allocating memory failed", return RCL_RET_ERROR); + // Initialize node impl. + // node name + size_t name_len = strlen(name); + if (name_len == 0) { + RCL_SET_ERROR_MSG("node name cannot be empty string"); + goto fail; + } + node->impl->name = (char *)allocator->allocate(strlen(name), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl->name, "allocating memory failed", goto fail); + strncpy(node->impl->name, name, strlen(name)); + // node options + node->impl->options = *options; + // node rmw_node_handle + // First determine the ROS_DOMAIN_ID. + // The result of rcl_impl_getenv on Windows is only valid until the next call to rcl_impl_getenv. + ret = rcl_impl_getenv("ROS_DOMAIN_ID", &ros_domain_id); + if (ret != RCL_RET_OK) { + goto fail; + } + if (ros_domain_id) { + auto number = strtoul(ros_domain_id, NULL, 0); + if (number == ULONG_MAX) { + RCL_SET_ERROR_MSG("failed to interpret ROS_DOMAIN_ID as integral number"); + goto fail; + } + domain_id = (size_t)number; + } + node->impl->rmw_node_handle = rmw_create_node(name, domain_id); + RCL_CHECK_FOR_NULL_WITH_MSG( + node->impl->rmw_node_handle, rmw_get_error_string_safe(), goto fail); + node->impl->rcl_instance_id = rcl_get_instance_id(); + return RCL_RET_OK; +fail: + if (node->impl->name) { + allocator->deallocate(node->impl->name, allocator->state); + } + if (node->impl) { + allocator->deallocate(node->impl, allocator->state); + } + return RCL_RET_ERROR; +} + +rcl_ret_t +rcl_node_fini(rcl_node_t * node) +{ + RCL_CHECK_PARAMETER_FOR_NULL(node, RCL_RET_ERROR); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return RCL_RET_ERROR); + rcl_ret_t result = RCL_RET_OK; + rmw_ret_t node_ret = rmw_destroy_node(node->impl->rmw_node_handle); + if (node_ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + result = RCL_RET_ERROR; + } + rcl_allocator_t allocator = node->impl->options.allocator; + allocator.deallocate(node->impl->name, allocator.state); + allocator.deallocate(node->impl, allocator.state); + return result; +} + +rcl_node_options_t +rcl_node_get_default_options() +{ + return {false, rcl_get_default_allocator()}; +} + +const char * +rcl_node_get_name(const rcl_node_t * node) +{ + RCL_CHECK_PARAMETER_FOR_NULL(node, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL); + if (node->impl->rcl_instance_id != rcl_get_instance_id()) { + RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match"); + return NULL; + } + return node->impl->name; +} + +const rcl_node_options_t * +rcl_node_get_options(const rcl_node_t * node) +{ + RCL_CHECK_PARAMETER_FOR_NULL(node, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL); + if (node->impl->rcl_instance_id != rcl_get_instance_id()) { + RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match"); + return NULL; + } + return &node->impl->options; +} + +rmw_node_t * +rcl_node_get_rmw_node_handle(const rcl_node_t * node) +{ + RCL_CHECK_PARAMETER_FOR_NULL(node, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL); + if (node->impl->rcl_instance_id != rcl_get_instance_id()) { + RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match"); + return NULL; + } + return node->impl->rmw_node_handle; +} + +uint64_t +rcl_node_get_rcl_instance_id(const rcl_node_t * node) +{ + RCL_CHECK_PARAMETER_FOR_NULL(node, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL); + return node->impl->rcl_instance_id; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c new file mode 100644 index 0000000..a8488ca --- /dev/null +++ b/rcl/src/rcl/rcl.c @@ -0,0 +1,108 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/rcl.h" + +#include +#include + +#include "rcl/error_handling.h" + +static atomic_bool __rcl_is_initialized = false; +static rcl_allocator_t __rcl_allocator = {0}; +static int __rcl_argc = 0; +static char ** __rcl_argv = NULL; +static atomic_uint_fast64_t __rcl_instance_id = 0; +static uint64_t __rcl_next_unique_id = 0; + +static inline void +__clean_up_init() +{ + if (__rcl_argv) { + for (size_t i = 0; i < __rcl_argc; ++i) { + if (__rcl_argv[i]) { + __rcl_allocator.deallocate(__rcl_argv[i], __rcl_allocator.state); + } + } + __rcl_allocator.deallocate(__rcl_argv, __rcl_allocator.state); + } + atomic_store(&__rcl_instance_id, 0); + atomic_store(&__rcl_is_initialized, false); +} + +rcl_ret_t +rcl_init(int argc, char ** argv, rcl_allocator_t allocator) +{ + bool was_initialized = atomic_exchange(&__rcl_is_initialized, true); + if (was_initialized) { + RCL_SET_ERROR_MSG("rcl_init called while already initialized"); + return RCL_RET_ALREADY_INIT; + } + // TODO(wjwwood): Remove rcl specific command line arguments. + // For now just copy the argc and argv. + __rcl_argc = argc; + __rcl_argv = (char **)__rcl_allocator.allocate(sizeof(char *) * argc, __rcl_allocator.state); + if (!__rcl_argv) { + RCL_SET_ERROR_MSG("allocation failed"); + goto fail; + } + memset(__rcl_argv, 0, sizeof(char **) * argc); + for (size_t i = 0; i < argc; ++i) { + __rcl_argv[i] = (char *)__rcl_allocator.allocate(strlen(argv[i]), __rcl_allocator.state); + strcpy(__rcl_argv[i], argv[i]); + } + atomic_store(&__rcl_instance_id, ++__rcl_next_unique_id); + if (atomic_load(&__rcl_instance_id) == 0) { + // Roll over occurred. + __rcl_next_unique_id--; // roll back to avoid the next call succeeding. + RCL_SET_ERROR_MSG("unique rcl instance ids exhausted"); + goto fail; + } + return RCL_RET_OK; +fail: + __clean_up_init(); + return RCL_RET_ERROR; +} + +rcl_ret_t +rcl_fini() +{ + if (!atomic_load(&__rcl_is_initialized)) { + RCL_SET_ERROR_MSG("rcl_fini called before rcl_init"); + return RCL_RET_NOT_INIT; + } + __clean_up_init(); + return RCL_RET_OK; +} + +uint64_t +rcl_get_instance_id() +{ + return atomic_load(&__rcl_instance_id); +} + +bool +rcl_ok() +{ + return atomic_load(&__rcl_is_initialized); +} + +#if __cplusplus +} +#endif From 4b55f8e1ccafa45c19960e414c3c4c4890836a2f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 24 Nov 2015 15:35:21 -0800 Subject: [PATCH 06/71] update allocator to properly support realloc --- rcl/include/rcl/allocator.h | 6 +++++- rcl/src/rcl/allocator.c | 9 ++++++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index 44b509d..1ae76b3 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -31,7 +31,11 @@ typedef struct rcl_allocator_t { /// Deallocate previously allocated memory, mimicking free(). void (*deallocate)(void * pointer, void * state); /// Reallocates if possible, otherwise it deallocates and allocates. - /* If unsupported then do deallocate and then allocate. */ + /* If unsupported then do deallocate and then allocate. + * This should behave as realloc is described, as opposed to reallocf, i.e. + * the memory given by pointer will not be free'd automatically if realloc + * fails. + */ void * (*reallocate)(void * pointer, size_t size, void * state); /// Implementation defined state storage. /* This is passed as the second parameter to the (de)allocate functions. */ diff --git a/rcl/src/rcl/allocator.c b/rcl/src/rcl/allocator.c index b4caf3e..93eee17 100644 --- a/rcl/src/rcl/allocator.c +++ b/rcl/src/rcl/allocator.c @@ -30,8 +30,15 @@ __default_deallocate(void * pointer, void * state) return free(pointer); } +static void * +__default_reallocate(void * pointer, size_t size, void * state) +{ + (void)state; // unused + return realloc(pointer, size); +} + rcl_allocator_t rcl_get_default_allocator() { - return {__default_allocate, __default_deallocate, NULL}; + return {__default_allocate, __default_deallocate, __default_reallocate, NULL}; } From c24a214403b40137c6e309190d90c4c7e4732c5b Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 1 Dec 2015 08:10:40 -0800 Subject: [PATCH 07/71] more changes (I will squash this commit) specifically I: - used more descriptive error codes - added some of the CMake logic - More .c files - Changed some of the documentation to be clearer --- rcl/CMakeLists.txt | 29 +++++- rcl/include/rcl/guard_condition.h | 22 +++-- rcl/include/rcl/node.h | 58 ++++++++++-- rcl/include/rcl/publisher.h | 4 - rcl/include/rcl/rcl.h | 14 +-- rcl/include/rcl/subscription.h | 16 +++- rcl/include/rcl/types.h | 3 + rcl/include/rcl/wait.h | 13 ++- rcl/package.xml | 5 + rcl/src/rcl/allocator.c | 8 +- rcl/src/rcl/common.h | 4 +- rcl/src/rcl/guard_condition.c | 150 ++++++++++++++++++++++++++++++ rcl/src/rcl/node.c | 66 +++++++------ rcl/src/rcl/publisher.c | 91 ++++++++++++++++++ rcl/src/rcl/rcl.c | 4 +- rcl/src/rcl/subscription.c | 26 ++++++ rcl/src/rcl/wait.c | 24 +++++ 17 files changed, 470 insertions(+), 67 deletions(-) create mode 100644 rcl/src/rcl/guard_condition.c create mode 100644 rcl/src/rcl/publisher.c create mode 100644 rcl/src/rcl/subscription.c create mode 100644 rcl/src/rcl/wait.c diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 8e83769..79c5af4 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -3,11 +3,36 @@ cmake_minimum_required(VERSION 2.8.3) project(rcl) find_package(ament_cmake REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(rmw REQUIRED) +find_package(rosidl_generator_cpp REQUIRED) -ament_export_dependencies(rmw) - +ament_export_dependencies(rmw rosidl_generator_cpp) ament_export_include_directories(include) +include_directories(include) + +if(NOT WIN32) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c11 -Wall -Wextra") +endif() + +set(${PROJECT_NAME}_sources + src/rcl/allocator.c + src/rcl/common.c + src/rcl/guard_condition.c + src/rcl/node.c + src/rcl/publisher.c + src/rcl/rcl.c + src/rcl/subscription.c + src/rcl/wait.c +) +add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_sources}) +ament_target_dependencies(${PROJECT_NAME} + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" +) + if(AMENT_ENABLE_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index b4a9b82..7e976c3 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/rcl/include/rcl/guard_condition.h @@ -72,10 +72,16 @@ rcl_get_zero_initialized_guard_condition(); * * This function is not thread-safe. * + * \TODO(wjwwood): does this function need a node to be passed to it? (same for fini) + * * \param[inout] guard_condition preallocated guard_condition structure * \param[in] node valid rcl node handle * \param[in] options the guard_condition's options - * \return RMW_RET_OK if guard_condition was initialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if guard_condition was initialized successfully, or + * RCL_RET_ALREADY_INIT if the guard condition is already initialized, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_guard_condition_init( @@ -83,16 +89,18 @@ rcl_guard_condition_init( const rcl_node_t * node, const rcl_guard_condition_options_t * options); -/// Deinitialize a rcl_guard_condition_t. +/// Finalize a rcl_guard_condition_t. /* After calling, calls to rcl_trigger_guard_condition will fail when using * this guard condition. * However, the given node handle is still valid. * * This function is not thread-safe. * - * \param[inout] guard_condition handle to the guard_condition to be deinitialized + * \param[inout] guard_condition handle to the guard_condition to be finalized * \param[in] node handle to the node used to create the guard_condition - * \return RMW_RET_OK if guard_condition was deinitialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if guard_condition was finalized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * node); @@ -104,7 +112,7 @@ rcl_guard_condition_get_default_options(); /// Trigger a rcl guard condition. /* This function can fail, and therefore return NULL, if the: * - guard condition is NULL - * - guard condition is invalid (never called init, called fini, or invalid node) + * - guard condition is invalid (never called init or called fini) * * A guard condition can be triggered from any thread. * @@ -112,7 +120,9 @@ rcl_guard_condition_get_default_options(); * with rcl_guard_condition_fini() on the same guard condition. * * \param[in] guard_condition handle to the guard_condition to be triggered - * \return RMW_RET_OK if the message was published, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if the guard condition was triggered, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition); diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 9d5f706..d1ad319 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -23,6 +23,9 @@ extern "C" #include "rcl/allocator.h" #include "rcl/types.h" +// Intentional underflow to get max size_t. +#define RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID (size_t)-1 + struct rcl_node_impl_t; /// Handle for a ROS node. @@ -42,6 +45,16 @@ typedef struct rcl_node_options_t { // rmw_qos_profile_t parameter_qos; /// If true, no parameter infrastructure will be setup. bool no_parameters; + /// If set, then this value overrides the ROS_DOMAIN_ID environment variable. + /* It defaults to RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID, which will cause the + * node to use the ROS domain ID set in the ROS_DOMAIN_ID environment + * variable, or on some systems 0 if the environment variable is not set. + * + * \TODO(wjwwood): Should we put a limit on the ROS_DOMAIN_ID value, that way + * we can have a safe value for the default + * RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID? (currently max size_t) + */ + size_t domain_id; /// Custom allocator used for incidental allocations, e.g. node name string. rcl_allocator_t allocator; } rcl_node_options_t; @@ -66,11 +79,13 @@ rcl_get_zero_initialized_node(); * * A node contains infrastructure for ROS parameters, which include advertising * publishers and service servers. - * So this function will create those external parameter interfaces even if + * This function will create those external parameter interfaces even if * parameters are not used later. * - * This function should be called on a rcl_node_t exactly once, and calling it - * multiple times is undefined behavior. + * The rcl_node_t given must be allocated and zero initalized. + * Passing an rcl_node_t which has already had this function called on it, more + * recently than rcl_node_fini, will fail. + * An allocated rcl_node_t with uninitialized memory is undefined behavior. * * Expected usage: * @@ -89,22 +104,28 @@ rcl_get_zero_initialized_node(); * \param[inout] node a preallocated node structure * \param[in] name the name of the node * \param[in] options the node options; pass null to use default options - * \return RMW_RET_OK if node was initialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if node was initialized successfully, or + * RCL_RET_ALREADY_INIT if the node has already be initialized, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options); -/// Deinitialize a rcl_node_t. +/// Finalized a rcl_node_t. /* Shuts down any automatically created infrastructure and deallocates memory. - * After calling, the rcl_node_t can be safely released. + * After calling, the rcl_node_t can be safely deallocated. * * Any middleware primitives created by the user, e.g. publishers, services, etc., * are invalid after deinitialization. * * This function is not thread-safe. * - * \param[in] node handle to the node to be deinitialized - * \return RMW_RET_OK if node was deinitialized successfully, otherwise RMW_RET_ERROR + * \param[in] node handle to the node to be finalized + * \return RCL_RET_OK if node was finalized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_node_fini(rcl_node_t * node); @@ -145,6 +166,27 @@ rcl_node_get_name(const rcl_node_t * node); const rcl_node_options_t * rcl_node_get_options(const rcl_node_t * node); +/// Return the ROS domain ID that the node is using. +/* This function returns the ROS domain ID that the node is in. + * + * This function should be used to determine what domain_id was used rather + * than checking the domin_id field in the node options, because if + * RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID is used when creating the node then + * it is not changed after creation, but this function will return the actual + * domain_id used. + * + * The domain_id field must point to an allocated size_t object to which the + * ROS domain ID will be written. + * + * \param[in] node the handle to the node being queried + * \return RCL_RET_OK if node the domain ID was retrieved successfully, or + * RCL_RET_NODE_INVALID if the node is invalid, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +rcl_ret_t +rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); + /// Return the rmw node handle. /* The handle returned is a pointer to the internally held rmw handle. * This function can fail, and therefore return NULL, if: diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 0ce2548..f4bf6f2 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -256,10 +256,6 @@ rcl_publisher_get_options(rcl_publisher_t * publisher); rmw_publisher_t * rcl_publisher_get_rmw_publisher_handle(rcl_publisher_t * publisher); -int main() { - -} - #if __cplusplus } #endif diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 485375e..b099360 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -29,13 +29,13 @@ extern "C" /* Unless otherwise noted, this must be called before using any rcl functions. * * This function can only be run once after starting the program, and once - * after each call to rcl_fini. + * after each call to rcl_shutdown. * Repeated calls will fail with RCL_RET_ALREADY_INIT. * This function is not thread safe. * - * This function can be called any time after rcl_fini is called, but it + * This function can be called any time after rcl_shutdown is called, but it * cannot be called from within a callback being executed by an rcl executor. - * For example, you can call rcl_fini from within a timer callback, but + * For example, you can call rcl_shutdown from within a timer callback, but * you have to return from the callback, and therefore exit any in-progress * call to a spin function, before calling rcl_init again. * @@ -47,7 +47,7 @@ extern "C" * * \param[in] argc number of strings in argv * \param[in] argv command line arguments; rcl specific arguments are removed - * \param[in] allocator allocator to be used during rcl_init and rcl_fini + * \param[in] allocator allocator to be used during rcl_init and rcl_shutdown * \return RCL_RET_OK if initialization is successful, or * RCL_RET_ALREADY_INIT if rcl_init has already been called, or * RCL_RET_ERROR if an unspecified error occurs. @@ -74,14 +74,14 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator); * RCL_RET_NOT_INIT if rcl_init has not yet been called */ rcl_ret_t -rcl_fini(); +rcl_shutdown(); /// Returns an uint64_t number that is unique for the latest rcl_init call. -/* If called before rcl_init or after rcl_fini then 0 will be returned. */ +/* If called before rcl_init or after rcl_shutdown then 0 will be returned. */ uint64_t rcl_get_instance_id(); -/// Return true until rcl_fini is called, then false. +/// Return true until rcl_shutdown is called, then false. /* This function is thread safe. */ bool rcl_ok(); diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index b2dd86f..f008864 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -117,7 +117,10 @@ rcl_get_zero_initialized_subscription(); * \param[in] type_support type support object for the topic's type * \param[in] topic_name the name of the topic * \param[in] options subscription options, including quality of service settings - * \return RMW_RET_OK if subscription was initialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if subscription was initialized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_subscription_init( @@ -140,7 +143,9 @@ rcl_subscription_init( * * \param[inout] subscription handle to the subscription to be deinitialized * \param[in] node handle to the node used to create the subscription - * \return RMW_RET_OK if subscription was deinitialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if subscription was deinitialized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node); @@ -157,7 +162,7 @@ rcl_subscription_get_default_options(); * be checked by this function and therefore no deliberate error will occur. * * TODO(wjwwood) blocking of take? - * TODO(wjwwood) pre- and post-conditions for message ownership? + * TODO(wjwwood) pre-, during-, and post-conditions for message ownership? * TODO(wjwwood) is rcl_take thread-safe? * * The ros_message pointer should point to an already allocated ROS message @@ -184,7 +189,10 @@ rcl_subscription_get_default_options(); * \param[inout] ros_message type-erased ptr to a allocated ROS message * \param[out] taken pointer to a bool, if set to false a message was not taken * \param[out] message_info rmw struct which contains meta-data for the message - * \return RMW_RET_OK if the message was published, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if the message was published, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_take( diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index cef999e..2dd33fe 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -24,7 +24,10 @@ typedef rmw_ret_t rcl_ret_t; // rcl specific ret codes start at 100 #define RCL_RET_ALREADY_INIT 100 #define RCL_RET_NOT_INIT 101 +#define RCL_RET_BAD_ALLOC 102 +#define RCL_RET_INVALID_ARGUMENT 103 // rcl node specific ret codes in 2XX +#define RCL_RET_NODE_INVALID 200 // rcl publisher specific ret codes in 3XX // rcl subscription specific ret codes in 4XX // rcl service client specific ret codes in 5XX diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index 2d415dc..7fc853c 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -45,7 +45,7 @@ typedef struct rcl_wait_set_t { /// If set to true, actions like add_subscription will fail until cleared. bool pruned; /// Implementation specific storage. - rcl_wait_set_impl_t * impl; + struct rcl_wait_set_impl_t * impl; } rcl_wait_set_t; /// Return a rcl_wait_set_t struct with members set to NULL. @@ -87,6 +87,8 @@ rcl_get_zero_initialized_wait_set(); * \param[in] allocator the allocator to use when allocating space in the sets * \return RCL_RET_OK if the wait set is initialized successfully, or * RCL_RET_ALREADY_INIT if the wait set is not zero initialized, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t @@ -110,7 +112,9 @@ rcl_wait_set_init( * This function is not thread-safe. * * \param[inout] wait_set the wait set struct to be finalized. - * \return RCL_RET_OK if the finalization was successful, otherwise RCL_RET_ERROR + * \return RCL_RET_OK if the finalization was successful, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_wait_set_fini(rcl_wait_set_t * wait_set); @@ -126,6 +130,7 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set); * \return RCL_RET_OK if added successfully, or * RCL_RET_WAIT_SET_FULL if the subscription set is full, or * RCL_RET_NOT_INIT if the wait set is zero initialized, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t @@ -144,6 +149,7 @@ rcl_wait_set_add_subscription( * \param[inout] wait_set struct to have its subscriptions cleared * \return RCL_RET_OK if cleared successfully, or * RCL_RET_NOT_INIT if the wait set is zero initialized, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t @@ -170,6 +176,8 @@ rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); * \param[inout] wait_set struct to have its subscriptions cleared * \param[in] size a size for the new set * \return RCL_RET_OK if resized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t @@ -282,6 +290,7 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); * RCL_RET_TIMEOUT timeout expired before something was ready, or * RCL_RET_NOT_INIT wait set is zero initialized, or * RCL_RET_WAIT_SET_EMPTY wait set contains no items, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or * RCL_RET_ERROR an unspecified error occur. */ rcl_ret_t diff --git a/rcl/package.xml b/rcl/package.xml index 034064c..700656b 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -11,9 +11,14 @@ ament_cmake rmw + + rmw_implementation rmw + rcl_interfaces + rosidl_generator_cpp + ament_lint_auto ament_lint_common diff --git a/rcl/src/rcl/allocator.c b/rcl/src/rcl/allocator.c index 93eee17..a51ae70 100644 --- a/rcl/src/rcl/allocator.c +++ b/rcl/src/rcl/allocator.c @@ -40,5 +40,11 @@ __default_reallocate(void * pointer, size_t size, void * state) rcl_allocator_t rcl_get_default_allocator() { - return {__default_allocate, __default_deallocate, __default_reallocate, NULL}; + static rcl_allocator_t default_allocator = { + __default_allocate, + __default_deallocate, + __default_reallocate, + NULL + }; + return default_allocator; } diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h index c62260b..827c33b 100644 --- a/rcl/src/rcl/common.h +++ b/rcl/src/rcl/common.h @@ -23,8 +23,8 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/types.h" -#define RCL_CHECK_PARAMETER_FOR_NULL(parameter, error_return_type) \ - RCL_CHECK_FOR_NULL_WITH_MSG(parameter, #parameter " parameter is null", return error_return_type) +#define RCL_CHECK_ARGUMENT_FOR_NULL(parameter, error_return_type) \ + RCL_CHECK_FOR_NULL_WITH_MSG(parameter, #parameter " argument is null", return error_return_type) #define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) if (!value) { \ RCL_SET_ERROR_MSG(msg); \ diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c new file mode 100644 index 0000000..a57db09 --- /dev/null +++ b/rcl/src/rcl/guard_condition.c @@ -0,0 +1,150 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/guard_condition.h" + +#include "./common.h" +#include "rcl/rcl.h" +#include "rmw/rmw.h" + +typedef struct rcl_guard_condition_impl_t { + rmw_guard_condition_t * rmw_handle; + rcl_guard_condition_options_t options; +} rcl_guard_condition_impl_t; + +rcl_guard_condition_t +rcl_get_zero_initialized_guard_condition() +{ + static rcl_guard_condition_t null_guard_condition = {0}; + return null_guard_condition; +} + +rcl_ret_t +rcl_guard_condition_init( + rcl_guard_condition_t * guard_condition, + const rcl_node_t * node, + const rcl_guard_condition_options_t * options) +{ + // Perform argument validation. + RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); + // Ensure the node is valid. + if (!node->impl || rcl_node_get_rcl_instance_id(node) != rcl_get_instance_id()) { + RCL_SET_ERROR_MSG("node handle is invalid"); + return RCL_RET_NODE_INVALID; + } + // Ensure the guard_condition handle is zero initialized. + if (guard_condition->impl) { + RCL_SET_ERROR_MSG("guard_condition already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + // Allocate space for the guard condition impl. + guard_condition->impl = (rcl_guard_condition_impl_t *)allocator->allocate( + sizeof(rcl_guard_condition_impl_t), allocator->state); + if (!guard_condition->impl) { + RCL_SET_ERROR_MSG("allocating memory failed"); + return RCL_RET_BAD_ALLOC; + } + // Create the rmw guard condition. + guard_condition->impl->rmw_handle = rmw_create_guard_condition(); + if (!guard_condition->impl->rmw_handle) { + // Deallocate impl and exit. + allocator->deallocate(guard_condition->impl, allocator->state); + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + // Copy options into impl. + guard_condition->impl->options = *options; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * node) +{ + // Perform argument validation. + RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + // Ensure the node is valid. + if (!node->impl || rcl_node_get_rcl_instance_id(node) != rcl_get_instance_id()) { + RCL_SET_ERROR_MSG("node handle is invalid"); + return RCL_RET_NODE_INVALID; + } + rcl_ret_t result = RCL_RET_OK; + if (guard_condition->impl) { + if (guard_condition->impl->rmw_handle) { + if (rmw_destroy_guard_condition(guard_condition->impl->rmw_handle) != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + result = RCL_RET_ERROR; + } + } + rcl_allocator_t allocator = guard_condition->impl->options.allocator; + if (allocator.deallocate) { + allocator.deallocate(guard_condition->impl, allocator.state); + } else { + RCL_SET_ERROR_MSG("deallocate not set"); + result = RCL_RET_ERROR; + } + } + return result; +} + +rcl_guard_condition_options_t +rcl_guard_condition_get_default_options() +{ + static rcl_guard_condition_options_t default_options = {0}; + // Must set the allocator after because it is not a compile time constant. + default_options.allocator = rcl_get_default_allocator(); + return default_options; +} + +rcl_ret_t +rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition) +{ + // Perform argument validation. + RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + guard_condition->impl, + "guard condition implementation is invalid", + return RCL_RET_INVALID_ARGUMENT); + // Trigger the guard condition. + if (rmw_trigger_guard_condition(guard_condition->impl->rmw_handle) != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +rmw_guard_condition_t * +rcl_guard_condition_get_rmw_guard_condition_handle(rcl_guard_condition_t * guard_condition) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + guard_condition->impl, "guard condition implementation is invalid", return NULL); + return guard_condition->impl->rmw_handle; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 6d2d19c..50e5aa9 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -17,18 +17,18 @@ extern "C" { #endif +#include "rcl/node.h" + #include #include #include -#include "rcl/node.h" #include "rcl/rcl.h" #include "rmw/rmw.h" #include "./common.h" typedef struct rcl_node_impl_t { - char * name; rcl_node_options_t options; rmw_node_t * rmw_node_handle; uint64_t rcl_instance_id; @@ -37,7 +37,8 @@ typedef struct rcl_node_impl_t { rcl_node_t rcl_get_uninitialized_node() { - return {0}; + static rcl_node_t null_node = {0}; + return null_node; } rcl_ret_t @@ -46,19 +47,22 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o size_t domain_id = 0; char * ros_domain_id; rcl_ret_t ret; - RCL_CHECK_PARAMETER_FOR_NULL(name, RCL_RET_ERROR); - RCL_CHECK_PARAMETER_FOR_NULL(options, RCL_RET_ERROR); - RCL_CHECK_PARAMETER_FOR_NULL(node, RCL_RET_ERROR); + rcl_ret_t fail_ret = RCL_RET_ERROR; + RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); if (node->impl) { - RCL_SET_ERROR_MSG( - "either double call to rcl_node_init or rcl_get_uninitialized_node was not used"); - return RCL_RET_ERROR; + RCL_SET_ERROR_MSG("node already initialized, or struct memory was unintialized"); + return RCL_RET_ALREADY_INIT; } const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); // Allocate space for the implementation struct. - RCL_CHECK_FOR_NULL_WITH_MSG(allocator->allocate, "allocate not set", return RCL_RET_ERROR); node->impl = (rcl_node_impl_t *)allocator->allocate(sizeof(rcl_node_impl_t), allocator->state); - RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "allocating memory failed", return RCL_RET_ERROR); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); // Initialize node impl. // node name size_t name_len = strlen(name); @@ -66,9 +70,6 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o RCL_SET_ERROR_MSG("node name cannot be empty string"); goto fail; } - node->impl->name = (char *)allocator->allocate(strlen(name), allocator->state); - RCL_CHECK_FOR_NULL_WITH_MSG(node->impl->name, "allocating memory failed", goto fail); - strncpy(node->impl->name, name, strlen(name)); // node options node->impl->options = *options; // node rmw_node_handle @@ -79,7 +80,7 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o goto fail; } if (ros_domain_id) { - auto number = strtoul(ros_domain_id, NULL, 0); + unsigned long number = strtoul(ros_domain_id, NULL, 0); if (number == ULONG_MAX) { RCL_SET_ERROR_MSG("failed to interpret ROS_DOMAIN_ID as integral number"); goto fail; @@ -92,20 +93,20 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o node->impl->rcl_instance_id = rcl_get_instance_id(); return RCL_RET_OK; fail: - if (node->impl->name) { - allocator->deallocate(node->impl->name, allocator->state); - } if (node->impl) { allocator->deallocate(node->impl, allocator->state); } - return RCL_RET_ERROR; + return fail_ret; } rcl_ret_t rcl_node_fini(rcl_node_t * node) { - RCL_CHECK_PARAMETER_FOR_NULL(node, RCL_RET_ERROR); - RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return RCL_RET_ERROR); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + if (!node->impl) { + // Repeat calls to fini or calling fini on a zero initialized node is ok. + return RCL_RET_OK; + } rcl_ret_t result = RCL_RET_OK; rmw_ret_t node_ret = rmw_destroy_node(node->impl->rmw_node_handle); if (node_ret != RMW_RET_OK) { @@ -113,7 +114,8 @@ rcl_node_fini(rcl_node_t * node) result = RCL_RET_ERROR; } rcl_allocator_t allocator = node->impl->options.allocator; - allocator.deallocate(node->impl->name, allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); allocator.deallocate(node->impl, allocator.state); return result; } @@ -121,25 +123,31 @@ rcl_node_fini(rcl_node_t * node) rcl_node_options_t rcl_node_get_default_options() { - return {false, rcl_get_default_allocator()}; + static rcl_node_options_t default_options = { + .no_parameters = false, + .domain_id = RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID, + }; + // Must set the allocator after because it is not a compile time constant. + default_options.allocator = rcl_get_default_allocator(); + return default_options; } const char * rcl_node_get_name(const rcl_node_t * node) { - RCL_CHECK_PARAMETER_FOR_NULL(node, NULL); + RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL); RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL); if (node->impl->rcl_instance_id != rcl_get_instance_id()) { RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match"); return NULL; } - return node->impl->name; + return node->impl->rmw_node_handle->name; } const rcl_node_options_t * rcl_node_get_options(const rcl_node_t * node) { - RCL_CHECK_PARAMETER_FOR_NULL(node, NULL); + RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL); RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL); if (node->impl->rcl_instance_id != rcl_get_instance_id()) { RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match"); @@ -151,7 +159,7 @@ rcl_node_get_options(const rcl_node_t * node) rmw_node_t * rcl_node_get_rmw_node_handle(const rcl_node_t * node) { - RCL_CHECK_PARAMETER_FOR_NULL(node, NULL); + RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL); RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL); if (node->impl->rcl_instance_id != rcl_get_instance_id()) { RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match"); @@ -163,8 +171,8 @@ rcl_node_get_rmw_node_handle(const rcl_node_t * node) uint64_t rcl_node_get_rcl_instance_id(const rcl_node_t * node) { - RCL_CHECK_PARAMETER_FOR_NULL(node, NULL); - RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL); + RCL_CHECK_ARGUMENT_FOR_NULL(node, 0); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return 0); return node->impl->rcl_instance_id; } diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c new file mode 100644 index 0000000..319b073 --- /dev/null +++ b/rcl/src/rcl/publisher.c @@ -0,0 +1,91 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/publisher.h" + +#include + +#include "./common.h" +#include "rmw/rmw.h" + +typedef struct rcl_publisher_impl_t { + rcl_publisher_options_t options; + rmw_publisher_t * rmw_handle; +} rcl_publisher_impl_t; + +rcl_publisher_t +rcl_get_zero_initialized_publisher() +{ + static rcl_publisher_t null_publisher = {0}; + return null_publisher; +} + +rcl_ret_t +rcl_publisher_init( + rcl_publisher_t * publisher, + const rcl_node_t * node, + const rosidl_message_type_support_t * type_support, + const char * topic_name, + const rcl_publisher_options_t * options) +{ + rcl_ret_t fail_ret = RCL_RET_ERROR; + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + if (publisher->impl) { + RCL_SET_ERROR_MSG("publisher already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); + // Allocate space for the implementation struct. + publisher->impl = (rcl_publisher_impl_t *)allocator->allocate( + sizeof(rcl_publisher_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + // Fill out implementation struct. + // rmw handle (create rmw publisher) + // TODO(wjwwood): pass along the allocator to rmw when it supports it + publisher->impl->rmw_handle = rmw_create_publisher( + rcl_node_get_rmw_node_handle(node), + type_support, + topic_name, + &rmw_qos_profile_default); + if (!publisher->impl->rmw_handle) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + goto fail; + } + // options + publisher->impl->options = *options; + return RCL_RET_OK; +fail: + if (publisher->impl) { + allocator->deallocate(publisher->impl, allocator->state); + } + return fail_ret; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index a8488ca..537b975 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -81,10 +81,10 @@ fail: } rcl_ret_t -rcl_fini() +rcl_shutdown() { if (!atomic_load(&__rcl_is_initialized)) { - RCL_SET_ERROR_MSG("rcl_fini called before rcl_init"); + RCL_SET_ERROR_MSG("rcl_shutdown called before rcl_init"); return RCL_RET_NOT_INIT; } __clean_up_init(); diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c new file mode 100644 index 0000000..3136616 --- /dev/null +++ b/rcl/src/rcl/subscription.c @@ -0,0 +1,26 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/subscription.h" + + + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c new file mode 100644 index 0000000..32577a2 --- /dev/null +++ b/rcl/src/rcl/wait.c @@ -0,0 +1,24 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/wait.h" + +#if __cplusplus +} +#endif From 0cb3bf3d9bd0227ed44919ff8adf7fb81c96a800 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 1 Dec 2015 09:42:17 -0800 Subject: [PATCH 08/71] fix up publisher --- rcl/include/rcl/publisher.h | 27 +++++++------- rcl/include/rcl/types.h | 1 + rcl/src/rcl/node.c | 4 +-- rcl/src/rcl/publisher.c | 72 +++++++++++++++++++++++++++++++++++++ 4 files changed, 90 insertions(+), 14 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index f4bf6f2..f229e7b 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -114,7 +114,12 @@ rcl_get_zero_initialized_publisher(); * \param[in] type_support type support object for the topic's type * \param[in] topic_name the name of the topic to publish on * \param[in] options publisher options, including quality of service settings - * \return RMW_RET_OK if publisher was initialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if the publisher was initialized successfully, or + * RCL_RET_NODE_INVALID if the node is invalid, or + * RCL_RET_ALREADY_INIT if the publisher is already initialized, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_BAD_ALLOC if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_publisher_init( @@ -135,7 +140,9 @@ rcl_publisher_init( * * \param[inout] publisher handle to the publisher to be deinitialized * \param[in] node handle to the node used to create the publisher - * \return RMW_RET_OK if publisher was deinitialized successfully, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if publisher was finalized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node); @@ -144,13 +151,6 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node); rcl_publisher_options_t rcl_publisher_get_default_options(); -// TODO(wjwwood): I have an idea on how to do a type-checking macro version of -// publish using C11's _Generic "generic selection", but it -// needs support from the generated code (look into it later) -// It might also use static assert, see: -// http://www.robertgamble.net/2012/01/c11-static-assertions.html -// #define RCL_PUBLISH(publisher, ros_message) ... - /// Publish a ROS message on a topic using a publisher. /* It is the job of the caller to ensure that the type of the ros_message * parameter and the type associate with the publisher (via the type support) @@ -190,7 +190,10 @@ rcl_publisher_get_default_options(); * * \param[in] publisher handle to the publisher which will do the publishing * \param[in] ros_message type-erased pointer to the ROS message - * \return RMW_RET_OK if the message was published, otherwise RMW_RET_ERROR + * \return RCL_RET_OK if the message was published successfully, or + * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or + * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_publish(const rcl_publisher_t * publisher, const void * ros_message); @@ -231,7 +234,7 @@ rcl_publisher_get_topic_name(const rcl_publisher_t * publisher); * \return options struct if successful, otherwise NULL */ const rcl_publisher_options_t * -rcl_publisher_get_options(rcl_publisher_t * publisher); +rcl_publisher_get_options(const rcl_publisher_t * publisher); /// Return the rmw publisher handle. /* The handle returned is a pointer to the internally held rmw handle. @@ -254,7 +257,7 @@ rcl_publisher_get_options(rcl_publisher_t * publisher); * \return rmw publisher handle if successful, otherwise NULL */ rmw_publisher_t * -rcl_publisher_get_rmw_publisher_handle(rcl_publisher_t * publisher); +rcl_publisher_get_rmw_publisher_handle(const rcl_publisher_t * publisher); #if __cplusplus } diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index 2dd33fe..e470aeb 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -29,6 +29,7 @@ typedef rmw_ret_t rcl_ret_t; // rcl node specific ret codes in 2XX #define RCL_RET_NODE_INVALID 200 // rcl publisher specific ret codes in 3XX +#define RCL_RET_PUBLISHER_INVALID 300 // rcl subscription specific ret codes in 4XX // rcl service client specific ret codes in 5XX // rcl service server specific ret codes in 6XX diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 50e5aa9..d6e94b3 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -108,8 +108,8 @@ rcl_node_fini(rcl_node_t * node) return RCL_RET_OK; } rcl_ret_t result = RCL_RET_OK; - rmw_ret_t node_ret = rmw_destroy_node(node->impl->rmw_node_handle); - if (node_ret != RMW_RET_OK) { + rmw_ret_t ret = rmw_destroy_node(node->impl->rmw_node_handle); + if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); result = RCL_RET_ERROR; } diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 319b073..af97185 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -86,6 +86,78 @@ fail: return fail_ret; } +rcl_ret_t +rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) +{ + rcl_ret_t result = RCL_RET_OK; + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + if (publisher->impl) { + rmw_ret_t ret = + rmw_destroy_publisher(rcl_node_get_rmw_node_handle(node), publisher->impl->rmw_handle); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + result = RCL_RET_ERROR; + } + rcl_allocator_t allocator = publisher->impl->options.allocator; + allocator.deallocate(publisher->impl, allocator.state); + } + return result; +} + +rcl_publisher_options_t +rcl_publisher_get_default_options() +{ + static rcl_publisher_options_t default_options = { + .qos = rmw_qos_profile_default, + }; + default_options.allocator = rcl_get_default_allocator(); + return default_options; +} + +rcl_ret_t +rcl_publish(const rcl_publisher_t * publisher, const void * ros_message) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl, "publisher is invalid", return RCL_RET_PUBLISHER_INVALID); + if (rmw_publish(publisher->impl->rmw_handle, ros_message) != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +const char * +rcl_publisher_get_topic_name(const rcl_publisher_t * publisher) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl, "publisher is invalid", return NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl->rmw_handle, "publisher is invalid", return NULL); + return publisher->impl->rmw_handle->topic_name; +} + +const rcl_publisher_options_t * +rcl_publisher_get_options(const rcl_publisher_t * publisher) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl, "publisher is invalid", return NULL); + return &publisher->impl->options; +} + +rmw_publisher_t * +rcl_publisher_get_rmw_publisher_handle(const rcl_publisher_t * publisher) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl, "publisher is invalid", return NULL); + return publisher->impl->rmw_handle; +} + #if __cplusplus } #endif From 29b50e282f3885cdfd3d65a2c354704f900584dd Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 1 Dec 2015 10:21:53 -0800 Subject: [PATCH 09/71] implement subscriptions --- rcl/include/rcl/subscription.h | 14 +-- rcl/include/rcl/types.h | 1 + rcl/src/rcl/common.h | 4 +- rcl/src/rcl/publisher.c | 6 +- rcl/src/rcl/subscription.c | 153 +++++++++++++++++++++++++++++++++ 5 files changed, 167 insertions(+), 11 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index f008864..6a9f867 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -156,7 +156,7 @@ rcl_subscription_get_default_options(); /// Take a ROS message from a topic using a rcl subscription. /* It is the job of the caller to ensure that the type of the ros_message - * parameter and the type associate with the subscription, via the type + * argument and the type associate with the subscription, via the type * support, match. * Passing a different type to rcl_take produces undefined behavior and cannot * be checked by this function and therefore no deliberate error will occur. @@ -164,6 +164,7 @@ rcl_subscription_get_default_options(); * TODO(wjwwood) blocking of take? * TODO(wjwwood) pre-, during-, and post-conditions for message ownership? * TODO(wjwwood) is rcl_take thread-safe? + * TODO(wjwwood) Should there be an rcl_message_info_t? * * The ros_message pointer should point to an already allocated ROS message * struct of the correct type, into which the taken ROS message will be copied @@ -181,9 +182,9 @@ rcl_subscription_get_default_options(); * message instance, like what the GUID of the publisher which published it * originally or whether or not the message received from within the same * process. - * The message_info parameter should be an already allocated rmw_message_info_t + * The message_info argument should be an already allocated rmw_message_info_t * structure. - * Passing NULL for message_info will be ignored. + * Passing NULL for message_info will result in the argument being ignored. * * \param[in] subscription the handle to the subscription from which to take * \param[inout] ros_message type-erased ptr to a allocated ROS message @@ -191,6 +192,7 @@ rcl_subscription_get_default_options(); * \param[out] message_info rmw struct which contains meta-data for the message * \return RCL_RET_OK if the message was published, or * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ @@ -205,7 +207,7 @@ rcl_take( /* This function returns the subscription's internal topic name string. * This function can fail, and therefore return NULL, if the: * - subscription is NULL - * - subscription is invalid (never called init, called fini, or invalid node) + * - subscription is invalid (never called init, called fini, or invalid) * * The returned string is only valid as long as the subscription is valid. * The value of the string may change if the topic name changes, and therefore @@ -224,7 +226,7 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription); /* This function returns the subscription's internal options struct. * This function can fail, and therefore return NULL, if the: * - subscription is NULL - * - subscription is invalid (never called init, called fini, or invalid node) + * - subscription is invalid (never called init, called fini, or invalid) * * The returned struct is only valid as long as the subscription is valid. * The values in the struct may change if the subscription's options change, @@ -243,7 +245,7 @@ rcl_subscription_get_options(rcl_subscription_t * subscription); /* The handle returned is a pointer to the internally held rmw handle. * This function can fail, and therefore return NULL, if the: * - subscription is NULL - * - subscription is invalid (never called init, called fini, or invalid node) + * - subscription is invalid (never called init, called fini, or invalid) * * The returned handle is only valid as long as the rcl subscription is valid. * diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index e470aeb..0abb885 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -31,6 +31,7 @@ typedef rmw_ret_t rcl_ret_t; // rcl publisher specific ret codes in 3XX #define RCL_RET_PUBLISHER_INVALID 300 // rcl subscription specific ret codes in 4XX +#define RCL_RET_SUBSCRIPTION_INVALID 400 // rcl service client specific ret codes in 5XX // rcl service server specific ret codes in 6XX // rcl guard condition specific ret codes in 7XX diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h index 827c33b..379611f 100644 --- a/rcl/src/rcl/common.h +++ b/rcl/src/rcl/common.h @@ -23,8 +23,8 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/types.h" -#define RCL_CHECK_ARGUMENT_FOR_NULL(parameter, error_return_type) \ - RCL_CHECK_FOR_NULL_WITH_MSG(parameter, #parameter " argument is null", return error_return_type) +#define RCL_CHECK_ARGUMENT_FOR_NULL(argument, error_return_type) \ + RCL_CHECK_FOR_NULL_WITH_MSG(argument, #argument " argument is null", return error_return_type) #define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) if (!value) { \ RCL_SET_ERROR_MSG(msg); \ diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index af97185..c7dc189 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -108,9 +108,9 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) rcl_publisher_options_t rcl_publisher_get_default_options() { - static rcl_publisher_options_t default_options = { - .qos = rmw_qos_profile_default, - }; + static rcl_publisher_options_t default_options; + // Must set the allocator and qos after because they are not a compile time constant. + default_options.qos = rmw_qos_profile_default; default_options.allocator = rcl_get_default_allocator(); return default_options; } diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 3136616..880bf31 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -19,7 +19,160 @@ extern "C" #include "rcl/subscription.h" +#include "rmw/rmw.h" +#include "./common.h" +typedef struct rcl_subscription_impl_t { + rcl_subscription_options_t options; + rmw_subscription_t * rmw_handle; +} rcl_subscription_impl_t; + +rcl_subscription_t +rcl_get_zero_initialized_subscription() +{ + static rcl_subscription_t null_subscription = {0}; + return null_subscription; +} + +rcl_ret_t +rcl_subscription_init( + rcl_subscription_t * subscription, + const rcl_node_t * node, + const rosidl_message_type_support_t * type_support, + const char * topic_name, + const rcl_subscription_options_t * options) +{ + rcl_ret_t fail_ret = RCL_RET_ERROR; + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + if (subscription->impl) { + RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninialized"); + return RCL_RET_ALREADY_INIT; + } + const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); + // Allocate memory for the implementation struct. + subscription->impl = (rcl_subscription_impl_t *)allocator->allocate( + sizeof(rcl_subscription_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + // Fill out the implemenation struct. + // rmw_handle + // TODO(wjwwood): pass allocator once supported in rmw api. + subscription->impl->rmw_handle = rmw_create_subscription( + rcl_node_get_rmw_node_handle(node), + type_support, + topic_name, + &rmw_qos_profile_default, + options->ignore_local_publications); + if (!subscription->impl->rmw_handle) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + goto fail; + } + // options + subscription->impl->options = *options; + return RCL_RET_OK; +fail: + if (subscription->impl) { + allocator->deallocate(subscription->impl, allocator->state); + } + return fail_ret; +} + +rcl_ret_t +rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) +{ + rcl_ret_t result = RCL_RET_OK; + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + if (subscription->impl) { + rmw_ret_t ret = + rmw_destroy_subscription(rcl_node_get_rmw_node_handle(node), subscription->impl->rmw_handle); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + result = RCL_RET_ERROR; + } + rcl_allocator_t allocator = subscription->impl->options.allocator; + allocator.deallocate(subscription->impl, allocator.state); + } + return result; +} + +rcl_subscription_options_t +rcl_subscription_get_default_options() +{ + static rcl_subscription_options_t default_options = { + .ignore_local_publications = false, + }; + // Must set the allocator and qos after because they are not a compile time constant. + default_options.qos = rmw_qos_profile_default; + default_options.allocator = rcl_get_default_allocator(); + return default_options; +} + +rcl_ret_t +rcl_take( + const rcl_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(taken, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl, "subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl->rmw_handle, + "subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID); + // If message_info is NULL, use a place holder which can be discarded. + rmw_message_info_t dummy_message_info; + rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info; + // Call rmw_take_with_info. + rmw_ret_t ret = + rmw_take_with_info(subscription->impl->rmw_handle, ros_message, taken, message_info); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +const char * +rcl_subscription_get_topic_name(const rcl_subscription_t * subscription) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl, "subscription is invalid", return NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl->rmw_handle, + "subscription is invalid", return NULL); + return subscription->impl->rmw_handle->topic_name; +} + +const rcl_subscription_options_t * +rcl_subscription_get_options(rcl_subscription_t * subscription) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl, "subscription is invalid", return NULL); + return &subscription->impl->options; +} + +rmw_subscription_t * +rcl_subscription_get_rmw_subscription_handle(rcl_subscription_t * subscription) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl, "subscription is invalid", return NULL); + return subscription->impl->rmw_handle; +} #if __cplusplus } From 483ddfdb86a05beedfc31c24eb569e93a4b839f3 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 1 Dec 2015 12:45:54 -0800 Subject: [PATCH 10/71] everything but rcl_wait --- rcl/include/rcl/allocator.h | 1 + rcl/include/rcl/types.h | 5 +- rcl/include/rcl/wait.h | 35 +++---- rcl/src/rcl/wait.c | 181 ++++++++++++++++++++++++++++++++++++ 4 files changed, 204 insertions(+), 18 deletions(-) diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index 1ae76b3..7742c50 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -35,6 +35,7 @@ typedef struct rcl_allocator_t { * This should behave as realloc is described, as opposed to reallocf, i.e. * the memory given by pointer will not be free'd automatically if realloc * fails. + * This function must be able to take an input pointer of NULL and succeed. */ void * (*reallocate)(void * pointer, size_t size, void * state); /// Implementation defined state storage. diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index 0abb885..9aecce3 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -36,7 +36,8 @@ typedef rmw_ret_t rcl_ret_t; // rcl service server specific ret codes in 6XX // rcl guard condition specific ret codes in 7XX // rcl wait and wait set specific ret codes in 8XX -#define RCL_RET_WAIT_SET_EMPTY 800 -#define RCL_RET_WAIT_SET_FULL 801 +#define RCL_RET_WAIT_SET_INVALID 800 +#define RCL_RET_WAIT_SET_EMPTY 801 +#define RCL_RET_WAIT_SET_FULL 802 #endif // RCL__TYPES_H_ diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index 7fc853c..d2f4c53 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -33,11 +33,11 @@ struct rcl_wait_set_impl_t; /// Container for subscription's, guard condition's, etc to be waited on. typedef struct rcl_wait_set_t { /// Storage for subscription pointers. - rcl_subscription_t ** subscriptions; + const rcl_subscription_t ** subscriptions; size_t size_of_subscriptions; size_t __current_subscription_offset; /// Storage for guard condition pointers. - rcl_guard_condition_t ** guard_conditions; + const rcl_guard_condition_t ** guard_conditions; size_t size_of_guard_conditions; size_t __current_guard_condition_offset; /// Allocator for storage. @@ -55,7 +55,7 @@ rcl_get_zero_initialized_wait_set(); /// Initialize a rcl wait set with space for items to be waited on. /* This function allocates space for the subscriptions and other wait-able * entities that can be stored in the wait set. - * It also sets the allocator to the given one and initializes the pruned + * It also sets the allocator to the given allocator and initializes the pruned * member to be false. * * The wait_set struct should be allocated and initialized to NULL. @@ -73,7 +73,7 @@ rcl_get_zero_initialized_wait_set(); * * rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); * rcl_ret_t ret = rcl_wait_set_init(&wait_set, 42, 42, rcl_get_default_allocator()); - * // ... error handling, and then after using call matching fini: + * // ... error handling, then use it, then call the matching fini: * ret = rcl_wait_set_fini(&wait_set); * // ... error handling * @@ -82,12 +82,12 @@ rcl_get_zero_initialized_wait_set(); * allocator is shared with other parts of the system. * * \param[inout] wait_set the wait set struct to be initialized - * \param[in] number_of_subscriptions size of the subscriptions set - * \param[in] number_of_guard_conditions size of the guard conditions set + * \param[in] number_of_subscriptions non-zero size of the subscriptions set + * \param[in] number_of_guard_conditions non-zero size of the guard conditions set * \param[in] allocator the allocator to use when allocating space in the sets * \return RCL_RET_OK if the wait set is initialized successfully, or * RCL_RET_ALREADY_INIT if the wait set is not zero initialized, or - * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ @@ -113,13 +113,13 @@ rcl_wait_set_init( * * \param[inout] wait_set the wait set struct to be finalized. * \return RCL_RET_OK if the finalization was successful, or - * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_wait_set_fini(rcl_wait_set_t * wait_set); -/// Stores a pointer to the given subscription in next empty spot in the set. +/// Stores a pointer to the given subscription in the next empty spot in the set. /* This function does not guarantee that the subscription is not already in the * wait set. * @@ -128,9 +128,9 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set); * \param[inout] wait_set struct in which the subscription is to be stored * \param[in] subscription the subscription to be added to the wait set * \return RCL_RET_OK if added successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or * RCL_RET_WAIT_SET_FULL if the subscription set is full, or - * RCL_RET_NOT_INIT if the wait set is zero initialized, or - * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t @@ -148,8 +148,8 @@ rcl_wait_set_add_subscription( * * \param[inout] wait_set struct to have its subscriptions cleared * \return RCL_RET_OK if cleared successfully, or - * RCL_RET_NOT_INIT if the wait set is zero initialized, or - * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t @@ -176,14 +176,14 @@ rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); * \param[inout] wait_set struct to have its subscriptions cleared * \param[in] size a size for the new set * \return RCL_RET_OK if resized successfully, or - * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size); -/// Stores a pointer to the guard condition in next empty spot in the set. +/// Stores a pointer to the guard condition in the next empty spot in the set. /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_add_subscription */ @@ -281,6 +281,9 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); * The timeout's value is not changed. * Passing a timeout struct with uninitialized memory is undefined behavior. * + * \TODO(wjwwood) this function should probably be thread-safe with itself but + * it's not clear to me what happens if the wait sets being + * waited on can be overlapping or not or if we can even check. * This function is not thread-safe and cannot be called concurrently, even if * the given wait sets are not the same and non-overlapping in contents. * @@ -290,7 +293,7 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); * RCL_RET_TIMEOUT timeout expired before something was ready, or * RCL_RET_NOT_INIT wait set is zero initialized, or * RCL_RET_WAIT_SET_EMPTY wait set contains no items, or - * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR an unspecified error occur. */ rcl_ret_t diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 32577a2..8ecc9bc 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -19,6 +19,187 @@ extern "C" #include "rcl/wait.h" +#include +#include + +#include "rcl/error_handling.h" +#include "rmw/rmw.h" +#include "./common.h" + +typedef struct rcl_wait_set_impl_t { + void * place_holder; // To prevent size differences between C and C++. +} rcl_wait_set_impl_t; + +rcl_wait_set_t +rcl_get_zero_initialized_wait_set() +{ + static rcl_wait_set_t null_wait_set = {0}; + return null_wait_set; +} + +static bool +__wait_set_is_valid(rcl_wait_set_t * wait_set) +{ + return wait_set && wait_set->impl; +} + +rcl_ret_t +rcl_wait_set_init( + rcl_wait_set_t * wait_set, + size_t number_of_subscriptions, + size_t number_of_guard_conditions, + rcl_allocator_t allocator) +{ + rcl_ret_t fail_ret = RCL_RET_ERROR; + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + if (__wait_set_is_valid(wait_set)) { + RCL_SET_ERROR_MSG("wait_set already initialized, or memory was uninitialized."); + return RCL_RET_ALREADY_INIT; + } + RCL_CHECK_FOR_NULL_WITH_MSG( + number_of_subscriptions, + "number_of_subscriptions cannot be 0", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + number_of_guard_conditions, + "number_of_guard_conditions cannot be 0", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator.allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator.reallocate, "reallocate not set", return RCL_RET_INVALID_ARGUMENT); + // Right now, the implementation struct is unused, but checked for NULL, so set it. + wait_set->impl = (rcl_wait_set_impl_t *)0xDEADBEEF; + // Initialize subscription space. + rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions); + rcl_wait_set_clear_subscriptions(wait_set); + // Initialize guard condition space. + rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions); + rcl_wait_set_clear_guard_conditions(wait_set); + // Set allocator. + wait_set->allocator = allocator; + // Initialize pruned. + wait_set->pruned = false; + return RCL_RET_OK; +fail: + if (wait_set->subscriptions) { + allocator.deallocate(wait_set->subscriptions, allocator.state); + } + if (wait_set->guard_conditions) { + allocator.deallocate(wait_set->guard_conditions, allocator.state); + } + return fail_ret; +} + +rcl_ret_t +rcl_wait_set_fini(rcl_wait_set_t * wait_set) +{ + rcl_ret_t result = RCL_RET_OK; + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + if (__wait_set_is_valid(wait_set)) { + if (wait_set->subscriptions) { + wait_set->allocator.deallocate(wait_set->subscriptions, wait_set->allocator.state); + } + if (wait_set->guard_conditions) { + wait_set->allocator.deallocate(wait_set->guard_conditions, wait_set->allocator.state); + } + if (wait_set->impl) { + wait_set->impl = NULL; + } + } + return result; +} + +#define SET_ADD(Type) \ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \ + RCL_CHECK_ARGUMENT_FOR_NULL(Type, RCL_RET_INVALID_ARGUMENT); \ + if (!__wait_set_is_valid(wait_set)) { \ + RCL_SET_ERROR_MSG("wait set is invalid"); \ + return RCL_RET_WAIT_SET_INVALID; \ + } \ + if (!(wait_set->__current_##Type##_offset < wait_set->size_of_##Type##s)) { \ + RCL_SET_ERROR_MSG(#Type "s set is full"); \ + return RCL_RET_WAIT_SET_FULL; \ + } \ + wait_set->Type##s[wait_set->__current_##Type##_offset++] = Type; \ + return RCL_RET_OK; + +#define SET_CLEAR(Type) \ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \ + if (!__wait_set_is_valid(wait_set)) { \ + RCL_SET_ERROR_MSG("wait set is invalid"); \ + return RCL_RET_WAIT_SET_INVALID; \ + } \ + memset(wait_set->Type##s, 0, sizeof(rcl_##Type##_t *) * wait_set->size_of_##Type##s); \ + wait_set->__current_##Type##_offset = 0; \ + return RCL_RET_OK; + +#define SET_RESIZE(Type) \ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \ + if (size == wait_set->size_of_##Type##s) { \ + return RCL_RET_OK; \ + } \ + if (size == 0) { \ + if (wait_set->Type##s) { \ + wait_set->allocator.deallocate(wait_set->Type##s, wait_set->allocator.state); \ + } \ + wait_set->Type##s = NULL; \ + return RCL_RET_OK; \ + } \ + wait_set->size_of_##Type##s = 0; \ + wait_set->Type##s = (const rcl_##Type##_t **)wait_set->allocator.reallocate( \ + wait_set->Type##s, sizeof(rcl_##Type##_t *) * size, wait_set->allocator.state); \ + RCL_CHECK_FOR_NULL_WITH_MSG( \ + wait_set->Type##s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \ + wait_set->size_of_##Type##s = size; \ + return RCL_RET_OK; + +rcl_ret_t +rcl_wait_set_add_subscription( + rcl_wait_set_t * wait_set, + const rcl_subscription_t * subscription) +{ + SET_ADD(subscription) +} + +rcl_ret_t +rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set) +{ + SET_CLEAR(subscription) +} + +rcl_ret_t +rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size) +{ + SET_RESIZE(subscription) +} + +rcl_ret_t +rcl_wait_set_add_guard_condition( + rcl_wait_set_t * wait_set, + const rcl_guard_condition_t * guard_condition) +{ + SET_ADD(guard_condition) +} + +rcl_ret_t +rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set) +{ + SET_CLEAR(guard_condition) +} + +rcl_ret_t +rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size) +{ + SET_RESIZE(guard_condition) +} + +rcl_ret_t +rcl_wait(rcl_wait_set_t * wait_set, const rcl_time_t * timeout) +{ + return RCL_RET_OK; +} + #if __cplusplus } #endif From ad21736113e9dd065e2b10eaa54c90d76465ce2f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 2 Dec 2015 15:44:36 -0800 Subject: [PATCH 11/71] narrow dependency on rosidl to C only --- rcl/CMakeLists.txt | 6 +++--- rcl/package.xml | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 79c5af4..2abd266 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -5,9 +5,9 @@ project(rcl) find_package(ament_cmake REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rmw REQUIRED) -find_package(rosidl_generator_cpp REQUIRED) +find_package(rosidl_generator_c REQUIRED) -ament_export_dependencies(rmw rosidl_generator_cpp) +ament_export_dependencies(rmw rosidl_generator_c) ament_export_include_directories(include) include_directories(include) @@ -30,7 +30,7 @@ add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_sources}) ament_target_dependencies(${PROJECT_NAME} "rcl_interfaces" "rmw" - "rosidl_generator_cpp" + "rosidl_generator_c" ) if(AMENT_ENABLE_TESTING) diff --git a/rcl/package.xml b/rcl/package.xml index 700656b..c0da176 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -17,7 +17,7 @@ rmw rcl_interfaces - rosidl_generator_cpp + rosidl_generator_c ament_lint_auto ament_lint_common From 3e5298bac043976d88d8d6e30bd7b92730307ede Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 4 Dec 2015 09:37:18 -0800 Subject: [PATCH 12/71] some small API changes and initial wait impl --- rcl/CMakeLists.txt | 1 + rcl/include/rcl/guard_condition.h | 2 +- rcl/include/rcl/node.h | 6 - rcl/include/rcl/subscription.h | 4 +- rcl/include/rcl/time.h | 14 ++ rcl/include/rcl/wait.h | 32 ++--- rcl/src/rcl/guard_condition.c | 2 +- rcl/src/rcl/subscription.c | 4 +- rcl/src/rcl/time.c | 42 ++++++ rcl/src/rcl/wait.c | 205 +++++++++++++++++++++++------- 10 files changed, 239 insertions(+), 73 deletions(-) create mode 100644 rcl/src/rcl/time.c diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 2abd266..0f16659 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -25,6 +25,7 @@ set(${PROJECT_NAME}_sources src/rcl/rcl.c src/rcl/subscription.c src/rcl/wait.c + src/rcl/time.c ) add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_sources}) ament_target_dependencies(${PROJECT_NAME} diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index 7e976c3..2f150ac 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/rcl/include/rcl/guard_condition.h @@ -141,7 +141,7 @@ rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition); * \return rmw guard_condition handle if successful, otherwise NULL */ rmw_guard_condition_t * -rcl_guard_condition_get_rmw_guard_condition_handle(rcl_guard_condition_t * guard_condition); +rcl_guard_condition_get_rmw_guard_condition_handle(const rcl_guard_condition_t * guard_condition); #if __cplusplus } diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index d1ad319..02ab38e 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -29,12 +29,6 @@ extern "C" struct rcl_node_impl_t; /// Handle for a ROS node. -/* This handle can be in one a few stats, which are referred to below: - * - uninitialized: memory allocated, but not initialized to anything - * - invalid: memory allocated but not usable - * - either zero initialized or shutdown (rcl_fini() or rcl_node_fini()) - * - valid: memory allocated and usable - */ typedef struct rcl_node_t { /// Private implementation pointer. struct rcl_node_impl_t * impl; diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 6a9f867..0b212d2 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -239,7 +239,7 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription); * \return options struct if successful, otherwise NULL */ const rcl_subscription_options_t * -rcl_subscription_get_options(rcl_subscription_t * subscription); +rcl_subscription_get_options(const rcl_subscription_t * subscription); /// Return the rmw subscription handle. /* The handle returned is a pointer to the internally held rmw handle. @@ -257,7 +257,7 @@ rcl_subscription_get_options(rcl_subscription_t * subscription); * \return rmw subscription handle if successful, otherwise NULL */ rmw_subscription_t * -rcl_subscription_get_rmw_subscription_handle(rcl_subscription_t * subscription); +rcl_subscription_get_rmw_subscription_handle(const rcl_subscription_t * subscription); #if __cplusplus } diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index 8596913..e25c923 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -22,8 +22,22 @@ extern "C" #include "rmw/rmw.h" +#define RCL_S_TO_NS(seconds) seconds * 1000000000 +#define RCL_MS_TO_NS(milliseconds) milliseconds * 1000000 +#define RCL_US_TO_NS(microseconds) microseconds * 1000 + +#define RCL_NS_TO_S(nanoseconds) nanoseconds / 1000000000 +#define RCL_NS_TO_MS(nanoseconds) nanoseconds / 1000000 +#define RCL_NS_TO_US(nanoseconds) nanoseconds / 1000 + typedef rmw_time_t rcl_time_t; +rcl_time_t +rcl_time_t_from_int64_t_nanoseconds(int64_t nanoseconds); + +rcl_time_t +rcl_time_t_from_uint64_t_nanoseconds(uint64_t nanoseconds); + #if __cplusplus } #endif diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index d2f4c53..b86d251 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -25,7 +25,6 @@ extern "C" #include "rcl/subscription.h" #include "rcl/guard_condition.h" -#include "rcl/time.h" #include "rcl/types.h" struct rcl_wait_set_impl_t; @@ -35,11 +34,9 @@ typedef struct rcl_wait_set_t { /// Storage for subscription pointers. const rcl_subscription_t ** subscriptions; size_t size_of_subscriptions; - size_t __current_subscription_offset; /// Storage for guard condition pointers. const rcl_guard_condition_t ** guard_conditions; size_t size_of_guard_conditions; - size_t __current_guard_condition_offset; /// Allocator for storage. rcl_allocator_t allocator; /// If set to true, actions like add_subscription will fail until cleared. @@ -240,8 +237,7 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); * // ... error handling * ret = rcl_wait_set_add_guard_condition(&wait_set, &gc1); * // ... error handling - * rcl_time_t timeout = {1, 0}; // 1 second and 0 nanoseconds. - * ret = rcl_wait(&wait_set, &timeout); + * ret = rcl_wait(&wait_set, RCL_MS_TO_NS(1000)); // 1000ms == 1s, passed as ns * if (ret == RCL_RET_TIMEOUT) { * continue; * } @@ -271,14 +267,14 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); * Passing an uninitialized (zero initialized) wait set struct will fail. * Passing a wait set struct with uninitialized memory is undefined behavior. * - * If the timeout pointer is NULL then this function will block indefinitely - * until something in the wait set is valid or it is interrupted. - * If the timeout contains 0 for seconds and nanoseconds this function will be - * non-blocking; checking what's ready but not waiting if nothing is ready yet. - * If the timeout contains a non-zero time then this function will return after - * that period of time has elapsed if something in the wait set does not become - * ready before then. - * The timeout's value is not changed. + * The unit of timeout is nanoseconds. + * If the timeout is negative then this function will block indefinitely until + * something in the wait set is valid or it is interrupted. + * If the timeout is 0 then this function will be non-blocking; checking what's + * ready now, but not waiting if nothing is ready yet. + * If the timeout is greater than 0 then this function will return after + * that period of time has elapsed or the wait set becomes ready, which ever + * comes first. * Passing a timeout struct with uninitialized memory is undefined behavior. * * \TODO(wjwwood) this function should probably be thread-safe with itself but @@ -288,16 +284,16 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); * the given wait sets are not the same and non-overlapping in contents. * * \param[inout] wait_set the set of things to be waited on and to be pruned if not ready - * \param[in] timeout the time duration to wait something in the wait set to be ready + * \param[in] timeout the duration to wait for the wait set to be ready, in nanoseconds * \return RCL_RET_OK something in the wait set became ready, or - * RCL_RET_TIMEOUT timeout expired before something was ready, or - * RCL_RET_NOT_INIT wait set is zero initialized, or - * RCL_RET_WAIT_SET_EMPTY wait set contains no items, or * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or + * RCL_RET_WAIT_SET_EMPTY if the wait set contains no items, or + * RCL_RET_TIMEOUT if the timeout expired before something was ready, or * RCL_RET_ERROR an unspecified error occur. */ rcl_ret_t -rcl_wait(rcl_wait_set_t * wait_set, const rcl_time_t * timeout); +rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout); #if __cplusplus } diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c index a57db09..595c58f 100644 --- a/rcl/src/rcl/guard_condition.c +++ b/rcl/src/rcl/guard_condition.c @@ -137,7 +137,7 @@ rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition) } rmw_guard_condition_t * -rcl_guard_condition_get_rmw_guard_condition_handle(rcl_guard_condition_t * guard_condition) +rcl_guard_condition_get_rmw_guard_condition_handle(const rcl_guard_condition_t * guard_condition) { RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, NULL); RCL_CHECK_FOR_NULL_WITH_MSG( diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 880bf31..6e8b3a0 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -157,7 +157,7 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription) } const rcl_subscription_options_t * -rcl_subscription_get_options(rcl_subscription_t * subscription) +rcl_subscription_get_options(const rcl_subscription_t * subscription) { RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL); RCL_CHECK_FOR_NULL_WITH_MSG( @@ -166,7 +166,7 @@ rcl_subscription_get_options(rcl_subscription_t * subscription) } rmw_subscription_t * -rcl_subscription_get_rmw_subscription_handle(rcl_subscription_t * subscription) +rcl_subscription_get_rmw_subscription_handle(const rcl_subscription_t * subscription) { RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL); RCL_CHECK_FOR_NULL_WITH_MSG( diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c new file mode 100644 index 0000000..429d7c8 --- /dev/null +++ b/rcl/src/rcl/time.c @@ -0,0 +1,42 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/time.h" + +rcl_time_t +rcl_time_t_from_int64_t_nanoseconds(int64_t nanoseconds) +{ + rcl_time_t result; + result.sec = RCL_NS_TO_S(nanoseconds); + result.nsec = nanoseconds % 1000000000; + return result; +} + +rcl_time_t +rcl_time_t_from_uint64_t_nanoseconds(uint64_t nanoseconds) +{ + rcl_time_t result; + result.sec = RCL_NS_TO_S(nanoseconds); + result.nsec = nanoseconds % 1000000000; + return result; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 8ecc9bc..cc9c10f 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -19,15 +19,21 @@ extern "C" #include "rcl/wait.h" +#include +#include #include #include #include "rcl/error_handling.h" +#include "rcl/time.h" #include "rmw/rmw.h" #include "./common.h" typedef struct rcl_wait_set_impl_t { - void * place_holder; // To prevent size differences between C and C++. + size_t subscription_index; + rmw_subscriptions_t rmw_subscriptions; + size_t guard_condition_index; + rmw_guard_conditions_t rmw_guard_conditions; } rcl_wait_set_impl_t; rcl_wait_set_t @@ -43,6 +49,21 @@ __wait_set_is_valid(rcl_wait_set_t * wait_set) return wait_set && wait_set->impl; } +static void +__wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator) +{ + if (wait_set->subscriptions) { + rcl_wait_set_resize_subscriptions(wait_set, 0); + } + if (wait_set->guard_conditions) { + rcl_wait_set_resize_guard_conditions(wait_set, 0); + } + if (wait_set->impl) { + allocator.deallocate(wait_set->impl, allocator.state); + wait_set->impl = NULL; + } +} + rcl_ret_t rcl_wait_set_init( rcl_wait_set_t * wait_set, @@ -68,26 +89,42 @@ rcl_wait_set_init( allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG( allocator.reallocate, "reallocate not set", return RCL_RET_INVALID_ARGUMENT); - // Right now, the implementation struct is unused, but checked for NULL, so set it. - wait_set->impl = (rcl_wait_set_impl_t *)0xDEADBEEF; + // Allocate space for the implementation struct. + wait_set->impl = (rcl_wait_set_impl_t *)allocator.allocate( + sizeof(rcl_wait_set_impl_t), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + wait_set->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + wait_set->impl->rmw_subscriptions.subscribers = NULL; + wait_set->impl->rmw_subscriptions.subscriber_count = 0; + wait_set->impl->rmw_guard_conditions.guard_conditions = NULL; + wait_set->impl->rmw_guard_conditions.guard_condition_count = 0; // Initialize subscription space. - rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions); - rcl_wait_set_clear_subscriptions(wait_set); + rcl_ret_t ret; + if ((ret = rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions)) != RCL_RET_OK) { + fail_ret = ret; + goto fail; + } + if ((ret = rcl_wait_set_clear_subscriptions(wait_set)) != RCL_RET_OK) { + fail_ret = ret; + goto fail; + } // Initialize guard condition space. - rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions); - rcl_wait_set_clear_guard_conditions(wait_set); + ret = rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions); + if (ret != RCL_RET_OK) { + fail_ret = ret; + goto fail; + } + if ((ret = rcl_wait_set_clear_guard_conditions(wait_set)) != RCL_RET_OK) { + fail_ret = ret; + goto fail; + } // Set allocator. wait_set->allocator = allocator; // Initialize pruned. wait_set->pruned = false; return RCL_RET_OK; fail: - if (wait_set->subscriptions) { - allocator.deallocate(wait_set->subscriptions, allocator.state); - } - if (wait_set->guard_conditions) { - allocator.deallocate(wait_set->guard_conditions, allocator.state); - } + __wait_set_clean_up(wait_set, allocator); return fail_ret; } @@ -97,61 +134,83 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set) rcl_ret_t result = RCL_RET_OK; RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); if (__wait_set_is_valid(wait_set)) { - if (wait_set->subscriptions) { - wait_set->allocator.deallocate(wait_set->subscriptions, wait_set->allocator.state); - } - if (wait_set->guard_conditions) { - wait_set->allocator.deallocate(wait_set->guard_conditions, wait_set->allocator.state); - } - if (wait_set->impl) { - wait_set->impl = NULL; - } + __wait_set_clean_up(wait_set, wait_set->allocator); } return result; } -#define SET_ADD(Type) \ +#define SET_ADD(Type, RMWStorage) \ RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \ RCL_CHECK_ARGUMENT_FOR_NULL(Type, RCL_RET_INVALID_ARGUMENT); \ if (!__wait_set_is_valid(wait_set)) { \ RCL_SET_ERROR_MSG("wait set is invalid"); \ return RCL_RET_WAIT_SET_INVALID; \ } \ - if (!(wait_set->__current_##Type##_offset < wait_set->size_of_##Type##s)) { \ + if (!(wait_set->impl->Type##_index < wait_set->size_of_##Type##s)) { \ RCL_SET_ERROR_MSG(#Type "s set is full"); \ return RCL_RET_WAIT_SET_FULL; \ } \ - wait_set->Type##s[wait_set->__current_##Type##_offset++] = Type; \ + size_t current_index = wait_set->impl->Type##_index++; \ + wait_set->Type##s[current_index] = Type; \ + /* Also place into rmw storage. */ \ + rmw_##Type##_t * rmw_handle = rcl_##Type##_get_rmw_##Type##_handle(Type); \ + RCL_CHECK_FOR_NULL_WITH_MSG( \ + rmw_handle, rcl_get_error_string_safe(), return RCL_RET_ERROR); \ + wait_set->impl->RMWStorage[current_index] = rmw_handle->data; \ return RCL_RET_OK; -#define SET_CLEAR(Type) \ +#define SET_CLEAR(Type, RMWStorage, RMWCount) \ RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \ if (!__wait_set_is_valid(wait_set)) { \ RCL_SET_ERROR_MSG("wait set is invalid"); \ return RCL_RET_WAIT_SET_INVALID; \ } \ memset(wait_set->Type##s, 0, sizeof(rcl_##Type##_t *) * wait_set->size_of_##Type##s); \ - wait_set->__current_##Type##_offset = 0; \ + wait_set->impl->Type##_index = 0; \ + /* Also clear the rmw storage. */ \ + memset( \ + wait_set->impl->RMWStorage, \ + 0, \ + sizeof(rmw_subscription_t *) * wait_set->impl->RMWCount); \ return RCL_RET_OK; -#define SET_RESIZE(Type) \ +#define SET_RESIZE(Type, RMWStorage, RMWCount) \ RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \ + RCL_CHECK_FOR_NULL_WITH_MSG( \ + wait_set->impl, "wait set is invalid", return RCL_RET_WAIT_SET_INVALID); \ if (size == wait_set->size_of_##Type##s) { \ return RCL_RET_OK; \ } \ if (size == 0) { \ if (wait_set->Type##s) { \ wait_set->allocator.deallocate(wait_set->Type##s, wait_set->allocator.state); \ + wait_set->Type##s = NULL; \ + } \ + /* Also deallocate the rmw storage. */ \ + if (wait_set->impl->RMWStorage) { \ + wait_set->allocator.deallocate(wait_set->impl->RMWStorage, wait_set->allocator.state); \ + wait_set->impl->RMWStorage = NULL; \ } \ - wait_set->Type##s = NULL; \ - return RCL_RET_OK; \ } \ - wait_set->size_of_##Type##s = 0; \ - wait_set->Type##s = (const rcl_##Type##_t **)wait_set->allocator.reallocate( \ - wait_set->Type##s, sizeof(rcl_##Type##_t *) * size, wait_set->allocator.state); \ - RCL_CHECK_FOR_NULL_WITH_MSG( \ - wait_set->Type##s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \ - wait_set->size_of_##Type##s = size; \ + else { \ + wait_set->size_of_##Type##s = 0; \ + wait_set->Type##s = (const rcl_##Type##_t **)wait_set->allocator.reallocate( \ + wait_set->Type##s, sizeof(rcl_##Type##_t *) * size, wait_set->allocator.state); \ + RCL_CHECK_FOR_NULL_WITH_MSG( \ + wait_set->Type##s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \ + wait_set->size_of_##Type##s = size; \ + /* Also resize the rmw storage. */ \ + wait_set->impl->RMWCount = 0; \ + wait_set->impl->RMWStorage = (void **)wait_set->allocator.reallocate( \ + wait_set->impl->RMWStorage, sizeof(rcl_##Type##_t *) * size, wait_set->allocator.state); \ + if (!wait_set->impl->RMWStorage) { \ + wait_set->allocator.deallocate(wait_set->Type##s, wait_set->allocator.state); \ + wait_set->size_of_##Type##s = 0; \ + RCL_SET_ERROR_MSG("allocating memory failed"); \ + return RCL_RET_BAD_ALLOC; \ + } \ + wait_set->impl->RMWCount = size; \ + } \ return RCL_RET_OK; rcl_ret_t @@ -159,19 +218,25 @@ rcl_wait_set_add_subscription( rcl_wait_set_t * wait_set, const rcl_subscription_t * subscription) { - SET_ADD(subscription) + SET_ADD(subscription, rmw_subscriptions.subscribers) } rcl_ret_t rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set) { - SET_CLEAR(subscription) + SET_CLEAR( + subscription, + rmw_subscriptions.subscribers, + rmw_subscriptions.subscriber_count) } rcl_ret_t rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size) { - SET_RESIZE(subscription) + SET_RESIZE( + subscription, + rmw_subscriptions.subscribers, + rmw_subscriptions.subscriber_count) } rcl_ret_t @@ -179,24 +244,78 @@ rcl_wait_set_add_guard_condition( rcl_wait_set_t * wait_set, const rcl_guard_condition_t * guard_condition) { - SET_ADD(guard_condition) + SET_ADD(guard_condition, rmw_guard_conditions.guard_conditions) } rcl_ret_t rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set) { - SET_CLEAR(guard_condition) + SET_CLEAR( + guard_condition, + rmw_guard_conditions.guard_conditions, + rmw_guard_conditions.guard_condition_count) } rcl_ret_t rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size) { - SET_RESIZE(guard_condition) + SET_RESIZE( + guard_condition, + rmw_guard_conditions.guard_conditions, + rmw_guard_conditions.guard_condition_count) } rcl_ret_t -rcl_wait(rcl_wait_set_t * wait_set, const rcl_time_t * timeout) +rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) { + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timeout, RCL_RET_INVALID_ARGUMENT); + if (!__wait_set_is_valid(wait_set)) { + RCL_SET_ERROR_MSG("wait set is invalid"); + return RCL_RET_WAIT_SET_INVALID; + } + if (wait_set->size_of_subscriptions == 0 && wait_set->size_of_guard_conditions == 0) { + RCL_SET_ERROR_MSG("wait set is empty"); + return RCL_RET_WAIT_SET_EMPTY; + } + // Create dummy sets for currently unsupported wait-ables. + static rmw_services_t dummy_services = {0, NULL}; + static rmw_clients_t dummy_clients = {0, NULL}; + rmw_time_t rmw_timeout = rcl_time_t_from_int64_t_nanoseconds(timeout); + // Wait. + rmw_ret_t ret = rmw_wait( + &wait_set->impl->rmw_subscriptions, + &wait_set->impl->rmw_guard_conditions, + &dummy_services, + &dummy_clients, + &rmw_timeout + ); + // Check for timeout. + if (ret == RMW_RET_TIMEOUT) { + // Assume none were set (because timeout was reached first), and clear all. + rcl_wait_set_clear_subscriptions(wait_set); + rcl_wait_set_clear_guard_conditions(wait_set); + return RCL_RET_TIMEOUT; + } + // Check for error. + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + // Set corresponding rcl subscription handles NULL. + for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) { + assert(i < wait_set->impl->rmw_subscriptions.subscriber_count); // Defensive. + if (!wait_set->impl->rmw_subscriptions.subscribers[i]) { + wait_set->subscriptions[i] = NULL; + } + } + // Set corresponding rcl guard_condition handles NULL. + for (size_t i = 0; i < wait_set->size_of_guard_conditions; ++i) { + assert(i < wait_set->impl->rmw_guard_conditions.guard_condition_count); // Defensive. + if (!wait_set->impl->rmw_guard_conditions.guard_conditions[i]) { + wait_set->guard_conditions[i] = NULL; + } + } return RCL_RET_OK; } From 821bbf7ce61bf03963b0a8f79166070f28a09bfc Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 4 Dec 2015 09:37:43 -0800 Subject: [PATCH 13/71] API for timers in timers.h --- rcl/include/rcl/timer.h | 308 ++++++++++++++++++++++++++++++++++++++++ rcl/include/rcl/types.h | 11 +- 2 files changed, 315 insertions(+), 4 deletions(-) create mode 100644 rcl/include/rcl/timer.h diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h new file mode 100644 index 0000000..1001851 --- /dev/null +++ b/rcl/include/rcl/timer.h @@ -0,0 +1,308 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__TIMER_H_ +#define RCL__TIMER_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#include + +#include "rcl/allocator.h" +#include "rcl/time.h" +#include "rcl/types.h" +#include "rmw/rmw.h" + +struct rcl_timer_impl_t; + +/// Handle for a ROS timer. +typedef struct rcl_timer_t { + /// Private implementation pointer. + rcl_timer_impl_t * impl; +} rcl_timer_t; + +/// User callback signature for timers. +/* The first argument the callback gets is a pointer to the timer. + * This can be used to cancel the timer, query about the time until the next + * timer callback, exchange the callback with a different one, etc. + * + * The only caveat is that the function rcl_timer_get_last_call_time will + * return the time just before the callback was called. + * Therefore the second argument given is the time when the previous callback + * was called, since that information is no longer accessible via the timer. + */ +typedef void (* rcl_timer_callback_t)(rcl_timer_t *, rcl_time_t); + +/// Return a zero initialized timer. +rcl_timer_t +rcl_get_zero_initialized_timer(); + +/// Initialize a timer. +/* A timer consists of a callback function and a period. + * A timer can be added to a wait set and waited on, such that the wait set + * will wake up when a timer is ready to be executed. + * + * A timer simply holds state and does not actively call periodically. + * It does not create any threads or register interrupts or signals. + * For blocking behavior it can be used in conjunction with a wait set and + * rcl_wait(). + * When rcl_timer_is_ready() returns true, the timer must still be called + * explicitly using rcl_timer_call(). + * + * The timer handle must be a pointer to an allocated and zero initialized + * timer struct. + * Calling this function on an already initialized timer will fail. + * Calling this function on a timer struct which has been allocated but not + * zero initialized is undefined behavior. + * + * The period is a timer duration (rather an absolute time in the future). + * If the period is 0 seconds and 0 nanoseconds then it will always be ready. + * + * The callback must be a function which returns void and takes two arguments, + * the first being a pointer to the associated timer, and the second a time + * struct which is the time of the previous call or when the timer was created + * if it is the first call to the callback. + * + * This function is not thread-safe. + * + * \param[inout] timer the timer handle to be initialized + * \param[in] period the duration between calls to the callback in nanoseconds + * \param[in] callback the user defined function to be called every period + * \param[in] allocator the allocator to use for allocations + * \return RCL_RET_OK if the timer was initialized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_init( + rcl_timer_t * timer, + uint64_t period, + const rcl_timer_callback_t callback, + rcl_allocator_t allocator); + +/// Finalize a timer. +/* This function will deallocate any memory and make the timer invalid. + * + * A timer that is already invalid (zero initialized) or NULL will not fail. + * + * This function is not thread-safe with any rcl_timer_* functions used on the + * same timer object. + * + * \param[inout] timer the handle to the timer to be finalized. + * \return RCL_RET_OK if the timer was finalized successfully, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_fini(rcl_timer_t * timer); + +/// Call the timer's callback and set the last call time to the current time. +/* This function will call the callback and change the last call time even if + * the timer's period has not yet elapsed. + * It is up to the calling code to make sure the period has elapsed by first + * calling rcl_timer_is_ready(). + * The order of operations in this command are as follows: + * + * - Ensure the timer has not been canceled. + * - Get the current time into a temporary rcl_time_t. + * - Exchange the current time with the last call time of the timer. + * - Call the callback, with this timer and the exchanged last call time. + * + * During the callback the timer can be canceled or have its period and/or + * callback modified. + * + * This function is thread-safe, but the user's callback may not be. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_int_least64_t, but the user's + * callback may not be lock-free. + * + * \param[inout] timer the handle to the timer to call + * \return RCL_RET_OK if the timer was called successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_TIMER_INVALID if the timer is invalid, or + * RCL_RET_TIMER_CANCELED if the timer has been canceled, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_call(rcl_timer_t * timer); + +/// Calculates whether or not the timer should be called. +/* The result is true if the time until next call is less than, or equal to, 0 + * and the timer has not been canceled. + * Otherwise the result is false, indicating the period has not elapsed. + * + * The is_ready argument must point to an allocated bool object, as the result + * is copied into it. + * + * This function is thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_int_least64_t. + * + * \param[in] timer the handle to the timer which is being checked + * \param[out] is_ready the bool used to store the result of the calculation + * \return RCL_RET_OK if the last call time was retrieved successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_TIMER_INVALID if the timer is invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready); + +/// Calculate and retrieve the time until the next call in nanoseconds. +/* This function calculates the time until the next call by adding the timer's + * period to the last call time and subtracting that sum from the current time. + * The calculated time until the next call can be positive, indicating that it + * is not ready to be called as the period has not elapsed since the last call. + * The calculated time until the next call can also be 0 or negative, + * indicating that the period has elapsed since the last call and the timer + * should be called. + * A negative value indicates the timer call is overdue by that amount. + * + * The time_until_next_call argument must point to an allocated int64_t, as the + * the time until is coped into that instance. + * + * This function is thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_int_least64_t. + * + * \param[in] timer the handle to the timer that is being queried + * \param[out] time_until_next_call the output variable for the result + * \return RCL_RET_OK if the timer until next call was successfully calculated, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_TIMER_INVALID if the timer is invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call); + +/// Retrieve the time of when the previous call to rcl_timer_call() occurred. +/* This function retrieves the internal time and copies it into the given + * rcl_time_t struct. + * + * Calling this function within a callback will not return the previous time + * but instead the time of when the current callback was called. + * + * The last_call_time argument must be a pointer to an already allocated + * rcl_time_t struct. + * + * This function is thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_int_least64_t. + * + * \param[in] timer the handle to the timer which is being queried + * \param[out] last_call_time the rcl_time_t struct in which the time is stored + * \return RCL_RET_OK if the last call time was retrieved successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_TIMER_INVALID if the timer is invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_get_last_call_time(const rcl_timer_t * timer, rcl_time_t * last_call_time); + +/// Return the current timer callback. +/* This function can fail, and therefore return NULL, if: + * - timer is NULL + * - timer has not been initialized (the implementation is invalid) + * + * This function is thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_uintptr_t. + * + * \param[in] timer handle to the timer from the callback should be returned + * \return function pointer to the callback, or NULL if an error occurred + */ +rcl_timer_callback_t +rcl_timer_get_callback(const rcl_timer_t * timer); + +/// Exchange the current timer callback and return the current callback. +/* This function can fail, and therefore return NULL, if: + * - timer is NULL + * - timer has not been initialized (the implementation is invalid) + * - the new_callback argument is NULL + * + * This function is thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_uintptr_t. + * + * \param[inout] timer handle to the timer from the callback should be returned + * \param[in] timer handle to the timer from the callback should be returned + * \return function pointer to the old callback, or NULL if an error occurred + */ +rcl_timer_callback_t +rcl_timer_exchange_callback(rcl_time_t * timer, const rcl_timer_callback_t new_callback); + +/// Cancel a timer. +/* When a timer is canceled, rcl_timer_is_ready() will return false for that + * timer, and rcl_timer_call() will fail with RCL_RET_TIMER_CANCELED. + * + * A canceled timer can be reset with rcl_timer_reset(), and then used again. + * Calling this function on an already canceled timer will succeed. + * + * This function is thread-safe. + * This function is lock-free. + * + * \param[inout] timer the timer to be canceled + * \return RCL_RET_OK if the last call time was retrieved successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_TIMER_INVALID if the timer is invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_cancel(rcl_timer_t * timer); + +/// Retrieve the canceled state of a timer. +/* If the timer is canceled true will be stored in the is_canceled argument. + * Otherwise false will be stored in the is_canceled argument. + * + * The is_canceled argument must point to an allocated bool, as the result is + * copied into this variable. + * + * This function is thread-safe. + * This function is lock-free. + * + * \param[in] timer the timer to be queried + * \return RCL_RET_OK if the last call time was retrieved successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_TIMER_INVALID if the timer is invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled); + +/// Reset a timer. +/* This function can be called on a timer, canceled or not. + * For all timers it will reset the last call time to now. + * For canceled timers it will additionally make the timer not canceled. + * + * This function is thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_int_least64_t. + * + * \param[inout] timer the timer to be reset + * \return RCL_RET_OK if the last call time was retrieved successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_TIMER_INVALID if the timer is invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_reset(rcl_timer_t * timer); + +#if __cplusplus +} +#endif + +#endif // RCL__TIMER_H_ diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h index 9aecce3..ba2d8a3 100644 --- a/rcl/include/rcl/types.h +++ b/rcl/include/rcl/types.h @@ -35,9 +35,12 @@ typedef rmw_ret_t rcl_ret_t; // rcl service client specific ret codes in 5XX // rcl service server specific ret codes in 6XX // rcl guard condition specific ret codes in 7XX -// rcl wait and wait set specific ret codes in 8XX -#define RCL_RET_WAIT_SET_INVALID 800 -#define RCL_RET_WAIT_SET_EMPTY 801 -#define RCL_RET_WAIT_SET_FULL 802 +// rcl timer specific ret codes in 8XX +#define RCL_RET_TIMER_INVALID 800 +#define RCL_RET_TIMER_CANCELED 801 +// rcl wait and wait set specific ret codes in 9XX +#define RCL_RET_WAIT_SET_INVALID 900 +#define RCL_RET_WAIT_SET_EMPTY 901 +#define RCL_RET_WAIT_SET_FULL 902 #endif // RCL__TYPES_H_ From 77406dea075ec3f4d28f5e57f0ba9d8cc6c15ac3 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 4 Dec 2015 12:19:15 -0800 Subject: [PATCH 14/71] fixup docs --- rcl/include/rcl/publisher.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index f229e7b..6f20616 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -84,8 +84,8 @@ rcl_get_zero_initialized_publisher(); * format. * * The options struct allows the user to set the quality of service settings as - * well as a custom allocator which is used when (de)initializing the publisher - * to allocate space for incidental things, e.g. the topic name string. + * well as a custom allocator which is used when initializing/finalizing the + * publisher to allocate space for incidentals, e.g. the topic name string. * * Expected usage (for C messages): * @@ -101,7 +101,7 @@ rcl_get_zero_initialized_publisher(); * rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); * rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options(); * ret = rcl_publisher_init(&publisher, &node, ts, "chatter", &publisher_ops); - * // ... error handling, and on shutdown do deinitialization: + * // ... error handling, and on shutdown do finalization: * ret = rcl_publisher_fini(&publisher, &node); * // ... error handling for rcl_publisher_fini() * ret = rcl_node_fini(&node); @@ -117,8 +117,8 @@ rcl_get_zero_initialized_publisher(); * \return RCL_RET_OK if the publisher was initialized successfully, or * RCL_RET_NODE_INVALID if the node is invalid, or * RCL_RET_ALREADY_INIT if the publisher is already initialized, or - * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or - * RCL_RET_BAD_ALLOC if any arugments are invalid, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_BAD_ALLOC if allocating memory fails, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t @@ -129,7 +129,7 @@ rcl_publisher_init( const char * topic_name, const rcl_publisher_options_t * options); -/// Deinitialize a rcl_publisher_t. +/// Finalize a rcl_publisher_t. /* After calling, the node will no longer be advertising that it is publishing * on this topic (assuming this is the only publisher on this topic). * @@ -138,10 +138,10 @@ rcl_publisher_init( * * This function is not thread-safe. * - * \param[inout] publisher handle to the publisher to be deinitialized + * \param[inout] publisher handle to the publisher to be finalized * \param[in] node handle to the node used to create the publisher * \return RCL_RET_OK if publisher was finalized successfully, or - * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t @@ -191,7 +191,7 @@ rcl_publisher_get_default_options(); * \param[in] publisher handle to the publisher which will do the publishing * \param[in] ros_message type-erased pointer to the ROS message * \return RCL_RET_OK if the message was published successfully, or - * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ From 1097b545b809181b14ceb567167777caa9f0c3a9 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 4 Dec 2015 12:21:03 -0800 Subject: [PATCH 15/71] implemented/updated some time utilities --- rcl/include/rcl/time.h | 42 +++++++++++- rcl/src/rcl/time.c | 147 ++++++++++++++++++++++++++++++++++++++++- rcl/src/rcl/wait.c | 2 +- 3 files changed, 186 insertions(+), 5 deletions(-) diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index e25c923..36d0d71 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -20,6 +20,7 @@ extern "C" { #endif +#include "rcl/types.h" #include "rmw/rmw.h" #define RCL_S_TO_NS(seconds) seconds * 1000000000 @@ -32,11 +33,48 @@ extern "C" typedef rmw_time_t rcl_time_t; +/// Create a rcl_time_t struct with a given signed number of nanoseconds. rcl_time_t -rcl_time_t_from_int64_t_nanoseconds(int64_t nanoseconds); +rcl_time_from_int64_t_nanoseconds(int64_t nanoseconds); +/// Create a rcl_time_t struct with a given unsigned number of nanoseconds. rcl_time_t -rcl_time_t_from_uint64_t_nanoseconds(uint64_t nanoseconds); +rcl_time_from_uint64_t_nanoseconds(uint64_t nanoseconds); + +/// Retrieve the current time as a rcl_time_t struct. +/* The now argument must point to an allocated rcl_time_t struct, as the + * result is copied into this variable. + * + * This function is thread-safe. + * This function is lock-free, with an exception on Windows. + * On Windows this is lock-free if the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_int_least64_t. + * + * \param[out] now a rcl_time_t struct in which the current time is stored + * \return RCL_RET_OK if the current time was successfully obtained, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_time_now(rcl_time_t * now); + +/// Normalize a time struct. +/* If there are more than 10^9 nanoseconds in the nsec field, the extra seconds + * are removed from the nsec field and added to the sec field. + * Overflow of the sec field due to this normalization is ignored. + * + * This function is thread-safe. + * This function is lock-free, with an exception on Windows. + * On Windows this is lock-free if the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_int_least64_t. + * + * \param[out] now a rcl_time_t struct to be normalized + * \return RCL_RET_OK if the struct was normalized successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_time_normalize(rcl_time_t * now); #if __cplusplus } diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index 429d7c8..eab5fdf 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -17,10 +17,30 @@ extern "C" { #endif +// Appropriate check accorind to: +// http://man7.org/linux/man-pages/man2/clock_gettime.2.html +#define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L) + #include "rcl/time.h" +#include +#include +#include + +#if defined(__MACH__) +#include +#include +#endif +#if defined(WIN32) +#include +#include +#endif + +#include "./common.h" +#include "rcl/error_handling.h" + rcl_time_t -rcl_time_t_from_int64_t_nanoseconds(int64_t nanoseconds) +rcl_time_from_int64_t_nanoseconds(int64_t nanoseconds) { rcl_time_t result; result.sec = RCL_NS_TO_S(nanoseconds); @@ -29,7 +49,7 @@ rcl_time_t_from_int64_t_nanoseconds(int64_t nanoseconds) } rcl_time_t -rcl_time_t_from_uint64_t_nanoseconds(uint64_t nanoseconds) +rcl_time_from_uint64_t_nanoseconds(uint64_t nanoseconds) { rcl_time_t result; result.sec = RCL_NS_TO_S(nanoseconds); @@ -37,6 +57,129 @@ rcl_time_t_from_uint64_t_nanoseconds(uint64_t nanoseconds) return result; } +static void +__timespec_get_now(struct timespec * timespec_now) +{ +#if defined(__MACH__) + // On OS X use clock_get_time. + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + timespec_now->tv_sec = mts.tv_sec; + timespec_now->tv_nsec = mts.tv_nsec; +#else // if defined(__MACH__) + // Otherwise use clock_gettime. + clock_gettime(CLOCK_REALTIME, timespec_now); +#endif // if defined(__MACH__) +} + +rcl_ret_t +rcl_time_now(rcl_time_t * now) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); +#ifndef WIN32 + // Unix implementations +#if HAS_CLOCK_GETTIME || defined(__MACH__) + // If clock_gettime is available or on OS X, use a timespec. + struct timespec timespec_now; + __timespec_get_now(×pec_now); + now->sec = timespec_now.tv_sec; + now->nsec = timespec_now.tv_nsec; +#else // if HAS_CLOCK_GETTIME || defined(__MACH__) + // Otherwise we have to fallback to gettimeofday. + struct timeval timeofday; + gettimeofday(&timeofday, NULL); + now->sec = timeofday.tv_sec; + now->nsec = timeofday.tv_usec * 1000; +#endif // if HAS_CLOCK_GETTIME || defined(__MACH__) +#else + // Windows implementation adapted from roscpp_core (3-clause BSD), see: + // https://github.com/ros/roscpp_core/blob/0.5.6/rostime/src/time.cpp#L96 + // Win32 implementation + // unless I've missed something obvious, the only way to get high-precision + // time on Windows is via the QueryPerformanceCounter() call. However, + // this is somewhat problematic in Windows XP on some processors, especially + // AMD, because the Windows implementation can freak out when the CPU clocks + // down to save power. Time can jump or even go backwards. Microsoft has + // fixed this bug for most systems now, but it can still show up if you have + // not installed the latest CPU drivers (an oxymoron). They fixed all these + // problems in Windows Vista, and this API is by far the most accurate that + // I know of in Windows, so I'll use it here despite all these caveats + LARGE_INTEGER cpu_freq; + static LARGE_INTEGER cpu_freq_global; + LARGE_INTEGER init_cpu_time; + static LARGE_INTEGER init_cpu_time_global; + static atomic_uint_least64_t start_ns = ATOMIC_VAR_INIT(0); + rcl_time_t start = {0, 0}; + // If start_ns (static/global) is 0, then set it up on the first call. + uint64_t start_ns_loaded = atomic_load(&start_ns); + if (start_ns_loaded == 0) { + QueryPerformanceFrequency(&cpu_freq); + if (cpu_freq.QuadPart == 0) { + RCL_SET_ERROR_MSG("no high performance timer found"); + return RCL_RET_ERROR; + } + QueryPerformanceCounter(&init_cpu_time); + // compute an offset from the Epoch using the lower-performance timer API + FILETIME ft; + GetSystemTimeAsFileTime(&ft); + LARGE_INTEGER start_li; + start_li.LowPart = ft.dwLowDateTime; + start_li.HighPart = ft.dwHighDateTime; + // why did they choose 1601 as the time zero, instead of 1970? + // there were no outstanding hard rock bands in 1601. +#ifdef _MSC_VER + start_li.QuadPart -= 116444736000000000Ui64; +#else + start_li.QuadPart -= 116444736000000000ULL; +#endif + start.sec = (uint64_t)(start_li.QuadPart / 10000000); // 100-ns units. odd. + start.nsec = (start_li.LowPart % 10000000) * 100; + if (atomic_compare_exchange_strong(&start_ns, 0, (start.sec * 1000000000) + start.nsec)) { + // If it matched 0 this call was first to setup, set the cpu_freq and init_cpu_time globals. + init_cpu_time_global = init_cpu_time; + cpu_freq_global = cpu_freq; + } else { + // Another concurrent first call managed to set this up first; reset start so it gets set. + start = {0, 0}; + } + } + if (start.sec == 0 && start.nsec == 0) { + start.sec = RCL_NS_TO_S(start_ns_loaded); + start.nsec = start_ns_loaded % 1000000000; + } + LARGE_INTEGER cur_time; + QueryPerformanceCounter(&cur_time); + LARGE_INTEGER delta_cpu_time; + delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time_global.QuadPart; + double d_delta_cpu_time = delta_cpu_time.QuadPart / (double) cpu_freq_global.QuadPart; + uint64_t delta_sec = (uint64_t) floor(d_delta_cpu_time); + uint64_t delta_nsec = (uint64_t) floor((d_delta_cpu_time - delta_sec) * 1e9); + + *now = start; + now->sec += delta_sec; + now->nsec += delta_nsec; + + // Normalize the time struct. + rcl_ret_t ret = rcl_time_normalize(now); + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } +#endif + return RCL_RET_OK; +} + +rcl_ret_t +rcl_time_normalize(rcl_time_t * now) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); + now->sec += now->nsec / 1000000000; + now->nsec = now->nsec % 1000000000; + return RCL_RET_OK; +} + #if __cplusplus } #endif diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index cc9c10f..c892d73 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -281,7 +281,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) // Create dummy sets for currently unsupported wait-ables. static rmw_services_t dummy_services = {0, NULL}; static rmw_clients_t dummy_clients = {0, NULL}; - rmw_time_t rmw_timeout = rcl_time_t_from_int64_t_nanoseconds(timeout); + rmw_time_t rmw_timeout = rcl_time_from_int64_t_nanoseconds(timeout); // Wait. rmw_ret_t ret = rmw_wait( &wait_set->impl->rmw_subscriptions, From 22761b65964f1d4558e76eed42e8ec2236271d34 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 4 Dec 2015 15:03:18 -0800 Subject: [PATCH 16/71] implement timer.c --- rcl/CMakeLists.txt | 1 + rcl/include/rcl/time.h | 17 ++- rcl/include/rcl/timer.h | 78 +++++++++++-- rcl/src/rcl/time.c | 9 ++ rcl/src/rcl/timer.c | 236 ++++++++++++++++++++++++++++++++++++++++ 5 files changed, 328 insertions(+), 13 deletions(-) create mode 100644 rcl/src/rcl/timer.c diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 0f16659..46f65b2 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -26,6 +26,7 @@ set(${PROJECT_NAME}_sources src/rcl/subscription.c src/rcl/wait.c src/rcl/time.c + src/rcl/timer.c ) add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_sources}) ament_target_dependencies(${PROJECT_NAME} diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index 36d0d71..bffb8be 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -23,13 +23,13 @@ extern "C" #include "rcl/types.h" #include "rmw/rmw.h" -#define RCL_S_TO_NS(seconds) seconds * 1000000000 -#define RCL_MS_TO_NS(milliseconds) milliseconds * 1000000 -#define RCL_US_TO_NS(microseconds) microseconds * 1000 +#define RCL_S_TO_NS(seconds) (seconds * 1000000000) +#define RCL_MS_TO_NS(milliseconds) (milliseconds * 1000000) +#define RCL_US_TO_NS(microseconds) (microseconds * 1000) -#define RCL_NS_TO_S(nanoseconds) nanoseconds / 1000000000 -#define RCL_NS_TO_MS(nanoseconds) nanoseconds / 1000000 -#define RCL_NS_TO_US(nanoseconds) nanoseconds / 1000 +#define RCL_NS_TO_S(nanoseconds) (nanoseconds / 1000000000) +#define RCL_NS_TO_MS(nanoseconds) (nanoseconds / 1000000) +#define RCL_NS_TO_US(nanoseconds) (nanoseconds / 1000) typedef rmw_time_t rcl_time_t; @@ -41,6 +41,11 @@ rcl_time_from_int64_t_nanoseconds(int64_t nanoseconds); rcl_time_t rcl_time_from_uint64_t_nanoseconds(uint64_t nanoseconds); +/// Convert a rcl_time_t struct into an unsigned number of nanoseconds. +/* This function will return 0 if the time argument is NULL. */ +uint64_t +rcl_time_to_uint64_t_nanoseconds(const rcl_time_t * time); + /// Retrieve the current time as a rcl_time_t struct. /* The now argument must point to an allocated rcl_time_t struct, as the * result is copied into this variable. diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index 1001851..d273ed5 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -32,7 +32,7 @@ struct rcl_timer_impl_t; /// Handle for a ROS timer. typedef struct rcl_timer_t { /// Private implementation pointer. - rcl_timer_impl_t * impl; + struct rcl_timer_impl_t * impl; } rcl_timer_t; /// User callback signature for timers. @@ -77,7 +77,25 @@ rcl_get_zero_initialized_timer(); * struct which is the time of the previous call or when the timer was created * if it is the first call to the callback. * + * Expected usage: + * + * #include + * + * void my_timer_callback(rcl_timer_t * timer, rcl_time_t last_call_time) + * { + * // Do timer work... + * // Optionally reconfigure, cancel, or reset the timer... + * } + * + * rcl_timer_t timer = rcl_get_zero_initialized_timer(); + * rcl_ret_t ret = + * rcl_timer_init(&timer, RCL_MS_TO_NS(100), my_timer_callback, rcl_get_default_allocator()); + * // ... error handling, use the timer with a wait set, or poll it manually, then cleanup + * ret = rcl_timer_fini(&timer); + * // ... error handling + * * This function is not thread-safe. + * This function is lock-free. * * \param[inout] timer the timer handle to be initialized * \param[in] period the duration between calls to the callback in nanoseconds @@ -85,6 +103,7 @@ rcl_get_zero_initialized_timer(); * \param[in] allocator the allocator to use for allocations * \return RCL_RET_OK if the timer was initialized successfully, or * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ALREADY_INIT if the timer was already initialized, or * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR an unspecified error occur. */ @@ -120,7 +139,8 @@ rcl_timer_fini(rcl_timer_t * timer); * - Ensure the timer has not been canceled. * - Get the current time into a temporary rcl_time_t. * - Exchange the current time with the last call time of the timer. - * - Call the callback, with this timer and the exchanged last call time. + * - Call the callback, with this timer and the last call time as arguments. + * - Return after the callback has completed. * * During the callback the timer can be canceled or have its period and/or * callback modified. @@ -213,6 +233,48 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt rcl_ret_t rcl_timer_get_last_call_time(const rcl_timer_t * timer, rcl_time_t * last_call_time); +/// Retrieve the period of the timer. +/* This function retrieves the period and copies it into the give variable. + * + * The period argument must be a pointer to an already allocated uint64_t. + * + * This function is thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_int_least64_t. + * + * \param[in] timer the handle to the timer which is being queried + * \param[out] period the uint64_t in which the period is stored + * \return RCL_RET_OK if the period was retrieved successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_TIMER_INVALID if the timer is invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period); + +/// Exchange the period of the timer and return the previous period. +/* This function exchanges the period in the timer and copies the old one into + * the give variable. + * + * Exchanging (changing) the period will not affect already waiting wait sets. + * + * The old_period argument must be a pointer to an already allocated uint64_t. + * + * This function is thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_int_least64_t. + * + * \param[in] timer the handle to the timer which is being modified + * \param[out] new_period the uint64_t to exchange into the timer + * \param[out] old_period the uint64_t in which the previous period is stored + * \return RCL_RET_OK if the period was retrieved successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_TIMER_INVALID if the timer is invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period); + /// Return the current timer callback. /* This function can fail, and therefore return NULL, if: * - timer is NULL @@ -238,12 +300,12 @@ rcl_timer_get_callback(const rcl_timer_t * timer); * This function is lock-free so long as the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_uintptr_t. * - * \param[inout] timer handle to the timer from the callback should be returned - * \param[in] timer handle to the timer from the callback should be returned + * \param[inout] timer handle to the timer from the callback should be exchanged + * \param[in] new_callback the callback to be exchanged into the timer * \return function pointer to the old callback, or NULL if an error occurred */ rcl_timer_callback_t -rcl_timer_exchange_callback(rcl_time_t * timer, const rcl_timer_callback_t new_callback); +rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback); /// Cancel a timer. /* When a timer is canceled, rcl_timer_is_ready() will return false for that @@ -253,7 +315,8 @@ rcl_timer_exchange_callback(rcl_time_t * timer, const rcl_timer_callback_t new_c * Calling this function on an already canceled timer will succeed. * * This function is thread-safe. - * This function is lock-free. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_bool. * * \param[inout] timer the timer to be canceled * \return RCL_RET_OK if the last call time was retrieved successfully, or @@ -272,7 +335,8 @@ rcl_timer_cancel(rcl_timer_t * timer); * copied into this variable. * * This function is thread-safe. - * This function is lock-free. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_bool. * * \param[in] timer the timer to be queried * \return RCL_RET_OK if the last call time was retrieved successfully, or diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index eab5fdf..be4c557 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -57,6 +57,15 @@ rcl_time_from_uint64_t_nanoseconds(uint64_t nanoseconds) return result; } +uint64_t +rcl_time_to_uint64_t_nanoseconds(const rcl_time_t * rcl_time) +{ + if (!rcl_time) { + return 0; + } + return RCL_S_TO_NS(rcl_time->sec) + rcl_time->nsec; +} + static void __timespec_get_now(struct timespec * timespec_now) { diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c new file mode 100644 index 0000000..a8df7ad --- /dev/null +++ b/rcl/src/rcl/timer.c @@ -0,0 +1,236 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/timer.h" + +#include + +#include "./common.h" + +typedef struct rcl_timer_impl_t { + // The user supplied callback. + atomic_uintptr_t callback; + // This is a duration in nanoseconds. + atomic_uint_least64_t period; + // This is an absolute time in nanoseconds since the unix epoch. + atomic_uint_least64_t last_call_time; + // A flag which indicates if the timer is canceled. + atomic_bool canceled; + // The user supplied allocator. + rcl_allocator_t allocator; +} rcl_timer_impl_t; + +rcl_timer_t +rcl_get_zero_initialized_timer() +{ + static rcl_timer_t null_timer = {0}; + return null_timer; +} + +rcl_ret_t +rcl_timer_init( + rcl_timer_t * timer, + uint64_t period, + const rcl_timer_callback_t callback, + rcl_allocator_t allocator) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT); + if (timer->impl) { + RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized"); + return RCL_RET_ALREADY_INIT; + } + rcl_time_t now; + rcl_ret_t now_ret = rcl_time_now(&now); + if (now_ret != RCL_RET_OK) { + return now_ret; // rcl error state should already be set. + } + rcl_timer_impl_t impl = { + .callback = ATOMIC_VAR_INIT((uintptr_t)callback), + .period = ATOMIC_VAR_INIT(period), + .last_call_time = ATOMIC_VAR_INIT(rcl_time_to_uint64_t_nanoseconds(&now)), + .canceled = ATOMIC_VAR_INIT(false), + .allocator = allocator, + }; + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator.allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); + timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + *timer->impl = impl; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_fini(rcl_timer_t * timer) +{ + if (!timer || !timer->impl) { + return RCL_RET_OK; + } + // Will return either RCL_RET_OK or RCL_RET_ERROR since the timer is valid. + rcl_ret_t result = rcl_timer_cancel(timer); + rcl_allocator_t allocator = timer->impl->allocator; + allocator.deallocate(timer->impl, allocator.state); + return result; +} + +rcl_ret_t +rcl_timer_call(rcl_timer_t * timer) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + if (atomic_load(&timer->impl->canceled)) { + RCL_SET_ERROR_MSG("timer is canceled"); + return RCL_RET_TIMER_CANCELED; + } + rcl_time_t now; + rcl_ret_t now_ret = rcl_time_now(&now); + if (now_ret != RCL_RET_OK) { + return now_ret; // rcl error state should already be set. + } + uint64_t previous_ns = + atomic_exchange(&timer->impl->last_call_time, rcl_time_to_uint64_t_nanoseconds(&now)); + rcl_time_t previous = rcl_time_from_uint64_t_nanoseconds(previous_ns); + rcl_timer_callback_t typed_callback = (rcl_timer_callback_t)atomic_load(&timer->impl->callback); + typed_callback(timer, previous); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_ready, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + int64_t time_until_next_call; + rcl_ret_t ret = rcl_timer_get_time_until_next_call(timer, &time_until_next_call); + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } + *is_ready = (time_until_next_call <= 0) && !atomic_load(&timer->impl->canceled); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + rcl_time_t now, last_call_time; + rcl_ret_t ret = rcl_time_now(&now); + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } + ret = rcl_timer_get_last_call_time(timer, &last_call_time); + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } + uint64_t period = atomic_load(&timer->impl->period); + int64_t next_call = rcl_time_to_uint64_t_nanoseconds(&last_call_time) + period; + *time_until_next_call = next_call - rcl_time_to_uint64_t_nanoseconds(&now); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_get_last_call_time(const rcl_timer_t * timer, rcl_time_t * last_call_time) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(last_call_time, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + uint64_t last_call_time_ns = atomic_load(&timer->impl->last_call_time); + *last_call_time = rcl_time_from_uint64_t_nanoseconds(last_call_time_ns); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(period, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + *period = atomic_load(&timer->impl->period); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + *old_period = atomic_exchange(&timer->impl->period, new_period); + return RCL_RET_OK; +} + +rcl_timer_callback_t +rcl_timer_get_callback(const rcl_timer_t * timer) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL); + return (rcl_timer_callback_t)atomic_load(&timer->impl->callback); +} + +rcl_timer_callback_t +rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL); + RCL_CHECK_ARGUMENT_FOR_NULL(new_callback, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL); + return (rcl_timer_callback_t)atomic_exchange(&timer->impl->callback, (uintptr_t)new_callback); +} + +rcl_ret_t +rcl_timer_cancel(rcl_timer_t * timer) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + atomic_store(&timer->impl->canceled, true); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_canceled, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + *is_canceled = atomic_load(&timer->impl->canceled); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_reset(rcl_timer_t * timer) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + rcl_time_t now; + rcl_ret_t now_ret = rcl_time_now(&now); + if (now_ret != RCL_RET_OK) { + return now_ret; // rcl error state should already be set. + } + atomic_store(&timer->impl->last_call_time, rcl_time_to_uint64_t_nanoseconds(&now)); + atomic_store(&timer->impl->canceled, false); + return RCL_RET_OK; +} + +#if __cplusplus +} +#endif From 7ab361a5fb62937e10200bf38c786f6c364bdd7f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 4 Dec 2015 17:22:48 -0800 Subject: [PATCH 17/71] update wait set and rcl_wait for timers --- rcl/include/rcl/timer.h | 3 + rcl/include/rcl/wait.h | 54 +++++++++-- rcl/src/rcl/wait.c | 194 ++++++++++++++++++++++++++++++---------- 3 files changed, 200 insertions(+), 51 deletions(-) diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index d273ed5..ba62062 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -122,6 +122,9 @@ rcl_timer_init( * This function is not thread-safe with any rcl_timer_* functions used on the * same timer object. * + * This function is not thread-safe. + * This function is lock-free. + * * \param[inout] timer the handle to the timer to be finalized. * \return RCL_RET_OK if the timer was finalized successfully, or * RCL_RET_ERROR an unspecified error occur. diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index b86d251..eb75555 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -25,6 +25,7 @@ extern "C" #include "rcl/subscription.h" #include "rcl/guard_condition.h" +#include "rcl/timer.h" #include "rcl/types.h" struct rcl_wait_set_impl_t; @@ -37,10 +38,9 @@ typedef struct rcl_wait_set_t { /// Storage for guard condition pointers. const rcl_guard_condition_t ** guard_conditions; size_t size_of_guard_conditions; - /// Allocator for storage. - rcl_allocator_t allocator; - /// If set to true, actions like add_subscription will fail until cleared. - bool pruned; + /// Storage for timer pointers. + const rcl_timer_t ** timers; + size_t size_of_timers; /// Implementation specific storage. struct rcl_wait_set_impl_t * impl; } rcl_wait_set_t; @@ -81,6 +81,7 @@ rcl_get_zero_initialized_wait_set(); * \param[inout] wait_set the wait set struct to be initialized * \param[in] number_of_subscriptions non-zero size of the subscriptions set * \param[in] number_of_guard_conditions non-zero size of the guard conditions set + * \param[in] number_of_timers non-zero size of the timers set * \param[in] allocator the allocator to use when allocating space in the sets * \return RCL_RET_OK if the wait set is initialized successfully, or * RCL_RET_ALREADY_INIT if the wait set is not zero initialized, or @@ -93,6 +94,7 @@ rcl_wait_set_init( rcl_wait_set_t * wait_set, size_t number_of_subscriptions, size_t number_of_guard_conditions, + size_t number_of_timers, rcl_allocator_t allocator); /// Finalize a rcl wait set. @@ -107,6 +109,7 @@ rcl_wait_set_init( * succeed. * * This function is not thread-safe. + * This function is lock-free. * * \param[inout] wait_set the wait set struct to be finalized. * \return RCL_RET_OK if the finalization was successful, or @@ -116,11 +119,28 @@ rcl_wait_set_init( rcl_ret_t rcl_wait_set_fini(rcl_wait_set_t * wait_set); +/// Retrieve the wait set's allocator. +/* The allocator must be an allocated rcl_allocator_t struct, as the result is + * copied into this variable. + * + * This function is not thread-safe. + * This function is lock-free. + * + * \param[in] wait_set the handle to the wait set + * \param[out] allocator the rcl_allocator_t struct to which the result is copied + * \return RCL_RET_OK if the allocator was successfully retrieved, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR if an unspecified error occurs. + */ +rcl_ret_t +rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * allocator); + /// Stores a pointer to the given subscription in the next empty spot in the set. /* This function does not guarantee that the subscription is not already in the * wait set. * * This function is not thread-safe. + * This function is lock-free. * * \param[inout] wait_set struct in which the subscription is to be stored * \param[in] subscription the subscription to be added to the wait set @@ -142,6 +162,7 @@ rcl_wait_set_add_subscription( * Calling this on an uninitialized (zero initialized) wait set will fail. * * This function is not thread-safe. + * This function is lock-free. * * \param[inout] wait_set struct to have its subscriptions cleared * \return RCL_RET_OK if cleared successfully, or @@ -196,13 +217,36 @@ rcl_wait_set_add_guard_condition( rcl_ret_t rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set); -/// Reallocates space for the subscriptions in the wait set. +/// Reallocates space for the guard conditions in the wait set. /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_resize_subscriptions */ rcl_ret_t rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); +/// Stores a pointer to the timer in the next empty spot in the set. +/* This function behaves exactly the same as for subscriptions. + * \see rcl_wait_set_add_subscription + */ +rcl_ret_t +rcl_wait_set_add_timer( + rcl_wait_set_t * wait_set, + const rcl_timer_t * timer); + +/// Removes (sets to NULL) the timers in the wait set. +/* This function behaves exactly the same as for subscriptions. + * \see rcl_wait_set_clear_subscriptions + */ +rcl_ret_t +rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set); + +/// Reallocates space for the timers in the wait set. +/* This function behaves exactly the same as for subscriptions. + * \see rcl_wait_set_resize_subscriptions + */ +rcl_ret_t +rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size); + /// Block until the wait set is ready or until the timeout has been exceeded. /* This function will collect the items in the rcl_wait_set_t and pass them * to the underlying rmw_wait function. diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index c892d73..c13cf38 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -34,6 +34,8 @@ typedef struct rcl_wait_set_impl_t { rmw_subscriptions_t rmw_subscriptions; size_t guard_condition_index; rmw_guard_conditions_t rmw_guard_conditions; + size_t timer_index; + rcl_allocator_t allocator; } rcl_wait_set_impl_t; rcl_wait_set_t @@ -44,7 +46,7 @@ rcl_get_zero_initialized_wait_set() } static bool -__wait_set_is_valid(rcl_wait_set_t * wait_set) +__wait_set_is_valid(const rcl_wait_set_t * wait_set) { return wait_set && wait_set->impl; } @@ -58,6 +60,9 @@ __wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator) if (wait_set->guard_conditions) { rcl_wait_set_resize_guard_conditions(wait_set, 0); } + if (wait_set->timers) { + rcl_wait_set_resize_timers(wait_set, 0); + } if (wait_set->impl) { allocator.deallocate(wait_set->impl, allocator.state); wait_set->impl = NULL; @@ -69,6 +74,7 @@ rcl_wait_set_init( rcl_wait_set_t * wait_set, size_t number_of_subscriptions, size_t number_of_guard_conditions, + size_t number_of_timers, rcl_allocator_t allocator) { rcl_ret_t fail_ret = RCL_RET_ERROR; @@ -118,10 +124,18 @@ rcl_wait_set_init( fail_ret = ret; goto fail; } + // Initialize timer space. + ret = rcl_wait_set_resize_timers(wait_set, number_of_timers); + if (ret != RCL_RET_OK) { + fail_ret = ret; + goto fail; + } + if ((ret = rcl_wait_set_clear_timers(wait_set)) != RCL_RET_OK) { + fail_ret = ret; + goto fail; + } // Set allocator. - wait_set->allocator = allocator; - // Initialize pruned. - wait_set->pruned = false; + wait_set->impl->allocator = allocator; return RCL_RET_OK; fail: __wait_set_clean_up(wait_set, allocator); @@ -134,12 +148,25 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set) rcl_ret_t result = RCL_RET_OK; RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); if (__wait_set_is_valid(wait_set)) { - __wait_set_clean_up(wait_set, wait_set->allocator); + __wait_set_clean_up(wait_set, wait_set->impl->allocator); } return result; } -#define SET_ADD(Type, RMWStorage) \ +rcl_ret_t +rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * allocator) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + if (!__wait_set_is_valid(wait_set)) { + RCL_SET_ERROR_MSG("wait set is invalid"); + return RCL_RET_WAIT_SET_INVALID; + } + *allocator = wait_set->impl->allocator; + return RCL_RET_OK; +} + +#define SET_ADD(Type) \ RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \ RCL_CHECK_ARGUMENT_FOR_NULL(Type, RCL_RET_INVALID_ARGUMENT); \ if (!__wait_set_is_valid(wait_set)) { \ @@ -151,15 +178,16 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set) return RCL_RET_WAIT_SET_FULL; \ } \ size_t current_index = wait_set->impl->Type##_index++; \ - wait_set->Type##s[current_index] = Type; \ + wait_set->Type##s[current_index] = Type; + +#define SET_ADD_RMW(Type, RMWStorage) \ /* Also place into rmw storage. */ \ rmw_##Type##_t * rmw_handle = rcl_##Type##_get_rmw_##Type##_handle(Type); \ RCL_CHECK_FOR_NULL_WITH_MSG( \ rmw_handle, rcl_get_error_string_safe(), return RCL_RET_ERROR); \ - wait_set->impl->RMWStorage[current_index] = rmw_handle->data; \ - return RCL_RET_OK; + wait_set->impl->RMWStorage[current_index] = rmw_handle->data; -#define SET_CLEAR(Type, RMWStorage, RMWCount) \ +#define SET_CLEAR(Type) \ RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \ if (!__wait_set_is_valid(wait_set)) { \ RCL_SET_ERROR_MSG("wait set is invalid"); \ @@ -167,67 +195,79 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set) } \ memset(wait_set->Type##s, 0, sizeof(rcl_##Type##_t *) * wait_set->size_of_##Type##s); \ wait_set->impl->Type##_index = 0; \ + +#define SET_CLEAR_RMW(Type, RMWStorage, RMWCount) \ /* Also clear the rmw storage. */ \ memset( \ wait_set->impl->RMWStorage, \ 0, \ - sizeof(rmw_subscription_t *) * wait_set->impl->RMWCount); \ - return RCL_RET_OK; + sizeof(rmw_subscription_t *) * wait_set->impl->RMWCount); -#define SET_RESIZE(Type, RMWStorage, RMWCount) \ +#define SET_RESIZE(Type, ExtraDealloc, ExtraRealloc) \ RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \ RCL_CHECK_FOR_NULL_WITH_MSG( \ wait_set->impl, "wait set is invalid", return RCL_RET_WAIT_SET_INVALID); \ if (size == wait_set->size_of_##Type##s) { \ return RCL_RET_OK; \ } \ + rcl_allocator_t allocator = wait_set->impl->allocator; \ if (size == 0) { \ if (wait_set->Type##s) { \ - wait_set->allocator.deallocate(wait_set->Type##s, wait_set->allocator.state); \ + allocator.deallocate(wait_set->Type##s, allocator.state); \ wait_set->Type##s = NULL; \ } \ - /* Also deallocate the rmw storage. */ \ - if (wait_set->impl->RMWStorage) { \ - wait_set->allocator.deallocate(wait_set->impl->RMWStorage, wait_set->allocator.state); \ - wait_set->impl->RMWStorage = NULL; \ - } \ + ExtraDealloc \ } \ else { \ wait_set->size_of_##Type##s = 0; \ - wait_set->Type##s = (const rcl_##Type##_t **)wait_set->allocator.reallocate( \ - wait_set->Type##s, sizeof(rcl_##Type##_t *) * size, wait_set->allocator.state); \ + wait_set->Type##s = (const rcl_##Type##_t **)allocator.reallocate( \ + wait_set->Type##s, sizeof(rcl_##Type##_t *) * size, allocator.state); \ RCL_CHECK_FOR_NULL_WITH_MSG( \ wait_set->Type##s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \ wait_set->size_of_##Type##s = size; \ - /* Also resize the rmw storage. */ \ - wait_set->impl->RMWCount = 0; \ - wait_set->impl->RMWStorage = (void **)wait_set->allocator.reallocate( \ - wait_set->impl->RMWStorage, sizeof(rcl_##Type##_t *) * size, wait_set->allocator.state); \ - if (!wait_set->impl->RMWStorage) { \ - wait_set->allocator.deallocate(wait_set->Type##s, wait_set->allocator.state); \ - wait_set->size_of_##Type##s = 0; \ - RCL_SET_ERROR_MSG("allocating memory failed"); \ - return RCL_RET_BAD_ALLOC; \ - } \ - wait_set->impl->RMWCount = size; \ + ExtraRealloc \ } \ return RCL_RET_OK; +#define SET_RESIZE_RMW_DEALLOC(RMWStorage, RMWCount) \ + /* Also deallocate the rmw storage. */ \ + if (wait_set->impl->RMWStorage) { \ + allocator.deallocate(wait_set->impl->RMWStorage, allocator.state); \ + wait_set->impl->RMWStorage = NULL; \ + } + +#define SET_RESIZE_RMW_REALLOC(Type, RMWStorage, RMWCount) \ + /* Also resize the rmw storage. */ \ + wait_set->impl->RMWCount = 0; \ + wait_set->impl->RMWStorage = (void **)allocator.reallocate( \ + wait_set->impl->RMWStorage, sizeof(rcl_##Type##_t *) * size, allocator.state); \ + if (!wait_set->impl->RMWStorage) { \ + allocator.deallocate(wait_set->Type##s, allocator.state); \ + wait_set->size_of_##Type##s = 0; \ + RCL_SET_ERROR_MSG("allocating memory failed"); \ + return RCL_RET_BAD_ALLOC; \ + } \ + wait_set->impl->RMWCount = size; + rcl_ret_t rcl_wait_set_add_subscription( rcl_wait_set_t * wait_set, const rcl_subscription_t * subscription) { - SET_ADD(subscription, rmw_subscriptions.subscribers) + SET_ADD(subscription) + SET_ADD_RMW(subscription, rmw_subscriptions.subscribers) + return RCL_RET_OK; } rcl_ret_t rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set) { - SET_CLEAR( + SET_CLEAR(subscription) + SET_CLEAR_RMW( subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count) + return RCL_RET_OK; } rcl_ret_t @@ -235,8 +275,11 @@ rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size) { SET_RESIZE( subscription, - rmw_subscriptions.subscribers, - rmw_subscriptions.subscriber_count) + SET_RESIZE_RMW_DEALLOC( + rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count), + SET_RESIZE_RMW_REALLOC( + subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count) + ) } rcl_ret_t @@ -244,16 +287,20 @@ rcl_wait_set_add_guard_condition( rcl_wait_set_t * wait_set, const rcl_guard_condition_t * guard_condition) { - SET_ADD(guard_condition, rmw_guard_conditions.guard_conditions) + SET_ADD(guard_condition) + SET_ADD_RMW(guard_condition, rmw_guard_conditions.guard_conditions) + return RCL_RET_OK; } rcl_ret_t rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set) { - SET_CLEAR( + SET_CLEAR(guard_condition) + SET_CLEAR_RMW( guard_condition, rmw_guard_conditions.guard_conditions, rmw_guard_conditions.guard_condition_count) + return RCL_RET_OK; } rcl_ret_t @@ -261,8 +308,35 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size) { SET_RESIZE( guard_condition, - rmw_guard_conditions.guard_conditions, - rmw_guard_conditions.guard_condition_count) + SET_RESIZE_RMW_DEALLOC( + rmw_guard_conditions.guard_conditions, rmw_guard_conditions.guard_condition_count), + SET_RESIZE_RMW_REALLOC( + guard_condition, + rmw_guard_conditions.guard_conditions, + rmw_guard_conditions.guard_condition_count) + ) +} + +rcl_ret_t +rcl_wait_set_add_timer( + rcl_wait_set_t * wait_set, + const rcl_timer_t * timer) +{ + SET_ADD(timer) + return RCL_RET_OK; +} + +rcl_ret_t +rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set) +{ + SET_CLEAR(timer) + return RCL_RET_OK; +} + +rcl_ret_t +rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size) +{ + SET_RESIZE(timer, ;, ;) } rcl_ret_t @@ -281,7 +355,24 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) // Create dummy sets for currently unsupported wait-ables. static rmw_services_t dummy_services = {0, NULL}; static rmw_clients_t dummy_clients = {0, NULL}; - rmw_time_t rmw_timeout = rcl_time_from_int64_t_nanoseconds(timeout); + // Calculate the timeout. + int64_t min_timeout = timeout; + if (min_timeout == 0) { // Do not consider timer timeouts if non-blocking. + for (size_t i = 0; i < wait_set->size_of_timers; ++i) { + if (!wait_set->timers[i]) { + continue; // Skip NULL timers. + } + int64_t timer_timeout; + rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout); + if (ret != RCL_RET_OK) { + return ret; // The rcl error state should already be set. + } + if (timer_timeout < min_timeout) { + min_timeout = timer_timeout; + } + } + } + rmw_time_t rmw_timeout = rcl_time_from_int64_t_nanoseconds(min_timeout); // Wait. rmw_ret_t ret = rmw_wait( &wait_set->impl->rmw_subscriptions, @@ -290,6 +381,22 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) &dummy_clients, &rmw_timeout ); + // Check for error. + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); + return RCL_RET_ERROR; + } + // Check for ready timers next, and set not ready timers to NULL. + for (size_t i = 0; i < wait_set->size_of_timers; ++i) { + bool is_ready = false; + rcl_ret_t ret = rcl_timer_is_ready(wait_set->timers[i], &is_ready); + if (ret != RCL_RET_OK) { + return ret; // The rcl error state should already be set. + } + if (!is_ready) { + wait_set->timers[i] = NULL; + } + } // Check for timeout. if (ret == RMW_RET_TIMEOUT) { // Assume none were set (because timeout was reached first), and clear all. @@ -297,11 +404,6 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) rcl_wait_set_clear_guard_conditions(wait_set); return RCL_RET_TIMEOUT; } - // Check for error. - if (ret != RMW_RET_OK) { - RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); - return RCL_RET_ERROR; - } // Set corresponding rcl subscription handles NULL. for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) { assert(i < wait_set->impl->rmw_subscriptions.subscriber_count); // Defensive. From 46551399e641c875219fdca7b76781a2244015fd Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 10 Dec 2015 16:11:16 -0800 Subject: [PATCH 18/71] refactored time and timer, and added tests --- rcl/CMakeLists.txt | 53 +++++- rcl/include/rcl/allocator.h | 7 +- rcl/include/rcl/time.h | 89 +++++++--- rcl/include/rcl/timer.h | 65 +++---- rcl/src/rcl/time.c | 181 +------------------- rcl/src/rcl/time_unix.c | 119 +++++++++++++ rcl/src/rcl/time_win32.c | 218 ++++++++++++++++++++++++ rcl/src/rcl/timer.c | 48 +++--- rcl/src/rcl/wait.c | 50 ++++-- rcl/test/memory_tools.cpp | 73 ++++++++ rcl/test/memory_tools.hpp | 50 ++++++ rcl/test/memory_tools_common.cpp | 150 ++++++++++++++++ rcl/test/memory_tools_osx_interpose.cpp | 54 ++++++ rcl/test/rcl/test_allocator.cpp | 74 ++++++++ rcl/test/rcl/test_time.cpp | 110 ++++++++++++ rcl/test/scope_exit.hpp | 40 +++++ rcl/test/test_memory_tools.cpp | 119 +++++++++++++ 17 files changed, 1221 insertions(+), 279 deletions(-) create mode 100644 rcl/src/rcl/time_unix.c create mode 100644 rcl/src/rcl/time_win32.c create mode 100644 rcl/test/memory_tools.cpp create mode 100644 rcl/test/memory_tools.hpp create mode 100644 rcl/test/memory_tools_common.cpp create mode 100644 rcl/test/memory_tools_osx_interpose.cpp create mode 100644 rcl/test/rcl/test_allocator.cpp create mode 100644 rcl/test/rcl/test_time.cpp create mode 100644 rcl/test/scope_exit.hpp create mode 100644 rcl/test/test_memory_tools.cpp diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 46f65b2..4a7d20c 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -13,7 +13,7 @@ ament_export_include_directories(include) include_directories(include) if(NOT WIN32) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c11 -Wall -Wextra") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") endif() set(${PROJECT_NAME}_sources @@ -34,10 +34,61 @@ ament_target_dependencies(${PROJECT_NAME} "rmw" "rosidl_generator_c" ) +set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-std=c11") if(AMENT_ENABLE_TESTING) + find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + set(extra_test_libraries ) + ament_find_gtest() # For GTEST_LIBRARIES + if(APPLE) + add_library(${PROJECT_NAME}_memory_tools_interpose SHARED test/memory_tools_osx_interpose.cpp) + target_link_libraries(${PROJECT_NAME}_memory_tools_interpose ${GTEST_LIBRARIES}) + set_target_properties(${PROJECT_NAME}_memory_tools_interpose PROPERTIES COMPILE_FLAGS "-std=c++11") + list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools_interpose) + endif() + add_library(${PROJECT_NAME}_memory_tools SHARED test/memory_tools.cpp) + set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") + target_link_libraries(${PROJECT_NAME}_memory_tools ${GTEST_LIBRARIES} ${extra_test_libraries}) + list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools) + + ament_add_gtest(test_memory_tools + test/test_memory_tools.cpp + ENV DYLD_INSERT_LIBRARIES=$) + if(TARGET test_memory_tools) + target_include_directories(test_memory_tools PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") + target_link_libraries(test_memory_tools ${PROJECT_NAME} ${extra_test_libraries}) + endif() + + ament_add_gtest(test_allocator + test/rcl/test_allocator.cpp + ENV DYLD_INSERT_LIBRARIES=$) + if(TARGET test_allocator) + target_include_directories(test_allocator PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + set_target_properties(test_allocator PROPERTIES COMPILE_FLAGS "-std=c++11") + target_link_libraries(test_allocator ${PROJECT_NAME} ${extra_test_libraries}) + endif() + + ament_add_gtest(test_time + test/rcl/test_time.cpp + ENV DYLD_INSERT_LIBRARIES=$) + if(TARGET test_time) + target_include_directories(test_time PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + set_target_properties(test_time PROPERTIES COMPILE_FLAGS "-std=c++11") + target_link_libraries(test_time ${PROJECT_NAME} ${extra_test_libraries}) + endif() endif() ament_package() diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index 7742c50..b47313f 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -23,7 +23,7 @@ extern "C" #include "rcl/types.h" /// Encapsulation of an allocator. -/* NULL for allocate implies that the system should use malloc and free. */ +/* To use malloc, free, and realloc use rcl_get_default_allocator */ typedef struct rcl_allocator_t { /// Allocate memory, given a size and state structure. /* An error should be indicated by returning null. */ @@ -44,7 +44,10 @@ typedef struct rcl_allocator_t { } rcl_allocator_t; /// Return a properly initialized rcl_allocator_t with default values. -/* This function does not allocate memory. */ +/* This function does not allocate memory. + * This function is thread-safe. + * This function is lock-free. + */ rcl_allocator_t rcl_get_default_allocator(); diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index bffb8be..fddbce4 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -21,7 +21,6 @@ extern "C" #endif #include "rcl/types.h" -#include "rmw/rmw.h" #define RCL_S_TO_NS(seconds) (seconds * 1000000000) #define RCL_MS_TO_NS(milliseconds) (milliseconds * 1000000) @@ -31,55 +30,89 @@ extern "C" #define RCL_NS_TO_MS(nanoseconds) (nanoseconds / 1000000) #define RCL_NS_TO_US(nanoseconds) (nanoseconds / 1000) -typedef rmw_time_t rcl_time_t; - -/// Create a rcl_time_t struct with a given signed number of nanoseconds. -rcl_time_t -rcl_time_from_int64_t_nanoseconds(int64_t nanoseconds); - -/// Create a rcl_time_t struct with a given unsigned number of nanoseconds. -rcl_time_t -rcl_time_from_uint64_t_nanoseconds(uint64_t nanoseconds); - -/// Convert a rcl_time_t struct into an unsigned number of nanoseconds. -/* This function will return 0 if the time argument is NULL. */ -uint64_t -rcl_time_to_uint64_t_nanoseconds(const rcl_time_t * time); - -/// Retrieve the current time as a rcl_time_t struct. -/* The now argument must point to an allocated rcl_time_t struct, as the - * result is copied into this variable. +/// Struct which encapsulates a point in time according to a system clock. +/* The system clock's point of reference is the Unix Epoch (January 1st, 1970). + * See: http://stackoverflow.com/a/32189845/671658 + * Therefore all times in this struct are positive and count the nanoseconds + * since the Unix epoch and this struct cannot be used on systems which report + * a current time before the Unix Epoch. + * Leap seconds are not considered. * + * The struct represents time as nanoseconds in an unsigned 64-bit integer. + * The struct is capable of representing any time until the year 2554 with + * nanosecond precisions. + */ +typedef struct rcl_system_time_point_t { + uint64_t nanoseconds; +} rcl_system_time_point_t; + +/// Struct which encapsulates a point in time according to a steady clock. +/* The steady clock's point of reference is system defined, but often the time + * that the process started plus a signed random offset. + * See: http://stackoverflow.com/a/32189845/671658 + * However, the clock is guaranteed to be monotonically increasing. + * Therefore all times in this struct are positive and count nanoseconds + * since an unspecified, but constant point in time. + * + * The struct represents time as nanoseconds in an unsigned 64-bit integer. + */ +typedef struct rcl_steady_time_point_t { + uint64_t nanoseconds; +} rcl_steady_time_point_t; + +/// Struct which encapsulates a duration of time between two points in time. +/* The struct can represent any time within the range [~292 years, ~-292 years] + * with nanosecond precision. + */ +typedef struct rcl_duration_t { + int64_t nanoseconds; +} rcl_duration_t; + +/// Retrieve the current time as a rcl_system_time_point_t struct. +/* This function returns the time from a system clock. + * The closest equivalent would be to std::chrono::system_clock::now(); + * + * The resolution (e.g. nanoseconds vs microseconds) is not guaranteed. + * + * The now argument must point to an allocated rcl_system_time_point_t struct, + * as the result is copied into this variable. + * + * This function may allocate heap memory when an error occurs. * This function is thread-safe. * This function is lock-free, with an exception on Windows. * On Windows this is lock-free if the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. * - * \param[out] now a rcl_time_t struct in which the current time is stored + * \param[out] now a struct in which the current time is stored * \return RCL_RET_OK if the current time was successfully obtained, or * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR an unspecified error occur. */ rcl_ret_t -rcl_time_now(rcl_time_t * now); +rcl_system_time_point_now(rcl_system_time_point_t * now); -/// Normalize a time struct. -/* If there are more than 10^9 nanoseconds in the nsec field, the extra seconds - * are removed from the nsec field and added to the sec field. - * Overflow of the sec field due to this normalization is ignored. +/// Retrieve the current time as a rcl_steady_time_point_t struct. +/* This function returns the time from a monotonically increasing clock. + * The closest equivalent would be to std::chrono::steady_clock::now(); * + * The resolution (e.g. nanoseconds vs microseconds) is not guaranteed. + * + * The now argument must point to an allocated rcl_steady_time_point_t struct, + * as the result is copied into this variable. + * + * This function may allocate heap memory when an error occurs. * This function is thread-safe. * This function is lock-free, with an exception on Windows. * On Windows this is lock-free if the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. * - * \param[out] now a rcl_time_t struct to be normalized - * \return RCL_RET_OK if the struct was normalized successfully, or + * \param[out] now a struct in which the current time is stored + * \return RCL_RET_OK if the current time was successfully obtained, or * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR an unspecified error occur. */ rcl_ret_t -rcl_time_normalize(rcl_time_t * now); +rcl_steady_time_point_now(rcl_steady_time_point_t * now); #if __cplusplus } diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index ba62062..ca921e4 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -37,15 +37,16 @@ typedef struct rcl_timer_t { /// User callback signature for timers. /* The first argument the callback gets is a pointer to the timer. - * This can be used to cancel the timer, query about the time until the next + * This can be used to cancel the timer, query the time until the next * timer callback, exchange the callback with a different one, etc. * - * The only caveat is that the function rcl_timer_get_last_call_time will - * return the time just before the callback was called. - * Therefore the second argument given is the time when the previous callback - * was called, since that information is no longer accessible via the timer. + * The only caveat is that the function rcl_timer_get_time_since_last_call will + * return the time since just before this callback was called, not the last. + * Therefore the second argument given is the time since the previous callback + * was called, because that information is no longer accessible via the timer. + * The time since the last callback call is given in nanoseconds. */ -typedef void (* rcl_timer_callback_t)(rcl_timer_t *, rcl_time_t); +typedef void (* rcl_timer_callback_t)(rcl_timer_t *, uint64_t); /// Return a zero initialized timer. rcl_timer_t @@ -56,32 +57,32 @@ rcl_get_zero_initialized_timer(); * A timer can be added to a wait set and waited on, such that the wait set * will wake up when a timer is ready to be executed. * - * A timer simply holds state and does not actively call periodically. - * It does not create any threads or register interrupts or signals. + * A timer simply holds state and does not automatically call callbacks. + * It does not create any threads, register interrupts, or consume signals. * For blocking behavior it can be used in conjunction with a wait set and * rcl_wait(). * When rcl_timer_is_ready() returns true, the timer must still be called * explicitly using rcl_timer_call(). * * The timer handle must be a pointer to an allocated and zero initialized - * timer struct. + * rcl_timer_t struct. * Calling this function on an already initialized timer will fail. * Calling this function on a timer struct which has been allocated but not * zero initialized is undefined behavior. * - * The period is a timer duration (rather an absolute time in the future). - * If the period is 0 seconds and 0 nanoseconds then it will always be ready. + * The period is a duration (rather an absolute time in the future). + * If the period is 0 then it will always be ready. * * The callback must be a function which returns void and takes two arguments, - * the first being a pointer to the associated timer, and the second a time - * struct which is the time of the previous call or when the timer was created + * the first being a pointer to the associated timer, and the second a uint64_t + * which is the time since the previous call, or since the timer was created * if it is the first call to the callback. * * Expected usage: * * #include * - * void my_timer_callback(rcl_timer_t * timer, rcl_time_t last_call_time) + * void my_timer_callback(rcl_timer_t * timer, uint64_t last_call_time) * { * // Do timer work... * // Optionally reconfigure, cancel, or reset the timer... @@ -94,8 +95,9 @@ rcl_get_zero_initialized_timer(); * ret = rcl_timer_fini(&timer); * // ... error handling * + * This function does allocate heap memory. * This function is not thread-safe. - * This function is lock-free. + * This function is not lock-free. * * \param[inout] timer the timer handle to be initialized * \param[in] period the duration between calls to the callback in nanoseconds @@ -122,8 +124,9 @@ rcl_timer_init( * This function is not thread-safe with any rcl_timer_* functions used on the * same timer object. * + * This function may allocate heap memory when an error occurs. * This function is not thread-safe. - * This function is lock-free. + * This function is not lock-free. * * \param[inout] timer the handle to the timer to be finalized. * \return RCL_RET_OK if the timer was finalized successfully, or @@ -132,7 +135,7 @@ rcl_timer_init( rcl_ret_t rcl_timer_fini(rcl_timer_t * timer); -/// Call the timer's callback and set the last call time to the current time. +/// Call the timer's callback and set the last call time. /* This function will call the callback and change the last call time even if * the timer's period has not yet elapsed. * It is up to the calling code to make sure the period has elapsed by first @@ -140,14 +143,15 @@ rcl_timer_fini(rcl_timer_t * timer); * The order of operations in this command are as follows: * * - Ensure the timer has not been canceled. - * - Get the current time into a temporary rcl_time_t. + * - Get the current time into a temporary rcl_steady_time_point_t. * - Exchange the current time with the last call time of the timer. - * - Call the callback, with this timer and the last call time as arguments. + * - Call the callback, passing this timer and the time since the last call. * - Return after the callback has completed. * * During the callback the timer can be canceled or have its period and/or * callback modified. * + * This function may allocate heap memory when an error occurs. * This function is thread-safe, but the user's callback may not be. * This function is lock-free so long as the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t, but the user's @@ -166,11 +170,12 @@ rcl_timer_call(rcl_timer_t * timer); /// Calculates whether or not the timer should be called. /* The result is true if the time until next call is less than, or equal to, 0 * and the timer has not been canceled. - * Otherwise the result is false, indicating the period has not elapsed. + * Otherwise the result is false, indicating the timer should not be called. * * The is_ready argument must point to an allocated bool object, as the result * is copied into it. * + * This function may allocate heap memory when an error occurs. * This function is thread-safe. * This function is lock-free so long as the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. @@ -198,6 +203,7 @@ rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready); * The time_until_next_call argument must point to an allocated int64_t, as the * the time until is coped into that instance. * + * This function may allocate heap memory when an error occurs. * This function is thread-safe. * This function is lock-free so long as the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. @@ -212,29 +218,30 @@ rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready); rcl_ret_t rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call); -/// Retrieve the time of when the previous call to rcl_timer_call() occurred. -/* This function retrieves the internal time and copies it into the given - * rcl_time_t struct. +/// Retrieve the time since the previous call to rcl_timer_call() occurred. +/* This function calculates the time since the last call and copies it into + * the given uint64_t variable. * - * Calling this function within a callback will not return the previous time - * but instead the time of when the current callback was called. + * Calling this function within a callback will not return the time since the + * previous call but instead the time since the current callback was called. * - * The last_call_time argument must be a pointer to an already allocated - * rcl_time_t struct. + * The time_since_last_call argument must be a pointer to an already allocated + * uint64_t. * + * This function may allocate heap memory when an error occurs. * This function is thread-safe. * This function is lock-free so long as the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. * * \param[in] timer the handle to the timer which is being queried - * \param[out] last_call_time the rcl_time_t struct in which the time is stored + * \param[out] time_since_last_call the struct in which the time is stored * \return RCL_RET_OK if the last call time was retrieved successfully, or * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_TIMER_INVALID if the timer is invalid, or * RCL_RET_ERROR an unspecified error occur. */ rcl_ret_t -rcl_timer_get_last_call_time(const rcl_timer_t * timer, rcl_time_t * last_call_time); +rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_since_last_call); /// Retrieve the period of the timer. /* This function retrieves the period and copies it into the give variable. diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index be4c557..ab08eda 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -12,183 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#if __cplusplus -extern "C" -{ -#endif - -// Appropriate check accorind to: -// http://man7.org/linux/man-pages/man2/clock_gettime.2.html -#define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L) - -#include "rcl/time.h" - -#include -#include -#include - -#if defined(__MACH__) -#include -#include -#endif -#if defined(WIN32) -#include -#include -#endif - -#include "./common.h" -#include "rcl/error_handling.h" - -rcl_time_t -rcl_time_from_int64_t_nanoseconds(int64_t nanoseconds) -{ - rcl_time_t result; - result.sec = RCL_NS_TO_S(nanoseconds); - result.nsec = nanoseconds % 1000000000; - return result; -} - -rcl_time_t -rcl_time_from_uint64_t_nanoseconds(uint64_t nanoseconds) -{ - rcl_time_t result; - result.sec = RCL_NS_TO_S(nanoseconds); - result.nsec = nanoseconds % 1000000000; - return result; -} - -uint64_t -rcl_time_to_uint64_t_nanoseconds(const rcl_time_t * rcl_time) -{ - if (!rcl_time) { - return 0; - } - return RCL_S_TO_NS(rcl_time->sec) + rcl_time->nsec; -} - -static void -__timespec_get_now(struct timespec * timespec_now) -{ -#if defined(__MACH__) - // On OS X use clock_get_time. - clock_serv_t cclock; - mach_timespec_t mts; - host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - timespec_now->tv_sec = mts.tv_sec; - timespec_now->tv_nsec = mts.tv_nsec; -#else // if defined(__MACH__) - // Otherwise use clock_gettime. - clock_gettime(CLOCK_REALTIME, timespec_now); -#endif // if defined(__MACH__) -} - -rcl_ret_t -rcl_time_now(rcl_time_t * now) -{ - RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); -#ifndef WIN32 - // Unix implementations -#if HAS_CLOCK_GETTIME || defined(__MACH__) - // If clock_gettime is available or on OS X, use a timespec. - struct timespec timespec_now; - __timespec_get_now(×pec_now); - now->sec = timespec_now.tv_sec; - now->nsec = timespec_now.tv_nsec; -#else // if HAS_CLOCK_GETTIME || defined(__MACH__) - // Otherwise we have to fallback to gettimeofday. - struct timeval timeofday; - gettimeofday(&timeofday, NULL); - now->sec = timeofday.tv_sec; - now->nsec = timeofday.tv_usec * 1000; -#endif // if HAS_CLOCK_GETTIME || defined(__MACH__) +#ifdef WIN32 +#include "./time_win32.c" #else - // Windows implementation adapted from roscpp_core (3-clause BSD), see: - // https://github.com/ros/roscpp_core/blob/0.5.6/rostime/src/time.cpp#L96 - // Win32 implementation - // unless I've missed something obvious, the only way to get high-precision - // time on Windows is via the QueryPerformanceCounter() call. However, - // this is somewhat problematic in Windows XP on some processors, especially - // AMD, because the Windows implementation can freak out when the CPU clocks - // down to save power. Time can jump or even go backwards. Microsoft has - // fixed this bug for most systems now, but it can still show up if you have - // not installed the latest CPU drivers (an oxymoron). They fixed all these - // problems in Windows Vista, and this API is by far the most accurate that - // I know of in Windows, so I'll use it here despite all these caveats - LARGE_INTEGER cpu_freq; - static LARGE_INTEGER cpu_freq_global; - LARGE_INTEGER init_cpu_time; - static LARGE_INTEGER init_cpu_time_global; - static atomic_uint_least64_t start_ns = ATOMIC_VAR_INIT(0); - rcl_time_t start = {0, 0}; - // If start_ns (static/global) is 0, then set it up on the first call. - uint64_t start_ns_loaded = atomic_load(&start_ns); - if (start_ns_loaded == 0) { - QueryPerformanceFrequency(&cpu_freq); - if (cpu_freq.QuadPart == 0) { - RCL_SET_ERROR_MSG("no high performance timer found"); - return RCL_RET_ERROR; - } - QueryPerformanceCounter(&init_cpu_time); - // compute an offset from the Epoch using the lower-performance timer API - FILETIME ft; - GetSystemTimeAsFileTime(&ft); - LARGE_INTEGER start_li; - start_li.LowPart = ft.dwLowDateTime; - start_li.HighPart = ft.dwHighDateTime; - // why did they choose 1601 as the time zero, instead of 1970? - // there were no outstanding hard rock bands in 1601. -#ifdef _MSC_VER - start_li.QuadPart -= 116444736000000000Ui64; -#else - start_li.QuadPart -= 116444736000000000ULL; -#endif - start.sec = (uint64_t)(start_li.QuadPart / 10000000); // 100-ns units. odd. - start.nsec = (start_li.LowPart % 10000000) * 100; - if (atomic_compare_exchange_strong(&start_ns, 0, (start.sec * 1000000000) + start.nsec)) { - // If it matched 0 this call was first to setup, set the cpu_freq and init_cpu_time globals. - init_cpu_time_global = init_cpu_time; - cpu_freq_global = cpu_freq; - } else { - // Another concurrent first call managed to set this up first; reset start so it gets set. - start = {0, 0}; - } - } - if (start.sec == 0 && start.nsec == 0) { - start.sec = RCL_NS_TO_S(start_ns_loaded); - start.nsec = start_ns_loaded % 1000000000; - } - LARGE_INTEGER cur_time; - QueryPerformanceCounter(&cur_time); - LARGE_INTEGER delta_cpu_time; - delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time_global.QuadPart; - double d_delta_cpu_time = delta_cpu_time.QuadPart / (double) cpu_freq_global.QuadPart; - uint64_t delta_sec = (uint64_t) floor(d_delta_cpu_time); - uint64_t delta_nsec = (uint64_t) floor((d_delta_cpu_time - delta_sec) * 1e9); - - *now = start; - now->sec += delta_sec; - now->nsec += delta_nsec; - - // Normalize the time struct. - rcl_ret_t ret = rcl_time_normalize(now); - if (ret != RCL_RET_OK) { - return ret; // rcl error state should already be set. - } -#endif - return RCL_RET_OK; -} - -rcl_ret_t -rcl_time_normalize(rcl_time_t * now) -{ - RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); - now->sec += now->nsec / 1000000000; - now->nsec = now->nsec % 1000000000; - return RCL_RET_OK; -} - -#if __cplusplus -} +#include "./time_unix.c" #endif diff --git a/rcl/src/rcl/time_unix.c b/rcl/src/rcl/time_unix.c new file mode 100644 index 0000000..5565d47 --- /dev/null +++ b/rcl/src/rcl/time_unix.c @@ -0,0 +1,119 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifdef WIN32 +#error time_unix.c is not intended to be used with win32 based systems +#endif + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/time.h" + +#if defined(__MACH__) +#include +#include +#endif +#include +#include + +#include "./common.h" +#include "rcl/error_handling.h" + +// This id an appropriate check for clock_gettime() according to: +// http://man7.org/linux/man-pages/man2/clock_gettime.2.html +#define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L) +#if !HAS_CLOCK_GETTIME && !defined(__MACH__) +#error no monotonic clock function available +#endif + +static void +__timespec_get_now_steady(struct timespec * timespec_now) +{ +#if defined(__MACH__) + // On OS X use clock_get_time. + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + timespec_now->tv_sec = mts.tv_sec; + timespec_now->tv_nsec = mts.tv_nsec; +#else // if defined(__MACH__) + // Otherwise use clock_gettime. +#ifdef CLOCK_MONOTONIC_RAW + clock_gettime(CLOCK_MONOTONIC_RAW, timespec_now); +#else + clock_gettime(CLOCK_MONOTONIC, timespec_now); +#endif // CLOCK_MONOTONIC_RAW +#endif // if defined(__MACH__) +} + +#define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) + +rcl_ret_t +rcl_system_time_point_now(rcl_system_time_point_t * now) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); + struct timespec timespec_now; +#if defined(__MACH__) + // On OS X use clock_get_time. + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + timespec_now.tv_sec = mts.tv_sec; + timespec_now.tv_nsec = mts.tv_nsec; +#else + // Otherwise use clock_gettime. + clock_gettime(CLOCK_REALTIME, ×pec_now); +#endif // if defined(__MACH__) + if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { + RCL_SET_ERROR_MSG("unexpected negative time"); + return RCL_RET_ERROR; + } + now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_steady_time_point_now(rcl_steady_time_point_t * now) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); +#ifndef WIN32 + // Unix implementations +#if HAS_CLOCK_GETTIME || defined(__MACH__) + // If clock_gettime is available or on OS X, use a timespec. + struct timespec timespec_now; + __timespec_get_now_steady(×pec_now); + if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { + RCL_SET_ERROR_MSG("unexpected negative time"); + return RCL_RET_ERROR; + } + now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; +#else // if HAS_CLOCK_GETTIME || defined(__MACH__) + // Cannot fallback to gettimeofday because it is not monotonic. +#error No monotonic clock detected. +#endif // if HAS_CLOCK_GETTIME || defined(__MACH__) +#else +#endif + return RCL_RET_OK; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c new file mode 100644 index 0000000..dcc8c20 --- /dev/null +++ b/rcl/src/rcl/time_win32.c @@ -0,0 +1,218 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef WIN32 +#error time_win32.c is only intended to be used with win32 based systems +#endif + +#if __cplusplus +extern "C" +{ +#endif + +// Appropriate check accorind to: +// http://man7.org/linux/man-pages/man2/clock_gettime.2.html +#define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L) + +#include "rcl/time.h" + +#include +#include +#include + +#if defined(__MACH__) +#include +#include +#endif +#if defined(WIN32) +#include +#include +#endif + +#include "./common.h" +#include "rcl/error_handling.h" + +static void +__timespec_get_now_system(struct timespec * timespec_now) +{ +#if defined(__MACH__) + // On OS X use clock_get_time. + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + timespec_now->tv_sec = mts.tv_sec; + timespec_now->tv_nsec = mts.tv_nsec; +#else // if defined(__MACH__) + // Otherwise use clock_gettime. + clock_gettime(CLOCK_REALTIME, timespec_now); +#endif // if defined(__MACH__) +} + +static void +__timespec_get_now_monotonic(struct timespec * timespec_now) +{ +#if defined(__MACH__) + // On OS X use clock_get_time. + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + timespec_now->tv_sec = mts.tv_sec; + timespec_now->tv_nsec = mts.tv_nsec; +#else // if defined(__MACH__) + // Otherwise use clock_gettime. +#ifdef CLOCK_MONOTONIC_RAW + clock_gettime(CLOCK_MONOTONIC_RAW, timespec_now); +#else + clock_gettime(CLOCK_MONOTONIC, timespec_now); +#endif // CLOCK_MONOTONIC_RAW +#endif // if defined(__MACH__) +} + +#define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) + +rcl_ret_t +rcl_system_time_point_now(rcl_system_time_point_t * now) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); +#ifndef WIN32 + // Unix implementations +#if HAS_CLOCK_GETTIME || defined(__MACH__) + // If clock_gettime is available or on OS X, use a timespec. + struct timespec timespec_now; + __timespec_get_now_system(×pec_now); + if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { + RCL_SET_ERROR_MSG("unexpected negative time"); + return RCL_RET_ERROR; + } + now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; +#else // if HAS_CLOCK_GETTIME || defined(__MACH__) + // Otherwise we have to fallback to gettimeofday. + struct timeval timeofday; + gettimeofday(&timeofday, NULL); + if (__WOULD_BE_NEGATIVE(timeofday.tv_sec, timeofday.tv_usec)) { + RCL_SET_ERROR_MSG("unexpected negative time"); + return RCL_RET_ERROR; + } + now->nanoseconds = RCL_S_TO_NS(timeofday.tv_sec) + timeofday.tv_usec * 1000; +#endif // if HAS_CLOCK_GETTIME || defined(__MACH__) +#else + /* Windows implementation adapted from roscpp_core (3-clause BSD), see: + * https://github.com/ros/roscpp_core/blob/0.5.6/rostime/src/time.cpp#L96 + * + * > Win32 implementation + * unless I've missed something obvious, the only way to get high-precision + * time on Windows is via the QueryPerformanceCounter() call. However, + * this is somewhat problematic in Windows XP on some processors, especially + * AMD, because the Windows implementation can freak out when the CPU clocks + * down to save power. Time can jump or even go backwards. Microsoft has + * fixed this bug for most systems now, but it can still show up if you have + * not installed the latest CPU drivers (an oxymoron). They fixed all these + * problems in Windows Vista, and this API is by far the most accurate that + * I know of in Windows, so I'll use it here despite all these caveats + * + * I've further modified it to be thread safe using a atomic_uint_least64_t. + */ + LARGE_INTEGER cpu_freq; + static LARGE_INTEGER cpu_freq_global; + LARGE_INTEGER init_cpu_time; + static LARGE_INTEGER init_cpu_time_global; + static atomic_uint_least64_t start_ns = ATOMIC_VAR_INIT(0); + rcl_time_t start = {0, 0}; + // If start_ns (static/global) is 0, then set it up on the first call. + uint64_t start_ns_loaded = atomic_load(&start_ns); + if (start_ns_loaded == 0) { + QueryPerformanceFrequency(&cpu_freq); + if (cpu_freq.QuadPart == 0) { + RCL_SET_ERROR_MSG("no high performance timer found"); + return RCL_RET_ERROR; + } + QueryPerformanceCounter(&init_cpu_time); + // compute an offset from the Epoch using the lower-performance timer API + FILETIME ft; + GetSystemTimeAsFileTime(&ft); + LARGE_INTEGER start_li; + start_li.LowPart = ft.dwLowDateTime; + start_li.HighPart = ft.dwHighDateTime; + // why did they choose 1601 as the time zero, instead of 1970? + // there were no outstanding hard rock bands in 1601. +#ifdef _MSC_VER + start_li.QuadPart -= 116444736000000000Ui64; +#else + start_li.QuadPart -= 116444736000000000ULL; +#endif + start.sec = (uint64_t)(start_li.QuadPart / 10000000); // 100-ns units. odd. + start.nsec = (start_li.LowPart % 10000000) * 100; + if (atomic_compare_exchange_strong(&start_ns, 0, RCL_S_TO_NS(start.sec) + start.nsec)) { + // If it matched 0 this call was first to setup, set the cpu_freq and init_cpu_time globals. + init_cpu_time_global = init_cpu_time; + cpu_freq_global = cpu_freq; + } else { + // Another concurrent first call managed to set this up first; reset start so it gets set. + start = {0, 0}; + } + } + if (start.sec == 0 && start.nsec == 0) { + start.sec = RCL_NS_TO_S(start_ns_loaded); + start.nsec = start_ns_loaded % 1000000000; + } + LARGE_INTEGER cur_time; + QueryPerformanceCounter(&cur_time); + LARGE_INTEGER delta_cpu_time; + delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time_global.QuadPart; + double d_delta_cpu_time = delta_cpu_time.QuadPart / (double) cpu_freq_global.QuadPart; + uint64_t delta_sec = (uint64_t) floor(d_delta_cpu_time); + uint64_t delta_nsec = (uint64_t) floor((d_delta_cpu_time - delta_sec) * 1e9); + + *now = start; + now->sec += delta_sec; + now->nsec += delta_nsec; + + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } +#endif + return RCL_RET_OK; +} + +rcl_ret_t +rcl_steady_time_point_now(rcl_steady_time_point_t * now) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); +#ifndef WIN32 + // Unix implementations +#if HAS_CLOCK_GETTIME || defined(__MACH__) + // If clock_gettime is available or on OS X, use a timespec. + struct timespec timespec_now; + __timespec_get_now_monotonic(×pec_now); + if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { + RCL_SET_ERROR_MSG("unexpected negative time"); + return RCL_RET_ERROR; + } + now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; +#else // if HAS_CLOCK_GETTIME || defined(__MACH__) + // Cannot fallback to gettimeofday because it is not monotonic. +#error No monotonic clock detected. +#endif // if HAS_CLOCK_GETTIME || defined(__MACH__) +#else +#endif + return RCL_RET_OK; +} + +#if __cplusplus +} +#endif diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index a8df7ad..cea2edb 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -28,7 +28,7 @@ typedef struct rcl_timer_impl_t { atomic_uintptr_t callback; // This is a duration in nanoseconds. atomic_uint_least64_t period; - // This is an absolute time in nanoseconds since the unix epoch. + // This is a time in nanoseconds since an unspecified time. atomic_uint_least64_t last_call_time; // A flag which indicates if the timer is canceled. atomic_bool canceled; @@ -56,15 +56,15 @@ rcl_timer_init( RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized"); return RCL_RET_ALREADY_INIT; } - rcl_time_t now; - rcl_ret_t now_ret = rcl_time_now(&now); + rcl_steady_time_point_t now_steady; + rcl_ret_t now_ret = rcl_steady_time_point_now(&now_steady); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } rcl_timer_impl_t impl = { .callback = ATOMIC_VAR_INIT((uintptr_t)callback), .period = ATOMIC_VAR_INIT(period), - .last_call_time = ATOMIC_VAR_INIT(rcl_time_to_uint64_t_nanoseconds(&now)), + .last_call_time = ATOMIC_VAR_INIT(now_steady.nanoseconds), .canceled = ATOMIC_VAR_INIT(false), .allocator = allocator, }; @@ -100,16 +100,15 @@ rcl_timer_call(rcl_timer_t * timer) RCL_SET_ERROR_MSG("timer is canceled"); return RCL_RET_TIMER_CANCELED; } - rcl_time_t now; - rcl_ret_t now_ret = rcl_time_now(&now); + rcl_steady_time_point_t now_steady; + rcl_ret_t now_ret = rcl_steady_time_point_now(&now_steady); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } - uint64_t previous_ns = - atomic_exchange(&timer->impl->last_call_time, rcl_time_to_uint64_t_nanoseconds(&now)); - rcl_time_t previous = rcl_time_from_uint64_t_nanoseconds(previous_ns); + uint64_t previous_ns = atomic_exchange(&timer->impl->last_call_time, now_steady.nanoseconds); + uint64_t since_last_call = now_steady.nanoseconds - previous_ns; rcl_timer_callback_t typed_callback = (rcl_timer_callback_t)atomic_load(&timer->impl->callback); - typed_callback(timer, previous); + typed_callback(timer, since_last_call); return RCL_RET_OK; } @@ -134,29 +133,28 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - rcl_time_t now, last_call_time; - rcl_ret_t ret = rcl_time_now(&now); - if (ret != RCL_RET_OK) { - return ret; // rcl error state should already be set. - } - ret = rcl_timer_get_last_call_time(timer, &last_call_time); + rcl_steady_time_point_t now; + rcl_ret_t ret = rcl_steady_time_point_now(&now); if (ret != RCL_RET_OK) { return ret; // rcl error state should already be set. } uint64_t period = atomic_load(&timer->impl->period); - int64_t next_call = rcl_time_to_uint64_t_nanoseconds(&last_call_time) + period; - *time_until_next_call = next_call - rcl_time_to_uint64_t_nanoseconds(&now); + *time_until_next_call = (atomic_load(&timer->impl->last_call_time) + period) - now.nanoseconds; return RCL_RET_OK; } rcl_ret_t -rcl_timer_get_last_call_time(const rcl_timer_t * timer, rcl_time_t * last_call_time) +rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_since_last_call) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(last_call_time, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(time_since_last_call, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - uint64_t last_call_time_ns = atomic_load(&timer->impl->last_call_time); - *last_call_time = rcl_time_from_uint64_t_nanoseconds(last_call_time_ns); + rcl_steady_time_point_t now; + rcl_ret_t ret = rcl_steady_time_point_now(&now); + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } + *time_since_last_call = (now.nanoseconds - atomic_load(&timer->impl->last_call_time)); return RCL_RET_OK; } @@ -221,12 +219,12 @@ rcl_timer_reset(rcl_timer_t * timer) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - rcl_time_t now; - rcl_ret_t now_ret = rcl_time_now(&now); + rcl_steady_time_point_t now; + rcl_ret_t now_ret = rcl_steady_time_point_now(&now); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } - atomic_store(&timer->impl->last_call_time, rcl_time_to_uint64_t_nanoseconds(&now)); + atomic_store(&timer->impl->last_call_time, now.nanoseconds); atomic_store(&timer->impl->canceled, false); return RCL_RET_OK; } diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index c13cf38..d7aff5e 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -355,31 +355,49 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) // Create dummy sets for currently unsupported wait-ables. static rmw_services_t dummy_services = {0, NULL}; static rmw_clients_t dummy_clients = {0, NULL}; - // Calculate the timeout. - int64_t min_timeout = timeout; - if (min_timeout == 0) { // Do not consider timer timeouts if non-blocking. - for (size_t i = 0; i < wait_set->size_of_timers; ++i) { - if (!wait_set->timers[i]) { - continue; // Skip NULL timers. - } - int64_t timer_timeout; - rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout); - if (ret != RCL_RET_OK) { - return ret; // The rcl error state should already be set. - } - if (timer_timeout < min_timeout) { - min_timeout = timer_timeout; + // Calculate the timeout argument. + rmw_time_t * timeout_argument; + rmw_time_t temporary_timeout_storage; + if (timeout < 0) { + // Pass NULL to wait to indicate block indefinitely. + timeout_argument = NULL; + } + if (timeout > 0) { + // Determine the nearest timeout (given or a timer). + uint64_t min_timeout = timeout; + if (min_timeout > 0) { // Do not consider timer timeouts if non-blocking. + for (size_t i = 0; i < wait_set->size_of_timers; ++i) { + if (!wait_set->timers[i]) { + continue; // Skip NULL timers. + } + int64_t timer_timeout; + rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout); + if (ret != RCL_RET_OK) { + return ret; // The rcl error state should already be set. + } + if (timer_timeout < min_timeout) { + min_timeout = timer_timeout; + } } } + // Set that in the temporary storage and point to that for the argument. + temporary_timeout_storage.sec = RCL_NS_TO_S(min_timeout); + temporary_timeout_storage.nsec = min_timeout % 1000000000; + timeout_argument = &temporary_timeout_storage; + } + if (timeout == 0) { + // Then it is non-blocking, so set the temporary storage to 0, 0 and pass it. + temporary_timeout_storage.sec = 0; + temporary_timeout_storage.nsec = 0; + timeout_argument = &temporary_timeout_storage; } - rmw_time_t rmw_timeout = rcl_time_from_int64_t_nanoseconds(min_timeout); // Wait. rmw_ret_t ret = rmw_wait( &wait_set->impl->rmw_subscriptions, &wait_set->impl->rmw_guard_conditions, &dummy_services, &dummy_clients, - &rmw_timeout + timeout_argument ); // Check for error. if (ret != RMW_RET_OK) { diff --git a/rcl/test/memory_tools.cpp b/rcl/test/memory_tools.cpp new file mode 100644 index 0000000..0a5c1e2 --- /dev/null +++ b/rcl/test/memory_tools.cpp @@ -0,0 +1,73 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if defined(__linux__) +/****************************************************************************** + * Begin Linux + *****************************************************************************/ + +// TODO(wjwwood): install custom malloc (and others) for Linux. + +#include "./memory_tools_common.cpp" + +/****************************************************************************** + * End Linux + *****************************************************************************/ +#elif defined(__APPLE__) +/****************************************************************************** + * Begin Apple + *****************************************************************************/ + +// The apple implementation is in a separate shared library, loaded with DYLD_INSERT_LIBRARIES. +// Therefore we do not include the common cpp file here. + +void osx_start_memory_checking(); +void osx_stop_memory_checking(); + +void start_memory_checking() +{ + return osx_start_memory_checking(); +} + +void stop_memory_checking() +{ + return osx_stop_memory_checking(); +} + +/****************************************************************************** + * End Apple + *****************************************************************************/ +#elif defined(WIN32) +/****************************************************************************** + * Begin Windows + *****************************************************************************/ + +// TODO(wjwwood): install custom malloc (and others) for Windows. + +/****************************************************************************** + * End Windows + *****************************************************************************/ +#else +// Default case: do nothing. + +void start_memory_checking() +{ + MALLOC_PRINTF("starting memory checking... not available\n"); +} +void stop_memory_checking() +{ + MALLOC_PRINTF("stopping memory checking... not available\n"); +} + +#endif // !defined(WIN32) diff --git a/rcl/test/memory_tools.hpp b/rcl/test/memory_tools.hpp new file mode 100644 index 0000000..ac20a65 --- /dev/null +++ b/rcl/test/memory_tools.hpp @@ -0,0 +1,50 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Code to do replacing of malloc/free/etc... inspired by: +// https://dxr.mozilla.org/mozilla-central/rev/ +// cc9c6cd756cb744596ba039dcc5ad3065a7cc3ea/memory/build/replace_malloc.c + +#ifndef TEST__MEMORY_TOOLS_HPP_ +#define TEST__MEMORY_TOOLS_HPP_ + +#include + +#include +#include + +typedef std::function UnexpectedCallbackType; + +void start_memory_checking(); + +#define ASSERT_NO_MALLOC(statements) assert_no_malloc_begin(); statements; assert_no_malloc_end(); +void assert_no_malloc_begin(); +void assert_no_malloc_end(); +void set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback); + +#define ASSERT_NO_REALLOC(statements) assert_no_realloc_begin(); statements; assert_no_realloc_end(); +void assert_no_realloc_begin(); +void assert_no_realloc_end(); +void set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback); + +#define ASSERT_NO_FREE(statements) assert_no_free_begin(); statements; assert_no_free_end(); +void assert_no_free_begin(); +void assert_no_free_end(); +void set_on_unepexcted_free_callback(UnexpectedCallbackType callback); + +void stop_memory_checking(); + +void memory_checking_thread_init(); + +#endif // TEST__MEMORY_TOOLS_HPP_ diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools_common.cpp new file mode 100644 index 0000000..731db36 --- /dev/null +++ b/rcl/test/memory_tools_common.cpp @@ -0,0 +1,150 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#if defined(__APPLE__) +#include +#define MALLOC_PRINTF malloc_printf +#else +#define MALLOC_PRINTF printf +#endif + +#include "./memory_tools.hpp" +#include "./scope_exit.hpp" + +static std::atomic enabled(false); + +static __thread bool malloc_expected = true; +static __thread UnexpectedCallbackType * unexpected_malloc_callback = nullptr; +void set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback) +{ + if (unexpected_malloc_callback) { + unexpected_malloc_callback->~UnexpectedCallbackType(); + free(unexpected_malloc_callback); + unexpected_malloc_callback = nullptr; + } + if (!callback) { + return; + } + if (!unexpected_malloc_callback) { + unexpected_malloc_callback = (UnexpectedCallbackType *)malloc(sizeof(UnexpectedCallbackType)); + if (!unexpected_malloc_callback) { + throw std::bad_alloc(); + } + new (unexpected_malloc_callback) UnexpectedCallbackType(); + } + *unexpected_malloc_callback = callback; +} + +void * +custom_malloc(size_t size) +{ + if (!enabled.load()) return malloc(size); + auto foo = SCOPE_EXIT(enabled.store(true);); + enabled.store(false); + if (!malloc_expected) { + if (unexpected_malloc_callback) { + (*unexpected_malloc_callback)(); + } + } + void * memory = malloc(size); + MALLOC_PRINTF( + "malloc expected(%s): %p %d\n", malloc_expected ? "true " : "false", memory, size); + return memory; +} + +static __thread bool realloc_expected = true; +static __thread UnexpectedCallbackType * unexpected_realloc_callback = nullptr; +void set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback) +{ + if (unexpected_realloc_callback) { + unexpected_realloc_callback->~UnexpectedCallbackType(); + free(unexpected_realloc_callback); + unexpected_realloc_callback = nullptr; + } + if (!callback) { + return; + } + if (!unexpected_realloc_callback) { + unexpected_realloc_callback = (UnexpectedCallbackType *)malloc(sizeof(UnexpectedCallbackType)); + if (!unexpected_realloc_callback) { + throw std::bad_alloc(); + } + new (unexpected_realloc_callback) UnexpectedCallbackType(); + } + *unexpected_realloc_callback = callback; +} + +void * +custom_realloc(void * memory_in, size_t size) +{ + if (!enabled.load()) return realloc(memory_in, size); + auto foo = SCOPE_EXIT(enabled.store(true);); + enabled.store(false); + if (!realloc_expected) { + if (unexpected_realloc_callback) { + (*unexpected_realloc_callback)(); + } + } + void * memory = realloc(memory_in, size); + MALLOC_PRINTF( + "realloc expected(%s): %p %p %d\n", + malloc_expected ? "true " : "false", memory_in, memory, size); + return memory; +} + +static __thread bool free_expected = true; +static __thread UnexpectedCallbackType * unexpected_free_callback = nullptr; +void set_on_unepexcted_free_callback(UnexpectedCallbackType callback) +{ + if (unexpected_free_callback) { + unexpected_free_callback->~UnexpectedCallbackType(); + free(unexpected_free_callback); + unexpected_free_callback = nullptr; + } + if (!callback) { + return; + } + if (!unexpected_free_callback) { + unexpected_free_callback = (UnexpectedCallbackType *)malloc(sizeof(UnexpectedCallbackType)); + if (!unexpected_free_callback) { + throw std::bad_alloc(); + } + new (unexpected_free_callback) UnexpectedCallbackType(); + } + *unexpected_free_callback = callback; +} + +void +custom_free(void * memory) +{ + if (!enabled.load()) return free(memory); + auto foo = SCOPE_EXIT(enabled.store(true);); + enabled.store(false); + if (!free_expected) { + if (unexpected_free_callback) { + (*unexpected_free_callback)(); + } + } + free(memory); + MALLOC_PRINTF("free expected(%s): %p\n", malloc_expected ? "true " : "false", memory); +} + +void assert_no_malloc_begin() {malloc_expected = false;} +void assert_no_malloc_end() {malloc_expected = true;} +void assert_no_realloc_begin() {realloc_expected = false;} +void assert_no_realloc_end() {realloc_expected = true;} +void assert_no_free_begin() {free_expected = false;} +void assert_no_free_end() {free_expected = true;} diff --git a/rcl/test/memory_tools_osx_interpose.cpp b/rcl/test/memory_tools_osx_interpose.cpp new file mode 100644 index 0000000..980b005 --- /dev/null +++ b/rcl/test/memory_tools_osx_interpose.cpp @@ -0,0 +1,54 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Pulled from: +// https://github.com/emeryberger/Heap-Layers/blob/ +// 076e9e7ef53b66380b159e40473b930f25cc353b/wrappers/macinterpose.h + +// The interposition data structure (just pairs of function pointers), +// used an interposition table like the following: +// + +typedef struct interpose_s { + void * new_func; + void * orig_func; +} interpose_t; + +#define OSX_INTERPOSE(newf,oldf) \ + __attribute__((used)) static const interpose_t \ + macinterpose##newf##oldf __attribute__ ((section("__DATA, __interpose"))) = { \ + (void *)newf, \ + (void *)oldf, \ + } + +// End Interpose. + +#include "./memory_tools_common.cpp" + +void osx_start_memory_checking() +{ + // No loading required, it is handled by DYLD_INSERT_LIBRARIES and dynamic library interposing. + MALLOC_PRINTF("starting memory checking...\n"); + enabled.store(true); +} + +void osx_stop_memory_checking() +{ + MALLOC_PRINTF("stopping memory checking...\n"); + enabled.store(false); +} + +OSX_INTERPOSE(custom_malloc, malloc); +OSX_INTERPOSE(custom_realloc, realloc); +OSX_INTERPOSE(custom_free, free); diff --git a/rcl/test/rcl/test_allocator.cpp b/rcl/test/rcl/test_allocator.cpp new file mode 100644 index 0000000..f63ec18 --- /dev/null +++ b/rcl/test/rcl/test_allocator.cpp @@ -0,0 +1,74 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/allocator.h" + +#include "../memory_tools.hpp" + +class TestAllocatorFixture : public ::testing::Test { +public: + TestAllocatorFixture() + { + start_memory_checking(); + stop_memory_checking(); + } + void SetUp() + { + set_on_unepexcted_malloc_callback([]() {EXPECT_FALSE(true) << "unexpected malloc";}); + set_on_unepexcted_realloc_callback([]() {EXPECT_FALSE(true) << "unexpected realloc";}); + set_on_unepexcted_free_callback([]() {EXPECT_FALSE(true) << "unexpected free";}); + start_memory_checking(); + } + + void TearDown() + { + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + set_on_unepexcted_malloc_callback(nullptr); + set_on_unepexcted_realloc_callback(nullptr); + set_on_unepexcted_free_callback(nullptr); + } +}; + +/* Tests the default allocator. + */ +TEST_F(TestAllocatorFixture, test_default_allocator_normal) +{ + ASSERT_NO_MALLOC( + rcl_allocator_t allocator = rcl_get_default_allocator(); + ) + size_t mallocs = 0; + size_t reallocs = 0; + size_t frees = 0; + set_on_unepexcted_malloc_callback([&mallocs]() {mallocs++;}); + set_on_unepexcted_realloc_callback([&reallocs]() {reallocs++;}); + set_on_unepexcted_free_callback([&frees]() {frees++;}); + assert_no_malloc_begin(); + assert_no_realloc_begin(); + assert_no_free_begin(); + void * allocated_memory = allocator.allocate(1024, allocator.state); + EXPECT_EQ(mallocs, 1); + EXPECT_NE(allocated_memory, nullptr); + allocated_memory = allocator.reallocate(allocated_memory, 2048, allocator.state); + EXPECT_EQ(reallocs, 1); + EXPECT_NE(allocated_memory, nullptr); + allocator.deallocate(allocated_memory, allocator.state); + EXPECT_EQ(mallocs, 1); + EXPECT_EQ(reallocs, 1); + EXPECT_EQ(frees, 1); +} diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp new file mode 100644 index 0000000..c86b74a --- /dev/null +++ b/rcl/test/rcl/test_time.cpp @@ -0,0 +1,110 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rcl/error_handling.h" +#include "rcl/time.h" + +#include "../memory_tools.hpp" + +class TestTimeFixture : public ::testing::Test { +public: + void SetUp() + { + set_on_unepexcted_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";}); + set_on_unepexcted_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";}); + set_on_unepexcted_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";}); + start_memory_checking(); + } + + void TearDown() + { + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + set_on_unepexcted_malloc_callback(nullptr); + set_on_unepexcted_realloc_callback(nullptr); + set_on_unepexcted_free_callback(nullptr); + } +}; + +/* Tests the rcl_system_time_point_now() function. + */ +TEST_F(TestTimeFixture, test_rcl_system_time_point_now) +{ + assert_no_realloc_begin(); + rcl_ret_t ret = RCL_RET_OK; + // Check for invalid argument error condition (allowed to alloc). + ret = rcl_system_time_point_now(nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); + rcl_reset_error(); + assert_no_malloc_begin(); + assert_no_free_begin(); + // Check for normal operation (not allowed to alloc). + rcl_system_time_point_t now = {0}; + ret = rcl_system_time_point_now(&now); + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + EXPECT_NE(now.nanoseconds, 0); + // Compare to std::chrono::system_clock time (within a second). + now = {0}; + ret = rcl_system_time_point_now(&now); + { + std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now(); + auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); + int64_t now_ns_int = now_ns.count(); + int64_t now_diff = now.nanoseconds - now_ns_int; + EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(100)) << "system_clock differs"; + } +} + +/* Tests the rcl_steady_time_point_now() function. + */ +TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) +{ + assert_no_realloc_begin(); + rcl_ret_t ret = RCL_RET_OK; + // Check for invalid argument error condition (allowed to alloc). + ret = rcl_steady_time_point_now(nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); + rcl_reset_error(); + assert_no_malloc_begin(); + assert_no_free_begin(); + // Check for normal operation (not allowed to alloc). + rcl_steady_time_point_t now = {0}; + ret = rcl_steady_time_point_now(&now); + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + EXPECT_NE(now.nanoseconds, 0); + // Compare to std::chrono::steady_clock time (within a second). + now = {0}; + ret = rcl_steady_time_point_now(&now); + { + std::chrono::steady_clock::time_point now_sc = std::chrono::steady_clock::now(); + auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); + int64_t now_ns_int = now_ns.count(); + int64_t now_diff = now.nanoseconds - now_ns_int; + EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(100)) << "steady_clock differs"; + } +} diff --git a/rcl/test/scope_exit.hpp b/rcl/test/scope_exit.hpp new file mode 100644 index 0000000..cb12fce --- /dev/null +++ b/rcl/test/scope_exit.hpp @@ -0,0 +1,40 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST__SCOPE_EXIT_HPP_ +#define TEST__SCOPE_EXIT_HPP_ + +#include + +template +struct ScopeExit +{ + explicit ScopeExit(Callable callable) + : callable_(callable) {} + ~ScopeExit() {callable_(); } + +private: + Callable callable_; +}; + +template +ScopeExit +make_scope_exit(Callable callable) +{ + return ScopeExit(callable); +} + +#define SCOPE_EXIT(code) make_scope_exit([&]() {code; }) + +#endif // TEST__SCOPE_EXIT_HPP_ diff --git a/rcl/test/test_memory_tools.cpp b/rcl/test/test_memory_tools.cpp new file mode 100644 index 0000000..770991e --- /dev/null +++ b/rcl/test/test_memory_tools.cpp @@ -0,0 +1,119 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "./memory_tools.hpp" + +/* Tests the allocatation checking tools. + */ +TEST(TestMemoryTools, test_allocation_checking_tools) +{ + size_t unexpected_mallocs = 0; + auto on_unexpected_malloc = [&unexpected_mallocs]() {unexpected_mallocs++;}; + set_on_unepexcted_malloc_callback(on_unexpected_malloc); + size_t unexpected_reallocs = 0; + auto on_unexpected_realloc = [&unexpected_reallocs]() {unexpected_reallocs++;}; + set_on_unepexcted_realloc_callback(on_unexpected_realloc); + size_t unexpected_frees = 0; + auto on_unexpected_free = [&unexpected_frees]() {unexpected_frees++;}; + set_on_unepexcted_free_callback(on_unexpected_free); + void * mem = nullptr; + // First try before enabling, should have no effect. + mem = malloc(1024); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + ASSERT_NE(mem, nullptr); + free(mem); + EXPECT_EQ(unexpected_mallocs, 0); + EXPECT_EQ(unexpected_reallocs, 0); + EXPECT_EQ(unexpected_frees, 0); + // Enable checking, but no assert, should have no effect. + start_memory_checking(); + mem = malloc(1024); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + ASSERT_NE(mem, nullptr); + free(mem); + EXPECT_EQ(unexpected_mallocs, 0); + EXPECT_EQ(unexpected_reallocs, 0); + EXPECT_EQ(unexpected_frees, 0); + // Enable no_* asserts, should increment all once. + assert_no_malloc_begin(); + assert_no_realloc_begin(); + assert_no_free_begin(); + mem = malloc(1024); + assert_no_malloc_end(); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + assert_no_realloc_end(); + ASSERT_NE(mem, nullptr); + free(mem); + assert_no_free_end(); + EXPECT_EQ(unexpected_mallocs, 1); + EXPECT_EQ(unexpected_reallocs, 1); + EXPECT_EQ(unexpected_frees, 1); + // Enable on malloc assert, only malloc should increment. + assert_no_malloc_begin(); + mem = malloc(1024); + assert_no_malloc_end(); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + ASSERT_NE(mem, nullptr); + free(mem); + EXPECT_EQ(unexpected_mallocs, 2); + EXPECT_EQ(unexpected_reallocs, 1); + EXPECT_EQ(unexpected_frees, 1); + // Enable on realloc assert, only realloc should increment. + assert_no_realloc_begin(); + mem = malloc(1024); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + assert_no_realloc_end(); + ASSERT_NE(mem, nullptr); + free(mem); + EXPECT_EQ(unexpected_mallocs, 2); + EXPECT_EQ(unexpected_reallocs, 2); + EXPECT_EQ(unexpected_frees, 1); + // Enable on free assert, only free should increment. + assert_no_free_begin(); + mem = malloc(1024); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + ASSERT_NE(mem, nullptr); + free(mem); + assert_no_free_end(); + EXPECT_EQ(unexpected_mallocs, 2); + EXPECT_EQ(unexpected_reallocs, 2); + EXPECT_EQ(unexpected_frees, 2); + // Go again, after disabling asserts, should have no effect. + mem = malloc(1024); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + ASSERT_NE(mem, nullptr); + free(mem); + EXPECT_EQ(unexpected_mallocs, 2); + EXPECT_EQ(unexpected_reallocs, 2); + EXPECT_EQ(unexpected_frees, 2); + // Go once more after disabling everything, should have no effect. + stop_memory_checking(); + mem = malloc(1024); + ASSERT_NE(mem, nullptr); + mem = realloc(mem, 2048); + ASSERT_NE(mem, nullptr); + free(mem); + EXPECT_EQ(unexpected_mallocs, 2); + EXPECT_EQ(unexpected_reallocs, 2); + EXPECT_EQ(unexpected_frees, 2); +} From fb9f66044396bfd8d1c57cf58d5758afc64965c6 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 10 Dec 2015 16:28:37 -0800 Subject: [PATCH 19/71] fixup; match the comment --- rcl/test/rcl/test_time.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index c86b74a..7c259bd 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -72,7 +72,7 @@ TEST_F(TestTimeFixture, test_rcl_system_time_point_now) auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); int64_t now_ns_int = now_ns.count(); int64_t now_diff = now.nanoseconds - now_ns_int; - EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(100)) << "system_clock differs"; + EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(1000)) << "system_clock differs"; } } @@ -105,6 +105,6 @@ TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); int64_t now_ns_int = now_ns.count(); int64_t now_diff = now.nanoseconds - now_ns_int; - EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(100)) << "steady_clock differs"; + EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(1000)) << "steady_clock differs"; } } From bd3e4e8f589e8ea86a9b804a1663866f98222c9f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 10 Dec 2015 17:06:18 -0800 Subject: [PATCH 20/71] cpplint --- rcl/CMakeLists.txt | 2 +- rcl/include/rcl/allocator.h | 5 ++- rcl/include/rcl/guard_condition.h | 6 ++- rcl/include/rcl/node.h | 6 ++- rcl/include/rcl/publisher.h | 6 ++- rcl/include/rcl/subscription.h | 6 ++- rcl/include/rcl/time.h | 9 ++-- rcl/include/rcl/timer.h | 3 +- rcl/include/rcl/wait.h | 3 +- rcl/src/rcl/common.h | 4 +- rcl/src/rcl/guard_condition.c | 3 +- rcl/src/rcl/node.c | 5 ++- rcl/src/rcl/publisher.c | 3 +- rcl/src/rcl/rcl.c | 2 +- rcl/src/rcl/subscription.c | 3 +- rcl/src/rcl/time_win32.c | 2 +- rcl/src/rcl/timer.c | 3 +- rcl/src/rcl/wait.c | 55 ++++++++++++------------- rcl/test/memory_tools.hpp | 21 ++++++---- rcl/test/memory_tools_common.cpp | 15 ++++--- rcl/test/memory_tools_osx_interpose.cpp | 11 ++--- rcl/test/rcl/test_allocator.cpp | 18 +++++--- rcl/test/rcl/test_time.cpp | 9 ++-- rcl/test/scope_exit.hpp | 6 +-- rcl/test/test_memory_tools.cpp | 15 ++++--- 25 files changed, 128 insertions(+), 93 deletions(-) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 4a7d20c..c60faf3 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -41,7 +41,7 @@ if(AMENT_ENABLE_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - set(extra_test_libraries ) + set(extra_test_libraries) ament_find_gtest() # For GTEST_LIBRARIES if(APPLE) add_library(${PROJECT_NAME}_memory_tools_interpose SHARED test/memory_tools_osx_interpose.cpp) diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index b47313f..0e1b2d6 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -24,12 +24,13 @@ extern "C" /// Encapsulation of an allocator. /* To use malloc, free, and realloc use rcl_get_default_allocator */ -typedef struct rcl_allocator_t { +typedef struct rcl_allocator_t +{ /// Allocate memory, given a size and state structure. /* An error should be indicated by returning null. */ void * (*allocate)(size_t size, void * state); /// Deallocate previously allocated memory, mimicking free(). - void (*deallocate)(void * pointer, void * state); + void (* deallocate)(void * pointer, void * state); /// Reallocates if possible, otherwise it deallocates and allocates. /* If unsupported then do deallocate and then allocate. * This should behave as realloc is described, as opposed to reallocf, i.e. diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index 2f150ac..de46b93 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/rcl/include/rcl/guard_condition.h @@ -26,12 +26,14 @@ extern "C" struct rcl_guard_condition_impl_t; /// Handle for a rcl guard condition. -typedef struct rcl_guard_condition_t { +typedef struct rcl_guard_condition_t +{ struct rcl_guard_condition_impl_t * impl; } rcl_guard_condition_t; /// Options available for a rcl guard condition. -typedef struct rcl_guard_condition_options_t { +typedef struct rcl_guard_condition_options_t +{ /// Custom allocator for the guard condition, used for incidental allocations. /* For default behavior (malloc/free), use: rcl_get_default_allocator() */ rcl_allocator_t allocator; diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 02ab38e..78a1fe9 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -29,12 +29,14 @@ extern "C" struct rcl_node_impl_t; /// Handle for a ROS node. -typedef struct rcl_node_t { +typedef struct rcl_node_t +{ /// Private implementation pointer. struct rcl_node_impl_t * impl; } rcl_node_t; -typedef struct rcl_node_options_t { +typedef struct rcl_node_options_t +{ // bool anonymous_name; // rmw_qos_profile_t parameter_qos; /// If true, no parameter infrastructure will be setup. diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 6f20616..bf57d1b 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -28,12 +28,14 @@ extern "C" struct rcl_publisher_impl_t; /// Handle for a rcl publisher. -typedef struct rcl_publisher_t { +typedef struct rcl_publisher_t +{ struct rcl_publisher_impl_t * impl; } rcl_publisher_t; /// Options available for a rcl publisher. -typedef struct rcl_publisher_options_t { +typedef struct rcl_publisher_options_t +{ /// Middleware quality of service settings for the publisher. rmw_qos_profile_t qos; /// Custom allocator for the publisher, used for incidental allocations. diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 0b212d2..8d6e6f7 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -28,12 +28,14 @@ extern "C" struct rcl_subscription_impl_t; /// Handle for a rcl subscription. -typedef struct rcl_subscription_t { +typedef struct rcl_subscription_t +{ struct rcl_subscription_impl_t * impl; } rcl_subscription_t; /// Options available for a rcl subscription. -typedef struct rcl_subscription_options_t { +typedef struct rcl_subscription_options_t +{ /// Middleware quality of service settings for the subscription. rmw_qos_profile_t qos; /// If true, messages published from within the same node are ignored. diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index fddbce4..b12f508 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -42,7 +42,8 @@ extern "C" * The struct is capable of representing any time until the year 2554 with * nanosecond precisions. */ -typedef struct rcl_system_time_point_t { +typedef struct rcl_system_time_point_t +{ uint64_t nanoseconds; } rcl_system_time_point_t; @@ -56,7 +57,8 @@ typedef struct rcl_system_time_point_t { * * The struct represents time as nanoseconds in an unsigned 64-bit integer. */ -typedef struct rcl_steady_time_point_t { +typedef struct rcl_steady_time_point_t +{ uint64_t nanoseconds; } rcl_steady_time_point_t; @@ -64,7 +66,8 @@ typedef struct rcl_steady_time_point_t { /* The struct can represent any time within the range [~292 years, ~-292 years] * with nanosecond precision. */ -typedef struct rcl_duration_t { +typedef struct rcl_duration_t +{ int64_t nanoseconds; } rcl_duration_t; diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index ca921e4..2298741 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -30,7 +30,8 @@ extern "C" struct rcl_timer_impl_t; /// Handle for a ROS timer. -typedef struct rcl_timer_t { +typedef struct rcl_timer_t +{ /// Private implementation pointer. struct rcl_timer_impl_t * impl; } rcl_timer_t; diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index eb75555..55e926b 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -31,7 +31,8 @@ extern "C" struct rcl_wait_set_impl_t; /// Container for subscription's, guard condition's, etc to be waited on. -typedef struct rcl_wait_set_t { +typedef struct rcl_wait_set_t +{ /// Storage for subscription pointers. const rcl_subscription_t ** subscriptions; size_t size_of_subscriptions; diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h index 379611f..03bf242 100644 --- a/rcl/src/rcl/common.h +++ b/rcl/src/rcl/common.h @@ -27,8 +27,8 @@ extern "C" RCL_CHECK_FOR_NULL_WITH_MSG(argument, #argument " argument is null", return error_return_type) #define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) if (!value) { \ - RCL_SET_ERROR_MSG(msg); \ - error_statement; \ + RCL_SET_ERROR_MSG(msg); \ + error_statement; \ } // This value put in env_value is only valid until the next call to rcl_impl_getenv (on Windows). diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c index 595c58f..f081d61 100644 --- a/rcl/src/rcl/guard_condition.c +++ b/rcl/src/rcl/guard_condition.c @@ -23,7 +23,8 @@ extern "C" #include "rcl/rcl.h" #include "rmw/rmw.h" -typedef struct rcl_guard_condition_impl_t { +typedef struct rcl_guard_condition_impl_t +{ rmw_guard_condition_t * rmw_handle; rcl_guard_condition_options_t options; } rcl_guard_condition_impl_t; diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index d6e94b3..57dab8e 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -28,7 +28,8 @@ extern "C" #include "./common.h" -typedef struct rcl_node_impl_t { +typedef struct rcl_node_impl_t +{ rcl_node_options_t options; rmw_node_t * rmw_node_handle; uint64_t rcl_instance_id; @@ -80,7 +81,7 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o goto fail; } if (ros_domain_id) { - unsigned long number = strtoul(ros_domain_id, NULL, 0); + unsigned long number = strtoul(ros_domain_id, NULL, 0); // NOLINT(runtime/int) if (number == ULONG_MAX) { RCL_SET_ERROR_MSG("failed to interpret ROS_DOMAIN_ID as integral number"); goto fail; diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index c7dc189..d0c2a67 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -24,7 +24,8 @@ extern "C" #include "./common.h" #include "rmw/rmw.h" -typedef struct rcl_publisher_impl_t { +typedef struct rcl_publisher_impl_t +{ rcl_publisher_options_t options; rmw_publisher_t * rmw_handle; } rcl_publisher_impl_t; diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index 537b975..53d4b25 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -65,7 +65,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator) memset(__rcl_argv, 0, sizeof(char **) * argc); for (size_t i = 0; i < argc; ++i) { __rcl_argv[i] = (char *)__rcl_allocator.allocate(strlen(argv[i]), __rcl_allocator.state); - strcpy(__rcl_argv[i], argv[i]); + strcpy(__rcl_argv[i], argv[i]); // NOLINT(runtime/printf) } atomic_store(&__rcl_instance_id, ++__rcl_next_unique_id); if (atomic_load(&__rcl_instance_id) == 0) { diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 6e8b3a0..57a6b8c 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -22,7 +22,8 @@ extern "C" #include "rmw/rmw.h" #include "./common.h" -typedef struct rcl_subscription_impl_t { +typedef struct rcl_subscription_impl_t +{ rcl_subscription_options_t options; rmw_subscription_t * rmw_handle; } rcl_subscription_impl_t; diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c index dcc8c20..b57a552 100644 --- a/rcl/src/rcl/time_win32.c +++ b/rcl/src/rcl/time_win32.c @@ -155,7 +155,7 @@ rcl_system_time_point_now(rcl_system_time_point_t * now) #else start_li.QuadPart -= 116444736000000000ULL; #endif - start.sec = (uint64_t)(start_li.QuadPart / 10000000); // 100-ns units. odd. + start.sec = (uint64_t)(start_li.QuadPart / 10000000); // 100-ns units. odd. start.nsec = (start_li.LowPart % 10000000) * 100; if (atomic_compare_exchange_strong(&start_ns, 0, RCL_S_TO_NS(start.sec) + start.nsec)) { // If it matched 0 this call was first to setup, set the cpu_freq and init_cpu_time globals. diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index cea2edb..a8c74fa 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -23,7 +23,8 @@ extern "C" #include "./common.h" -typedef struct rcl_timer_impl_t { +typedef struct rcl_timer_impl_t +{ // The user supplied callback. atomic_uintptr_t callback; // This is a duration in nanoseconds. diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index d7aff5e..25d4807 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -29,7 +29,8 @@ extern "C" #include "rmw/rmw.h" #include "./common.h" -typedef struct rcl_wait_set_impl_t { +typedef struct rcl_wait_set_impl_t +{ size_t subscription_index; rmw_subscriptions_t rmw_subscriptions; size_t guard_condition_index; @@ -173,16 +174,16 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al RCL_SET_ERROR_MSG("wait set is invalid"); \ return RCL_RET_WAIT_SET_INVALID; \ } \ - if (!(wait_set->impl->Type##_index < wait_set->size_of_##Type##s)) { \ + if (!(wait_set->impl->Type ## _index < wait_set->size_of_ ## Type ## s)) { \ RCL_SET_ERROR_MSG(#Type "s set is full"); \ return RCL_RET_WAIT_SET_FULL; \ } \ - size_t current_index = wait_set->impl->Type##_index++; \ - wait_set->Type##s[current_index] = Type; + size_t current_index = wait_set->impl->Type ## _index++; \ + wait_set->Type ## s[current_index] = Type; #define SET_ADD_RMW(Type, RMWStorage) \ /* Also place into rmw storage. */ \ - rmw_##Type##_t * rmw_handle = rcl_##Type##_get_rmw_##Type##_handle(Type); \ + rmw_ ## Type ## _t * rmw_handle = rcl_ ## Type ## _get_rmw_ ## Type ## _handle(Type); \ RCL_CHECK_FOR_NULL_WITH_MSG( \ rmw_handle, rcl_get_error_string_safe(), return RCL_RET_ERROR); \ wait_set->impl->RMWStorage[current_index] = rmw_handle->data; @@ -193,8 +194,8 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al RCL_SET_ERROR_MSG("wait set is invalid"); \ return RCL_RET_WAIT_SET_INVALID; \ } \ - memset(wait_set->Type##s, 0, sizeof(rcl_##Type##_t *) * wait_set->size_of_##Type##s); \ - wait_set->impl->Type##_index = 0; \ + memset(wait_set->Type ## s, 0, sizeof(rcl_ ## Type ## _t *) * wait_set->size_of_ ## Type ## s); \ + wait_set->impl->Type ## _index = 0; \ #define SET_CLEAR_RMW(Type, RMWStorage, RMWCount) \ /* Also clear the rmw storage. */ \ @@ -207,43 +208,42 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \ RCL_CHECK_FOR_NULL_WITH_MSG( \ wait_set->impl, "wait set is invalid", return RCL_RET_WAIT_SET_INVALID); \ - if (size == wait_set->size_of_##Type##s) { \ + if (size == wait_set->size_of_ ## Type ## s) { \ return RCL_RET_OK; \ } \ rcl_allocator_t allocator = wait_set->impl->allocator; \ if (size == 0) { \ - if (wait_set->Type##s) { \ - allocator.deallocate(wait_set->Type##s, allocator.state); \ - wait_set->Type##s = NULL; \ + if (wait_set->Type ## s) { \ + allocator.deallocate(wait_set->Type ## s, allocator.state); \ + wait_set->Type ## s = NULL; \ } \ ExtraDealloc \ - } \ - else { \ - wait_set->size_of_##Type##s = 0; \ - wait_set->Type##s = (const rcl_##Type##_t **)allocator.reallocate( \ - wait_set->Type##s, sizeof(rcl_##Type##_t *) * size, allocator.state); \ + } else { \ + wait_set->size_of_ ## Type ## s = 0; \ + wait_set->Type ## s = (const rcl_ ## Type ## _t * *)allocator.reallocate( \ + wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * size, allocator.state); \ RCL_CHECK_FOR_NULL_WITH_MSG( \ - wait_set->Type##s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \ - wait_set->size_of_##Type##s = size; \ + wait_set->Type ## s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \ + wait_set->size_of_ ## Type ## s = size; \ ExtraRealloc \ } \ return RCL_RET_OK; #define SET_RESIZE_RMW_DEALLOC(RMWStorage, RMWCount) \ /* Also deallocate the rmw storage. */ \ - if (wait_set->impl->RMWStorage) { \ - allocator.deallocate(wait_set->impl->RMWStorage, allocator.state); \ - wait_set->impl->RMWStorage = NULL; \ - } + if (wait_set->impl->RMWStorage) { \ + allocator.deallocate(wait_set->impl->RMWStorage, allocator.state); \ + wait_set->impl->RMWStorage = NULL; \ + } #define SET_RESIZE_RMW_REALLOC(Type, RMWStorage, RMWCount) \ /* Also resize the rmw storage. */ \ wait_set->impl->RMWCount = 0; \ wait_set->impl->RMWStorage = (void **)allocator.reallocate( \ - wait_set->impl->RMWStorage, sizeof(rcl_##Type##_t *) * size, allocator.state); \ + wait_set->impl->RMWStorage, sizeof(rcl_ ## Type ## _t *) * size, allocator.state); \ if (!wait_set->impl->RMWStorage) { \ - allocator.deallocate(wait_set->Type##s, allocator.state); \ - wait_set->size_of_##Type##s = 0; \ + allocator.deallocate(wait_set->Type ## s, allocator.state); \ + wait_set->size_of_ ## Type ## s = 0; \ RCL_SET_ERROR_MSG("allocating memory failed"); \ return RCL_RET_BAD_ALLOC; \ } \ @@ -336,7 +336,7 @@ rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set) rcl_ret_t rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size) { - SET_RESIZE(timer, ;, ;) + SET_RESIZE(timer,;,;) // NOLINT } rcl_ret_t @@ -397,8 +397,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) &wait_set->impl->rmw_guard_conditions, &dummy_services, &dummy_clients, - timeout_argument - ); + timeout_argument); // Check for error. if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); diff --git a/rcl/test/memory_tools.hpp b/rcl/test/memory_tools.hpp index ac20a65..f220baf 100644 --- a/rcl/test/memory_tools.hpp +++ b/rcl/test/memory_tools.hpp @@ -16,29 +16,32 @@ // https://dxr.mozilla.org/mozilla-central/rev/ // cc9c6cd756cb744596ba039dcc5ad3065a7cc3ea/memory/build/replace_malloc.c -#ifndef TEST__MEMORY_TOOLS_HPP_ -#define TEST__MEMORY_TOOLS_HPP_ +#ifndef MEMORY_TOOLS_HPP_ +#define MEMORY_TOOLS_HPP_ #include - -#include #include -typedef std::function UnexpectedCallbackType; +#include + +typedef std::function UnexpectedCallbackType; void start_memory_checking(); -#define ASSERT_NO_MALLOC(statements) assert_no_malloc_begin(); statements; assert_no_malloc_end(); +#define ASSERT_NO_MALLOC(statements) \ + assert_no_malloc_begin(); statements; assert_no_malloc_end(); void assert_no_malloc_begin(); void assert_no_malloc_end(); void set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback); -#define ASSERT_NO_REALLOC(statements) assert_no_realloc_begin(); statements; assert_no_realloc_end(); +#define ASSERT_NO_REALLOC(statements) \ + assert_no_realloc_begin(); statements; assert_no_realloc_end(); void assert_no_realloc_begin(); void assert_no_realloc_end(); void set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback); -#define ASSERT_NO_FREE(statements) assert_no_free_begin(); statements; assert_no_free_end(); +#define ASSERT_NO_FREE(statements) \ + assert_no_free_begin(); statements; assert_no_free_end(); void assert_no_free_begin(); void assert_no_free_end(); void set_on_unepexcted_free_callback(UnexpectedCallbackType callback); @@ -47,4 +50,4 @@ void stop_memory_checking(); void memory_checking_thread_init(); -#endif // TEST__MEMORY_TOOLS_HPP_ +#endif // MEMORY_TOOLS_HPP_ diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools_common.cpp index 731db36..18c3ef0 100644 --- a/rcl/test/memory_tools_common.cpp +++ b/rcl/test/memory_tools_common.cpp @@ -39,7 +39,8 @@ void set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback) return; } if (!unexpected_malloc_callback) { - unexpected_malloc_callback = (UnexpectedCallbackType *)malloc(sizeof(UnexpectedCallbackType)); + unexpected_malloc_callback = + reinterpret_cast(malloc(sizeof(UnexpectedCallbackType))); if (!unexpected_malloc_callback) { throw std::bad_alloc(); } @@ -51,7 +52,7 @@ void set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback) void * custom_malloc(size_t size) { - if (!enabled.load()) return malloc(size); + if (!enabled.load()) {return malloc(size);} auto foo = SCOPE_EXIT(enabled.store(true);); enabled.store(false); if (!malloc_expected) { @@ -78,7 +79,8 @@ void set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback) return; } if (!unexpected_realloc_callback) { - unexpected_realloc_callback = (UnexpectedCallbackType *)malloc(sizeof(UnexpectedCallbackType)); + unexpected_realloc_callback = + reinterpret_cast(malloc(sizeof(UnexpectedCallbackType))); if (!unexpected_realloc_callback) { throw std::bad_alloc(); } @@ -90,7 +92,7 @@ void set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback) void * custom_realloc(void * memory_in, size_t size) { - if (!enabled.load()) return realloc(memory_in, size); + if (!enabled.load()) {return realloc(memory_in, size);} auto foo = SCOPE_EXIT(enabled.store(true);); enabled.store(false); if (!realloc_expected) { @@ -118,7 +120,8 @@ void set_on_unepexcted_free_callback(UnexpectedCallbackType callback) return; } if (!unexpected_free_callback) { - unexpected_free_callback = (UnexpectedCallbackType *)malloc(sizeof(UnexpectedCallbackType)); + unexpected_free_callback = + reinterpret_cast(malloc(sizeof(UnexpectedCallbackType))); if (!unexpected_free_callback) { throw std::bad_alloc(); } @@ -130,7 +133,7 @@ void set_on_unepexcted_free_callback(UnexpectedCallbackType callback) void custom_free(void * memory) { - if (!enabled.load()) return free(memory); + if (!enabled.load()) {return free(memory);} auto foo = SCOPE_EXIT(enabled.store(true);); enabled.store(false); if (!free_expected) { diff --git a/rcl/test/memory_tools_osx_interpose.cpp b/rcl/test/memory_tools_osx_interpose.cpp index 980b005..91579cf 100644 --- a/rcl/test/memory_tools_osx_interpose.cpp +++ b/rcl/test/memory_tools_osx_interpose.cpp @@ -20,16 +20,17 @@ // used an interposition table like the following: // -typedef struct interpose_s { +typedef struct interpose_s +{ void * new_func; void * orig_func; } interpose_t; -#define OSX_INTERPOSE(newf,oldf) \ +#define OSX_INTERPOSE(newf, oldf) \ __attribute__((used)) static const interpose_t \ - macinterpose##newf##oldf __attribute__ ((section("__DATA, __interpose"))) = { \ - (void *)newf, \ - (void *)oldf, \ + macinterpose ## newf ## oldf __attribute__ ((section("__DATA, __interpose"))) = { \ + reinterpret_cast(newf), \ + reinterpret_cast(oldf), \ } // End Interpose. diff --git a/rcl/test/rcl/test_allocator.cpp b/rcl/test/rcl/test_allocator.cpp index f63ec18..0294267 100644 --- a/rcl/test/rcl/test_allocator.cpp +++ b/rcl/test/rcl/test_allocator.cpp @@ -18,7 +18,8 @@ #include "../memory_tools.hpp" -class TestAllocatorFixture : public ::testing::Test { +class TestAllocatorFixture : public::testing::Test +{ public: TestAllocatorFixture() { @@ -47,17 +48,22 @@ public: /* Tests the default allocator. */ -TEST_F(TestAllocatorFixture, test_default_allocator_normal) -{ +TEST_F(TestAllocatorFixture, test_default_allocator_normal) { ASSERT_NO_MALLOC( rcl_allocator_t allocator = rcl_get_default_allocator(); ) size_t mallocs = 0; size_t reallocs = 0; size_t frees = 0; - set_on_unepexcted_malloc_callback([&mallocs]() {mallocs++;}); - set_on_unepexcted_realloc_callback([&reallocs]() {reallocs++;}); - set_on_unepexcted_free_callback([&frees]() {frees++;}); + set_on_unepexcted_malloc_callback([&mallocs]() { + mallocs++; + }); + set_on_unepexcted_realloc_callback([&reallocs]() { + reallocs++; + }); + set_on_unepexcted_free_callback([&frees]() { + frees++; + }); assert_no_malloc_begin(); assert_no_realloc_begin(); assert_no_free_begin(); diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index 7c259bd..99851fe 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -21,7 +21,8 @@ #include "../memory_tools.hpp" -class TestTimeFixture : public ::testing::Test { +class TestTimeFixture : public::testing::Test +{ public: void SetUp() { @@ -45,8 +46,7 @@ public: /* Tests the rcl_system_time_point_now() function. */ -TEST_F(TestTimeFixture, test_rcl_system_time_point_now) -{ +TEST_F(TestTimeFixture, test_rcl_system_time_point_now) { assert_no_realloc_begin(); rcl_ret_t ret = RCL_RET_OK; // Check for invalid argument error condition (allowed to alloc). @@ -78,8 +78,7 @@ TEST_F(TestTimeFixture, test_rcl_system_time_point_now) /* Tests the rcl_steady_time_point_now() function. */ -TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) -{ +TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) { assert_no_realloc_begin(); rcl_ret_t ret = RCL_RET_OK; // Check for invalid argument error condition (allowed to alloc). diff --git a/rcl/test/scope_exit.hpp b/rcl/test/scope_exit.hpp index cb12fce..056e1fa 100644 --- a/rcl/test/scope_exit.hpp +++ b/rcl/test/scope_exit.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TEST__SCOPE_EXIT_HPP_ -#define TEST__SCOPE_EXIT_HPP_ +#ifndef SCOPE_EXIT_HPP_ +#define SCOPE_EXIT_HPP_ #include @@ -37,4 +37,4 @@ make_scope_exit(Callable callable) #define SCOPE_EXIT(code) make_scope_exit([&]() {code; }) -#endif // TEST__SCOPE_EXIT_HPP_ +#endif // SCOPE_EXIT_HPP_ diff --git a/rcl/test/test_memory_tools.cpp b/rcl/test/test_memory_tools.cpp index 770991e..1714c4f 100644 --- a/rcl/test/test_memory_tools.cpp +++ b/rcl/test/test_memory_tools.cpp @@ -18,16 +18,21 @@ /* Tests the allocatation checking tools. */ -TEST(TestMemoryTools, test_allocation_checking_tools) -{ +TEST(TestMemoryTools, test_allocation_checking_tools) { size_t unexpected_mallocs = 0; - auto on_unexpected_malloc = [&unexpected_mallocs]() {unexpected_mallocs++;}; + auto on_unexpected_malloc = ([&unexpected_mallocs]() { + unexpected_mallocs++; + }); set_on_unepexcted_malloc_callback(on_unexpected_malloc); size_t unexpected_reallocs = 0; - auto on_unexpected_realloc = [&unexpected_reallocs]() {unexpected_reallocs++;}; + auto on_unexpected_realloc = ([&unexpected_reallocs]() { + unexpected_reallocs++; + }); set_on_unepexcted_realloc_callback(on_unexpected_realloc); size_t unexpected_frees = 0; - auto on_unexpected_free = [&unexpected_frees]() {unexpected_frees++;}; + auto on_unexpected_free = ([&unexpected_frees]() { + unexpected_frees++; + }); set_on_unepexcted_free_callback(on_unexpected_free); void * mem = nullptr; // First try before enabling, should have no effect. From 902172d4c9b94fe9b16b7fc2d0be4558fd2559d0 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 10 Dec 2015 18:17:44 -0800 Subject: [PATCH 21/71] cppcheck --- rcl/src/rcl/common.c | 2 +- rcl/src/rcl/time_win32.c | 77 --------------------------------- rcl/test/rcl/test_allocator.cpp | 6 +-- rcl/test/test_memory_tools.cpp | 57 ++++++++++++++---------- 4 files changed, 37 insertions(+), 105 deletions(-) diff --git a/rcl/src/rcl/common.c b/rcl/src/rcl/common.c index 563921c..8a22c43 100644 --- a/rcl/src/rcl/common.c +++ b/rcl/src/rcl/common.c @@ -28,7 +28,7 @@ static char __env_buffer[1024]; rcl_ret_t rcl_impl_getenv(const char * env_name, char ** env_value) { - env_value = NULL; + *env_value = NULL; #if !defined(WIN32) *env_value = getenv(env_name); #else diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c index b57a552..8e43c26 100644 --- a/rcl/src/rcl/time_win32.c +++ b/rcl/src/rcl/time_win32.c @@ -21,96 +21,20 @@ extern "C" { #endif -// Appropriate check accorind to: -// http://man7.org/linux/man-pages/man2/clock_gettime.2.html -#define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L) - #include "rcl/time.h" -#include -#include -#include - -#if defined(__MACH__) -#include -#include -#endif -#if defined(WIN32) #include #include -#endif #include "./common.h" #include "rcl/error_handling.h" -static void -__timespec_get_now_system(struct timespec * timespec_now) -{ -#if defined(__MACH__) - // On OS X use clock_get_time. - clock_serv_t cclock; - mach_timespec_t mts; - host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - timespec_now->tv_sec = mts.tv_sec; - timespec_now->tv_nsec = mts.tv_nsec; -#else // if defined(__MACH__) - // Otherwise use clock_gettime. - clock_gettime(CLOCK_REALTIME, timespec_now); -#endif // if defined(__MACH__) -} - -static void -__timespec_get_now_monotonic(struct timespec * timespec_now) -{ -#if defined(__MACH__) - // On OS X use clock_get_time. - clock_serv_t cclock; - mach_timespec_t mts; - host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - timespec_now->tv_sec = mts.tv_sec; - timespec_now->tv_nsec = mts.tv_nsec; -#else // if defined(__MACH__) - // Otherwise use clock_gettime. -#ifdef CLOCK_MONOTONIC_RAW - clock_gettime(CLOCK_MONOTONIC_RAW, timespec_now); -#else - clock_gettime(CLOCK_MONOTONIC, timespec_now); -#endif // CLOCK_MONOTONIC_RAW -#endif // if defined(__MACH__) -} - #define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) rcl_ret_t rcl_system_time_point_now(rcl_system_time_point_t * now) { RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); -#ifndef WIN32 - // Unix implementations -#if HAS_CLOCK_GETTIME || defined(__MACH__) - // If clock_gettime is available or on OS X, use a timespec. - struct timespec timespec_now; - __timespec_get_now_system(×pec_now); - if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { - RCL_SET_ERROR_MSG("unexpected negative time"); - return RCL_RET_ERROR; - } - now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; -#else // if HAS_CLOCK_GETTIME || defined(__MACH__) - // Otherwise we have to fallback to gettimeofday. - struct timeval timeofday; - gettimeofday(&timeofday, NULL); - if (__WOULD_BE_NEGATIVE(timeofday.tv_sec, timeofday.tv_usec)) { - RCL_SET_ERROR_MSG("unexpected negative time"); - return RCL_RET_ERROR; - } - now->nanoseconds = RCL_S_TO_NS(timeofday.tv_sec) + timeofday.tv_usec * 1000; -#endif // if HAS_CLOCK_GETTIME || defined(__MACH__) -#else /* Windows implementation adapted from roscpp_core (3-clause BSD), see: * https://github.com/ros/roscpp_core/blob/0.5.6/rostime/src/time.cpp#L96 * @@ -185,7 +109,6 @@ rcl_system_time_point_now(rcl_system_time_point_t * now) if (ret != RCL_RET_OK) { return ret; // rcl error state should already be set. } -#endif return RCL_RET_OK; } diff --git a/rcl/test/rcl/test_allocator.cpp b/rcl/test/rcl/test_allocator.cpp index 0294267..5477713 100644 --- a/rcl/test/rcl/test_allocator.cpp +++ b/rcl/test/rcl/test_allocator.cpp @@ -28,9 +28,9 @@ public: } void SetUp() { - set_on_unepexcted_malloc_callback([]() {EXPECT_FALSE(true) << "unexpected malloc";}); - set_on_unepexcted_realloc_callback([]() {EXPECT_FALSE(true) << "unexpected realloc";}); - set_on_unepexcted_free_callback([]() {EXPECT_FALSE(true) << "unexpected free";}); + set_on_unepexcted_malloc_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED MALLOC";}); + set_on_unepexcted_realloc_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED REALLOC";}); + set_on_unepexcted_free_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED FREE";}); start_memory_checking(); } diff --git a/rcl/test/test_memory_tools.cpp b/rcl/test/test_memory_tools.cpp index 1714c4f..1c03b03 100644 --- a/rcl/test/test_memory_tools.cpp +++ b/rcl/test/test_memory_tools.cpp @@ -35,12 +35,14 @@ TEST(TestMemoryTools, test_allocation_checking_tools) { }); set_on_unepexcted_free_callback(on_unexpected_free); void * mem = nullptr; + void * remem = nullptr; // First try before enabling, should have no effect. mem = malloc(1024); ASSERT_NE(mem, nullptr); - mem = realloc(mem, 2048); - ASSERT_NE(mem, nullptr); - free(mem); + remem = realloc(mem, 2048); + ASSERT_NE(remem, nullptr); + if (!remem) {free(mem);} + free(remem); EXPECT_EQ(unexpected_mallocs, 0); EXPECT_EQ(unexpected_reallocs, 0); EXPECT_EQ(unexpected_frees, 0); @@ -48,9 +50,10 @@ TEST(TestMemoryTools, test_allocation_checking_tools) { start_memory_checking(); mem = malloc(1024); ASSERT_NE(mem, nullptr); - mem = realloc(mem, 2048); - ASSERT_NE(mem, nullptr); - free(mem); + remem = realloc(mem, 2048); + ASSERT_NE(remem, nullptr); + if (!remem) {free(mem);} + free(remem); EXPECT_EQ(unexpected_mallocs, 0); EXPECT_EQ(unexpected_reallocs, 0); EXPECT_EQ(unexpected_frees, 0); @@ -61,10 +64,11 @@ TEST(TestMemoryTools, test_allocation_checking_tools) { mem = malloc(1024); assert_no_malloc_end(); ASSERT_NE(mem, nullptr); - mem = realloc(mem, 2048); + remem = realloc(mem, 2048); assert_no_realloc_end(); - ASSERT_NE(mem, nullptr); - free(mem); + ASSERT_NE(remem, nullptr); + if (!remem) {free(mem);} + free(remem); assert_no_free_end(); EXPECT_EQ(unexpected_mallocs, 1); EXPECT_EQ(unexpected_reallocs, 1); @@ -74,9 +78,10 @@ TEST(TestMemoryTools, test_allocation_checking_tools) { mem = malloc(1024); assert_no_malloc_end(); ASSERT_NE(mem, nullptr); - mem = realloc(mem, 2048); - ASSERT_NE(mem, nullptr); - free(mem); + remem = realloc(mem, 2048); + ASSERT_NE(remem, nullptr); + if (!remem) {free(mem);} + free(remem); EXPECT_EQ(unexpected_mallocs, 2); EXPECT_EQ(unexpected_reallocs, 1); EXPECT_EQ(unexpected_frees, 1); @@ -84,10 +89,11 @@ TEST(TestMemoryTools, test_allocation_checking_tools) { assert_no_realloc_begin(); mem = malloc(1024); ASSERT_NE(mem, nullptr); - mem = realloc(mem, 2048); + remem = realloc(mem, 2048); assert_no_realloc_end(); - ASSERT_NE(mem, nullptr); - free(mem); + ASSERT_NE(remem, nullptr); + if (!remem) {free(mem);} + free(remem); EXPECT_EQ(unexpected_mallocs, 2); EXPECT_EQ(unexpected_reallocs, 2); EXPECT_EQ(unexpected_frees, 1); @@ -95,9 +101,10 @@ TEST(TestMemoryTools, test_allocation_checking_tools) { assert_no_free_begin(); mem = malloc(1024); ASSERT_NE(mem, nullptr); - mem = realloc(mem, 2048); - ASSERT_NE(mem, nullptr); - free(mem); + remem = realloc(mem, 2048); + ASSERT_NE(remem, nullptr); + if (!remem) {free(mem);} + free(remem); assert_no_free_end(); EXPECT_EQ(unexpected_mallocs, 2); EXPECT_EQ(unexpected_reallocs, 2); @@ -105,9 +112,10 @@ TEST(TestMemoryTools, test_allocation_checking_tools) { // Go again, after disabling asserts, should have no effect. mem = malloc(1024); ASSERT_NE(mem, nullptr); - mem = realloc(mem, 2048); - ASSERT_NE(mem, nullptr); - free(mem); + remem = realloc(mem, 2048); + ASSERT_NE(remem, nullptr); + if (!remem) {free(mem);} + free(remem); EXPECT_EQ(unexpected_mallocs, 2); EXPECT_EQ(unexpected_reallocs, 2); EXPECT_EQ(unexpected_frees, 2); @@ -115,9 +123,10 @@ TEST(TestMemoryTools, test_allocation_checking_tools) { stop_memory_checking(); mem = malloc(1024); ASSERT_NE(mem, nullptr); - mem = realloc(mem, 2048); - ASSERT_NE(mem, nullptr); - free(mem); + remem = realloc(mem, 2048); + ASSERT_NE(remem, nullptr); + if (!remem) {free(mem);} + free(remem); EXPECT_EQ(unexpected_mallocs, 2); EXPECT_EQ(unexpected_reallocs, 2); EXPECT_EQ(unexpected_frees, 2); From 9c0208e4d55bb45ac87217a496afbf6138951b6c Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 10 Dec 2015 18:21:19 -0800 Subject: [PATCH 22/71] clean up time_unix.c --- rcl/src/rcl/time_unix.c | 49 ++++++++++++++--------------------------- 1 file changed, 17 insertions(+), 32 deletions(-) diff --git a/rcl/src/rcl/time_unix.c b/rcl/src/rcl/time_unix.c index 5565d47..c89f295 100644 --- a/rcl/src/rcl/time_unix.c +++ b/rcl/src/rcl/time_unix.c @@ -40,28 +40,6 @@ extern "C" #error no monotonic clock function available #endif -static void -__timespec_get_now_steady(struct timespec * timespec_now) -{ -#if defined(__MACH__) - // On OS X use clock_get_time. - clock_serv_t cclock; - mach_timespec_t mts; - host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - timespec_now->tv_sec = mts.tv_sec; - timespec_now->tv_nsec = mts.tv_nsec; -#else // if defined(__MACH__) - // Otherwise use clock_gettime. -#ifdef CLOCK_MONOTONIC_RAW - clock_gettime(CLOCK_MONOTONIC_RAW, timespec_now); -#else - clock_gettime(CLOCK_MONOTONIC, timespec_now); -#endif // CLOCK_MONOTONIC_RAW -#endif // if defined(__MACH__) -} - #define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) rcl_ret_t @@ -94,23 +72,30 @@ rcl_ret_t rcl_steady_time_point_now(rcl_steady_time_point_t * now) { RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); -#ifndef WIN32 - // Unix implementations -#if HAS_CLOCK_GETTIME || defined(__MACH__) // If clock_gettime is available or on OS X, use a timespec. struct timespec timespec_now; - __timespec_get_now_steady(×pec_now); +#if defined(__MACH__) + // On OS X use clock_get_time. + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + timespec_now.tv_sec = mts.tv_sec; + timespec_now.tv_nsec = mts.tv_nsec; +#else + // Otherwise use clock_gettime. +#ifdef CLOCK_MONOTONIC_RAW + clock_gettime(CLOCK_MONOTONIC_RAW, ×pec_now); +#else + clock_gettime(CLOCK_MONOTONIC, ×pec_now); +#endif // CLOCK_MONOTONIC_RAW +#endif // if defined(__MACH__) if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { RCL_SET_ERROR_MSG("unexpected negative time"); return RCL_RET_ERROR; } now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; -#else // if HAS_CLOCK_GETTIME || defined(__MACH__) - // Cannot fallback to gettimeofday because it is not monotonic. -#error No monotonic clock detected. -#endif // if HAS_CLOCK_GETTIME || defined(__MACH__) -#else -#endif return RCL_RET_OK; } From 6985fc04df082e40687dbe3ae81009ba0b16a497 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 11 Dec 2015 11:25:46 -0800 Subject: [PATCH 23/71] add tests for, and clean up, common --- rcl/CMakeLists.txt | 16 +++++++++++ rcl/src/rcl/common.c | 10 ++++--- rcl/src/rcl/common.h | 20 ++++++++++++-- rcl/src/rcl/node.c | 2 +- rcl/test/rcl/test_common.cpp | 51 ++++++++++++++++++++++++++++++++++++ 5 files changed, 93 insertions(+), 6 deletions(-) create mode 100644 rcl/test/rcl/test_common.cpp diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index c60faf3..b600c5b 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -89,6 +89,22 @@ if(AMENT_ENABLE_TESTING) set_target_properties(test_time PROPERTIES COMPILE_FLAGS "-std=c++11") target_link_libraries(test_time ${PROJECT_NAME} ${extra_test_libraries}) endif() + + ament_add_gtest(test_common + test/rcl/test_common.cpp + ENV + DYLD_INSERT_LIBRARIES=$ + EMPTY_TEST= + NORMAL_TEST=foo + ) + if(TARGET test_common) + target_include_directories(test_common PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + set_target_properties(test_common PROPERTIES COMPILE_FLAGS "-std=c++11") + target_link_libraries(test_common ${PROJECT_NAME} ${extra_test_libraries}) + endif() endif() ament_package() diff --git a/rcl/src/rcl/common.c b/rcl/src/rcl/common.c index 8a22c43..cae778c 100644 --- a/rcl/src/rcl/common.c +++ b/rcl/src/rcl/common.c @@ -22,12 +22,15 @@ extern "C" #include #if defined(WIN32) -static char __env_buffer[1024]; +#define WINDOWS_ENV_BUFFER_SIZE 2048 +static char __env_buffer[WINDOWS_ENV_BUFFER_SIZE]; #endif rcl_ret_t -rcl_impl_getenv(const char * env_name, char ** env_value) +rcl_impl_getenv(const char * env_name, const char ** env_value) { + RCL_CHECK_ARGUMENT_FOR_NULL(env_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(env_value, RCL_RET_INVALID_ARGUMENT); *env_value = NULL; #if !defined(WIN32) *env_value = getenv(env_name); @@ -38,7 +41,8 @@ rcl_impl_getenv(const char * env_name, char ** env_value) RCL_SET_ERROR_MSG("value in env variable too large to read in"); return RCL_RET_ERROR; } - env_value = __env_buffer; + __env_buffer[WINDOWS_ENV_BUFFER_SIZE - 1] = '\0'; + *env_value = __env_buffer; #endif return RCL_RET_OK; } diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h index 03bf242..b27232f 100644 --- a/rcl/src/rcl/common.h +++ b/rcl/src/rcl/common.h @@ -31,9 +31,25 @@ extern "C" error_statement; \ } -// This value put in env_value is only valid until the next call to rcl_impl_getenv (on Windows). +/// Retrieve the value of the given environment variable if it exists, or NULL. +/* The returned char is only valid until the next time this function is called, + * because the returned char * is a direct pointer to the static storage. + * The returned value char * variable should never have free() called on it. + * + * Environment variable values will be truncated at 2048 characters on Windows. + * + * This function does not allocate heap memory, but the system calls might. + * This function is not thread-safe. + * This function is not lock-free. + * + * \param[in] env_name the name of the environment variable + * \param[out] env_value pointer to the value cstring + * \return RCL_RET_OK if the value is retrieved successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ rcl_ret_t -rcl_impl_getenv(const char * env_name, char ** env_value); +rcl_impl_getenv(const char * env_name, const char ** env_value); #if __cplusplus } diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 57dab8e..5ef8a87 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -46,7 +46,7 @@ rcl_ret_t rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options) { size_t domain_id = 0; - char * ros_domain_id; + const char * ros_domain_id; rcl_ret_t ret; rcl_ret_t fail_ret = RCL_RET_ERROR; RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT); diff --git a/rcl/test/rcl/test_common.cpp b/rcl/test/rcl/test_common.cpp new file mode 100644 index 0000000..b4732eb --- /dev/null +++ b/rcl/test/rcl/test_common.cpp @@ -0,0 +1,51 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "../../src/rcl/common.h" + +/* Tests the default allocator. + * + * Expected environment variables must be set by the calling code: + * + * - EMPTY_TEST= + * - NORMAL_TEST=foo + * + * These are set in the call to `ament_add_gtest()` in the `CMakeLists.txt`. + */ +TEST(TestCommon, test_getenv) { + const char * env; + rcl_ret_t ret; + ret = rcl_impl_getenv("NORMAL_TEST", NULL); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + ret = rcl_impl_getenv(NULL, &env); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + ret = rcl_impl_getenv("SHOULD_NOT_EXIST_TEST", &env); + EXPECT_EQ(ret, RCL_RET_OK); + EXPECT_EQ(env, nullptr) << std::string(env); + rcl_reset_error(); + ret = rcl_impl_getenv("NORMAL_TEST", &env); + EXPECT_EQ(ret, RCL_RET_OK); + ASSERT_NE(env, nullptr); + EXPECT_EQ(std::string(env), "foo"); + ret = rcl_impl_getenv("EMPTY_TEST", &env); + EXPECT_EQ(ret, RCL_RET_OK); + ASSERT_NE(env, nullptr); + EXPECT_EQ(std::string(env), ""); +} From f528b7a92584b4e952bc23dc6f6640d90ddee94c Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 11 Dec 2015 13:38:31 -0800 Subject: [PATCH 24/71] fixup guard_condition and other docs, start on time_win32.c --- rcl/include/rcl/allocator.h | 9 ++++---- rcl/include/rcl/guard_condition.h | 36 ++++++++++++++++--------------- rcl/include/rcl/node.h | 11 ++++------ rcl/src/rcl/guard_condition.c | 7 +++--- rcl/src/rcl/time_win32.c | 26 ++++++++-------------- 5 files changed, 40 insertions(+), 49 deletions(-) diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index 0e1b2d6..3dc3034 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -23,16 +23,17 @@ extern "C" #include "rcl/types.h" /// Encapsulation of an allocator. -/* To use malloc, free, and realloc use rcl_get_default_allocator */ +/* To use malloc, free, and realloc use rcl_get_default_allocator(). */ typedef struct rcl_allocator_t { /// Allocate memory, given a size and state structure. - /* An error should be indicated by returning null. */ + /* An error should be indicated by returning NULL. */ void * (*allocate)(size_t size, void * state); /// Deallocate previously allocated memory, mimicking free(). void (* deallocate)(void * pointer, void * state); /// Reallocates if possible, otherwise it deallocates and allocates. /* If unsupported then do deallocate and then allocate. + * \TODO(wjwwood): should this behave as reallocf? * This should behave as realloc is described, as opposed to reallocf, i.e. * the memory given by pointer will not be free'd automatically if realloc * fails. @@ -40,12 +41,12 @@ typedef struct rcl_allocator_t */ void * (*reallocate)(void * pointer, size_t size, void * state); /// Implementation defined state storage. - /* This is passed as the second parameter to the (de)allocate functions. */ + /* This is passed as the second parameter to other allocator functions. */ void * state; } rcl_allocator_t; /// Return a properly initialized rcl_allocator_t with default values. -/* This function does not allocate memory. +/* This function does not allocate heap memory. * This function is thread-safe. * This function is lock-free. */ diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index de46b93..7c6ed0e 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/rcl/include/rcl/guard_condition.h @@ -34,23 +34,17 @@ typedef struct rcl_guard_condition_t /// Options available for a rcl guard condition. typedef struct rcl_guard_condition_options_t { - /// Custom allocator for the guard condition, used for incidental allocations. - /* For default behavior (malloc/free), use: rcl_get_default_allocator() */ + /// Custom allocator for the guard condition, used for internal allocations. rcl_allocator_t allocator; } rcl_guard_condition_options_t; /// Return a rcl_guard_condition_t struct with members set to NULL. -/* Should be called to get a null rcl_guard_condition_t before passing to - * rcl_initalize_guard_condition(). - * It's also possible to use calloc() instead of this if the rcl guard condition is - * being allocated on the heap. - */ rcl_guard_condition_t rcl_get_zero_initialized_guard_condition(); /// Initialize a rcl guard_condition. /* After calling this function on a rcl_guard_condition_t, it can be passed to - * rcl_wait and can be triggered to wake rcl_wait up. + * rcl_wait() and then concurrently it can be triggered to wake-up rcl_wait(). * * The given rcl_node_t must be valid and the resulting rcl_guard_condition_t * is only valid as long as the given rcl_node_t remains valid. @@ -64,15 +58,17 @@ rcl_get_zero_initialized_guard_condition(); * rcl_ret_t ret = rcl_node_init(&node, "node_name", &node_ops); * // ... error handling * rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition(); - * rcl_guard_condition_options_t guard_condition_ops = rcl_guard_condition_get_default_options(); - * ret = rcl_guard_condition_init(&guard_condition, &node, ts, "chatter", &guard_condition_ops); + * ret = rcl_guard_condition_init( + * &guard_condition, &node, rcl_guard_condition_get_default_options()); * // ... error handling, and on shutdown do deinitialization: * ret = rcl_guard_condition_fini(&guard_condition, &node); * // ... error handling for rcl_guard_condition_fini() * ret = rcl_node_fini(&node); * // ... error handling for rcl_deinitialize_node() * + * This function does allocate heap memory. * This function is not thread-safe. + * This function is lock-free. * * \TODO(wjwwood): does this function need a node to be passed to it? (same for fini) * @@ -89,14 +85,16 @@ rcl_ret_t rcl_guard_condition_init( rcl_guard_condition_t * guard_condition, const rcl_node_t * node, - const rcl_guard_condition_options_t * options); + const rcl_guard_condition_options_t options); /// Finalize a rcl_guard_condition_t. -/* After calling, calls to rcl_trigger_guard_condition will fail when using +/* After calling, calls to rcl_guard_condition_trigger() will fail when using * this guard condition. * However, the given node handle is still valid. * - * This function is not thread-safe. + * This function does free heap memory and can allocate memory on errors. + * This function is not thread-safe with rcl_guard_condition_trigger(). + * This function is lock-free. * * \param[inout] guard_condition handle to the guard_condition to be finalized * \param[in] node handle to the node used to create the guard_condition @@ -107,7 +105,11 @@ rcl_guard_condition_init( rcl_ret_t rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * node); -/// Return the default guard_condition options in a rcl_guard_condition_options_t. +/// Return the default options in a rcl_guard_condition_options_t struct. +/* This function does not allocate heap memory. + * This function is thread-safe. + * This function is lock-free. + */ rcl_guard_condition_options_t rcl_guard_condition_get_default_options(); @@ -118,8 +120,10 @@ rcl_guard_condition_get_default_options(); * * A guard condition can be triggered from any thread. * + * This function does not allocate heap memory, but can on errors. * This function is thread-safe with itself, but cannot be called concurrently * with rcl_guard_condition_fini() on the same guard condition. + * This function is lock-free, but the underlying system calls may not be. * * \param[in] guard_condition handle to the guard_condition to be triggered * \return RCL_RET_OK if the guard condition was triggered, or @@ -127,7 +131,7 @@ rcl_guard_condition_get_default_options(); * RCL_RET_ERROR if an unspecified error occurs. */ rcl_ret_t -rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition); +rcl_guard_condition_trigger(const rcl_guard_condition_t * guard_condition); /// Return the rmw guard condition handle. /* The handle returned is a pointer to the internally held rmw handle. @@ -137,8 +141,6 @@ rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition); * * The returned handle is only valid as long as the given guard_condition is valid. * - * \TODO(wjwwood) should the return value of this be const? - * * \param[in] guard_condition pointer to the rcl guard_condition * \return rmw guard_condition handle if successful, otherwise NULL */ diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 78a1fe9..fae9274 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -20,11 +20,12 @@ extern "C" { #endif +#include + #include "rcl/allocator.h" #include "rcl/types.h" -// Intentional underflow to get max size_t. -#define RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID (size_t)-1 +#define RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID SIZE_MAX struct rcl_node_impl_t; @@ -51,15 +52,11 @@ typedef struct rcl_node_options_t * RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID? (currently max size_t) */ size_t domain_id; - /// Custom allocator used for incidental allocations, e.g. node name string. + /// Custom allocator used for internal allocations. rcl_allocator_t allocator; } rcl_node_options_t; /// Return a rcl_node_t struct with members initialized to NULL. -/* Should be called to get rcl_node_t before passing to rcl_node_init(). - * It's also possible to use calloc() instead of this if the rcl_node is being - * allocated on the heap. - */ rcl_node_t rcl_get_zero_initialized_node(); diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c index f081d61..c2e48fd 100644 --- a/rcl/src/rcl/guard_condition.c +++ b/rcl/src/rcl/guard_condition.c @@ -40,13 +40,12 @@ rcl_ret_t rcl_guard_condition_init( rcl_guard_condition_t * guard_condition, const rcl_node_t * node, - const rcl_guard_condition_options_t * options) + const rcl_guard_condition_options_t options) { // Perform argument validation. RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); - const rcl_allocator_t * allocator = &options->allocator; + const rcl_allocator_t * allocator = &options.allocator; RCL_CHECK_FOR_NULL_WITH_MSG( allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG( @@ -77,7 +76,7 @@ rcl_guard_condition_init( return RCL_RET_ERROR; } // Copy options into impl. - guard_condition->impl->options = *options; + guard_condition->impl->options = options; return RCL_RET_OK; } diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c index 8e43c26..7d0f9b9 100644 --- a/rcl/src/rcl/time_win32.c +++ b/rcl/src/rcl/time_win32.c @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifdef WIN32 #ifndef WIN32 #error time_win32.c is only intended to be used with win32 based systems #endif @@ -33,6 +34,10 @@ extern "C" rcl_ret_t rcl_system_time_point_now(rcl_system_time_point_t * now) +{ + +} +#if 0 { RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); /* Windows implementation adapted from roscpp_core (3-clause BSD), see: @@ -111,31 +116,18 @@ rcl_system_time_point_now(rcl_system_time_point_t * now) } return RCL_RET_OK; } +#endif rcl_ret_t rcl_steady_time_point_now(rcl_steady_time_point_t * now) { RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); -#ifndef WIN32 - // Unix implementations -#if HAS_CLOCK_GETTIME || defined(__MACH__) - // If clock_gettime is available or on OS X, use a timespec. - struct timespec timespec_now; - __timespec_get_now_monotonic(×pec_now); - if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { - RCL_SET_ERROR_MSG("unexpected negative time"); - return RCL_RET_ERROR; - } - now->nanoseconds = RCL_S_TO_NS(timespec_now.tv_sec) + timespec_now.tv_nsec; -#else // if HAS_CLOCK_GETTIME || defined(__MACH__) - // Cannot fallback to gettimeofday because it is not monotonic. -#error No monotonic clock detected. -#endif // if HAS_CLOCK_GETTIME || defined(__MACH__) -#else -#endif + WINAPI ret = QueryPerformanceFrequency(); return RCL_RET_OK; } #if __cplusplus } #endif + +#endif From 3e516c8c8f11f01c18cb05c3f785aeb1600a0606 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 11 Dec 2015 17:47:49 -0800 Subject: [PATCH 25/71] windows wip --- rcl/CMakeLists.txt | 44 ++- rcl/src/rcl/common.c | 2 +- rcl/src/rcl/rcl.c | 10 +- .../rcl/stdatomic_helper/win32/stdatomic.h | 325 ++++++++++++++++++ rcl/src/rcl/stdatomics_helper.h | 26 ++ 5 files changed, 386 insertions(+), 21 deletions(-) create mode 100644 rcl/src/rcl/stdatomic_helper/win32/stdatomic.h create mode 100644 rcl/src/rcl/stdatomics_helper.h diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index b600c5b..92d6b8e 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -34,7 +34,9 @@ ament_target_dependencies(${PROJECT_NAME} "rmw" "rosidl_generator_c" ) -set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-std=c11") +if(NOT WIN32) + set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-std=c11") +endif() if(AMENT_ENABLE_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -42,58 +44,64 @@ if(AMENT_ENABLE_TESTING) ament_lint_auto_find_test_dependencies() set(extra_test_libraries) + set(extra_memory_tools_env PHONY=) # Use a phony env var so there is always at least one. ament_find_gtest() # For GTEST_LIBRARIES if(APPLE) add_library(${PROJECT_NAME}_memory_tools_interpose SHARED test/memory_tools_osx_interpose.cpp) target_link_libraries(${PROJECT_NAME}_memory_tools_interpose ${GTEST_LIBRARIES}) - set_target_properties(${PROJECT_NAME}_memory_tools_interpose PROPERTIES COMPILE_FLAGS "-std=c++11") + set_target_properties(${PROJECT_NAME}_memory_tools_interpose + PROPERTIES COMPILE_FLAGS "-std=c++11") list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools_interpose) + list(APPEND extra_memory_tools_env + DYLD_INSERT_LIBRARIES=$) endif() add_library(${PROJECT_NAME}_memory_tools SHARED test/memory_tools.cpp) - set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") + if(NOT WIN32) + set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() target_link_libraries(${PROJECT_NAME}_memory_tools ${GTEST_LIBRARIES} ${extra_test_libraries}) list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools) - ament_add_gtest(test_memory_tools - test/test_memory_tools.cpp - ENV DYLD_INSERT_LIBRARIES=$) + ament_add_gtest(test_memory_tools test/test_memory_tools.cpp ENV ${extra_memory_tools_env}) if(TARGET test_memory_tools) target_include_directories(test_memory_tools PUBLIC ${rcl_interfaces_INCLUDE_DIRS} ${rmw_INCLUDE_DIRS} ) - set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") + if(NOT WIN32) + set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() target_link_libraries(test_memory_tools ${PROJECT_NAME} ${extra_test_libraries}) endif() - ament_add_gtest(test_allocator - test/rcl/test_allocator.cpp - ENV DYLD_INSERT_LIBRARIES=$) + ament_add_gtest(test_allocator test/rcl/test_allocator.cpp ENV ${extra_memory_tools_env}) if(TARGET test_allocator) target_include_directories(test_allocator PUBLIC ${rcl_interfaces_INCLUDE_DIRS} ${rmw_INCLUDE_DIRS} ) - set_target_properties(test_allocator PROPERTIES COMPILE_FLAGS "-std=c++11") + if(NOT WIN32) + set_target_properties(test_allocator PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() target_link_libraries(test_allocator ${PROJECT_NAME} ${extra_test_libraries}) endif() - ament_add_gtest(test_time - test/rcl/test_time.cpp - ENV DYLD_INSERT_LIBRARIES=$) + ament_add_gtest(test_time test/rcl/test_time.cpp ENV ${extra_memory_tools_env}) if(TARGET test_time) target_include_directories(test_time PUBLIC ${rcl_interfaces_INCLUDE_DIRS} ${rmw_INCLUDE_DIRS} ) - set_target_properties(test_time PROPERTIES COMPILE_FLAGS "-std=c++11") + if(NOT WIN32) + set_target_properties(test_time PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() target_link_libraries(test_time ${PROJECT_NAME} ${extra_test_libraries}) endif() ament_add_gtest(test_common test/rcl/test_common.cpp ENV - DYLD_INSERT_LIBRARIES=$ + ${extra_memory_tools_env} EMPTY_TEST= NORMAL_TEST=foo ) @@ -102,7 +110,9 @@ if(AMENT_ENABLE_TESTING) ${rcl_interfaces_INCLUDE_DIRS} ${rmw_INCLUDE_DIRS} ) - set_target_properties(test_common PROPERTIES COMPILE_FLAGS "-std=c++11") + if(NOT WIN32) + set_target_properties(test_common PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() target_link_libraries(test_common ${PROJECT_NAME} ${extra_test_libraries}) endif() endif() diff --git a/rcl/src/rcl/common.c b/rcl/src/rcl/common.c index cae778c..89a667f 100644 --- a/rcl/src/rcl/common.c +++ b/rcl/src/rcl/common.c @@ -36,7 +36,7 @@ rcl_impl_getenv(const char * env_name, const char ** env_value) *env_value = getenv(env_name); #else size_t required_size; - error_t ret = getenv_s(&required_size, __env_buffer, sizeof(__env_buffer), env_name); + errno_t ret = getenv_s(&required_size, __env_buffer, sizeof(__env_buffer), env_name); if (ret != 0) { RCL_SET_ERROR_MSG("value in env variable too large to read in"); return RCL_RET_ERROR; diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index 53d4b25..da8c303 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -19,19 +19,23 @@ extern "C" #include "rcl/rcl.h" +#ifndef WIN32 #include +#else +#include "./stdatomic/win32/stdatomic.h" +#endif #include #include "rcl/error_handling.h" -static atomic_bool __rcl_is_initialized = false; +static atomic_bool __rcl_is_initialized = ATOMIC_VAR_INIT(false); static rcl_allocator_t __rcl_allocator = {0}; static int __rcl_argc = 0; static char ** __rcl_argv = NULL; -static atomic_uint_fast64_t __rcl_instance_id = 0; +static atomic_uint_fast64_t __rcl_instance_id = ATOMIC_VAR_INIT(0); static uint64_t __rcl_next_unique_id = 0; -static inline void +static void __clean_up_init() { if (__rcl_argv) { diff --git a/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h new file mode 100644 index 0000000..0fa16ac --- /dev/null +++ b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h @@ -0,0 +1,325 @@ +/* + * An implementation of C11 stdatomic.h for Win32, part borrowed from FreeBSD + * (original copyright follows), with major modifications for + * portability to Win32 systems. + * + * Caveats and limitations: + * - Only the ``_Atomic parentheses'' notation is implemented, while + * the ``_Atomic space'' one is not. + * - _Atomic types must be typedef'ed, or programs using them will + * not type check correctly (incompatible anonymous structure + * types). + * - Support is limited to 16, 32, and 64 bit types only. + * - Stripped down to only the functions used locally in rcl. + */ + +/*- + * Copyright (c) 2011 Ed Schouten + * David Chisnall + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * $FreeBSD: src/include/stdatomic.h,v 1.10.2.2 2012/05/30 19:21:54 theraven Exp $ + */ + +#ifndef _STDATOMIC_H_ +#define _STDATOMIC_H_ + +#include + +#include +#include + +#if !defined(WIN32) +#error "this stdatomic.h does not support your compiler" +#endif + +// In MSVC, correct alignment of each type is already ensured. +#define _Atomic(T) struct { T __val; } + +/* + * 7.17.2 Initialization. + */ + +#define ATOMIC_VAR_INIT(value) { .__val = (value) } +#define atomic_init(obj, value) do { \ + (obj)->__val = (value); \ +} while (0) + +/* + * Clang and recent GCC both provide predefined macros for the memory + * orderings. If we are using a compiler that doesn't define them, use the + * clang values - these will be ignored in the fallback path. + */ + +#ifndef __ATOMIC_RELAXED +#define __ATOMIC_RELAXED 0 +#endif +#ifndef __ATOMIC_CONSUME +#define __ATOMIC_CONSUME 1 +#endif +#ifndef __ATOMIC_ACQUIRE +#define __ATOMIC_ACQUIRE 2 +#endif +#ifndef __ATOMIC_RELEASE +#define __ATOMIC_RELEASE 3 +#endif +#ifndef __ATOMIC_ACQ_REL +#define __ATOMIC_ACQ_REL 4 +#endif +#ifndef __ATOMIC_SEQ_CST +#define __ATOMIC_SEQ_CST 5 +#endif + +/* + * 7.17.3 Order and consistency. + * + * The memory_order_* constants that denote the barrier behaviour of the + * atomic operations. + */ + +enum memory_order { + memory_order_relaxed = __ATOMIC_RELAXED, + memory_order_consume = __ATOMIC_CONSUME, + memory_order_acquire = __ATOMIC_ACQUIRE, + memory_order_release = __ATOMIC_RELEASE, + memory_order_acq_rel = __ATOMIC_ACQ_REL, + memory_order_seq_cst = __ATOMIC_SEQ_CST +}; + +typedef enum memory_order memory_order; + +/* + * 7.17.4 Fences. + */ + +#define atomic_thread_fence(order) MemoryBarrier() +#define atomic_signal_fence(order) _ReadWriteBarrier() + +/* + * 7.17.5 Lock-free property. + */ + +#define atomic_is_lock_free(obj) (sizeof((obj)->__val) <= sizeof(void *)) + +/* + * 7.17.6 Atomic integer types. + */ + +typedef _Atomic(_Bool) atomic_bool; +typedef _Atomic(char) atomic_char; +typedef _Atomic(signed char) atomic_schar; +typedef _Atomic(unsigned char) atomic_uchar; +typedef _Atomic(short) atomic_short; +typedef _Atomic(unsigned short) atomic_ushort; +typedef _Atomic(int) atomic_int; +typedef _Atomic(unsigned int) atomic_uint; +typedef _Atomic(long) atomic_long; +typedef _Atomic(unsigned long) atomic_ulong; +typedef _Atomic(long long) atomic_llong; +typedef _Atomic(unsigned long long) atomic_ullong; +#if 0 +typedef _Atomic(char16_t) atomic_char16_t; +typedef _Atomic(char32_t) atomic_char32_t; +typedef _Atomic(wchar_t) atomic_wchar_t; +typedef _Atomic(int_least8_t) atomic_int_least8_t; +typedef _Atomic(uint_least8_t) atomic_uint_least8_t; +#endif +typedef _Atomic(int_least16_t) atomic_int_least16_t; +typedef _Atomic(uint_least16_t) atomic_uint_least16_t; +typedef _Atomic(int_least32_t) atomic_int_least32_t; +typedef _Atomic(uint_least32_t) atomic_uint_least32_t; +typedef _Atomic(int_least64_t) atomic_int_least64_t; +typedef _Atomic(uint_least64_t) atomic_uint_least64_t; +#if 0 +typedef _Atomic(int_fast8_t) atomic_int_fast8_t; +typedef _Atomic(uint_fast8_t) atomic_uint_fast8_t; +#endif +typedef _Atomic(int_fast16_t) atomic_int_fast16_t; +typedef _Atomic(uint_fast16_t) atomic_uint_fast16_t; +typedef _Atomic(int_fast32_t) atomic_int_fast32_t; +typedef _Atomic(uint_fast32_t) atomic_uint_fast32_t; +typedef _Atomic(int_fast64_t) atomic_int_fast64_t; +typedef _Atomic(uint_fast64_t) atomic_uint_fast64_t; +typedef _Atomic(intptr_t) atomic_intptr_t; +typedef _Atomic(uintptr_t) atomic_uintptr_t; +typedef _Atomic(size_t) atomic_size_t; +typedef _Atomic(ptrdiff_t) atomic_ptrdiff_t; +typedef _Atomic(intmax_t) atomic_intmax_t; +typedef _Atomic(uintmax_t) atomic_uintmax_t; + +/* + * 7.17.7 Operations on atomic types. (pruned modified for Windows' crappy C compiler) + */ + +#define rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) \ + do { \ + switch(sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedCompareExchange64((LONGLONG *) object, desired, expected); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedCompareExchange((LONG *) object, desired, expected); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedCompareExchange16((SHORT *) object, desired, expected); \ + break; \ + default: \ + break; \ + }; \ + } while(0) + +#define rcl_win32_atomic_compare_exchange_weak(object, out, expected, desired) \ + rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) + +#define rcl_win32_atomic_exchange((object), out, desired) \ + do { \ + switch(sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedExchange64((LONGLONG *) object, desired); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedExchange((LONG *) object, desired); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedExchange16((SHORT *) object, desired); \ + break; \ + default: \ + break; \ + }; \ + } while(0) + +#define rcl_win32_atomic_fetch_add(object, out, operand) \ + do { \ + switch(sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedExchangeAdd64((LONGLONG *) object, operand); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedExchangeAdd((LONG *) object, operand); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedExchangeAdd16((SHORT *) object, operand); \ + break; \ + default: \ + break; \ + }; \ + } while(0) + +#define rcl_win32_atomic_fetch_and(object, out, operand) \ + do { \ + switch(sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedAnd64((LONGLONG *) object, operand); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedAnd((LONG *) object, operand); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedAnd16((SHORT *) object, operand); \ + break; \ + default: \ + break; \ + }; \ + } while(0) + +#define rcl_win32_atomic_fetch_or(object, out, operand) \ + do { \ + switch(sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedOr64((LONGLONG *) object, operand); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedOr((LONG *) object, operand); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedOr16((SHORT *) object, operand); \ + break; \ + default: \ + break; \ + }; \ + } while(0) + +#define rcl_win32_atomic_fetch_sub(object, out, operand) \ + rcl_win32_atomic_fetch_add(object, out, -(operand)) + +#define rcl_win32_atomic_fetch_xor(object, out, operand) \ + do { \ + switch(sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedXor64((LONGLONG *) object, operand); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedXor((LONG *) object, operand); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedXor16((SHORT *) object, operand); \ + break; \ + default: \ + break; \ + }; \ + } while(0) + +#define rcl_win32_atomic_load(object) \ + do { \ + switch(sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedExchangeAdd64((LONGLONG *) object, 0); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedExchangeAdd((LONG *) object, 0); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedExchangeAdd16((SHORT *) object, 0); \ + break; \ + default: \ + break; \ + }; \ + } while(0) + +#define rcl_win32_atomic_store(object, desired) \ + do { \ + MemoryBarrier(); \ + object = (desired); \ + MemoryBarrier(); \ + } while (0) + +/* + * 7.17.8 Atomic flag type and operations. (disabled for now) + */ + +// typedef atomic_bool atomic_flag; + +// #define ATOMIC_FLAG_INIT ATOMIC_VAR_INIT(0) + +// #define atomic_flag_clear_explicit(object, order) \ +// atomic_store_explicit(object, 0, order) +// #define atomic_flag_test_and_set_explicit(object, order) \ +// atomic_compare_exchange_strong_explicit(object, 0, 1, order, order) + +// #define atomic_flag_clear(object) \ +// atomic_flag_clear_explicit(object, memory_order_seq_cst) +// #define atomic_flag_test_and_set(object) \ +// atomic_flag_test_and_set_explicit(object, memory_order_seq_cst) + +#endif /* !_STDATOMIC_H_ */ diff --git a/rcl/src/rcl/stdatomics_helper.h b/rcl/src/rcl/stdatomics_helper.h new file mode 100644 index 0000000..a7d2352 --- /dev/null +++ b/rcl/src/rcl/stdatomics_helper.h @@ -0,0 +1,26 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__STDATOMICS_HELPER_H_ +#define RCL__STDATOMICS_HELPER_H_ + +#if !defined(WIN32) + + + +#else + +#endif + +#endif // RCL__STDATOMICS_HELPER_H_ From 879f0a1ced6b5b0362adbffe4d17db9445945a0a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 11 Dec 2015 17:51:16 -0800 Subject: [PATCH 26/71] rename file wip --- rcl/src/rcl/rcl.c | 6 +----- rcl/src/rcl/{stdatomics_helper.h => stdatomic_helper.h} | 2 +- 2 files changed, 2 insertions(+), 6 deletions(-) rename rcl/src/rcl/{stdatomics_helper.h => stdatomic_helper.h} (96%) diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index da8c303..a2b5c51 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -19,13 +19,9 @@ extern "C" #include "rcl/rcl.h" -#ifndef WIN32 -#include -#else -#include "./stdatomic/win32/stdatomic.h" -#endif #include +#include "./stdatomic_helper.h" #include "rcl/error_handling.h" static atomic_bool __rcl_is_initialized = ATOMIC_VAR_INIT(false); diff --git a/rcl/src/rcl/stdatomics_helper.h b/rcl/src/rcl/stdatomic_helper.h similarity index 96% rename from rcl/src/rcl/stdatomics_helper.h rename to rcl/src/rcl/stdatomic_helper.h index a7d2352..f3822bd 100644 --- a/rcl/src/rcl/stdatomics_helper.h +++ b/rcl/src/rcl/stdatomic_helper.h @@ -17,7 +17,7 @@ #if !defined(WIN32) - +// #include #else From d281ab3d284779ee682f38b6ac627947fd462203 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 11 Dec 2015 18:44:46 -0800 Subject: [PATCH 27/71] wip --- rcl/src/rcl/rcl.c | 17 +- rcl/src/rcl/stdatomic_helper.h | 66 +++- .../rcl/stdatomic_helper/win32/stdatomic.h | 305 +++++++++--------- rcl/src/rcl/time_win32.c | 8 +- rcl/src/rcl/timer.c | 38 ++- rcl/src/rcl/wait.c | 4 +- 6 files changed, 259 insertions(+), 179 deletions(-) diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index a2b5c51..7fc8745 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -42,14 +42,15 @@ __clean_up_init() } __rcl_allocator.deallocate(__rcl_argv, __rcl_allocator.state); } - atomic_store(&__rcl_instance_id, 0); - atomic_store(&__rcl_is_initialized, false); + rcl_atomic_store(&__rcl_instance_id, 0); + rcl_atomic_store(&__rcl_is_initialized, false); } rcl_ret_t rcl_init(int argc, char ** argv, rcl_allocator_t allocator) { - bool was_initialized = atomic_exchange(&__rcl_is_initialized, true); + bool was_initialized; + rcl_atomic_exchange(&__rcl_is_initialized, was_initialized, true); if (was_initialized) { RCL_SET_ERROR_MSG("rcl_init called while already initialized"); return RCL_RET_ALREADY_INIT; @@ -67,8 +68,8 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator) __rcl_argv[i] = (char *)__rcl_allocator.allocate(strlen(argv[i]), __rcl_allocator.state); strcpy(__rcl_argv[i], argv[i]); // NOLINT(runtime/printf) } - atomic_store(&__rcl_instance_id, ++__rcl_next_unique_id); - if (atomic_load(&__rcl_instance_id) == 0) { + rcl_atomic_store(&__rcl_instance_id, ++__rcl_next_unique_id); + if (rcl_atomic_load_uint64_t(&__rcl_instance_id) == 0) { // Roll over occurred. __rcl_next_unique_id--; // roll back to avoid the next call succeeding. RCL_SET_ERROR_MSG("unique rcl instance ids exhausted"); @@ -83,7 +84,7 @@ fail: rcl_ret_t rcl_shutdown() { - if (!atomic_load(&__rcl_is_initialized)) { + if (!rcl_ok()) { RCL_SET_ERROR_MSG("rcl_shutdown called before rcl_init"); return RCL_RET_NOT_INIT; } @@ -94,13 +95,13 @@ rcl_shutdown() uint64_t rcl_get_instance_id() { - return atomic_load(&__rcl_instance_id); + return rcl_atomic_load_uint64_t(&__rcl_instance_id); } bool rcl_ok() { - return atomic_load(&__rcl_is_initialized); + return rcl_atomic_load_bool(&__rcl_is_initialized); } #if __cplusplus diff --git a/rcl/src/rcl/stdatomic_helper.h b/rcl/src/rcl/stdatomic_helper.h index f3822bd..913e438 100644 --- a/rcl/src/rcl/stdatomic_helper.h +++ b/rcl/src/rcl/stdatomic_helper.h @@ -12,15 +12,73 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCL__STDATOMICS_HELPER_H_ -#define RCL__STDATOMICS_HELPER_H_ +#ifndef RCL__STDATOMIC_HELPER_H_ +#define RCL__STDATOMIC_HELPER_H_ #if !defined(WIN32) -// #include +#include + +#define rcl_atomic_load(object, out) (out) = atomic_load(object) + +#define rcl_atomic_compare_exchange_strong(object, out, expected, desired) \ + (out) = atomic_compare_exchange_strong(object, expected, desired) + +#define rcl_atomic_exchange(object, out, desired) (out) = atomic_exchange(object, desired) + +#define rcl_atomic_store(object, desired) atomic_store(object, desired) #else #endif -#endif // RCL__STDATOMICS_HELPER_H_ +static inline bool +rcl_atomic_load_bool(atomic_bool * a_bool) +{ + bool result; + rcl_atomic_load(a_bool, result); + return result; +} + +static inline uint64_t +rcl_atomic_load_uint64_t(atomic_uint_least64_t * a_uint64_t) +{ + uint64_t result; + rcl_atomic_load(a_uint64_t, result); + return result; +} + +static inline uintptr_t +rcl_atomic_load_uintptr_t(atomic_uintptr_t * a_uintptr_t) +{ + uintptr_t result; + rcl_atomic_load(a_uintptr_t, result); + return result; +} + +static inline bool +rcl_atomic_compare_exchange_strong_uint_least64_t( + atomic_uint_least64_t * a_uint_least64_t, uint64_t * expected, uint64_t desired) +{ + bool result; + rcl_atomic_compare_exchange_strong(a_uint_least64_t, result, expected, desired); + return result; +} + +static inline uint64_t +rcl_atomic_exchange_uint64_t(atomic_uint_least64_t * a_uint64_t, uint64_t desired) +{ + uint64_t result; + rcl_atomic_exchange(a_uint64_t, result, desired); + return result; +} + +static inline uint64_t +rcl_atomic_exchange_uintptr_t(atomic_uintptr_t * a_uintptr_t, uintptr_t desired) +{ + uintptr_t result; + rcl_atomic_exchange(a_uintptr_t, result, desired); + return result; +} + +#endif // RCL__STDATOMIC_HELPER_H_ diff --git a/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h index 0fa16ac..42b6f74 100644 --- a/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h +++ b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h @@ -1,3 +1,17 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + /* * An implementation of C11 stdatomic.h for Win32, part borrowed from FreeBSD * (original copyright follows), with major modifications for @@ -42,18 +56,18 @@ * $FreeBSD: src/include/stdatomic.h,v 1.10.2.2 2012/05/30 19:21:54 theraven Exp $ */ -#ifndef _STDATOMIC_H_ -#define _STDATOMIC_H_ +#if !defined(WIN32) +#error "this stdatomic.h does not support your compiler" +#endif + +#ifndef RCL__STDATOMIC_HELPER__WIN32__STDATOMIC_H_ +#define RCL__STDATOMIC_HELPER__WIN32__STDATOMIC_H_ #include #include #include -#if !defined(WIN32) -#error "this stdatomic.h does not support your compiler" -#endif - // In MSVC, correct alignment of each type is already ensured. #define _Atomic(T) struct { T __val; } @@ -61,9 +75,9 @@ * 7.17.2 Initialization. */ -#define ATOMIC_VAR_INIT(value) { .__val = (value) } +#define ATOMIC_VAR_INIT(value) {.__val = (value)} #define atomic_init(obj, value) do { \ - (obj)->__val = (value); \ + (obj)->__val = (value); \ } while (0) /* @@ -98,7 +112,8 @@ * atomic operations. */ -enum memory_order { +enum memory_order +{ memory_order_relaxed = __ATOMIC_RELAXED, memory_order_consume = __ATOMIC_CONSUME, memory_order_acquire = __ATOMIC_ACQUIRE, @@ -126,47 +141,47 @@ typedef enum memory_order memory_order; * 7.17.6 Atomic integer types. */ -typedef _Atomic(_Bool) atomic_bool; -typedef _Atomic(char) atomic_char; -typedef _Atomic(signed char) atomic_schar; -typedef _Atomic(unsigned char) atomic_uchar; -typedef _Atomic(short) atomic_short; -typedef _Atomic(unsigned short) atomic_ushort; -typedef _Atomic(int) atomic_int; -typedef _Atomic(unsigned int) atomic_uint; -typedef _Atomic(long) atomic_long; -typedef _Atomic(unsigned long) atomic_ulong; -typedef _Atomic(long long) atomic_llong; -typedef _Atomic(unsigned long long) atomic_ullong; +typedef _Atomic (_Bool) atomic_bool; +typedef _Atomic (char) atomic_char; +typedef _Atomic (signed char) atomic_schar; +typedef _Atomic (unsigned char) atomic_uchar; +typedef _Atomic (short) atomic_short; // NOLINT +typedef _Atomic (unsigned short) atomic_ushort; // NOLINT +typedef _Atomic (int) atomic_int; +typedef _Atomic (unsigned int) atomic_uint; +typedef _Atomic (long) atomic_long; // NOLINT +typedef _Atomic (unsigned long) atomic_ulong; // NOLINT +typedef _Atomic (long long) atomic_llong; // NOLINT +typedef _Atomic (unsigned long long) atomic_ullong; // NOLINT #if 0 -typedef _Atomic(char16_t) atomic_char16_t; -typedef _Atomic(char32_t) atomic_char32_t; -typedef _Atomic(wchar_t) atomic_wchar_t; -typedef _Atomic(int_least8_t) atomic_int_least8_t; -typedef _Atomic(uint_least8_t) atomic_uint_least8_t; +typedef _Atomic (char16_t) atomic_char16_t; +typedef _Atomic (char32_t) atomic_char32_t; +typedef _Atomic (wchar_t) atomic_wchar_t; +typedef _Atomic (int_least8_t) atomic_int_least8_t; +typedef _Atomic (uint_least8_t) atomic_uint_least8_t; #endif -typedef _Atomic(int_least16_t) atomic_int_least16_t; -typedef _Atomic(uint_least16_t) atomic_uint_least16_t; -typedef _Atomic(int_least32_t) atomic_int_least32_t; -typedef _Atomic(uint_least32_t) atomic_uint_least32_t; -typedef _Atomic(int_least64_t) atomic_int_least64_t; -typedef _Atomic(uint_least64_t) atomic_uint_least64_t; +typedef _Atomic (int_least16_t) atomic_int_least16_t; +typedef _Atomic (uint_least16_t) atomic_uint_least16_t; +typedef _Atomic (int_least32_t) atomic_int_least32_t; +typedef _Atomic (uint_least32_t) atomic_uint_least32_t; +typedef _Atomic (int_least64_t) atomic_int_least64_t; +typedef _Atomic (uint_least64_t) atomic_uint_least64_t; #if 0 -typedef _Atomic(int_fast8_t) atomic_int_fast8_t; -typedef _Atomic(uint_fast8_t) atomic_uint_fast8_t; +typedef _Atomic (int_fast8_t) atomic_int_fast8_t; +typedef _Atomic (uint_fast8_t) atomic_uint_fast8_t; #endif -typedef _Atomic(int_fast16_t) atomic_int_fast16_t; -typedef _Atomic(uint_fast16_t) atomic_uint_fast16_t; -typedef _Atomic(int_fast32_t) atomic_int_fast32_t; -typedef _Atomic(uint_fast32_t) atomic_uint_fast32_t; -typedef _Atomic(int_fast64_t) atomic_int_fast64_t; -typedef _Atomic(uint_fast64_t) atomic_uint_fast64_t; -typedef _Atomic(intptr_t) atomic_intptr_t; -typedef _Atomic(uintptr_t) atomic_uintptr_t; -typedef _Atomic(size_t) atomic_size_t; -typedef _Atomic(ptrdiff_t) atomic_ptrdiff_t; -typedef _Atomic(intmax_t) atomic_intmax_t; -typedef _Atomic(uintmax_t) atomic_uintmax_t; +typedef _Atomic (int_fast16_t) atomic_int_fast16_t; +typedef _Atomic (uint_fast16_t) atomic_uint_fast16_t; +typedef _Atomic (int_fast32_t) atomic_int_fast32_t; +typedef _Atomic (uint_fast32_t) atomic_uint_fast32_t; +typedef _Atomic (int_fast64_t) atomic_int_fast64_t; +typedef _Atomic (uint_fast64_t) atomic_uint_fast64_t; +typedef _Atomic (intptr_t) atomic_intptr_t; +typedef _Atomic (uintptr_t) atomic_uintptr_t; +typedef _Atomic (size_t) atomic_size_t; +typedef _Atomic (ptrdiff_t) atomic_ptrdiff_t; +typedef _Atomic (intmax_t) atomic_intmax_t; +typedef _Atomic (uintmax_t) atomic_uintmax_t; /* * 7.17.7 Operations on atomic types. (pruned modified for Windows' crappy C compiler) @@ -174,128 +189,128 @@ typedef _Atomic(uintmax_t) atomic_uintmax_t; #define rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) \ do { \ - switch(sizeof(object)) { \ - case sizeof(uint64_t): \ - out = _InterlockedCompareExchange64((LONGLONG *) object, desired, expected); \ - break; \ - case sizeof(uint32_t): \ - out = _InterlockedCompareExchange((LONG *) object, desired, expected); \ - break; \ - case sizeof(uint16_t): \ - out = _InterlockedCompareExchange16((SHORT *) object, desired, expected); \ - break; \ - default: \ - break; \ - }; \ - } while(0) + switch (sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedCompareExchange64((LONGLONG *) object, desired, expected); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedCompareExchange((LONG *) object, desired, expected); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedCompareExchange16((SHORT *) object, desired, expected); \ + break; \ + default: \ + break; \ + } \ + } while (0) #define rcl_win32_atomic_compare_exchange_weak(object, out, expected, desired) \ rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) #define rcl_win32_atomic_exchange((object), out, desired) \ do { \ - switch(sizeof(object)) { \ - case sizeof(uint64_t): \ - out = _InterlockedExchange64((LONGLONG *) object, desired); \ - break; \ - case sizeof(uint32_t): \ - out = _InterlockedExchange((LONG *) object, desired); \ - break; \ - case sizeof(uint16_t): \ - out = _InterlockedExchange16((SHORT *) object, desired); \ - break; \ - default: \ - break; \ - }; \ - } while(0) + switch (sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedExchange64((LONGLONG *) object, desired); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedExchange((LONG *) object, desired); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedExchange16((SHORT *) object, desired); \ + break; \ + default: \ + break; \ + } \ + } while (0) #define rcl_win32_atomic_fetch_add(object, out, operand) \ do { \ - switch(sizeof(object)) { \ - case sizeof(uint64_t): \ - out = _InterlockedExchangeAdd64((LONGLONG *) object, operand); \ - break; \ - case sizeof(uint32_t): \ - out = _InterlockedExchangeAdd((LONG *) object, operand); \ - break; \ - case sizeof(uint16_t): \ - out = _InterlockedExchangeAdd16((SHORT *) object, operand); \ - break; \ - default: \ - break; \ - }; \ - } while(0) + switch (sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedExchangeAdd64((LONGLONG *) object, operand); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedExchangeAdd((LONG *) object, operand); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedExchangeAdd16((SHORT *) object, operand); \ + break; \ + default: \ + break; \ + } \ + } while (0) #define rcl_win32_atomic_fetch_and(object, out, operand) \ do { \ - switch(sizeof(object)) { \ - case sizeof(uint64_t): \ - out = _InterlockedAnd64((LONGLONG *) object, operand); \ - break; \ - case sizeof(uint32_t): \ - out = _InterlockedAnd((LONG *) object, operand); \ - break; \ - case sizeof(uint16_t): \ - out = _InterlockedAnd16((SHORT *) object, operand); \ - break; \ - default: \ - break; \ - }; \ - } while(0) + switch (sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedAnd64((LONGLONG *) object, operand); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedAnd((LONG *) object, operand); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedAnd16((SHORT *) object, operand); \ + break; \ + default: \ + break; \ + } \ + } while (0) #define rcl_win32_atomic_fetch_or(object, out, operand) \ do { \ - switch(sizeof(object)) { \ - case sizeof(uint64_t): \ - out = _InterlockedOr64((LONGLONG *) object, operand); \ - break; \ - case sizeof(uint32_t): \ - out = _InterlockedOr((LONG *) object, operand); \ - break; \ - case sizeof(uint16_t): \ - out = _InterlockedOr16((SHORT *) object, operand); \ - break; \ - default: \ - break; \ - }; \ - } while(0) + switch (sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedOr64((LONGLONG *) object, operand); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedOr((LONG *) object, operand); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedOr16((SHORT *) object, operand); \ + break; \ + default: \ + break; \ + } \ + } while (0) #define rcl_win32_atomic_fetch_sub(object, out, operand) \ rcl_win32_atomic_fetch_add(object, out, -(operand)) #define rcl_win32_atomic_fetch_xor(object, out, operand) \ do { \ - switch(sizeof(object)) { \ - case sizeof(uint64_t): \ - out = _InterlockedXor64((LONGLONG *) object, operand); \ - break; \ - case sizeof(uint32_t): \ - out = _InterlockedXor((LONG *) object, operand); \ - break; \ - case sizeof(uint16_t): \ - out = _InterlockedXor16((SHORT *) object, operand); \ - break; \ - default: \ - break; \ - }; \ - } while(0) + switch (sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedXor64((LONGLONG *) object, operand); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedXor((LONG *) object, operand); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedXor16((SHORT *) object, operand); \ + break; \ + default: \ + break; \ + } \ + } while (0) #define rcl_win32_atomic_load(object) \ do { \ - switch(sizeof(object)) { \ - case sizeof(uint64_t): \ - out = _InterlockedExchangeAdd64((LONGLONG *) object, 0); \ - break; \ - case sizeof(uint32_t): \ - out = _InterlockedExchangeAdd((LONG *) object, 0); \ - break; \ - case sizeof(uint16_t): \ - out = _InterlockedExchangeAdd16((SHORT *) object, 0); \ - break; \ - default: \ - break; \ - }; \ - } while(0) + switch (sizeof(object)) { \ + case sizeof(uint64_t): \ + out = _InterlockedExchangeAdd64((LONGLONG *) object, 0); \ + break; \ + case sizeof(uint32_t): \ + out = _InterlockedExchangeAdd((LONG *) object, 0); \ + break; \ + case sizeof(uint16_t): \ + out = _InterlockedExchangeAdd16((SHORT *) object, 0); \ + break; \ + default: \ + break; \ + } \ + } while (0) #define rcl_win32_atomic_store(object, desired) \ do { \ @@ -322,4 +337,4 @@ typedef _Atomic(uintmax_t) atomic_uintmax_t; // #define atomic_flag_test_and_set(object) \ // atomic_flag_test_and_set_explicit(object, memory_order_seq_cst) -#endif /* !_STDATOMIC_H_ */ +#endif // RCL__STDATOMIC_HELPER__WIN32__STDATOMIC_H_ diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c index 7d0f9b9..55c03e5 100644 --- a/rcl/src/rcl/time_win32.c +++ b/rcl/src/rcl/time_win32.c @@ -24,10 +24,10 @@ extern "C" #include "rcl/time.h" -#include #include #include "./common.h" +#include "./stdatomic_helper.h" #include "rcl/error_handling.h" #define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) @@ -63,7 +63,7 @@ rcl_system_time_point_now(rcl_system_time_point_t * now) static atomic_uint_least64_t start_ns = ATOMIC_VAR_INIT(0); rcl_time_t start = {0, 0}; // If start_ns (static/global) is 0, then set it up on the first call. - uint64_t start_ns_loaded = atomic_load(&start_ns); + uint64_t start_ns_loaded = rcl_atomic_load(&start_ns); if (start_ns_loaded == 0) { QueryPerformanceFrequency(&cpu_freq); if (cpu_freq.QuadPart == 0) { @@ -86,7 +86,9 @@ rcl_system_time_point_now(rcl_system_time_point_t * now) #endif start.sec = (uint64_t)(start_li.QuadPart / 10000000); // 100-ns units. odd. start.nsec = (start_li.LowPart % 10000000) * 100; - if (atomic_compare_exchange_strong(&start_ns, 0, RCL_S_TO_NS(start.sec) + start.nsec)) { + static uint64_t expected = 0; + uint64_t desired = RCL_S_TO_NS(start.sec) + start.nsec; + if (rcl_atomic_compare_exchange_strong_uint_least64_t(&start_ns, &expected, desired)) { // If it matched 0 this call was first to setup, set the cpu_freq and init_cpu_time globals. init_cpu_time_global = init_cpu_time; cpu_freq_global = cpu_freq; diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index a8c74fa..dc2fb45 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -19,9 +19,8 @@ extern "C" #include "rcl/timer.h" -#include - #include "./common.h" +#include "./stdatomic_helper.h" typedef struct rcl_timer_impl_t { @@ -97,7 +96,7 @@ rcl_timer_call(rcl_timer_t * timer) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - if (atomic_load(&timer->impl->canceled)) { + if (rcl_atomic_load_bool(&timer->impl->canceled)) { RCL_SET_ERROR_MSG("timer is canceled"); return RCL_RET_TIMER_CANCELED; } @@ -106,9 +105,11 @@ rcl_timer_call(rcl_timer_t * timer) if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } - uint64_t previous_ns = atomic_exchange(&timer->impl->last_call_time, now_steady.nanoseconds); + uint64_t previous_ns = + rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady.nanoseconds); uint64_t since_last_call = now_steady.nanoseconds - previous_ns; - rcl_timer_callback_t typed_callback = (rcl_timer_callback_t)atomic_load(&timer->impl->callback); + rcl_timer_callback_t typed_callback = + (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback); typed_callback(timer, since_last_call); return RCL_RET_OK; } @@ -124,7 +125,7 @@ rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready) if (ret != RCL_RET_OK) { return ret; // rcl error state should already be set. } - *is_ready = (time_until_next_call <= 0) && !atomic_load(&timer->impl->canceled); + *is_ready = (time_until_next_call <= 0) && !rcl_atomic_load_bool(&timer->impl->canceled); return RCL_RET_OK; } @@ -139,8 +140,9 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt if (ret != RCL_RET_OK) { return ret; // rcl error state should already be set. } - uint64_t period = atomic_load(&timer->impl->period); - *time_until_next_call = (atomic_load(&timer->impl->last_call_time) + period) - now.nanoseconds; + uint64_t period = rcl_atomic_load_uint64_t(&timer->impl->period); + *time_until_next_call = + (rcl_atomic_load_uint64_t(&timer->impl->last_call_time) + period) - now.nanoseconds; return RCL_RET_OK; } @@ -155,7 +157,8 @@ rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_si if (ret != RCL_RET_OK) { return ret; // rcl error state should already be set. } - *time_since_last_call = (now.nanoseconds - atomic_load(&timer->impl->last_call_time)); + *time_since_last_call = + (now.nanoseconds - rcl_atomic_load_uint64_t(&timer->impl->last_call_time)); return RCL_RET_OK; } @@ -165,7 +168,7 @@ rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period) RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(period, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - *period = atomic_load(&timer->impl->period); + *period = rcl_atomic_load_uint64_t(&timer->impl->period); return RCL_RET_OK; } @@ -175,7 +178,7 @@ rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64 RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - *old_period = atomic_exchange(&timer->impl->period, new_period); + *old_period = rcl_atomic_exchange_uint64_t(&timer->impl->period, new_period); return RCL_RET_OK; } @@ -184,7 +187,7 @@ rcl_timer_get_callback(const rcl_timer_t * timer) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL); - return (rcl_timer_callback_t)atomic_load(&timer->impl->callback); + return (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback); } rcl_timer_callback_t @@ -193,7 +196,8 @@ rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_ RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL); RCL_CHECK_ARGUMENT_FOR_NULL(new_callback, NULL); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL); - return (rcl_timer_callback_t)atomic_exchange(&timer->impl->callback, (uintptr_t)new_callback); + return (rcl_timer_callback_t)rcl_atomic_exchange_uintptr_t( + &timer->impl->callback, (uintptr_t)new_callback); } rcl_ret_t @@ -201,7 +205,7 @@ rcl_timer_cancel(rcl_timer_t * timer) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - atomic_store(&timer->impl->canceled, true); + rcl_atomic_store(&timer->impl->canceled, true); return RCL_RET_OK; } @@ -211,7 +215,7 @@ rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled) RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(is_canceled, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); - *is_canceled = atomic_load(&timer->impl->canceled); + *is_canceled = rcl_atomic_load_bool(&timer->impl->canceled); return RCL_RET_OK; } @@ -225,8 +229,8 @@ rcl_timer_reset(rcl_timer_t * timer) if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } - atomic_store(&timer->impl->last_call_time, now.nanoseconds); - atomic_store(&timer->impl->canceled, false); + rcl_atomic_store(&timer->impl->last_call_time, now.nanoseconds); + rcl_atomic_store(&timer->impl->canceled, false); return RCL_RET_OK; } diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 25d4807..b41fb75 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -20,14 +20,14 @@ extern "C" #include "rcl/wait.h" #include -#include #include #include +#include "./common.h" +#include "./stdatomic_helper.h" #include "rcl/error_handling.h" #include "rcl/time.h" #include "rmw/rmw.h" -#include "./common.h" typedef struct rcl_wait_set_impl_t { From 02f926c2c7fcfb2f410811908d168aba38df7591 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sat, 12 Dec 2015 12:12:48 -0800 Subject: [PATCH 28/71] fixes while testing on Windows --- rcl/src/rcl/rcl.c | 8 ++-- rcl/src/rcl/stdatomic_helper.h | 29 ++++++++++-- .../rcl/stdatomic_helper/win32/stdatomic.h | 47 ++++++++++++++----- rcl/src/rcl/time_win32.c | 2 +- rcl/src/rcl/wait.c | 15 +++--- 5 files changed, 71 insertions(+), 30 deletions(-) diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index 7fc8745..72a8a12 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -28,7 +28,7 @@ static atomic_bool __rcl_is_initialized = ATOMIC_VAR_INIT(false); static rcl_allocator_t __rcl_allocator = {0}; static int __rcl_argc = 0; static char ** __rcl_argv = NULL; -static atomic_uint_fast64_t __rcl_instance_id = ATOMIC_VAR_INIT(0); +static atomic_uint_least64_t __rcl_instance_id = ATOMIC_VAR_INIT(0); static uint64_t __rcl_next_unique_id = 0; static void @@ -49,9 +49,7 @@ __clean_up_init() rcl_ret_t rcl_init(int argc, char ** argv, rcl_allocator_t allocator) { - bool was_initialized; - rcl_atomic_exchange(&__rcl_is_initialized, was_initialized, true); - if (was_initialized) { + if (rcl_atomic_exchange_bool(&__rcl_is_initialized, true)) { RCL_SET_ERROR_MSG("rcl_init called while already initialized"); return RCL_RET_ALREADY_INIT; } @@ -66,7 +64,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator) memset(__rcl_argv, 0, sizeof(char **) * argc); for (size_t i = 0; i < argc; ++i) { __rcl_argv[i] = (char *)__rcl_allocator.allocate(strlen(argv[i]), __rcl_allocator.state); - strcpy(__rcl_argv[i], argv[i]); // NOLINT(runtime/printf) + memcpy(__rcl_argv[i], argv[i], strlen(argv[i])); } rcl_atomic_store(&__rcl_instance_id, ++__rcl_next_unique_id); if (rcl_atomic_load_uint64_t(&__rcl_instance_id) == 0) { diff --git a/rcl/src/rcl/stdatomic_helper.h b/rcl/src/rcl/stdatomic_helper.h index 913e438..fef4bff 100644 --- a/rcl/src/rcl/stdatomic_helper.h +++ b/rcl/src/rcl/stdatomic_helper.h @@ -28,14 +28,25 @@ #define rcl_atomic_store(object, desired) atomic_store(object, desired) -#else +#else // !defined(WIN32) -#endif +#include "./stdatomic_helper/win32/stdatomic.h" + +#define rcl_atomic_load(object, out) rcl_win32_atomic_load(object, out) + +#define rcl_atomic_compare_exchange_strong(object, out, expected, desired) \ + rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) + +#define rcl_atomic_exchange(object, out, desired) rcl_win32_atomic_exchange(object, out, desired) + +#define rcl_atomic_store(object, desired) rcl_win32_atomic_store(object, desired) + +#endif // !defined(WIN32) static inline bool rcl_atomic_load_bool(atomic_bool * a_bool) { - bool result; + bool result = false; rcl_atomic_load(a_bool, result); return result; } @@ -43,7 +54,7 @@ rcl_atomic_load_bool(atomic_bool * a_bool) static inline uint64_t rcl_atomic_load_uint64_t(atomic_uint_least64_t * a_uint64_t) { - uint64_t result; + uint64_t result = 0; rcl_atomic_load(a_uint64_t, result); return result; } @@ -51,7 +62,7 @@ rcl_atomic_load_uint64_t(atomic_uint_least64_t * a_uint64_t) static inline uintptr_t rcl_atomic_load_uintptr_t(atomic_uintptr_t * a_uintptr_t) { - uintptr_t result; + uintptr_t result = 0; rcl_atomic_load(a_uintptr_t, result); return result; } @@ -65,6 +76,14 @@ rcl_atomic_compare_exchange_strong_uint_least64_t( return result; } +static inline bool +rcl_atomic_exchange_bool(atomic_bool * a_bool, bool desired) +{ + bool result; + rcl_atomic_exchange(a_bool, result, desired); + return result; +} + static inline uint64_t rcl_atomic_exchange_uint64_t(atomic_uint_least64_t * a_uint64_t, uint64_t desired) { diff --git a/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h index 42b6f74..c346025 100644 --- a/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h +++ b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h @@ -188,26 +188,31 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; */ #define rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) \ + __pragma(warning( push )) \ + __pragma(warning( disable : 4244 )) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ - out = _InterlockedCompareExchange64((LONGLONG *) object, desired, expected); \ + out = _InterlockedCompareExchange64((LONGLONG *) object, desired, *expected); \ break; \ case sizeof(uint32_t): \ - out = _InterlockedCompareExchange((LONG *) object, desired, expected); \ + out = _InterlockedCompareExchange((LONG *) object, desired, *expected); \ break; \ case sizeof(uint16_t): \ - out = _InterlockedCompareExchange16((SHORT *) object, desired, expected); \ + out = _InterlockedCompareExchange16((SHORT *) object, desired, *expected); \ break; \ default: \ break; \ } \ - } while (0) + } while (0) \ + __pragma(warning( pop )) #define rcl_win32_atomic_compare_exchange_weak(object, out, expected, desired) \ rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) -#define rcl_win32_atomic_exchange((object), out, desired) \ +#define rcl_win32_atomic_exchange(object, out, desired) \ + __pragma(warning( push )) \ + __pragma(warning( disable : 4244 )) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -222,9 +227,12 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) + } while (0) \ + __pragma(warning( pop )) #define rcl_win32_atomic_fetch_add(object, out, operand) \ + __pragma(warning( push )) \ + __pragma(warning( disable : 4244 )) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -239,9 +247,12 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) + } while (0) \ + __pragma(warning( pop )) #define rcl_win32_atomic_fetch_and(object, out, operand) \ + __pragma(warning( push )) \ + __pragma(warning( disable : 4244 )) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -256,9 +267,12 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) + } while (0) \ + __pragma(warning( pop )) #define rcl_win32_atomic_fetch_or(object, out, operand) \ + __pragma(warning( push )) \ + __pragma(warning( disable : 4244 )) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -273,12 +287,15 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) + } while (0) \ + __pragma(warning( pop )) #define rcl_win32_atomic_fetch_sub(object, out, operand) \ rcl_win32_atomic_fetch_add(object, out, -(operand)) #define rcl_win32_atomic_fetch_xor(object, out, operand) \ + __pragma(warning( push )) \ + __pragma(warning( disable : 4244 )) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -293,9 +310,12 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) + } while (0) \ + __pragma(warning( pop )) -#define rcl_win32_atomic_load(object) \ +#define rcl_win32_atomic_load(object, out) \ + __pragma(warning( push )) \ + __pragma(warning( disable : 4244 )) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -310,12 +330,13 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) + } while (0) \ + __pragma(warning( pop )) #define rcl_win32_atomic_store(object, desired) \ do { \ MemoryBarrier(); \ - object = (desired); \ + (object)->__val = (desired); \ MemoryBarrier(); \ } while (0) diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c index 55c03e5..8db5d26 100644 --- a/rcl/src/rcl/time_win32.c +++ b/rcl/src/rcl/time_win32.c @@ -124,7 +124,7 @@ rcl_ret_t rcl_steady_time_point_now(rcl_steady_time_point_t * now) { RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); - WINAPI ret = QueryPerformanceFrequency(); + // WINAPI ret = QueryPerformanceFrequency(); return RCL_RET_OK; } diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index b41fb75..9e6589f 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -194,7 +194,10 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al RCL_SET_ERROR_MSG("wait set is invalid"); \ return RCL_RET_WAIT_SET_INVALID; \ } \ - memset(wait_set->Type ## s, 0, sizeof(rcl_ ## Type ## _t *) * wait_set->size_of_ ## Type ## s); \ + memset( \ + (void *)wait_set->Type ## s, \ + 0, \ + sizeof(rcl_ ## Type ## _t *) * wait_set->size_of_ ## Type ## s); \ wait_set->impl->Type ## _index = 0; \ #define SET_CLEAR_RMW(Type, RMWStorage, RMWCount) \ @@ -214,14 +217,14 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al rcl_allocator_t allocator = wait_set->impl->allocator; \ if (size == 0) { \ if (wait_set->Type ## s) { \ - allocator.deallocate(wait_set->Type ## s, allocator.state); \ + allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ wait_set->Type ## s = NULL; \ } \ ExtraDealloc \ } else { \ wait_set->size_of_ ## Type ## s = 0; \ wait_set->Type ## s = (const rcl_ ## Type ## _t * *)allocator.reallocate( \ - wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * size, allocator.state); \ + (void *)wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * size, allocator.state); \ RCL_CHECK_FOR_NULL_WITH_MSG( \ wait_set->Type ## s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \ wait_set->size_of_ ## Type ## s = size; \ @@ -232,7 +235,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al #define SET_RESIZE_RMW_DEALLOC(RMWStorage, RMWCount) \ /* Also deallocate the rmw storage. */ \ if (wait_set->impl->RMWStorage) { \ - allocator.deallocate(wait_set->impl->RMWStorage, allocator.state); \ + allocator.deallocate((void *)wait_set->impl->RMWStorage, allocator.state); \ wait_set->impl->RMWStorage = NULL; \ } @@ -242,7 +245,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al wait_set->impl->RMWStorage = (void **)allocator.reallocate( \ wait_set->impl->RMWStorage, sizeof(rcl_ ## Type ## _t *) * size, allocator.state); \ if (!wait_set->impl->RMWStorage) { \ - allocator.deallocate(wait_set->Type ## s, allocator.state); \ + allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ wait_set->size_of_ ## Type ## s = 0; \ RCL_SET_ERROR_MSG("allocating memory failed"); \ return RCL_RET_BAD_ALLOC; \ @@ -375,7 +378,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) if (ret != RCL_RET_OK) { return ret; // The rcl error state should already be set. } - if (timer_timeout < min_timeout) { + if ((uint64_t)timer_timeout < min_timeout) { min_timeout = timer_timeout; } } From 34ae477e284c25be1da3ccc0da0f8e9cbf07ad2a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sat, 12 Dec 2015 16:29:34 -0800 Subject: [PATCH 29/71] refactoring since type support changes --- rcl/CMakeLists.txt | 118 ++++++++++++---------------------------- rcl/package.xml | 15 +++-- rcl/test/CMakeLists.txt | 74 +++++++++++++++++++++++++ 3 files changed, 117 insertions(+), 90 deletions(-) create mode 100644 rcl/test/CMakeLists.txt diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 92d6b8e..d967013 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -5,11 +5,10 @@ project(rcl) find_package(ament_cmake REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rmw REQUIRED) +find_package(rmw_implementation REQUIRED) +find_package(rmw_implementation_cmake REQUIRED) find_package(rosidl_generator_c REQUIRED) -ament_export_dependencies(rmw rosidl_generator_c) -ament_export_include_directories(include) - include_directories(include) if(NOT WIN32) @@ -28,93 +27,44 @@ set(${PROJECT_NAME}_sources src/rcl/time.c src/rcl/timer.c ) -add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_sources}) -ament_target_dependencies(${PROJECT_NAME} - "rcl_interfaces" - "rmw" - "rosidl_generator_c" -) -if(NOT WIN32) - set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-std=c11") -endif() + +macro(target) + add_library(${PROJECT_NAME}${target_suffix} SHARED ${${PROJECT_NAME}_sources}) + ament_target_dependencies(${PROJECT_NAME}${target_suffix} + "rcl_interfaces" + "rmw" + "rosidl_generator_c" + "${rmw_implementation}" + ) + + if(NOT WIN32) + set_target_properties(${PROJECT_NAME}${target_suffix} PROPERTIES COMPILE_FLAGS "-std=c11") + endif() + + install( + TARGETS ${PROJECT_NAME}${target_suffix} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) +endmacro() + +call_for_each_rmw_implementation(target GENERATE_DEFAULT) + +ament_export_dependencies(rcl_interfaces) +ament_export_dependencies(rmw) +ament_export_dependencies(rmw_implementation) +ament_export_dependencies(rosidl_generator_c) + +ament_export_include_directories(include) + +ament_export_libraries(${PROJECT_NAME}) if(AMENT_ENABLE_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - set(extra_test_libraries) - set(extra_memory_tools_env PHONY=) # Use a phony env var so there is always at least one. - ament_find_gtest() # For GTEST_LIBRARIES - if(APPLE) - add_library(${PROJECT_NAME}_memory_tools_interpose SHARED test/memory_tools_osx_interpose.cpp) - target_link_libraries(${PROJECT_NAME}_memory_tools_interpose ${GTEST_LIBRARIES}) - set_target_properties(${PROJECT_NAME}_memory_tools_interpose - PROPERTIES COMPILE_FLAGS "-std=c++11") - list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools_interpose) - list(APPEND extra_memory_tools_env - DYLD_INSERT_LIBRARIES=$) - endif() - add_library(${PROJECT_NAME}_memory_tools SHARED test/memory_tools.cpp) - if(NOT WIN32) - set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") - endif() - target_link_libraries(${PROJECT_NAME}_memory_tools ${GTEST_LIBRARIES} ${extra_test_libraries}) - list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools) - - ament_add_gtest(test_memory_tools test/test_memory_tools.cpp ENV ${extra_memory_tools_env}) - if(TARGET test_memory_tools) - target_include_directories(test_memory_tools PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ) - if(NOT WIN32) - set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") - endif() - target_link_libraries(test_memory_tools ${PROJECT_NAME} ${extra_test_libraries}) - endif() - - ament_add_gtest(test_allocator test/rcl/test_allocator.cpp ENV ${extra_memory_tools_env}) - if(TARGET test_allocator) - target_include_directories(test_allocator PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ) - if(NOT WIN32) - set_target_properties(test_allocator PROPERTIES COMPILE_FLAGS "-std=c++11") - endif() - target_link_libraries(test_allocator ${PROJECT_NAME} ${extra_test_libraries}) - endif() - - ament_add_gtest(test_time test/rcl/test_time.cpp ENV ${extra_memory_tools_env}) - if(TARGET test_time) - target_include_directories(test_time PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ) - if(NOT WIN32) - set_target_properties(test_time PROPERTIES COMPILE_FLAGS "-std=c++11") - endif() - target_link_libraries(test_time ${PROJECT_NAME} ${extra_test_libraries}) - endif() - - ament_add_gtest(test_common - test/rcl/test_common.cpp - ENV - ${extra_memory_tools_env} - EMPTY_TEST= - NORMAL_TEST=foo - ) - if(TARGET test_common) - target_include_directories(test_common PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ) - if(NOT WIN32) - set_target_properties(test_common PROPERTIES COMPILE_FLAGS "-std=c++11") - endif() - target_link_libraries(test_common ${PROJECT_NAME} ${extra_test_libraries}) - endif() + add_subdirectory(test) endif() ament_package() diff --git a/rcl/package.xml b/rcl/package.xml index c0da176..493e2c1 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -10,17 +10,20 @@ ament_cmake - rmw - - rmw_implementation - rmw - rcl_interfaces - rosidl_generator_c + rcl_interfaces + rmw_implementation_cmake + rosidl_generator_c + rcl_interfaces + rosidl_generator_c + rmw_implementation + + ament_cmake_gtest ament_lint_auto ament_lint_common + rmw ament_cmake diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt new file mode 100644 index 0000000..8f10bc2 --- /dev/null +++ b/rcl/test/CMakeLists.txt @@ -0,0 +1,74 @@ +if(AMENT_ENABLE_TESTING) + set(extra_test_libraries) + set(extra_memory_tools_env PHONY=) # Use a phony env var so there is always at least one. + ament_find_gtest() # For GTEST_LIBRARIES + if(APPLE) + add_library(${PROJECT_NAME}_memory_tools_interpose SHARED memory_tools_osx_interpose.cpp) + target_link_libraries(${PROJECT_NAME}_memory_tools_interpose ${GTEST_LIBRARIES}) + set_target_properties(${PROJECT_NAME}_memory_tools_interpose + PROPERTIES COMPILE_FLAGS "-std=c++11") + list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools_interpose) + list(APPEND extra_memory_tools_env + DYLD_INSERT_LIBRARIES=$) + endif() + add_library(${PROJECT_NAME}_memory_tools SHARED memory_tools.cpp) + if(NOT WIN32) + set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() + target_link_libraries(${PROJECT_NAME}_memory_tools ${GTEST_LIBRARIES} ${extra_test_libraries}) + list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools) + + ament_add_gtest(test_memory_tools test_memory_tools.cpp ENV ${extra_memory_tools_env}) + if(TARGET test_memory_tools) + target_include_directories(test_memory_tools PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + if(NOT WIN32) + set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() + target_link_libraries(test_memory_tools ${PROJECT_NAME} ${extra_test_libraries}) + endif() + + ament_add_gtest(test_allocator rcl/test_allocator.cpp ENV ${extra_memory_tools_env}) + if(TARGET test_allocator) + target_include_directories(test_allocator PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + if(NOT WIN32) + set_target_properties(test_allocator PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() + target_link_libraries(test_allocator ${PROJECT_NAME} ${extra_test_libraries}) + endif() + + ament_add_gtest(test_time rcl/test_time.cpp ENV ${extra_memory_tools_env}) + if(TARGET test_time) + target_include_directories(test_time PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + if(NOT WIN32) + set_target_properties(test_time PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() + target_link_libraries(test_time ${PROJECT_NAME} ${extra_test_libraries}) + endif() + + ament_add_gtest(test_common + rcl/test_common.cpp + ENV + ${extra_memory_tools_env} + EMPTY_TEST= + NORMAL_TEST=foo + ) + if(TARGET test_common) + target_include_directories(test_common PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + if(NOT WIN32) + set_target_properties(test_common PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() + target_link_libraries(test_common ${PROJECT_NAME} ${extra_test_libraries}) + endif() +endif() From 20373465feb2593aca3835d5bd3e66b896ccaaab Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sat, 12 Dec 2015 16:29:43 -0800 Subject: [PATCH 30/71] uncrustify --- .../rcl/stdatomic_helper/win32/stdatomic.h | 56 +++++++++---------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h index c346025..d814300 100644 --- a/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h +++ b/rcl/src/rcl/stdatomic_helper/win32/stdatomic.h @@ -188,8 +188,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; */ #define rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) \ - __pragma(warning( push )) \ - __pragma(warning( disable : 4244 )) \ + __pragma(warning(push)) \ + __pragma(warning(disable: 4244)) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -204,15 +204,15 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) \ - __pragma(warning( pop )) + } while (0); \ + __pragma(warning(pop)) #define rcl_win32_atomic_compare_exchange_weak(object, out, expected, desired) \ rcl_win32_atomic_compare_exchange_strong(object, out, expected, desired) #define rcl_win32_atomic_exchange(object, out, desired) \ - __pragma(warning( push )) \ - __pragma(warning( disable : 4244 )) \ + __pragma(warning(push)) \ + __pragma(warning(disable: 4244)) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -227,12 +227,12 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) \ - __pragma(warning( pop )) + } while (0); \ + __pragma(warning(pop)) #define rcl_win32_atomic_fetch_add(object, out, operand) \ - __pragma(warning( push )) \ - __pragma(warning( disable : 4244 )) \ + __pragma(warning(push)) \ + __pragma(warning(disable: 4244)) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -247,12 +247,12 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) \ - __pragma(warning( pop )) + } while (0); \ + __pragma(warning(pop)) #define rcl_win32_atomic_fetch_and(object, out, operand) \ - __pragma(warning( push )) \ - __pragma(warning( disable : 4244 )) \ + __pragma(warning(push)) \ + __pragma(warning(disable: 4244)) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -267,12 +267,12 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) \ - __pragma(warning( pop )) + } while (0); \ + __pragma(warning(pop)) #define rcl_win32_atomic_fetch_or(object, out, operand) \ - __pragma(warning( push )) \ - __pragma(warning( disable : 4244 )) \ + __pragma(warning(push)) \ + __pragma(warning(disable: 4244)) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -287,15 +287,15 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) \ - __pragma(warning( pop )) + } while (0); \ + __pragma(warning(pop)) #define rcl_win32_atomic_fetch_sub(object, out, operand) \ rcl_win32_atomic_fetch_add(object, out, -(operand)) #define rcl_win32_atomic_fetch_xor(object, out, operand) \ - __pragma(warning( push )) \ - __pragma(warning( disable : 4244 )) \ + __pragma(warning(push)) \ + __pragma(warning(disable: 4244)) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -310,12 +310,12 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) \ - __pragma(warning( pop )) + } while (0); \ + __pragma(warning(pop)) #define rcl_win32_atomic_load(object, out) \ - __pragma(warning( push )) \ - __pragma(warning( disable : 4244 )) \ + __pragma(warning(push)) \ + __pragma(warning(disable: 4244)) \ do { \ switch (sizeof(object)) { \ case sizeof(uint64_t): \ @@ -330,8 +330,8 @@ typedef _Atomic (uintmax_t) atomic_uintmax_t; default: \ break; \ } \ - } while (0) \ - __pragma(warning( pop )) + } while (0); \ + __pragma(warning(pop)) #define rcl_win32_atomic_store(object, desired) \ do { \ From 654ded894ff692f61ec29a23b02c47fab7346f1b Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sun, 13 Dec 2015 18:52:56 -0800 Subject: [PATCH 31/71] implement time_win32.c --- rcl/src/rcl/allocator.c | 2 +- rcl/src/rcl/time_win32.c | 105 ++++++++------------------------------- 2 files changed, 22 insertions(+), 85 deletions(-) diff --git a/rcl/src/rcl/allocator.c b/rcl/src/rcl/allocator.c index a51ae70..14ec8b8 100644 --- a/rcl/src/rcl/allocator.c +++ b/rcl/src/rcl/allocator.c @@ -27,7 +27,7 @@ static void __default_deallocate(void * pointer, void * state) { (void)state; // unused - return free(pointer); + free(pointer); } static void * diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c index 8db5d26..debdaf4 100644 --- a/rcl/src/rcl/time_win32.c +++ b/rcl/src/rcl/time_win32.c @@ -30,101 +30,38 @@ extern "C" #include "./stdatomic_helper.h" #include "rcl/error_handling.h" -#define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) - rcl_ret_t rcl_system_time_point_now(rcl_system_time_point_t * now) -{ - -} -#if 0 { RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); - /* Windows implementation adapted from roscpp_core (3-clause BSD), see: - * https://github.com/ros/roscpp_core/blob/0.5.6/rostime/src/time.cpp#L96 - * - * > Win32 implementation - * unless I've missed something obvious, the only way to get high-precision - * time on Windows is via the QueryPerformanceCounter() call. However, - * this is somewhat problematic in Windows XP on some processors, especially - * AMD, because the Windows implementation can freak out when the CPU clocks - * down to save power. Time can jump or even go backwards. Microsoft has - * fixed this bug for most systems now, but it can still show up if you have - * not installed the latest CPU drivers (an oxymoron). They fixed all these - * problems in Windows Vista, and this API is by far the most accurate that - * I know of in Windows, so I'll use it here despite all these caveats - * - * I've further modified it to be thread safe using a atomic_uint_least64_t. - */ - LARGE_INTEGER cpu_freq; - static LARGE_INTEGER cpu_freq_global; - LARGE_INTEGER init_cpu_time; - static LARGE_INTEGER init_cpu_time_global; - static atomic_uint_least64_t start_ns = ATOMIC_VAR_INIT(0); - rcl_time_t start = {0, 0}; - // If start_ns (static/global) is 0, then set it up on the first call. - uint64_t start_ns_loaded = rcl_atomic_load(&start_ns); - if (start_ns_loaded == 0) { - QueryPerformanceFrequency(&cpu_freq); - if (cpu_freq.QuadPart == 0) { - RCL_SET_ERROR_MSG("no high performance timer found"); - return RCL_RET_ERROR; - } - QueryPerformanceCounter(&init_cpu_time); - // compute an offset from the Epoch using the lower-performance timer API - FILETIME ft; - GetSystemTimeAsFileTime(&ft); - LARGE_INTEGER start_li; - start_li.LowPart = ft.dwLowDateTime; - start_li.HighPart = ft.dwHighDateTime; - // why did they choose 1601 as the time zero, instead of 1970? - // there were no outstanding hard rock bands in 1601. -#ifdef _MSC_VER - start_li.QuadPart -= 116444736000000000Ui64; -#else - start_li.QuadPart -= 116444736000000000ULL; -#endif - start.sec = (uint64_t)(start_li.QuadPart / 10000000); // 100-ns units. odd. - start.nsec = (start_li.LowPart % 10000000) * 100; - static uint64_t expected = 0; - uint64_t desired = RCL_S_TO_NS(start.sec) + start.nsec; - if (rcl_atomic_compare_exchange_strong_uint_least64_t(&start_ns, &expected, desired)) { - // If it matched 0 this call was first to setup, set the cpu_freq and init_cpu_time globals. - init_cpu_time_global = init_cpu_time; - cpu_freq_global = cpu_freq; - } else { - // Another concurrent first call managed to set this up first; reset start so it gets set. - start = {0, 0}; - } - } - if (start.sec == 0 && start.nsec == 0) { - start.sec = RCL_NS_TO_S(start_ns_loaded); - start.nsec = start_ns_loaded % 1000000000; - } - LARGE_INTEGER cur_time; - QueryPerformanceCounter(&cur_time); - LARGE_INTEGER delta_cpu_time; - delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time_global.QuadPart; - double d_delta_cpu_time = delta_cpu_time.QuadPart / (double) cpu_freq_global.QuadPart; - uint64_t delta_sec = (uint64_t) floor(d_delta_cpu_time); - uint64_t delta_nsec = (uint64_t) floor((d_delta_cpu_time - delta_sec) * 1e9); - - *now = start; - now->sec += delta_sec; - now->nsec += delta_nsec; - - if (ret != RCL_RET_OK) { - return ret; // rcl error state should already be set. - } + FILETIME ft; + GetSystemTimeAsFileTime(&ft); + ULARGE_INTEGER uli; + uli.LowPart = ft.dwLowDateTime; + uli.HighPart = ft.dwHighDateTime; + // Adjust for January 1st, 1970, see: + // https://support.microsoft.com/en-us/kb/167296 + uli.QuadPart -= 116444736000000000; + // Convert to nanoseconds from 100's of nanoseconds. + now->nanoseconds = uli.QuadPart * 100; return RCL_RET_OK; } -#endif rcl_ret_t rcl_steady_time_point_now(rcl_steady_time_point_t * now) { RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT); - // WINAPI ret = QueryPerformanceFrequency(); + LARGE_INTEGER cpu_frequency, performance_count; + // These should not ever fail since XP is already end of life: + // From https://msdn.microsoft.com/en-us/library/windows/desktop/ms644905(v=vs.85).aspx and + // https://msdn.microsoft.com/en-us/library/windows/desktop/ms644904(v=vs.85).aspx: + // "On systems that run Windows XP or later, the function will always succeed and will + // thus never return zero." + QueryPerformanceFrequency(&cpu_frequency); + QueryPerformanceCounter(&performance_count); + // Convert to nanoseconds before converting from ticks to avoid precision loss. + now->nanoseconds = RCL_S_TO_NS(performance_count.QuadPart); + now->nanoseconds /= cpu_frequency.QuadPart; return RCL_RET_OK; } From 43c1824909f881d15938ba94a5aaa0bf0ba8b6db Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 14 Dec 2015 00:36:35 -0800 Subject: [PATCH 32/71] add visibility control macros to rcl --- rcl/CMakeLists.txt | 3 ++ rcl/include/rcl/allocator.h | 2 + rcl/include/rcl/guard_condition.h | 7 +++ rcl/include/rcl/node.h | 10 ++++ rcl/include/rcl/publisher.h | 9 ++++ rcl/include/rcl/rcl.h | 5 ++ rcl/include/rcl/subscription.h | 9 ++++ rcl/include/rcl/time.h | 3 ++ rcl/include/rcl/timer.h | 14 +++++ rcl/include/rcl/visibility_control.h | 58 ++++++++++++++++++++ rcl/include/rcl/wait.h | 15 ++++++ rcl/src/rcl/common.c | 3 ++ rcl/src/rcl/common.h | 11 ++-- rcl/test/CMakeLists.txt | 22 ++++---- rcl/test/memory_tools.cpp | 30 +++++++++-- rcl/test/memory_tools.hpp | 80 +++++++++++++++++++++++----- rcl/test/rcl/test_allocator.cpp | 4 ++ rcl/test/rcl/test_common.cpp | 21 ++++---- rcl/test/rcl/test_time.cpp | 16 +++--- 19 files changed, 274 insertions(+), 48 deletions(-) create mode 100644 rcl/include/rcl/visibility_control.h diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index d967013..f4cf14a 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -40,6 +40,9 @@ macro(target) if(NOT WIN32) set_target_properties(${PROJECT_NAME}${target_suffix} PROPERTIES COMPILE_FLAGS "-std=c11") endif() + # Causes the visibility macros to use dllexport rather than dllimport, + # which is appropriate when building the dll but not consuming it. + target_compile_definitions(${PROJECT_NAME}${target_suffix} PRIVATE "RCL_BUILDING_LIBRARY") install( TARGETS ${PROJECT_NAME}${target_suffix} diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index 3dc3034..4e849fe 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -21,6 +21,7 @@ extern "C" #endif #include "rcl/types.h" +#include "rcl/visibility_control.h" /// Encapsulation of an allocator. /* To use malloc, free, and realloc use rcl_get_default_allocator(). */ @@ -50,6 +51,7 @@ typedef struct rcl_allocator_t * This function is thread-safe. * This function is lock-free. */ +RCL_PUBLIC rcl_allocator_t rcl_get_default_allocator(); diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index 7c6ed0e..83b3c2c 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/rcl/include/rcl/guard_condition.h @@ -21,6 +21,7 @@ extern "C" #endif #include "rcl/node.h" +#include "rcl/visibility_control.h" /// Internal rcl guard condition implementation struct. struct rcl_guard_condition_impl_t; @@ -39,6 +40,7 @@ typedef struct rcl_guard_condition_options_t } rcl_guard_condition_options_t; /// Return a rcl_guard_condition_t struct with members set to NULL. +RCL_PUBLIC rcl_guard_condition_t rcl_get_zero_initialized_guard_condition(); @@ -81,6 +83,7 @@ rcl_get_zero_initialized_guard_condition(); * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_guard_condition_init( rcl_guard_condition_t * guard_condition, @@ -102,6 +105,7 @@ rcl_guard_condition_init( * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * node); @@ -110,6 +114,7 @@ rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * n * This function is thread-safe. * This function is lock-free. */ +RCL_PUBLIC rcl_guard_condition_options_t rcl_guard_condition_get_default_options(); @@ -130,6 +135,7 @@ rcl_guard_condition_get_default_options(); * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_guard_condition_trigger(const rcl_guard_condition_t * guard_condition); @@ -144,6 +150,7 @@ rcl_guard_condition_trigger(const rcl_guard_condition_t * guard_condition); * \param[in] guard_condition pointer to the rcl guard_condition * \return rmw guard_condition handle if successful, otherwise NULL */ +RCL_PUBLIC rmw_guard_condition_t * rcl_guard_condition_get_rmw_guard_condition_handle(const rcl_guard_condition_t * guard_condition); diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index fae9274..daeb219 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -24,6 +24,7 @@ extern "C" #include "rcl/allocator.h" #include "rcl/types.h" +#include "rcl/visibility_control.h" #define RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID SIZE_MAX @@ -57,6 +58,7 @@ typedef struct rcl_node_options_t } rcl_node_options_t; /// Return a rcl_node_t struct with members initialized to NULL. +RCL_PUBLIC rcl_node_t rcl_get_zero_initialized_node(); @@ -103,6 +105,7 @@ rcl_get_zero_initialized_node(); * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options); @@ -120,10 +123,12 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_node_fini(rcl_node_t * node); /// Return the default node options in a rcl_node_options_t. +RCL_PUBLIC rcl_node_options_t rcl_node_get_default_options(); @@ -140,6 +145,7 @@ rcl_node_get_default_options(); * \param[in] node pointer to the node * \return name string if successful, otherwise NULL */ +RCL_PUBLIC const char * rcl_node_get_name(const rcl_node_t * node); @@ -156,6 +162,7 @@ rcl_node_get_name(const rcl_node_t * node); * \param[in] node pointer to the node * \return options struct if successful, otherwise NULL */ +RCL_PUBLIC const rcl_node_options_t * rcl_node_get_options(const rcl_node_t * node); @@ -177,6 +184,7 @@ rcl_node_get_options(const rcl_node_t * node); * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); @@ -193,6 +201,7 @@ rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); * \param[in] node pointer to the rcl node * \return rmw node handle if successful, otherwise NULL */ +RCL_PUBLIC rmw_node_t * rcl_node_get_rmw_node_handle(const rcl_node_t * node); @@ -211,6 +220,7 @@ rcl_node_get_rmw_node_handle(const rcl_node_t * node); * \param[in] node pointer to the rcl node * \return rcl instance id captured at node creation or 0 if there was an error */ +RCL_PUBLIC uint64_t rcl_node_get_rcl_instance_id(const rcl_node_t * node); diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index bf57d1b..4d51e99 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -23,6 +23,7 @@ extern "C" #include "rosidl_generator_c/message_type_support.h" #include "rcl/node.h" +#include "rcl/visibility_control.h" /// Internal rcl publisher implementation struct. struct rcl_publisher_impl_t; @@ -49,6 +50,7 @@ typedef struct rcl_publisher_options_t * It's also possible to use calloc() instead of this if the rcl_publisher is * being allocated on the heap. */ +RCL_PUBLIC rcl_publisher_t rcl_get_zero_initialized_publisher(); @@ -123,6 +125,7 @@ rcl_get_zero_initialized_publisher(); * RCL_RET_BAD_ALLOC if allocating memory fails, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_publisher_init( rcl_publisher_t * publisher, @@ -146,10 +149,12 @@ rcl_publisher_init( * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node); /// Return the default publisher options in a rcl_publisher_options_t. +RCL_PUBLIC rcl_publisher_options_t rcl_publisher_get_default_options(); @@ -197,6 +202,7 @@ rcl_publisher_get_default_options(); * RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_publish(const rcl_publisher_t * publisher, const void * ros_message); @@ -216,6 +222,7 @@ rcl_publish(const rcl_publisher_t * publisher, const void * ros_message); * \param[in] publisher pointer to the publisher * \return name string if successful, otherwise NULL */ +RCL_PUBLIC const char * rcl_publisher_get_topic_name(const rcl_publisher_t * publisher); @@ -235,6 +242,7 @@ rcl_publisher_get_topic_name(const rcl_publisher_t * publisher); * \param[in] publisher pointer to the publisher * \return options struct if successful, otherwise NULL */ +RCL_PUBLIC const rcl_publisher_options_t * rcl_publisher_get_options(const rcl_publisher_t * publisher); @@ -258,6 +266,7 @@ rcl_publisher_get_options(const rcl_publisher_t * publisher); * \param[in] publisher pointer to the rcl publisher * \return rmw publisher handle if successful, otherwise NULL */ +RCL_PUBLIC rmw_publisher_t * rcl_publisher_get_rmw_publisher_handle(const rcl_publisher_t * publisher); diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index b099360..4c27d73 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -24,6 +24,7 @@ extern "C" #include "rcl/publisher.h" #include "rcl/subscription.h" #include "rcl/types.h" +#include "rcl/visibility_control.h" /// Global initialization of rcl. /* Unless otherwise noted, this must be called before using any rcl functions. @@ -52,6 +53,7 @@ extern "C" * RCL_RET_ALREADY_INIT if rcl_init has already been called, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_init(int argc, char ** argv, rcl_allocator_t allocator); @@ -73,16 +75,19 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator); * \return RCL_RET_OK if shutdown is successful, otherwise RCL_RET_ERROR or * RCL_RET_NOT_INIT if rcl_init has not yet been called */ +RCL_PUBLIC rcl_ret_t rcl_shutdown(); /// Returns an uint64_t number that is unique for the latest rcl_init call. /* If called before rcl_init or after rcl_shutdown then 0 will be returned. */ +RCL_PUBLIC uint64_t rcl_get_instance_id(); /// Return true until rcl_shutdown is called, then false. /* This function is thread safe. */ +RCL_PUBLIC bool rcl_ok(); diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 8d6e6f7..0bdee17 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -23,6 +23,7 @@ extern "C" #include "rosidl_generator_c/message_type_support.h" #include "rcl/node.h" +#include "rcl/visibility_control.h" /// Internal rcl implementation struct. struct rcl_subscription_impl_t; @@ -51,6 +52,7 @@ typedef struct rcl_subscription_options_t * It's also possible to use calloc() instead of this if the rcl_subscription_t * is being allocated on the heap. */ +RCL_PUBLIC rcl_subscription_t rcl_get_zero_initialized_subscription(); @@ -124,6 +126,7 @@ rcl_get_zero_initialized_subscription(); * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_subscription_init( rcl_subscription_t * subscription, @@ -149,10 +152,12 @@ rcl_subscription_init( * RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node); /// Return the default subscription options in a rcl_subscription_options_t. +RCL_PUBLIC rcl_subscription_options_t rcl_subscription_get_default_options(); @@ -198,6 +203,7 @@ rcl_subscription_get_default_options(); * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_take( const rcl_subscription_t * subscription, @@ -221,6 +227,7 @@ rcl_take( * \param[in] subscription the pointer to the subscription * \return name string if successful, otherwise NULL */ +RCL_PUBLIC const char * rcl_subscription_get_topic_name(const rcl_subscription_t * subscription); @@ -240,6 +247,7 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription); * \param[in] subscription pointer to the subscription * \return options struct if successful, otherwise NULL */ +RCL_PUBLIC const rcl_subscription_options_t * rcl_subscription_get_options(const rcl_subscription_t * subscription); @@ -258,6 +266,7 @@ rcl_subscription_get_options(const rcl_subscription_t * subscription); * \param[in] subscription pointer to the rcl subscription * \return rmw subscription handle if successful, otherwise NULL */ +RCL_PUBLIC rmw_subscription_t * rcl_subscription_get_rmw_subscription_handle(const rcl_subscription_t * subscription); diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index b12f508..3fa7144 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -21,6 +21,7 @@ extern "C" #endif #include "rcl/types.h" +#include "rcl/visibility_control.h" #define RCL_S_TO_NS(seconds) (seconds * 1000000000) #define RCL_MS_TO_NS(milliseconds) (milliseconds * 1000000) @@ -91,6 +92,7 @@ typedef struct rcl_duration_t * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_system_time_point_now(rcl_system_time_point_t * now); @@ -114,6 +116,7 @@ rcl_system_time_point_now(rcl_system_time_point_t * now); * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_steady_time_point_now(rcl_steady_time_point_t * now); diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index 2298741..a8b72a0 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -50,6 +50,7 @@ typedef struct rcl_timer_t typedef void (* rcl_timer_callback_t)(rcl_timer_t *, uint64_t); /// Return a zero initialized timer. +RCL_PUBLIC rcl_timer_t rcl_get_zero_initialized_timer(); @@ -110,6 +111,7 @@ rcl_get_zero_initialized_timer(); * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_timer_init( rcl_timer_t * timer, @@ -133,6 +135,7 @@ rcl_timer_init( * \return RCL_RET_OK if the timer was finalized successfully, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_timer_fini(rcl_timer_t * timer); @@ -165,6 +168,7 @@ rcl_timer_fini(rcl_timer_t * timer); * RCL_RET_TIMER_CANCELED if the timer has been canceled, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_timer_call(rcl_timer_t * timer); @@ -188,6 +192,7 @@ rcl_timer_call(rcl_timer_t * timer); * RCL_RET_TIMER_INVALID if the timer is invalid, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready); @@ -216,6 +221,7 @@ rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready); * RCL_RET_TIMER_INVALID if the timer is invalid, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call); @@ -241,6 +247,7 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt * RCL_RET_TIMER_INVALID if the timer is invalid, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_since_last_call); @@ -260,6 +267,7 @@ rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_si * RCL_RET_TIMER_INVALID if the timer is invalid, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period); @@ -283,6 +291,7 @@ rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period); * RCL_RET_TIMER_INVALID if the timer is invalid, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period); @@ -298,6 +307,7 @@ rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64 * \param[in] timer handle to the timer from the callback should be returned * \return function pointer to the callback, or NULL if an error occurred */ +RCL_PUBLIC rcl_timer_callback_t rcl_timer_get_callback(const rcl_timer_t * timer); @@ -315,6 +325,7 @@ rcl_timer_get_callback(const rcl_timer_t * timer); * \param[in] new_callback the callback to be exchanged into the timer * \return function pointer to the old callback, or NULL if an error occurred */ +RCL_PUBLIC rcl_timer_callback_t rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback); @@ -335,6 +346,7 @@ rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_ * RCL_RET_TIMER_INVALID if the timer is invalid, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_timer_cancel(rcl_timer_t * timer); @@ -355,6 +367,7 @@ rcl_timer_cancel(rcl_timer_t * timer); * RCL_RET_TIMER_INVALID if the timer is invalid, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled); @@ -373,6 +386,7 @@ rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled); * RCL_RET_TIMER_INVALID if the timer is invalid, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_timer_reset(rcl_timer_t * timer); diff --git a/rcl/include/rcl/visibility_control.h b/rcl/include/rcl/visibility_control.h new file mode 100644 index 0000000..24b3a25 --- /dev/null +++ b/rcl/include/rcl/visibility_control.h @@ -0,0 +1,58 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__VISIBILITY_CONTROL_H_ +#define RCL__VISIBILITY_CONTROL_H_ + +#if __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RCL_EXPORT __attribute__ ((dllexport)) + #define RCL_IMPORT __attribute__ ((dllimport)) + #else + #define RCL_EXPORT __declspec(dllexport) + #define RCL_IMPORT __declspec(dllimport) + #endif + #ifdef RCL_BUILDING_DLL + #define RCL_PUBLIC RCL_EXPORT + #else + #define RCL_PUBLIC RCL_IMPORT + #endif + #define RCL_PUBLIC_TYPE RCL_PUBLIC + #define RCL_LOCAL +#else + #define RCL_EXPORT __attribute__ ((visibility("default"))) + #define RCL_IMPORT + #if __GNUC__ >= 4 + #define RCL_PUBLIC __attribute__ ((visibility("default"))) + #define RCL_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RCL_PUBLIC + #define RCL_LOCAL + #endif + #define RCL_PUBLIC_TYPE +#endif + +#if __cplusplus +} +#endif + +#endif // RCL__VISIBILITY_CONTROL_H_ diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index 55e926b..0453f91 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -27,6 +27,7 @@ extern "C" #include "rcl/guard_condition.h" #include "rcl/timer.h" #include "rcl/types.h" +#include "rcl/visibility_control.h" struct rcl_wait_set_impl_t; @@ -47,6 +48,7 @@ typedef struct rcl_wait_set_t } rcl_wait_set_t; /// Return a rcl_wait_set_t struct with members set to NULL. +RCL_PUBLIC rcl_wait_set_t rcl_get_zero_initialized_wait_set(); @@ -90,6 +92,7 @@ rcl_get_zero_initialized_wait_set(); * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_wait_set_init( rcl_wait_set_t * wait_set, @@ -117,6 +120,7 @@ rcl_wait_set_init( * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_wait_set_fini(rcl_wait_set_t * wait_set); @@ -133,6 +137,7 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set); * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * allocator); @@ -151,6 +156,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al * RCL_RET_WAIT_SET_FULL if the subscription set is full, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_wait_set_add_subscription( rcl_wait_set_t * wait_set, @@ -171,6 +177,7 @@ rcl_wait_set_add_subscription( * RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); @@ -199,6 +206,7 @@ rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC rcl_ret_t rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size); @@ -206,6 +214,7 @@ rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size); /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_add_subscription */ +RCL_PUBLIC rcl_ret_t rcl_wait_set_add_guard_condition( rcl_wait_set_t * wait_set, @@ -215,6 +224,7 @@ rcl_wait_set_add_guard_condition( /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_clear_subscriptions */ +RCL_PUBLIC rcl_ret_t rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set); @@ -222,6 +232,7 @@ rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set); /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_resize_subscriptions */ +RCL_PUBLIC rcl_ret_t rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); @@ -229,6 +240,7 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_add_subscription */ +RCL_PUBLIC rcl_ret_t rcl_wait_set_add_timer( rcl_wait_set_t * wait_set, @@ -238,6 +250,7 @@ rcl_wait_set_add_timer( /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_clear_subscriptions */ +RCL_PUBLIC rcl_ret_t rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set); @@ -245,6 +258,7 @@ rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set); /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_resize_subscriptions */ +RCL_PUBLIC rcl_ret_t rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size); @@ -337,6 +351,7 @@ rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size); * RCL_RET_TIMEOUT if the timeout expired before something was ready, or * RCL_RET_ERROR an unspecified error occur. */ +RCL_PUBLIC rcl_ret_t rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout); diff --git a/rcl/src/rcl/common.c b/rcl/src/rcl/common.c index 89a667f..5cec065 100644 --- a/rcl/src/rcl/common.c +++ b/rcl/src/rcl/common.c @@ -34,6 +34,9 @@ rcl_impl_getenv(const char * env_name, const char ** env_value) *env_value = NULL; #if !defined(WIN32) *env_value = getenv(env_name); + if (*env_value == NULL) { + *env_value = ""; + } #else size_t required_size; errno_t ret = getenv_s(&required_size, __env_buffer, sizeof(__env_buffer), env_name); diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h index b27232f..2b2d5f7 100644 --- a/rcl/src/rcl/common.h +++ b/rcl/src/rcl/common.h @@ -31,19 +31,20 @@ extern "C" error_statement; \ } -/// Retrieve the value of the given environment variable if it exists, or NULL. -/* The returned char is only valid until the next time this function is called, - * because the returned char * is a direct pointer to the static storage. +/// Retrieve the value of the given environment variable if it exists, or "". +/* The returned cstring is only valid until the next time this function is + * called, because it is a direct pointer to the static storage. * The returned value char * variable should never have free() called on it. + * If the environment variable is not set, an empty string will be returned. * - * Environment variable values will be truncated at 2048 characters on Windows. + * Environment variables will be truncated at 2048 characters on Windows. * * This function does not allocate heap memory, but the system calls might. * This function is not thread-safe. * This function is not lock-free. * * \param[in] env_name the name of the environment variable - * \param[out] env_value pointer to the value cstring + * \param[out] env_value pointer to the value cstring, or "" if unset * \return RCL_RET_OK if the value is retrieved successfully, or * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or * RCL_RET_ERROR an unspecified error occur. diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 8f10bc2..5fbd8dc 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -16,18 +16,22 @@ if(AMENT_ENABLE_TESTING) set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") endif() target_link_libraries(${PROJECT_NAME}_memory_tools ${GTEST_LIBRARIES} ${extra_test_libraries}) + target_compile_definitions(${PROJECT_NAME}_memory_tools + PRIVATE "RCL_MEMORY_TOOLS_BUILDING_DLL") list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools) - ament_add_gtest(test_memory_tools test_memory_tools.cpp ENV ${extra_memory_tools_env}) - if(TARGET test_memory_tools) - target_include_directories(test_memory_tools PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ) - if(NOT WIN32) - set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") + if(NOT WIN32) + ament_add_gtest(test_memory_tools test_memory_tools.cpp ENV ${extra_memory_tools_env}) + if(TARGET test_memory_tools) + target_include_directories(test_memory_tools PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + if(NOT WIN32) + set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() + target_link_libraries(test_memory_tools ${PROJECT_NAME} ${extra_test_libraries}) endif() - target_link_libraries(test_memory_tools ${PROJECT_NAME} ${extra_test_libraries}) endif() ament_add_gtest(test_allocator rcl/test_allocator.cpp ENV ${extra_memory_tools_env}) diff --git a/rcl/test/memory_tools.cpp b/rcl/test/memory_tools.cpp index 0a5c1e2..5e1887d 100644 --- a/rcl/test/memory_tools.cpp +++ b/rcl/test/memory_tools.cpp @@ -48,7 +48,7 @@ void stop_memory_checking() /****************************************************************************** * End Apple *****************************************************************************/ -#elif defined(WIN32) +// #elif defined(WIN32) /****************************************************************************** * Begin Windows *****************************************************************************/ @@ -61,13 +61,37 @@ void stop_memory_checking() #else // Default case: do nothing. +#include "./memory_tools.hpp" + +#include + void start_memory_checking() { - MALLOC_PRINTF("starting memory checking... not available\n"); + printf("starting memory checking... not available\n"); } void stop_memory_checking() { - MALLOC_PRINTF("stopping memory checking... not available\n"); + printf("stopping memory checking... not available\n"); } +void assert_no_malloc_begin() {} + +void assert_no_malloc_end() {} + +void set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback) {} + +void assert_no_realloc_begin() {} + +void assert_no_realloc_end() {} + +void set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback) {} + +void assert_no_free_begin() {} + +void assert_no_free_end() {} + +void set_on_unepexcted_free_callback(UnexpectedCallbackType callback) {} + +void memory_checking_thread_init() {} + #endif // !defined(WIN32) diff --git a/rcl/test/memory_tools.hpp b/rcl/test/memory_tools.hpp index f220baf..9941f63 100644 --- a/rcl/test/memory_tools.hpp +++ b/rcl/test/memory_tools.hpp @@ -19,35 +19,89 @@ #ifndef MEMORY_TOOLS_HPP_ #define MEMORY_TOOLS_HPP_ -#include #include #include +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RCL_MEMORY_TOOLS_EXPORT __attribute__ ((dllexport)) + #define RCL_MEMORY_TOOLS_IMPORT __attribute__ ((dllimport)) + #else + #define RCL_MEMORY_TOOLS_EXPORT __declspec(dllexport) + #define RCL_MEMORY_TOOLS_IMPORT __declspec(dllimport) + #endif + #ifdef RCL_MEMORY_TOOLS_BUILDING_DLL + #define RCL_MEMORY_TOOLS_PUBLIC RCL_MEMORY_TOOLS_EXPORT + #else + #define RCL_MEMORY_TOOLS_PUBLIC RCL_MEMORY_TOOLS_IMPORT + #endif + #define RCL_MEMORY_TOOLS_PUBLIC_TYPE RCL_MEMORY_TOOLS_PUBLIC + #define RCL_MEMORY_TOOLS_LOCAL +#else + #define RCL_MEMORY_TOOLS_EXPORT __attribute__ ((visibility("default"))) + #define RCL_MEMORY_TOOLS_IMPORT + #if __GNUC__ >= 4 + #define RCL_MEMORY_TOOLS_PUBLIC __attribute__ ((visibility("default"))) + #define RCL_MEMORY_TOOLS_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RCL_MEMORY_TOOLS_PUBLIC + #define RCL_MEMORY_TOOLS_LOCAL + #endif + #define RCL_MEMORY_TOOLS_PUBLIC_TYPE +#endif + typedef std::function UnexpectedCallbackType; -void start_memory_checking(); +RCL_MEMORY_TOOLS_PUBLIC +void +start_memory_checking(); #define ASSERT_NO_MALLOC(statements) \ assert_no_malloc_begin(); statements; assert_no_malloc_end(); -void assert_no_malloc_begin(); -void assert_no_malloc_end(); -void set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback); +RCL_MEMORY_TOOLS_PUBLIC +void +assert_no_malloc_begin(); +RCL_MEMORY_TOOLS_PUBLIC +void +assert_no_malloc_end(); +RCL_MEMORY_TOOLS_PUBLIC +void +set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback); #define ASSERT_NO_REALLOC(statements) \ assert_no_realloc_begin(); statements; assert_no_realloc_end(); -void assert_no_realloc_begin(); -void assert_no_realloc_end(); -void set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback); +RCL_MEMORY_TOOLS_PUBLIC +void +assert_no_realloc_begin(); +RCL_MEMORY_TOOLS_PUBLIC +void +assert_no_realloc_end(); +RCL_MEMORY_TOOLS_PUBLIC +void +set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback); #define ASSERT_NO_FREE(statements) \ assert_no_free_begin(); statements; assert_no_free_end(); -void assert_no_free_begin(); -void assert_no_free_end(); -void set_on_unepexcted_free_callback(UnexpectedCallbackType callback); +RCL_MEMORY_TOOLS_PUBLIC +void +assert_no_free_begin(); +RCL_MEMORY_TOOLS_PUBLIC +void +assert_no_free_end(); +RCL_MEMORY_TOOLS_PUBLIC +void +set_on_unepexcted_free_callback(UnexpectedCallbackType callback); -void stop_memory_checking(); +RCL_MEMORY_TOOLS_PUBLIC +void +stop_memory_checking(); -void memory_checking_thread_init(); +RCL_MEMORY_TOOLS_PUBLIC +void +memory_checking_thread_init(); #endif // MEMORY_TOOLS_HPP_ diff --git a/rcl/test/rcl/test_allocator.cpp b/rcl/test/rcl/test_allocator.cpp index 5477713..5bb65b6 100644 --- a/rcl/test/rcl/test_allocator.cpp +++ b/rcl/test/rcl/test_allocator.cpp @@ -49,6 +49,10 @@ public: /* Tests the default allocator. */ TEST_F(TestAllocatorFixture, test_default_allocator_normal) { +#ifdef WIN32 + printf("Allocator tests disabled on Windows.\n"); + return; +#endif ASSERT_NO_MALLOC( rcl_allocator_t allocator = rcl_get_default_allocator(); ) diff --git a/rcl/test/rcl/test_common.cpp b/rcl/test/rcl/test_common.cpp index b4732eb..d7571cf 100644 --- a/rcl/test/rcl/test_common.cpp +++ b/rcl/test/rcl/test_common.cpp @@ -17,6 +17,7 @@ #include #include "../../src/rcl/common.h" +#include "../../src/rcl/common.c" /* Tests the default allocator. * @@ -31,21 +32,21 @@ TEST(TestCommon, test_getenv) { const char * env; rcl_ret_t ret; ret = rcl_impl_getenv("NORMAL_TEST", NULL); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); rcl_reset_error(); ret = rcl_impl_getenv(NULL, &env); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); rcl_reset_error(); ret = rcl_impl_getenv("SHOULD_NOT_EXIST_TEST", &env); - EXPECT_EQ(ret, RCL_RET_OK); - EXPECT_EQ(env, nullptr) << std::string(env); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ("", std::string(env)) << std::string(env); rcl_reset_error(); ret = rcl_impl_getenv("NORMAL_TEST", &env); - EXPECT_EQ(ret, RCL_RET_OK); - ASSERT_NE(env, nullptr); - EXPECT_EQ(std::string(env), "foo"); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_NE(nullptr, env); + EXPECT_EQ("foo", std::string(env)); ret = rcl_impl_getenv("EMPTY_TEST", &env); - EXPECT_EQ(ret, RCL_RET_OK); - ASSERT_NE(env, nullptr); - EXPECT_EQ(std::string(env), ""); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_NE(nullptr, env); + EXPECT_EQ("", std::string(env)); } diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index 99851fe..7a769a6 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -96,14 +96,14 @@ TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) { stop_memory_checking(); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); EXPECT_NE(now.nanoseconds, 0); - // Compare to std::chrono::steady_clock time (within a second). + // Compare to std::chrono::steady_clock difference of two times (within a second). now = {0}; ret = rcl_steady_time_point_now(&now); - { - std::chrono::steady_clock::time_point now_sc = std::chrono::steady_clock::now(); - auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); - int64_t now_ns_int = now_ns.count(); - int64_t now_diff = now.nanoseconds - now_ns_int; - EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(1000)) << "steady_clock differs"; - } + std::chrono::steady_clock::time_point now_sc = std::chrono::steady_clock::now(); + rcl_steady_time_point_t later; + ret = rcl_steady_time_point_now(&later); + std::chrono::steady_clock::time_point later_sc = std::chrono::steady_clock::now(); + uint64_t steady_diff = later.nanoseconds - now.nanoseconds; + uint64_t sc_diff = (now_sc - later_sc).count(); + EXPECT_LE(llabs(steady_diff - sc_diff), RCL_MS_TO_NS(1000)) << "steady_clock differs"; } From 5c07b9eec365faabf18978a6bcdcc6069b2d4419 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 14 Dec 2015 00:42:23 -0800 Subject: [PATCH 33/71] fixup --- rcl/test/memory_tools_common.cpp | 1 + rcl/test/rcl/test_time.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools_common.cpp index 18c3ef0..6e3d016 100644 --- a/rcl/test/memory_tools_common.cpp +++ b/rcl/test/memory_tools_common.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #if defined(__APPLE__) diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index 7a769a6..54b33fc 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -103,7 +103,7 @@ TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) { rcl_steady_time_point_t later; ret = rcl_steady_time_point_now(&later); std::chrono::steady_clock::time_point later_sc = std::chrono::steady_clock::now(); - uint64_t steady_diff = later.nanoseconds - now.nanoseconds; - uint64_t sc_diff = (now_sc - later_sc).count(); + int64_t steady_diff = later.nanoseconds - now.nanoseconds; + int64_t sc_diff = (now_sc - later_sc).count(); EXPECT_LE(llabs(steady_diff - sc_diff), RCL_MS_TO_NS(1000)) << "steady_clock differs"; } From 4ccaefb602537611d4a7178e4f02cc1fdfc538ec Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 14 Dec 2015 12:33:10 -0800 Subject: [PATCH 34/71] add stdatomic compatability header for older gcc and clang versions --- rcl/src/rcl/stdatomic_helper.h | 8 + rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h | 393 +++++++++++++++++++ 2 files changed, 401 insertions(+) create mode 100644 rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h diff --git a/rcl/src/rcl/stdatomic_helper.h b/rcl/src/rcl/stdatomic_helper.h index fef4bff..1c1c7a7 100644 --- a/rcl/src/rcl/stdatomic_helper.h +++ b/rcl/src/rcl/stdatomic_helper.h @@ -17,7 +17,15 @@ #if !defined(WIN32) +#if defined(__GNUC__) && __GNUC__ <= 4 && __GNUC_MINOR__ <= 9 +// If GCC and below GCC-4.9, use the compatability header. +#include "stdatomic_helper/gcc/stdatomic.h" +#elif defined(__clang__) && defined(__has_feature) && !__has_feature(c_atomic) +// If Clang and no c_atomics (true for some older versions), use the compatability header. +#include "stdatomic_helper/gcc/stdatomic.h" +#else #include +#endif #define rcl_atomic_load(object, out) (out) = atomic_load(object) diff --git a/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h b/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h new file mode 100644 index 0000000..e6c318a --- /dev/null +++ b/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h @@ -0,0 +1,393 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This file was taken from: + * https://gist.github.com/nhatminhle/5181506 + * It is a modification of the FreeBSD stdatomic header with some modifications + * so that it will work with gcc 4.7+ and Clang 3.1+. + * The original FreeBSD header can be found here: + * https://github.com/freebsd/freebsd/blob/release/10.2.0/sys/sys/stdatomic.h + */ + +/**** Start included file. ****/ + +/* + * An implementation of C11 stdatomic.h directly borrowed from FreeBSD + * (original copyright follows), with minor modifications for + * portability to other systems. Works for recent Clang (that + * implement the feature c_atomic) and GCC 4.7+; includes + * compatibility for GCC below 4.7 but I wouldn't recommend it. + * + * Caveats and limitations: + * - Only the ``_Atomic parentheses'' notation is implemented, while + * the ``_Atomic space'' one is not. + * - _Atomic types must be typedef'ed, or programs using them will + * not type check correctly (incompatible anonymous structure + * types). + * - Non-scalar _Atomic types would require runtime support for + * runtime locking, which, as far as I know, is not currently + * available on any system. + */ + +/*- + * Copyright (c) 2011 Ed Schouten + * David Chisnall + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * $FreeBSD: src/include/stdatomic.h,v 1.10.2.2 2012/05/30 19:21:54 theraven Exp $ + */ + +#ifndef _STDATOMIC_H_ +#define _STDATOMIC_H_ + +#include +#include + +#if !defined(__has_feature) +#define __has_feature(x) 0 +#endif +#if !defined(__has_builtin) +#define __has_builtin(x) 0 +#endif +#if !defined(__GNUC_PREREQ__) +#if defined(__GNUC__) && defined(__GNUC_MINOR__) +#define __GNUC_PREREQ__(maj, min) \ + ((__GNUC__ << 16) + __GNUC_MINOR__ >= ((maj) << 16) + (min)) +#else +#define __GNUC_PREREQ__(maj, min) 0 +#endif +#endif + +#if !defined(__CLANG_ATOMICS) && !defined(__GNUC_ATOMICS) +#if __has_feature(c_atomic) +#define __CLANG_ATOMICS +#elif __GNUC_PREREQ__(4, 7) +#define __GNUC_ATOMICS +#elif !defined(__GNUC__) +#error "stdatomic.h does not support your compiler" +#endif +#endif + +#if !defined(__CLANG_ATOMICS) +#define _Atomic(T) struct { volatile __typeof__(T) __val; } +#endif + +/* + * 7.17.2 Initialization. + */ + +#if defined(__CLANG_ATOMICS) +#define ATOMIC_VAR_INIT(value) (value) +#define atomic_init(obj, value) __c11_atomic_init(obj, value) +#else +#define ATOMIC_VAR_INIT(value) { .__val = (value) } +#define atomic_init(obj, value) do { \ + (obj)->__val = (value); \ +} while (0) +#endif + +/* + * Clang and recent GCC both provide predefined macros for the memory + * orderings. If we are using a compiler that doesn't define them, use the + * clang values - these will be ignored in the fallback path. + */ + +#ifndef __ATOMIC_RELAXED +#define __ATOMIC_RELAXED 0 +#endif +#ifndef __ATOMIC_CONSUME +#define __ATOMIC_CONSUME 1 +#endif +#ifndef __ATOMIC_ACQUIRE +#define __ATOMIC_ACQUIRE 2 +#endif +#ifndef __ATOMIC_RELEASE +#define __ATOMIC_RELEASE 3 +#endif +#ifndef __ATOMIC_ACQ_REL +#define __ATOMIC_ACQ_REL 4 +#endif +#ifndef __ATOMIC_SEQ_CST +#define __ATOMIC_SEQ_CST 5 +#endif + +/* + * 7.17.3 Order and consistency. + * + * The memory_order_* constants that denote the barrier behaviour of the + * atomic operations. + */ + +enum memory_order { + memory_order_relaxed = __ATOMIC_RELAXED, + memory_order_consume = __ATOMIC_CONSUME, + memory_order_acquire = __ATOMIC_ACQUIRE, + memory_order_release = __ATOMIC_RELEASE, + memory_order_acq_rel = __ATOMIC_ACQ_REL, + memory_order_seq_cst = __ATOMIC_SEQ_CST +}; + +typedef enum memory_order memory_order; + +/* + * 7.17.4 Fences. + */ + +#ifdef __CLANG_ATOMICS +#define atomic_thread_fence(order) __c11_atomic_thread_fence(order) +#define atomic_signal_fence(order) __c11_atomic_signal_fence(order) +#elif defined(__GNUC_ATOMICS) +#define atomic_thread_fence(order) __atomic_thread_fence(order) +#define atomic_signal_fence(order) __atomic_signal_fence(order) +#else +#define atomic_thread_fence(order) __sync_synchronize() +#define atomic_signal_fence(order) __asm volatile ("" : : : "memory") +#endif + +/* + * 7.17.5 Lock-free property. + */ + +#if defined(__CLANG_ATOMICS) +#define atomic_is_lock_free(obj) \ + __c11_atomic_is_lock_free(sizeof(obj)) +#elif defined(__GNUC_ATOMICS) +#define atomic_is_lock_free(obj) \ + __atomic_is_lock_free(sizeof((obj)->__val)) +#else +#define atomic_is_lock_free(obj) \ + (sizeof((obj)->__val) <= sizeof(void *)) +#endif + +/* + * 7.17.6 Atomic integer types. + */ + +typedef _Atomic(_Bool) atomic_bool; +typedef _Atomic(char) atomic_char; +typedef _Atomic(signed char) atomic_schar; +typedef _Atomic(unsigned char) atomic_uchar; +typedef _Atomic(short) atomic_short; +typedef _Atomic(unsigned short) atomic_ushort; +typedef _Atomic(int) atomic_int; +typedef _Atomic(unsigned int) atomic_uint; +typedef _Atomic(long) atomic_long; +typedef _Atomic(unsigned long) atomic_ulong; +typedef _Atomic(long long) atomic_llong; +typedef _Atomic(unsigned long long) atomic_ullong; +#if 0 +typedef _Atomic(char16_t) atomic_char16_t; +typedef _Atomic(char32_t) atomic_char32_t; +#endif +typedef _Atomic(wchar_t) atomic_wchar_t; +typedef _Atomic(int_least8_t) atomic_int_least8_t; +typedef _Atomic(uint_least8_t) atomic_uint_least8_t; +typedef _Atomic(int_least16_t) atomic_int_least16_t; +typedef _Atomic(uint_least16_t) atomic_uint_least16_t; +typedef _Atomic(int_least32_t) atomic_int_least32_t; +typedef _Atomic(uint_least32_t) atomic_uint_least32_t; +typedef _Atomic(int_least64_t) atomic_int_least64_t; +typedef _Atomic(uint_least64_t) atomic_uint_least64_t; +typedef _Atomic(int_fast8_t) atomic_int_fast8_t; +typedef _Atomic(uint_fast8_t) atomic_uint_fast8_t; +typedef _Atomic(int_fast16_t) atomic_int_fast16_t; +typedef _Atomic(uint_fast16_t) atomic_uint_fast16_t; +typedef _Atomic(int_fast32_t) atomic_int_fast32_t; +typedef _Atomic(uint_fast32_t) atomic_uint_fast32_t; +typedef _Atomic(int_fast64_t) atomic_int_fast64_t; +typedef _Atomic(uint_fast64_t) atomic_uint_fast64_t; +typedef _Atomic(intptr_t) atomic_intptr_t; +typedef _Atomic(uintptr_t) atomic_uintptr_t; +typedef _Atomic(size_t) atomic_size_t; +typedef _Atomic(ptrdiff_t) atomic_ptrdiff_t; +typedef _Atomic(intmax_t) atomic_intmax_t; +typedef _Atomic(uintmax_t) atomic_uintmax_t; + +/* + * 7.17.7 Operations on atomic types. + */ + +/* + * Compiler-specific operations. + */ + +#if defined(__CLANG_ATOMICS) +#define atomic_compare_exchange_strong_explicit(object, expected, \ + desired, success, failure) \ + __c11_atomic_compare_exchange_strong(object, expected, desired, \ + success, failure) +#define atomic_compare_exchange_weak_explicit(object, expected, \ + desired, success, failure) \ + __c11_atomic_compare_exchange_weak(object, expected, desired, \ + success, failure) +#define atomic_exchange_explicit(object, desired, order) \ + __c11_atomic_exchange(object, desired, order) +#define atomic_fetch_add_explicit(object, operand, order) \ + __c11_atomic_fetch_add(object, operand, order) +#define atomic_fetch_and_explicit(object, operand, order) \ + __c11_atomic_fetch_and(object, operand, order) +#define atomic_fetch_or_explicit(object, operand, order) \ + __c11_atomic_fetch_or(object, operand, order) +#define atomic_fetch_sub_explicit(object, operand, order) \ + __c11_atomic_fetch_sub(object, operand, order) +#define atomic_fetch_xor_explicit(object, operand, order) \ + __c11_atomic_fetch_xor(object, operand, order) +#define atomic_load_explicit(object, order) \ + __c11_atomic_load(object, order) +#define atomic_store_explicit(object, desired, order) \ + __c11_atomic_store(object, desired, order) +#elif defined(__GNUC_ATOMICS) +#define atomic_compare_exchange_strong_explicit(object, expected, \ + desired, success, failure) \ + __atomic_compare_exchange_n(&(object)->__val, expected, \ + desired, 0, success, failure) +#define atomic_compare_exchange_weak_explicit(object, expected, \ + desired, success, failure) \ + __atomic_compare_exchange_n(&(object)->__val, expected, \ + desired, 1, success, failure) +#define atomic_exchange_explicit(object, desired, order) \ + __atomic_exchange_n(&(object)->__val, desired, order) +#define atomic_fetch_add_explicit(object, operand, order) \ + __atomic_fetch_add(&(object)->__val, operand, order) +#define atomic_fetch_and_explicit(object, operand, order) \ + __atomic_fetch_and(&(object)->__val, operand, order) +#define atomic_fetch_or_explicit(object, operand, order) \ + __atomic_fetch_or(&(object)->__val, operand, order) +#define atomic_fetch_sub_explicit(object, operand, order) \ + __atomic_fetch_sub(&(object)->__val, operand, order) +#define atomic_fetch_xor_explicit(object, operand, order) \ + __atomic_fetch_xor(&(object)->__val, operand, order) +#define atomic_load_explicit(object, order) \ + __atomic_load_n(&(object)->__val, order) +#define atomic_store_explicit(object, desired, order) \ + __atomic_store_n(&(object)->__val, desired, order) +#else +#define atomic_compare_exchange_strong_explicit(object, expected, \ + desired, success, failure) ({ \ + __typeof__((object)->__val) __v; \ + _Bool __r; \ + __v = __sync_val_compare_and_swap(&(object)->__val, \ + *(expected), desired); \ + __r = *(expected) == __v; \ + *(expected) = __v; \ + __r; \ +}) + +#define atomic_compare_exchange_weak_explicit(object, expected, \ + desired, success, failure) \ + atomic_compare_exchange_strong_explicit(object, expected, \ + desired, success, failure) +#if __has_builtin(__sync_swap) +/* Clang provides a full-barrier atomic exchange - use it if available. */ +#define atomic_exchange_explicit(object, desired, order) \ + __sync_swap(&(object)->__val, desired) +#else +/* + * __sync_lock_test_and_set() is only an acquire barrier in theory (although in + * practice it is usually a full barrier) so we need an explicit barrier after + * it. + */ +#define atomic_exchange_explicit(object, desired, order) ({ \ + __typeof__((object)->__val) __v; \ + __v = __sync_lock_test_and_set(&(object)->__val, desired); \ + __sync_synchronize(); \ + __v; \ +}) +#endif +#define atomic_fetch_add_explicit(object, operand, order) \ + __sync_fetch_and_add(&(object)->__val, operand) +#define atomic_fetch_and_explicit(object, operand, order) \ + __sync_fetch_and_and(&(object)->__val, operand) +#define atomic_fetch_or_explicit(object, operand, order) \ + __sync_fetch_and_or(&(object)->__val, operand) +#define atomic_fetch_sub_explicit(object, operand, order) \ + __sync_fetch_and_sub(&(object)->__val, operand) +#define atomic_fetch_xor_explicit(object, operand, order) \ + __sync_fetch_and_xor(&(object)->__val, operand) +#define atomic_load_explicit(object, order) \ + __sync_fetch_and_add(&(object)->__val, 0) +#define atomic_store_explicit(object, desired, order) do { \ + __sync_synchronize(); \ + (object)->__val = (desired); \ + __sync_synchronize(); \ +} while (0) +#endif + +/* + * Convenience functions. + */ + +#define atomic_compare_exchange_strong(object, expected, desired) \ + atomic_compare_exchange_strong_explicit(object, expected, \ + desired, memory_order_seq_cst, memory_order_seq_cst) +#define atomic_compare_exchange_weak(object, expected, desired) \ + atomic_compare_exchange_weak_explicit(object, expected, \ + desired, memory_order_seq_cst, memory_order_seq_cst) +#define atomic_exchange(object, desired) \ + atomic_exchange_explicit(object, desired, memory_order_seq_cst) +#define atomic_fetch_add(object, operand) \ + atomic_fetch_add_explicit(object, operand, memory_order_seq_cst) +#define atomic_fetch_and(object, operand) \ + atomic_fetch_and_explicit(object, operand, memory_order_seq_cst) +#define atomic_fetch_or(object, operand) \ + atomic_fetch_or_explicit(object, operand, memory_order_seq_cst) +#define atomic_fetch_sub(object, operand) \ + atomic_fetch_sub_explicit(object, operand, memory_order_seq_cst) +#define atomic_fetch_xor(object, operand) \ + atomic_fetch_xor_explicit(object, operand, memory_order_seq_cst) +#define atomic_load(object) \ + atomic_load_explicit(object, memory_order_seq_cst) +#define atomic_store(object, desired) \ + atomic_store_explicit(object, desired, memory_order_seq_cst) + +/* + * 7.17.8 Atomic flag type and operations. + */ + +typedef atomic_bool atomic_flag; + +#define ATOMIC_FLAG_INIT ATOMIC_VAR_INIT(0) + +#define atomic_flag_clear_explicit(object, order) \ + atomic_store_explicit(object, 0, order) +#define atomic_flag_test_and_set_explicit(object, order) \ + atomic_compare_exchange_strong_explicit(object, 0, 1, order, order) + +#define atomic_flag_clear(object) \ + atomic_flag_clear_explicit(object, memory_order_seq_cst) +#define atomic_flag_test_and_set(object) \ + atomic_flag_test_and_set_explicit(object, memory_order_seq_cst) + +#endif /* !_STDATOMIC_H_ */ \ No newline at end of file From 536b80cac2fe081a63dd7b8372e3d7cd0d77ad17 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 14 Dec 2015 13:11:41 -0800 Subject: [PATCH 35/71] fixup warnings --- rcl/test/memory_tools_common.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools_common.cpp index 6e3d016..ab49f65 100644 --- a/rcl/test/memory_tools_common.cpp +++ b/rcl/test/memory_tools_common.cpp @@ -63,7 +63,7 @@ custom_malloc(size_t size) } void * memory = malloc(size); MALLOC_PRINTF( - "malloc expected(%s): %p %d\n", malloc_expected ? "true " : "false", memory, size); + "malloc expected(%s): %p %zu\n", malloc_expected ? "true " : "false", memory, size); return memory; } @@ -103,7 +103,7 @@ custom_realloc(void * memory_in, size_t size) } void * memory = realloc(memory_in, size); MALLOC_PRINTF( - "realloc expected(%s): %p %p %d\n", + "realloc expected(%s): %p %p %zu\n", malloc_expected ? "true " : "false", memory_in, memory, size); return memory; } From c4d30221047e54896b384b6866a9d00dec0c6207 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 14 Dec 2015 16:08:45 -0800 Subject: [PATCH 36/71] implement memory tools on linux and fixes --- rcl/CMakeLists.txt | 3 -- rcl/src/rcl/rcl.c | 6 ++- rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h | 18 ++++---- rcl/src/rcl/time_unix.c | 6 ++- rcl/src/rcl/wait.c | 10 +++-- rcl/test/CMakeLists.txt | 6 ++- rcl/test/memory_tools.cpp | 46 +++++++++++++++++++- rcl/test/memory_tools_common.cpp | 2 +- 8 files changed, 75 insertions(+), 22 deletions(-) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index f4cf14a..3baf6cb 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -37,9 +37,6 @@ macro(target) "${rmw_implementation}" ) - if(NOT WIN32) - set_target_properties(${PROJECT_NAME}${target_suffix} PROPERTIES COMPILE_FLAGS "-std=c11") - endif() # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME}${target_suffix} PRIVATE "RCL_BUILDING_LIBRARY") diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index 72a8a12..839a81c 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -35,7 +35,8 @@ static void __clean_up_init() { if (__rcl_argv) { - for (size_t i = 0; i < __rcl_argc; ++i) { + size_t i; + for (i = 0; i < __rcl_argc; ++i) { if (__rcl_argv[i]) { __rcl_allocator.deallocate(__rcl_argv[i], __rcl_allocator.state); } @@ -62,7 +63,8 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator) goto fail; } memset(__rcl_argv, 0, sizeof(char **) * argc); - for (size_t i = 0; i < argc; ++i) { + size_t i; + for (i = 0; i < argc; ++i) { __rcl_argv[i] = (char *)__rcl_allocator.allocate(strlen(argv[i]), __rcl_allocator.state); memcpy(__rcl_argv[i], argv[i], strlen(argv[i])); } diff --git a/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h b/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h index e6c318a..9b6cb20 100644 --- a/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h +++ b/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h @@ -20,6 +20,8 @@ * https://github.com/freebsd/freebsd/blob/release/10.2.0/sys/sys/stdatomic.h */ +// *INDENT-OFF* (disable uncrustify) + /**** Start included file. ****/ /* @@ -69,7 +71,7 @@ * $FreeBSD: src/include/stdatomic.h,v 1.10.2.2 2012/05/30 19:21:54 theraven Exp $ */ -#ifndef _STDATOMIC_H_ +#ifndef _STDATOMIC_H_ // NOLINT #define _STDATOMIC_H_ #include @@ -199,14 +201,14 @@ typedef _Atomic(_Bool) atomic_bool; typedef _Atomic(char) atomic_char; typedef _Atomic(signed char) atomic_schar; typedef _Atomic(unsigned char) atomic_uchar; -typedef _Atomic(short) atomic_short; -typedef _Atomic(unsigned short) atomic_ushort; +typedef _Atomic(short) atomic_short; // NOLINT +typedef _Atomic(unsigned short) atomic_ushort; // NOLINT typedef _Atomic(int) atomic_int; typedef _Atomic(unsigned int) atomic_uint; -typedef _Atomic(long) atomic_long; -typedef _Atomic(unsigned long) atomic_ulong; -typedef _Atomic(long long) atomic_llong; -typedef _Atomic(unsigned long long) atomic_ullong; +typedef _Atomic(long) atomic_long; // NOLINT +typedef _Atomic(unsigned long) atomic_ulong; // NOLINT +typedef _Atomic(long long) atomic_llong; // NOLINT +typedef _Atomic(unsigned long long) atomic_ullong; // NOLINT #if 0 typedef _Atomic(char16_t) atomic_char16_t; typedef _Atomic(char32_t) atomic_char32_t; @@ -390,4 +392,4 @@ typedef atomic_bool atomic_flag; #define atomic_flag_test_and_set(object) \ atomic_flag_test_and_set_explicit(object, memory_order_seq_cst) -#endif /* !_STDATOMIC_H_ */ \ No newline at end of file +#endif /* !_STDATOMIC_H_ */ // NOLINT diff --git a/rcl/src/rcl/time_unix.c b/rcl/src/rcl/time_unix.c index c89f295..7cd438d 100644 --- a/rcl/src/rcl/time_unix.c +++ b/rcl/src/rcl/time_unix.c @@ -29,16 +29,18 @@ extern "C" #endif #include #include +#include #include "./common.h" #include "rcl/error_handling.h" +#if !defined(__MACH__) // Assume clock_get_time is available on OS X. // This id an appropriate check for clock_gettime() according to: // http://man7.org/linux/man-pages/man2/clock_gettime.2.html -#define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L) -#if !HAS_CLOCK_GETTIME && !defined(__MACH__) +#if (!defined(_POSIX_TIMERS) || !_POSIX_TIMERS) #error no monotonic clock function available #endif +#endif #define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 9e6589f..c14178a 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -369,7 +369,8 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) // Determine the nearest timeout (given or a timer). uint64_t min_timeout = timeout; if (min_timeout > 0) { // Do not consider timer timeouts if non-blocking. - for (size_t i = 0; i < wait_set->size_of_timers; ++i) { + size_t i; + for (i = 0; i < wait_set->size_of_timers; ++i) { if (!wait_set->timers[i]) { continue; // Skip NULL timers. } @@ -407,7 +408,8 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) return RCL_RET_ERROR; } // Check for ready timers next, and set not ready timers to NULL. - for (size_t i = 0; i < wait_set->size_of_timers; ++i) { + size_t i; + for (i = 0; i < wait_set->size_of_timers; ++i) { bool is_ready = false; rcl_ret_t ret = rcl_timer_is_ready(wait_set->timers[i], &is_ready); if (ret != RCL_RET_OK) { @@ -425,14 +427,14 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) return RCL_RET_TIMEOUT; } // Set corresponding rcl subscription handles NULL. - for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) { + for (i = 0; i < wait_set->size_of_subscriptions; ++i) { assert(i < wait_set->impl->rmw_subscriptions.subscriber_count); // Defensive. if (!wait_set->impl->rmw_subscriptions.subscribers[i]) { wait_set->subscriptions[i] = NULL; } } // Set corresponding rcl guard_condition handles NULL. - for (size_t i = 0; i < wait_set->size_of_guard_conditions; ++i) { + for (i = 0; i < wait_set->size_of_guard_conditions; ++i) { assert(i < wait_set->impl->rmw_guard_conditions.guard_condition_count); // Defensive. if (!wait_set->impl->rmw_guard_conditions.guard_conditions[i]) { wait_set->guard_conditions[i] = NULL; diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 5fbd8dc..11f393a 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -15,7 +15,11 @@ if(AMENT_ENABLE_TESTING) if(NOT WIN32) set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") endif() - target_link_libraries(${PROJECT_NAME}_memory_tools ${GTEST_LIBRARIES} ${extra_test_libraries}) + if(UNIX AND NOT APPLE) + list(APPEND extra_test_libraries dl) + list(APPEND extra_memory_tools_env DL_PRELOAD=$) + endif() + target_link_libraries(${PROJECT_NAME}_memory_tools ${extra_test_libraries}) target_compile_definitions(${PROJECT_NAME}_memory_tools PRIVATE "RCL_MEMORY_TOOLS_BUILDING_DLL") list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools) diff --git a/rcl/test/memory_tools.cpp b/rcl/test/memory_tools.cpp index 5e1887d..e87c67e 100644 --- a/rcl/test/memory_tools.cpp +++ b/rcl/test/memory_tools.cpp @@ -17,10 +17,54 @@ * Begin Linux *****************************************************************************/ -// TODO(wjwwood): install custom malloc (and others) for Linux. +#include +#include +#include #include "./memory_tools_common.cpp" +void * +malloc(size_t size) +{ + void * (* libc_malloc)(size_t) = (void *(*)(size_t))dlsym(RTLD_NEXT, "malloc"); + if (enabled.load()) { + return custom_malloc(size); + } + return libc_malloc(size); +} + +void * +realloc(void * pointer, size_t size) +{ + void * (* libc_realloc)(void *, size_t) = (void *(*)(void *, size_t))dlsym(RTLD_NEXT, "realloc"); + if (enabled.load()) { + return custom_realloc(pointer, size); + } + return libc_realloc(pointer, size); +} + +void +free(void * pointer) +{ + void (* libc_free)(void *) = (void (*)(void *))dlsym(RTLD_NEXT, "free"); + if (enabled.load()) { + return custom_free(pointer); + } + return libc_free(pointer); +} + +void start_memory_checking() +{ + printf("starting memory checking...\n"); + enabled.store(true); +} + +void stop_memory_checking() +{ + printf("stopping memory checking...\n"); + enabled.store(false); +} + /****************************************************************************** * End Linux *****************************************************************************/ diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools_common.cpp index ab49f65..30949b4 100644 --- a/rcl/test/memory_tools_common.cpp +++ b/rcl/test/memory_tools_common.cpp @@ -142,8 +142,8 @@ custom_free(void * memory) (*unexpected_free_callback)(); } } - free(memory); MALLOC_PRINTF("free expected(%s): %p\n", malloc_expected ? "true " : "false", memory); + free(memory); } void assert_no_malloc_begin() {malloc_expected = false;} From a02229956877374c47c78d044e8f846f4442c1d8 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 14 Dec 2015 23:27:14 -0800 Subject: [PATCH 37/71] fixup --- rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h b/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h index 9b6cb20..dd0c674 100644 --- a/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h +++ b/rcl/src/rcl/stdatomic_helper/gcc/stdatomic.h @@ -74,6 +74,11 @@ #ifndef _STDATOMIC_H_ // NOLINT #define _STDATOMIC_H_ +#if __cplusplus +// This will suppress warnings about _Bool not being defined. +typedef bool _Bool; +#endif + #include #include From 0ae4ca735ddbc3889b9591973da6887e5ee6793c Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 15 Dec 2015 15:59:16 -0800 Subject: [PATCH 38/71] implement tests for rcl --- rcl/include/rcl/rcl.h | 34 +++++- rcl/src/rcl/rcl.c | 19 +++- rcl/test/CMakeLists.txt | 12 +++ rcl/test/memory_tools_common.cpp | 4 +- rcl/test/rcl/test_rcl.cpp | 180 +++++++++++++++++++++++++++++++ 5 files changed, 241 insertions(+), 8 deletions(-) create mode 100644 rcl/test/rcl/test_rcl.cpp diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 4c27d73..21d5f04 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -46,11 +46,17 @@ extern "C" * be ignored. * If argc is 0 and argv is NULL no parameters will be parsed. * + * This function allocates heap memory. + * This function is not thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_uint_least64_t. + * * \param[in] argc number of strings in argv * \param[in] argv command line arguments; rcl specific arguments are removed * \param[in] allocator allocator to be used during rcl_init and rcl_shutdown * \return RCL_RET_OK if initialization is successful, or * RCL_RET_ALREADY_INIT if rcl_init has already been called, or + * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC @@ -72,21 +78,39 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator); * - No new work (executing callbacks) will be done in executors. * - Currently running work in executors will be finished. * - * \return RCL_RET_OK if shutdown is successful, otherwise RCL_RET_ERROR or - * RCL_RET_NOT_INIT if rcl_init has not yet been called + * This function does not allocate heap memory. + * This function is thread-safe, except with rcl_init(). + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_uint_least64_t. + * + * \return RCL_RET_OK if the shutdown was completed successfully, or + * RCL_RET_NOT_INIT if rcl is not currently initialized, or + * RCL_RET_ERROR if an unspecified error occur. */ RCL_PUBLIC rcl_ret_t rcl_shutdown(); /// Returns an uint64_t number that is unique for the latest rcl_init call. -/* If called before rcl_init or after rcl_shutdown then 0 will be returned. */ +/* If called before rcl_init or after rcl_shutdown then 0 will be returned. + * + * This function does not allocate memory. + * This function is thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_uint_least64_t. + * + * \return a unique id specific to this rcl instance, or 0 if not initialized. + */ RCL_PUBLIC uint64_t rcl_get_instance_id(); -/// Return true until rcl_shutdown is called, then false. -/* This function is thread safe. */ +/// Return true if rcl is currently initialized, otherwise false. +/* This function does not allocate memory. + * This function is thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_uint_least64_t. + */ RCL_PUBLIC bool rcl_ok(); diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index 839a81c..13ec6a8 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -21,6 +21,7 @@ extern "C" #include +#include "./common.h" #include "./stdatomic_helper.h" #include "rcl/error_handling.h" @@ -38,11 +39,15 @@ __clean_up_init() size_t i; for (i = 0; i < __rcl_argc; ++i) { if (__rcl_argv[i]) { + // Use the old allocator. __rcl_allocator.deallocate(__rcl_argv[i], __rcl_allocator.state); } } + // Use the old allocator. __rcl_allocator.deallocate(__rcl_argv, __rcl_allocator.state); } + __rcl_argc = 0; + __rcl_argv = NULL; rcl_atomic_store(&__rcl_instance_id, 0); rcl_atomic_store(&__rcl_is_initialized, false); } @@ -50,16 +55,28 @@ __clean_up_init() rcl_ret_t rcl_init(int argc, char ** argv, rcl_allocator_t allocator) { + rcl_ret_t fail_ret = RCL_RET_ERROR; + if (argc > 0) { + RCL_CHECK_ARGUMENT_FOR_NULL(argv, RCL_RET_INVALID_ARGUMENT); + } + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator.allocate, + "invalid allocator, allocate not set", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator.deallocate, + "invalid allocator, deallocate not set", return RCL_RET_INVALID_ARGUMENT); if (rcl_atomic_exchange_bool(&__rcl_is_initialized, true)) { RCL_SET_ERROR_MSG("rcl_init called while already initialized"); return RCL_RET_ALREADY_INIT; } // TODO(wjwwood): Remove rcl specific command line arguments. // For now just copy the argc and argv. + __rcl_allocator = allocator; // Set the new allocator. __rcl_argc = argc; __rcl_argv = (char **)__rcl_allocator.allocate(sizeof(char *) * argc, __rcl_allocator.state); if (!__rcl_argv) { RCL_SET_ERROR_MSG("allocation failed"); + fail_ret = RCL_RET_BAD_ALLOC; goto fail; } memset(__rcl_argv, 0, sizeof(char **) * argc); @@ -78,7 +95,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator) return RCL_RET_OK; fail: __clean_up_init(); - return RCL_RET_ERROR; + return fail_ret; } rcl_ret_t diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 11f393a..83f96b6 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -79,4 +79,16 @@ if(AMENT_ENABLE_TESTING) endif() target_link_libraries(test_common ${PROJECT_NAME} ${extra_test_libraries}) endif() + + ament_add_gtest(test_rcl rcl/test_rcl.cpp ENV ${extra_memory_tools_env}) + if(TARGET test_rcl) + target_include_directories(test_rcl PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + if(NOT WIN32) + set_target_properties(test_rcl PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() + target_link_libraries(test_rcl ${PROJECT_NAME} ${extra_test_libraries}) + endif() endif() diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools_common.cpp index 30949b4..a2672ed 100644 --- a/rcl/test/memory_tools_common.cpp +++ b/rcl/test/memory_tools_common.cpp @@ -63,7 +63,7 @@ custom_malloc(size_t size) } void * memory = malloc(size); MALLOC_PRINTF( - "malloc expected(%s): %p %zu\n", malloc_expected ? "true " : "false", memory, size); + "malloc expected(%s): %p %llu\n", malloc_expected ? "true " : "false", memory, size); return memory; } @@ -103,7 +103,7 @@ custom_realloc(void * memory_in, size_t size) } void * memory = realloc(memory_in, size); MALLOC_PRINTF( - "realloc expected(%s): %p %p %zu\n", + "realloc expected(%s): %p %p %llu\n", malloc_expected ? "true " : "false", memory_in, memory, size); return memory; } diff --git a/rcl/test/rcl/test_rcl.cpp b/rcl/test/rcl/test_rcl.cpp new file mode 100644 index 0000000..b5517a1 --- /dev/null +++ b/rcl/test/rcl/test_rcl.cpp @@ -0,0 +1,180 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/rcl.h" + +#include "../memory_tools.hpp" +#include "rcl/error_handling.h" + +class TestRCLFixture : public::testing::Test +{ +public: + void SetUp() + { + set_on_unepexcted_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";}); + set_on_unepexcted_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";}); + set_on_unepexcted_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";}); + start_memory_checking(); + } + + void TearDown() + { + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + set_on_unepexcted_malloc_callback(nullptr); + set_on_unepexcted_realloc_callback(nullptr); + set_on_unepexcted_free_callback(nullptr); + } +}; + +void * +failing_malloc(size_t size, void * state) +{ + (void)(size); + (void)(state); + return nullptr; +} + +void * +failing_realloc(void * pointer, size_t size, void * state) +{ + (void)(pointer); + (void)(size); + (void)(state); + return nullptr; +} + +void +failing_free(void * pointer, void * state) +{ + (void)pointer; + (void)state; + return; +} + +struct FakeTestArgv +{ + FakeTestArgv() : argc(2) + { + this->argv = (char **)malloc(2 * sizeof(char *)); + if (!this->argv) { + throw std::bad_alloc(); + } + this->argv[0] = (char *)malloc(10 * sizeof(char)); + sprintf(this->argv[0], "foo"); + this->argv[1] = (char *)malloc(10 * sizeof(char)); + sprintf(this->argv[1], "bar"); + } + + ~FakeTestArgv() + { + if (this->argv) { + if (this->argv > 0) { + size_t unsigned_argc = this->argc; + for (size_t i = 0; i < unsigned_argc; --i) { + free(this->argv[i]); + } + } + } + free(this->argv); + } + + int argc; + char ** argv; +}; + +/* Tests the rcl_init() and rcl_shutdown() functions. + */ +TEST_F(TestRCLFixture, test_rcl_init_and_shutdown) +{ + rcl_ret_t ret; + // A shutdown before any init has been called should fail. + ret = rcl_shutdown(); + EXPECT_EQ(RCL_RET_NOT_INIT, ret); + rcl_reset_error(); + ASSERT_EQ(false, rcl_ok()); + // If argc is not 0, but argv is, it should be an invalid argument. + ret = rcl_init(42, nullptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ASSERT_EQ(false, rcl_ok()); + // If either the allocate or deallocate function pointers are not set, it should be invalid arg. + rcl_allocator_t invalid_allocator = rcl_get_default_allocator(); + invalid_allocator.allocate = nullptr; + ret = rcl_init(0, nullptr, invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ASSERT_EQ(false, rcl_ok()); + invalid_allocator.allocate = rcl_get_default_allocator().allocate; + invalid_allocator.deallocate = nullptr; + ret = rcl_init(0, nullptr, invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ASSERT_EQ(false, rcl_ok()); + // If the malloc call fails (with some valid arguments to copy), it should be a bad alloc. + { + FakeTestArgv test_args; + rcl_allocator_t failing_allocator = rcl_get_default_allocator(); + failing_allocator.allocate = &failing_malloc; + failing_allocator.deallocate = failing_free; + failing_allocator.reallocate = failing_realloc; + ret = rcl_init(test_args.argc, test_args.argv, failing_allocator); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + rcl_reset_error(); + ASSERT_EQ(false, rcl_ok()); + } + // If argc is 0 and argv is nullptr and the allocator is valid, it should succeed. + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_EQ(true, rcl_ok()); + // Then shutdown should work. + ret = rcl_shutdown(); + EXPECT_EQ(ret, RCL_RET_OK); + ASSERT_EQ(false, rcl_ok()); + // Valid argc/argv values and a valid allocator should succeed. + { + FakeTestArgv test_args; + ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_EQ(true, rcl_ok()); + } + // Then shutdown should work. + ret = rcl_shutdown(); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_EQ(false, rcl_ok()); + // A repeat call to shutdown should not work. + ret = rcl_shutdown(); + EXPECT_EQ(RCL_RET_NOT_INIT, ret); + rcl_reset_error(); + ASSERT_EQ(false, rcl_ok()); + // Repeat, but valid, calls to rcl_init() should fail. + { + FakeTestArgv test_args; + ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_EQ(true, rcl_ok()); + ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret); + rcl_reset_error(); + ASSERT_EQ(true, rcl_ok()); + } + // But shutdown should still work. + ret = rcl_shutdown(); + EXPECT_EQ(ret, RCL_RET_OK); + ASSERT_EQ(false, rcl_ok()); +} From ff64d077029eac254611bfa6aa3765db205b486a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 15 Dec 2015 16:09:07 -0800 Subject: [PATCH 39/71] [style] fixup warnings and style problems --- rcl/test/memory_tools_common.cpp | 13 +++++++++---- rcl/test/rcl/test_rcl.cpp | 20 ++++++++++---------- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools_common.cpp index a2672ed..c05e4e3 100644 --- a/rcl/test/memory_tools_common.cpp +++ b/rcl/test/memory_tools_common.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include + #include +#include #if defined(__APPLE__) #include @@ -62,8 +64,10 @@ custom_malloc(size_t size) } } void * memory = malloc(size); + uint64_t fw_size = size; MALLOC_PRINTF( - "malloc expected(%s): %p %llu\n", malloc_expected ? "true " : "false", memory, size); + "malloc expected(%s): %p %" PRIu64 "\n", + malloc_expected ? "true " : "false", memory, fw_size); return memory; } @@ -102,9 +106,10 @@ custom_realloc(void * memory_in, size_t size) } } void * memory = realloc(memory_in, size); + uint64_t fw_size = size; MALLOC_PRINTF( - "realloc expected(%s): %p %p %llu\n", - malloc_expected ? "true " : "false", memory_in, memory, size); + "realloc expected(%s): %p %p %" PRIu64 "\n", + malloc_expected ? "true " : "false", memory_in, memory, fw_size); return memory; } diff --git a/rcl/test/rcl/test_rcl.cpp b/rcl/test/rcl/test_rcl.cpp index b5517a1..3c02c5e 100644 --- a/rcl/test/rcl/test_rcl.cpp +++ b/rcl/test/rcl/test_rcl.cpp @@ -64,27 +64,28 @@ failing_free(void * pointer, void * state) { (void)pointer; (void)state; - return; } struct FakeTestArgv { - FakeTestArgv() : argc(2) + FakeTestArgv() + : argc(2) { - this->argv = (char **)malloc(2 * sizeof(char *)); + this->argv = reinterpret_cast(malloc(2 * sizeof(char *))); if (!this->argv) { throw std::bad_alloc(); } - this->argv[0] = (char *)malloc(10 * sizeof(char)); - sprintf(this->argv[0], "foo"); - this->argv[1] = (char *)malloc(10 * sizeof(char)); - sprintf(this->argv[1], "bar"); + static const size_t size = 10; + this->argv[0] = reinterpret_cast(malloc(size * sizeof(char))); + snprintf(this->argv[0], size, "foo"); + this->argv[1] = reinterpret_cast(malloc(size * sizeof(char))); + snprintf(this->argv[1], size, "bar"); } ~FakeTestArgv() { if (this->argv) { - if (this->argv > 0) { + if (this->argc > 0) { size_t unsigned_argc = this->argc; for (size_t i = 0; i < unsigned_argc; --i) { free(this->argv[i]); @@ -100,8 +101,7 @@ struct FakeTestArgv /* Tests the rcl_init() and rcl_shutdown() functions. */ -TEST_F(TestRCLFixture, test_rcl_init_and_shutdown) -{ +TEST_F(TestRCLFixture, test_rcl_init_and_shutdown) { rcl_ret_t ret; // A shutdown before any init has been called should fail. ret = rcl_shutdown(); From abc1f87f6376cce8762d3e80cac9cc6c46becf7d Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 15 Dec 2015 17:09:16 -0800 Subject: [PATCH 40/71] add more tests for rcl.c --- rcl/test/rcl/test_rcl.cpp | 57 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 55 insertions(+), 2 deletions(-) diff --git a/rcl/test/rcl/test_rcl.cpp b/rcl/test/rcl/test_rcl.cpp index 3c02c5e..aa8c7b4 100644 --- a/rcl/test/rcl/test_rcl.cpp +++ b/rcl/test/rcl/test_rcl.cpp @@ -99,9 +99,9 @@ struct FakeTestArgv char ** argv; }; -/* Tests the rcl_init() and rcl_shutdown() functions. +/* Tests the rcl_init(), rcl_ok(), and rcl_shutdown() functions. */ -TEST_F(TestRCLFixture, test_rcl_init_and_shutdown) { +TEST_F(TestRCLFixture, test_rcl_init_and_ok_and_shutdown) { rcl_ret_t ret; // A shutdown before any init has been called should fail. ret = rcl_shutdown(); @@ -178,3 +178,56 @@ TEST_F(TestRCLFixture, test_rcl_init_and_shutdown) { EXPECT_EQ(ret, RCL_RET_OK); ASSERT_EQ(false, rcl_ok()); } + +/* Tests the rcl_get_instance_id() and rcl_ok() functions. + */ +TEST_F(TestRCLFixture, test_rcl_get_instance_id_and_ok) { + rcl_ret_t ret; + // Instance id should be 0 before rcl_init(). + EXPECT_EQ(0, rcl_get_instance_id()); + ASSERT_EQ(false, rcl_ok()); + // It should still return 0 after an invalid init. + ret = rcl_init(1, nullptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_EQ(0, rcl_get_instance_id()); + ASSERT_EQ(false, rcl_ok()); + // A non-zero instance id should be returned after a valid init. + { + FakeTestArgv test_args; + ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_EQ(true, rcl_ok()); + } + // And it should be allocation free. + uint64_t first_instance_id; + assert_no_malloc_begin(); + assert_no_realloc_begin(); + assert_no_free_begin(); + first_instance_id = rcl_get_instance_id(); + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + EXPECT_NE(0, first_instance_id); + EXPECT_EQ(first_instance_id, rcl_get_instance_id()); // Repeat calls should return the same. + EXPECT_EQ(true, rcl_ok()); + // Calling after a shutdown should return 0. + ret = rcl_shutdown(); + EXPECT_EQ(ret, RCL_RET_OK); + EXPECT_EQ(0, rcl_get_instance_id()); + ASSERT_EQ(false, rcl_ok()); + // It should return a different value after another valid init. + { + FakeTestArgv test_args; + ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_EQ(true, rcl_ok()); + } + EXPECT_NE(0, rcl_get_instance_id()); + EXPECT_NE(first_instance_id, rcl_get_instance_id()); + ASSERT_EQ(true, rcl_ok()); + // Shutting down a second time should result in 0 again. + ret = rcl_shutdown(); + EXPECT_EQ(ret, RCL_RET_OK); + EXPECT_EQ(0, rcl_get_instance_id()); + ASSERT_EQ(false, rcl_ok()); +} From aa83632ffe80a7af09ba781eed7c31ba17311fa9 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 15 Dec 2015 18:50:34 -0800 Subject: [PATCH 41/71] fixup comments on #endif statements --- rcl/src/rcl/common.c | 6 +++--- rcl/src/rcl/time.c | 6 +++--- rcl/src/rcl/time_unix.c | 26 +++++++++++++------------- rcl/src/rcl/time_win32.c | 3 --- rcl/test/memory_tools.cpp | 2 +- rcl/test/memory_tools_common.cpp | 4 ++-- rcl/test/rcl/test_allocator.cpp | 4 ++-- 7 files changed, 24 insertions(+), 27 deletions(-) diff --git a/rcl/src/rcl/common.c b/rcl/src/rcl/common.c index 5cec065..ab9f523 100644 --- a/rcl/src/rcl/common.c +++ b/rcl/src/rcl/common.c @@ -24,7 +24,7 @@ extern "C" #if defined(WIN32) #define WINDOWS_ENV_BUFFER_SIZE 2048 static char __env_buffer[WINDOWS_ENV_BUFFER_SIZE]; -#endif +#endif // defined(WIN32) rcl_ret_t rcl_impl_getenv(const char * env_name, const char ** env_value) @@ -37,7 +37,7 @@ rcl_impl_getenv(const char * env_name, const char ** env_value) if (*env_value == NULL) { *env_value = ""; } -#else +#else // !defined(WIN32) size_t required_size; errno_t ret = getenv_s(&required_size, __env_buffer, sizeof(__env_buffer), env_name); if (ret != 0) { @@ -46,7 +46,7 @@ rcl_impl_getenv(const char * env_name, const char ** env_value) } __env_buffer[WINDOWS_ENV_BUFFER_SIZE - 1] = '\0'; *env_value = __env_buffer; -#endif +#endif // !defined(WIN32) return RCL_RET_OK; } diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index ab08eda..7226b2c 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifdef WIN32 +#if defined(WIN32) #include "./time_win32.c" -#else +#else // defined(WIN32) #include "./time_unix.c" -#endif +#endif // defined(WIN32) diff --git a/rcl/src/rcl/time_unix.c b/rcl/src/rcl/time_unix.c index 7cd438d..765742c 100644 --- a/rcl/src/rcl/time_unix.c +++ b/rcl/src/rcl/time_unix.c @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifdef WIN32 +#if defined(WIN32) #error time_unix.c is not intended to be used with win32 based systems -#endif +#endif // defined(WIN32) #if __cplusplus extern "C" @@ -26,7 +26,7 @@ extern "C" #if defined(__MACH__) #include #include -#endif +#endif // defined(__MACH__) #include #include #include @@ -37,10 +37,10 @@ extern "C" #if !defined(__MACH__) // Assume clock_get_time is available on OS X. // This id an appropriate check for clock_gettime() according to: // http://man7.org/linux/man-pages/man2/clock_gettime.2.html -#if (!defined(_POSIX_TIMERS) || !_POSIX_TIMERS) +#if !defined(_POSIX_TIMERS) || !_POSIX_TIMERS #error no monotonic clock function available -#endif -#endif +#endif // !defined(_POSIX_TIMERS) || !_POSIX_TIMERS +#endif // !defined(__MACH__) #define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) @@ -58,10 +58,10 @@ rcl_system_time_point_now(rcl_system_time_point_t * now) mach_port_deallocate(mach_task_self(), cclock); timespec_now.tv_sec = mts.tv_sec; timespec_now.tv_nsec = mts.tv_nsec; -#else +#else // defined(__MACH__) // Otherwise use clock_gettime. clock_gettime(CLOCK_REALTIME, ×pec_now); -#endif // if defined(__MACH__) +#endif // defined(__MACH__) if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { RCL_SET_ERROR_MSG("unexpected negative time"); return RCL_RET_ERROR; @@ -85,14 +85,14 @@ rcl_steady_time_point_now(rcl_steady_time_point_t * now) mach_port_deallocate(mach_task_self(), cclock); timespec_now.tv_sec = mts.tv_sec; timespec_now.tv_nsec = mts.tv_nsec; -#else +#else // defined(__MACH__) // Otherwise use clock_gettime. -#ifdef CLOCK_MONOTONIC_RAW +#if defined(CLOCK_MONOTONIC_RAW) clock_gettime(CLOCK_MONOTONIC_RAW, ×pec_now); -#else +#else // defined(CLOCK_MONOTONIC_RAW) clock_gettime(CLOCK_MONOTONIC, ×pec_now); -#endif // CLOCK_MONOTONIC_RAW -#endif // if defined(__MACH__) +#endif // defined(CLOCK_MONOTONIC_RAW) +#endif // defined(__MACH__) if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) { RCL_SET_ERROR_MSG("unexpected negative time"); return RCL_RET_ERROR; diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c index debdaf4..987ac68 100644 --- a/rcl/src/rcl/time_win32.c +++ b/rcl/src/rcl/time_win32.c @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifdef WIN32 #ifndef WIN32 #error time_win32.c is only intended to be used with win32 based systems #endif @@ -68,5 +67,3 @@ rcl_steady_time_point_now(rcl_steady_time_point_t * now) #if __cplusplus } #endif - -#endif diff --git a/rcl/test/memory_tools.cpp b/rcl/test/memory_tools.cpp index e87c67e..b95338f 100644 --- a/rcl/test/memory_tools.cpp +++ b/rcl/test/memory_tools.cpp @@ -138,4 +138,4 @@ void set_on_unepexcted_free_callback(UnexpectedCallbackType callback) {} void memory_checking_thread_init() {} -#endif // !defined(WIN32) +#endif // if defined(__linux__) elif defined(__APPLE__) elif defined(WIN32) else ... diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools_common.cpp index c05e4e3..03a2772 100644 --- a/rcl/test/memory_tools_common.cpp +++ b/rcl/test/memory_tools_common.cpp @@ -20,9 +20,9 @@ #if defined(__APPLE__) #include #define MALLOC_PRINTF malloc_printf -#else +#else // defined(__APPLE__) #define MALLOC_PRINTF printf -#endif +#endif // defined(__APPLE__) #include "./memory_tools.hpp" #include "./scope_exit.hpp" diff --git a/rcl/test/rcl/test_allocator.cpp b/rcl/test/rcl/test_allocator.cpp index 5bb65b6..ecf1bd6 100644 --- a/rcl/test/rcl/test_allocator.cpp +++ b/rcl/test/rcl/test_allocator.cpp @@ -49,10 +49,10 @@ public: /* Tests the default allocator. */ TEST_F(TestAllocatorFixture, test_default_allocator_normal) { -#ifdef WIN32 +#if defined(WIN32) printf("Allocator tests disabled on Windows.\n"); return; -#endif +#endif // defined(WIN32) ASSERT_NO_MALLOC( rcl_allocator_t allocator = rcl_get_default_allocator(); ) From 52da897e225a8141035e97215f5f39f155de0c36 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 16 Dec 2015 09:47:20 -0800 Subject: [PATCH 42/71] Spelling --- rcl/include/rcl/allocator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index 4e849fe..53494cd 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -32,7 +32,7 @@ typedef struct rcl_allocator_t void * (*allocate)(size_t size, void * state); /// Deallocate previously allocated memory, mimicking free(). void (* deallocate)(void * pointer, void * state); - /// Reallocates if possible, otherwise it deallocates and allocates. + /// Reallocate if possible, otherwise it deallocates and allocates. /* If unsupported then do deallocate and then allocate. * \TODO(wjwwood): should this behave as reallocf? * This should behave as realloc is described, as opposed to reallocf, i.e. From 88c503e7e596fbc6caa2f7e9ae13f6a1e746fd85 Mon Sep 17 00:00:00 2001 From: Jackie Kay Date: Wed, 16 Dec 2015 10:15:00 -0800 Subject: [PATCH 43/71] Correct a few typos --- rcl/src/rcl/subscription.c | 2 +- rcl/test/memory_tools.cpp | 6 +++--- rcl/test/memory_tools.hpp | 6 +++--- rcl/test/memory_tools_common.cpp | 6 +++--- rcl/test/rcl/test_allocator.cpp | 18 +++++++++--------- rcl/test/rcl/test_rcl.cpp | 12 ++++++------ rcl/test/rcl/test_time.cpp | 12 ++++++------ rcl/test/test_memory_tools.cpp | 6 +++--- 8 files changed, 34 insertions(+), 34 deletions(-) diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 57a6b8c..72d46c8 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -50,7 +50,7 @@ rcl_subscription_init( RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); if (subscription->impl) { - RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninialized"); + RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized"); return RCL_RET_ALREADY_INIT; } const rcl_allocator_t * allocator = &options->allocator; diff --git a/rcl/test/memory_tools.cpp b/rcl/test/memory_tools.cpp index b95338f..17fd508 100644 --- a/rcl/test/memory_tools.cpp +++ b/rcl/test/memory_tools.cpp @@ -122,19 +122,19 @@ void assert_no_malloc_begin() {} void assert_no_malloc_end() {} -void set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback) {} +void set_on_unexpected_malloc_callback(UnexpectedCallbackType callback) {} void assert_no_realloc_begin() {} void assert_no_realloc_end() {} -void set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback) {} +void set_on_unexpected_realloc_callback(UnexpectedCallbackType callback) {} void assert_no_free_begin() {} void assert_no_free_end() {} -void set_on_unepexcted_free_callback(UnexpectedCallbackType callback) {} +void set_on_unexpected_free_callback(UnexpectedCallbackType callback) {} void memory_checking_thread_init() {} diff --git a/rcl/test/memory_tools.hpp b/rcl/test/memory_tools.hpp index 9941f63..001c110 100644 --- a/rcl/test/memory_tools.hpp +++ b/rcl/test/memory_tools.hpp @@ -70,7 +70,7 @@ void assert_no_malloc_end(); RCL_MEMORY_TOOLS_PUBLIC void -set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback); +set_on_unexpected_malloc_callback(UnexpectedCallbackType callback); #define ASSERT_NO_REALLOC(statements) \ assert_no_realloc_begin(); statements; assert_no_realloc_end(); @@ -82,7 +82,7 @@ void assert_no_realloc_end(); RCL_MEMORY_TOOLS_PUBLIC void -set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback); +set_on_unexpected_realloc_callback(UnexpectedCallbackType callback); #define ASSERT_NO_FREE(statements) \ assert_no_free_begin(); statements; assert_no_free_end(); @@ -94,7 +94,7 @@ void assert_no_free_end(); RCL_MEMORY_TOOLS_PUBLIC void -set_on_unepexcted_free_callback(UnexpectedCallbackType callback); +set_on_unexpected_free_callback(UnexpectedCallbackType callback); RCL_MEMORY_TOOLS_PUBLIC void diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools_common.cpp index 03a2772..8789a75 100644 --- a/rcl/test/memory_tools_common.cpp +++ b/rcl/test/memory_tools_common.cpp @@ -31,7 +31,7 @@ static std::atomic enabled(false); static __thread bool malloc_expected = true; static __thread UnexpectedCallbackType * unexpected_malloc_callback = nullptr; -void set_on_unepexcted_malloc_callback(UnexpectedCallbackType callback) +void set_on_unexpected_malloc_callback(UnexpectedCallbackType callback) { if (unexpected_malloc_callback) { unexpected_malloc_callback->~UnexpectedCallbackType(); @@ -73,7 +73,7 @@ custom_malloc(size_t size) static __thread bool realloc_expected = true; static __thread UnexpectedCallbackType * unexpected_realloc_callback = nullptr; -void set_on_unepexcted_realloc_callback(UnexpectedCallbackType callback) +void set_on_unexpected_realloc_callback(UnexpectedCallbackType callback) { if (unexpected_realloc_callback) { unexpected_realloc_callback->~UnexpectedCallbackType(); @@ -115,7 +115,7 @@ custom_realloc(void * memory_in, size_t size) static __thread bool free_expected = true; static __thread UnexpectedCallbackType * unexpected_free_callback = nullptr; -void set_on_unepexcted_free_callback(UnexpectedCallbackType callback) +void set_on_unexpected_free_callback(UnexpectedCallbackType callback) { if (unexpected_free_callback) { unexpected_free_callback->~UnexpectedCallbackType(); diff --git a/rcl/test/rcl/test_allocator.cpp b/rcl/test/rcl/test_allocator.cpp index ecf1bd6..b9ca12a 100644 --- a/rcl/test/rcl/test_allocator.cpp +++ b/rcl/test/rcl/test_allocator.cpp @@ -28,9 +28,9 @@ public: } void SetUp() { - set_on_unepexcted_malloc_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED MALLOC";}); - set_on_unepexcted_realloc_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED REALLOC";}); - set_on_unepexcted_free_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED FREE";}); + set_on_unexpected_malloc_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED MALLOC";}); + set_on_unexpected_realloc_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED REALLOC";}); + set_on_unexpected_free_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED FREE";}); start_memory_checking(); } @@ -40,9 +40,9 @@ public: assert_no_realloc_end(); assert_no_free_end(); stop_memory_checking(); - set_on_unepexcted_malloc_callback(nullptr); - set_on_unepexcted_realloc_callback(nullptr); - set_on_unepexcted_free_callback(nullptr); + set_on_unexpected_malloc_callback(nullptr); + set_on_unexpected_realloc_callback(nullptr); + set_on_unexpected_free_callback(nullptr); } }; @@ -59,13 +59,13 @@ TEST_F(TestAllocatorFixture, test_default_allocator_normal) { size_t mallocs = 0; size_t reallocs = 0; size_t frees = 0; - set_on_unepexcted_malloc_callback([&mallocs]() { + set_on_unexpected_malloc_callback([&mallocs]() { mallocs++; }); - set_on_unepexcted_realloc_callback([&reallocs]() { + set_on_unexpected_realloc_callback([&reallocs]() { reallocs++; }); - set_on_unepexcted_free_callback([&frees]() { + set_on_unexpected_free_callback([&frees]() { frees++; }); assert_no_malloc_begin(); diff --git a/rcl/test/rcl/test_rcl.cpp b/rcl/test/rcl/test_rcl.cpp index aa8c7b4..42b634c 100644 --- a/rcl/test/rcl/test_rcl.cpp +++ b/rcl/test/rcl/test_rcl.cpp @@ -24,9 +24,9 @@ class TestRCLFixture : public::testing::Test public: void SetUp() { - set_on_unepexcted_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";}); - set_on_unepexcted_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";}); - set_on_unepexcted_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";}); + set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";}); + set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";}); + set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";}); start_memory_checking(); } @@ -36,9 +36,9 @@ public: assert_no_realloc_end(); assert_no_free_end(); stop_memory_checking(); - set_on_unepexcted_malloc_callback(nullptr); - set_on_unepexcted_realloc_callback(nullptr); - set_on_unepexcted_free_callback(nullptr); + set_on_unexpected_malloc_callback(nullptr); + set_on_unexpected_realloc_callback(nullptr); + set_on_unexpected_free_callback(nullptr); } }; diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index 54b33fc..0ff333c 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -26,9 +26,9 @@ class TestTimeFixture : public::testing::Test public: void SetUp() { - set_on_unepexcted_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";}); - set_on_unepexcted_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";}); - set_on_unepexcted_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";}); + set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";}); + set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";}); + set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";}); start_memory_checking(); } @@ -38,9 +38,9 @@ public: assert_no_realloc_end(); assert_no_free_end(); stop_memory_checking(); - set_on_unepexcted_malloc_callback(nullptr); - set_on_unepexcted_realloc_callback(nullptr); - set_on_unepexcted_free_callback(nullptr); + set_on_unexpected_malloc_callback(nullptr); + set_on_unexpected_realloc_callback(nullptr); + set_on_unexpected_free_callback(nullptr); } }; diff --git a/rcl/test/test_memory_tools.cpp b/rcl/test/test_memory_tools.cpp index 1c03b03..dcddaa3 100644 --- a/rcl/test/test_memory_tools.cpp +++ b/rcl/test/test_memory_tools.cpp @@ -23,17 +23,17 @@ TEST(TestMemoryTools, test_allocation_checking_tools) { auto on_unexpected_malloc = ([&unexpected_mallocs]() { unexpected_mallocs++; }); - set_on_unepexcted_malloc_callback(on_unexpected_malloc); + set_on_unexpected_malloc_callback(on_unexpected_malloc); size_t unexpected_reallocs = 0; auto on_unexpected_realloc = ([&unexpected_reallocs]() { unexpected_reallocs++; }); - set_on_unepexcted_realloc_callback(on_unexpected_realloc); + set_on_unexpected_realloc_callback(on_unexpected_realloc); size_t unexpected_frees = 0; auto on_unexpected_free = ([&unexpected_frees]() { unexpected_frees++; }); - set_on_unepexcted_free_callback(on_unexpected_free); + set_on_unexpected_free_callback(on_unexpected_free); void * mem = nullptr; void * remem = nullptr; // First try before enabling, should have no effect. From e2cbacebe47466ae2c581cea0f6c47d1d79b470a Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 16 Dec 2015 10:37:29 -0800 Subject: [PATCH 44/71] spelling --- rcl/include/rcl/wait.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index 0453f91..a9aa117 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -141,7 +141,7 @@ RCL_PUBLIC rcl_ret_t rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * allocator); -/// Stores a pointer to the given subscription in the next empty spot in the set. +/// Store a pointer to the given subscription in the next empty spot in the set. /* This function does not guarantee that the subscription is not already in the * wait set. * From 7b36a604d83963ca743761a271ca941586fe3b6c Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 16 Dec 2015 10:38:26 -0800 Subject: [PATCH 45/71] spelling --- rcl/include/rcl/wait.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index a9aa117..ca67d19 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -162,7 +162,7 @@ rcl_wait_set_add_subscription( rcl_wait_set_t * wait_set, const rcl_subscription_t * subscription); -/// Removes (sets to NULL) the subscriptions in the wait set. +/// Remove (sets to NULL) the subscriptions in the wait set. /* This function should be used after passing using rcl_wait, but before * adding new subscriptions to the set. * @@ -181,7 +181,7 @@ RCL_PUBLIC rcl_ret_t rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); -/// Reallocates space for the subscriptions in the wait set. +/// Reallocate space for the subscriptions in the wait set. /* This function will deallocate and reallocate the memory for the * subscriptions set. * @@ -210,7 +210,7 @@ RCL_PUBLIC rcl_ret_t rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size); -/// Stores a pointer to the guard condition in the next empty spot in the set. +/// Store a pointer to the guard condition in the next empty spot in the set. /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_add_subscription */ @@ -220,7 +220,7 @@ rcl_wait_set_add_guard_condition( rcl_wait_set_t * wait_set, const rcl_guard_condition_t * guard_condition); -/// Removes (sets to NULL) the guard conditions in the wait set. +/// Remove (sets to NULL) the guard conditions in the wait set. /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_clear_subscriptions */ @@ -228,7 +228,7 @@ RCL_PUBLIC rcl_ret_t rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set); -/// Reallocates space for the guard conditions in the wait set. +/// Reallocate space for the guard conditions in the wait set. /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_resize_subscriptions */ @@ -236,7 +236,7 @@ RCL_PUBLIC rcl_ret_t rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); -/// Stores a pointer to the timer in the next empty spot in the set. +/// Store a pointer to the timer in the next empty spot in the set. /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_add_subscription */ @@ -246,7 +246,7 @@ rcl_wait_set_add_timer( rcl_wait_set_t * wait_set, const rcl_timer_t * timer); -/// Removes (sets to NULL) the timers in the wait set. +/// Remove (sets to NULL) the timers in the wait set. /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_clear_subscriptions */ @@ -254,7 +254,7 @@ RCL_PUBLIC rcl_ret_t rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set); -/// Reallocates space for the timers in the wait set. +/// Reallocate space for the timers in the wait set. /* This function behaves exactly the same as for subscriptions. * \see rcl_wait_set_resize_subscriptions */ From b205c360f5d4a8616e8942e0059bf861f489696d Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 12:17:44 -0800 Subject: [PATCH 46/71] add note about allocator life time --- rcl/include/rcl/allocator.h | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index 53494cd..d246ce7 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -24,7 +24,16 @@ extern "C" #include "rcl/visibility_control.h" /// Encapsulation of an allocator. -/* To use malloc, free, and realloc use rcl_get_default_allocator(). */ +/* To use malloc, free, and realloc use rcl_get_default_allocator(). + * + * The allocator should be trivially copyable. + * Meaning that the struct should continue to work after being assignment + * copied into a new struct. + * Specifically the object pointed to by the state pointer should remain valid + * until all uses of the allocator have been made. + * Particular care should be taken when giving an allocator to rcl_init_* where + * it is stored within another object and used later. + */ typedef struct rcl_allocator_t { /// Allocate memory, given a size and state structure. From cf450f597a0fe331c5239eb6c6c9b69309c70353 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 12:18:14 -0800 Subject: [PATCH 47/71] remove resolved todo question --- rcl/include/rcl/publisher.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 4d51e99..d59760c 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -217,7 +217,6 @@ rcl_publish(const rcl_publisher_t * publisher, const void * ros_message); * copying the string is recommended if this is a concern. * * This function is not thread-safe, and copying the result is not thread-safe. - * \TODO(wjwwood): should we provide a thread-safe copy_topic_name? * * \param[in] publisher pointer to the publisher * \return name string if successful, otherwise NULL @@ -237,7 +236,6 @@ rcl_publisher_get_topic_name(const rcl_publisher_t * publisher); * and therefore copying the struct is recommended if this is a concern. * * This function is not thread-safe, and copying the result is not thread-safe. - * \TODO(wjwwood): should we provide a thread-safe copy_publisher_options? * * \param[in] publisher pointer to the publisher * \return options struct if successful, otherwise NULL From 8ea6b75b420a4bab1622142762a80bcf8c12867e Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 12:22:22 -0800 Subject: [PATCH 48/71] [style] fixup --- rcl/src/rcl/timer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index dc2fb45..30d73c9 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -158,7 +158,7 @@ rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_si return ret; // rcl error state should already be set. } *time_since_last_call = - (now.nanoseconds - rcl_atomic_load_uint64_t(&timer->impl->last_call_time)); + now.nanoseconds - rcl_atomic_load_uint64_t(&timer->impl->last_call_time); return RCL_RET_OK; } From 68facdb9353775da23a02fe6fe3cdb67a558a875 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 12:30:39 -0800 Subject: [PATCH 49/71] remove extra check for 0 initial size for sets --- rcl/src/rcl/wait.c | 6 ------ 1 file changed, 6 deletions(-) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index c14178a..0fba416 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -84,12 +84,6 @@ rcl_wait_set_init( RCL_SET_ERROR_MSG("wait_set already initialized, or memory was uninitialized."); return RCL_RET_ALREADY_INIT; } - RCL_CHECK_FOR_NULL_WITH_MSG( - number_of_subscriptions, - "number_of_subscriptions cannot be 0", return RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_FOR_NULL_WITH_MSG( - number_of_guard_conditions, - "number_of_guard_conditions cannot be 0", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG( allocator.allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_FOR_NULL_WITH_MSG( From 2398e98f66f2e494e69c363b43afdf09b1c48128 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 12:37:17 -0800 Subject: [PATCH 50/71] fix building dll definition --- rcl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 3baf6cb..f79fe59 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -39,7 +39,7 @@ macro(target) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. - target_compile_definitions(${PROJECT_NAME}${target_suffix} PRIVATE "RCL_BUILDING_LIBRARY") + target_compile_definitions(${PROJECT_NAME}${target_suffix} PRIVATE "RCL_BUILDING_DLL") install( TARGETS ${PROJECT_NAME}${target_suffix} From 898f3890199c40780e40ca8e644ffe15a16b55b1 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 12:55:33 -0800 Subject: [PATCH 51/71] resolve reallocf todo --- rcl/include/rcl/allocator.h | 10 +++++++++- rcl/src/rcl/allocator.c | 15 +++++++++++++++ 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index d246ce7..e9014b6 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -43,10 +43,10 @@ typedef struct rcl_allocator_t void (* deallocate)(void * pointer, void * state); /// Reallocate if possible, otherwise it deallocates and allocates. /* If unsupported then do deallocate and then allocate. - * \TODO(wjwwood): should this behave as reallocf? * This should behave as realloc is described, as opposed to reallocf, i.e. * the memory given by pointer will not be free'd automatically if realloc * fails. + * For reallocf behavior use rcl_reallocf(). * This function must be able to take an input pointer of NULL and succeed. */ void * (*reallocate)(void * pointer, size_t size, void * state); @@ -64,6 +64,14 @@ RCL_PUBLIC rcl_allocator_t rcl_get_default_allocator(); +/// Emulate the behavior of reallocf. +/* This function will return NULL if the allocator is NULL or has NULL for + * function pointer fields. + */ +RCL_PUBLIC +void * +rcl_reallocf(void * pointer, size_t size, rcl_allocator_t * allocator); + #if __cplusplus } #endif diff --git a/rcl/src/rcl/allocator.c b/rcl/src/rcl/allocator.c index 14ec8b8..32c6406 100644 --- a/rcl/src/rcl/allocator.c +++ b/rcl/src/rcl/allocator.c @@ -15,6 +15,7 @@ #include #include "rcl/allocator.h" +#include "rcl/error_handling.h" static void * __default_allocate(size_t size, void * state) @@ -48,3 +49,17 @@ rcl_get_default_allocator() }; return default_allocator; } + +void * +rcl_reallocf(void * pointer, size_t size, rcl_allocator_t * allocator) +{ + if (!allocator || !allocator->reallocate || !allocator->deallocate) { + RCL_SET_ERROR_MSG("invalid allocator or allocator function pointers"); + return NULL; + } + void * new_pointer = allocator->reallocate(pointer, size, allocator->state); + if (!new_pointer) { + allocator->deallocate(pointer, allocator->state); + } + return new_pointer; +} From 09a2aad44f18e454aa70716f6e5d2fe85e0fb1e4 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 14:41:31 -0800 Subject: [PATCH 52/71] fix test of steady clock --- rcl/test/rcl/test_time.cpp | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index 0ff333c..1e89f10 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -14,7 +14,10 @@ #include +#include + #include +#include #include "rcl/error_handling.h" #include "rcl/time.h" @@ -48,7 +51,7 @@ public: */ TEST_F(TestTimeFixture, test_rcl_system_time_point_now) { assert_no_realloc_begin(); - rcl_ret_t ret = RCL_RET_OK; + rcl_ret_t ret; // Check for invalid argument error condition (allowed to alloc). ret = rcl_system_time_point_now(nullptr); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); @@ -72,7 +75,8 @@ TEST_F(TestTimeFixture, test_rcl_system_time_point_now) { auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); int64_t now_ns_int = now_ns.count(); int64_t now_diff = now.nanoseconds - now_ns_int; - EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(1000)) << "system_clock differs"; + const int k_tolerance_ms = 1000; + EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "system_clock differs"; } } @@ -80,7 +84,7 @@ TEST_F(TestTimeFixture, test_rcl_system_time_point_now) { */ TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) { assert_no_realloc_begin(); - rcl_ret_t ret = RCL_RET_OK; + rcl_ret_t ret; // Check for invalid argument error condition (allowed to alloc). ret = rcl_steady_time_point_now(nullptr); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); @@ -100,10 +104,17 @@ TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) { now = {0}; ret = rcl_steady_time_point_now(&now); std::chrono::steady_clock::time_point now_sc = std::chrono::steady_clock::now(); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); + // Wait for a little while. + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // Then take a new timestamp with each and compare. rcl_steady_time_point_t later; ret = rcl_steady_time_point_now(&later); std::chrono::steady_clock::time_point later_sc = std::chrono::steady_clock::now(); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe(); int64_t steady_diff = later.nanoseconds - now.nanoseconds; - int64_t sc_diff = (now_sc - later_sc).count(); - EXPECT_LE(llabs(steady_diff - sc_diff), RCL_MS_TO_NS(1000)) << "steady_clock differs"; + int64_t sc_diff = + std::chrono::duration_cast(later_sc - now_sc).count(); + const int k_tolerance_ms = 1; + EXPECT_LE(llabs(steady_diff - sc_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "steady_clock differs"; } From a44c14ce2bf53215b451311fa46363e105d7aca3 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 14:49:22 -0800 Subject: [PATCH 53/71] resolve some todo questions --- rcl/include/rcl/guard_condition.h | 8 +++++++- rcl/include/rcl/node.h | 15 ++++++++++----- rcl/include/rcl/publisher.h | 17 +++++++---------- rcl/include/rcl/subscription.h | 12 +++++++----- 4 files changed, 31 insertions(+), 21 deletions(-) diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index 83b3c2c..bd45b95 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/rcl/include/rcl/guard_condition.h @@ -145,7 +145,13 @@ rcl_guard_condition_trigger(const rcl_guard_condition_t * guard_condition); * - guard_condition is NULL * - guard_condition is invalid (never called init, called fini, or invalid node) * - * The returned handle is only valid as long as the given guard_condition is valid. + * The returned handle is made invalid if the guard condition is finalized or + * if rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * guard condition as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the guard condition using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. * * \param[in] guard_condition pointer to the rcl guard_condition * \return rmw guard_condition handle if successful, otherwise NULL diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index daeb219..9e82f3d 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -63,7 +63,7 @@ rcl_node_t rcl_get_zero_initialized_node(); /// Initialize a ROS node. -/* Calling this on a rcl_node_t makes it a valid node handle until rcl_fini +/* Calling this on a rcl_node_t makes it a valid node handle until rcl_shutdown * is called or until rcl_node_fini is called on it. * * After calling the ROS node object can be used to create other middleware @@ -194,9 +194,14 @@ rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); * - node is NULL * - node has not been initialized (the implementation is invalid) * - * The returned handle is only valid as long as the given node is valid. + * The returned handle is made invalid if the node is finalized or if + * rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * node as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the node using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. * - * \TODO(wjwwood) should the return value of this be const? * * \param[in] node pointer to the rcl node * \return rmw node handle if successful, otherwise NULL @@ -214,8 +219,8 @@ rcl_node_get_rmw_node_handle(const rcl_node_t * node); * - node is NULL * - node has not been initialized (the implementation is invalid) * - * This function will succeed, however, even if rcl_fini has been called since - * the node was created. + * This function will succeed, however, even if rcl_shutdown has been called + * since the node was created. * * \param[in] node pointer to the rcl node * \return rcl instance id captured at node creation or 0 if there was an error diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index d59760c..c67d842 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -250,16 +250,13 @@ rcl_publisher_get_options(const rcl_publisher_t * publisher); * - publisher is NULL * - publisher is invalid (never called init, called fini, or invalid node) * - * The returned handle is only valid as long as the given publisher is valid. - * - * \TODO(wjwwood) will the publisher ever need to remove and recreate the rmw - * handle under the hood? Perhaps not now, but if we add the - * ability to reconfigure, alias, or remap topics, and don't - * implement that into rmw, then we should consider the - * implications here. This argument applies to other parts of - * the interface, but I'll just mention it here. - * - * \TODO(wjwwood) should the return value of this be const? + * The returned handle is made invalid if the publisher is finalized or if + * rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * publisher as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the publisher using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. * * \param[in] publisher pointer to the rcl publisher * \return rmw publisher handle if successful, otherwise NULL diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 0bdee17..0c7a3c5 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -222,7 +222,6 @@ rcl_take( * copying the string is recommended if this is a concern. * * This function is not thread-safe, and copying the result is not thread-safe. - * \TODO(wjwwood): should we provide a thread-safe copy_topic_name? * * \param[in] subscription the pointer to the subscription * \return name string if successful, otherwise NULL @@ -242,7 +241,6 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription); * and therefore copying the struct is recommended if this is a concern. * * This function is not thread-safe, and copying the result is not thread-safe. - * \TODO(wjwwood): should we provide a thread-safe copy_subscription_options? * * \param[in] subscription pointer to the subscription * \return options struct if successful, otherwise NULL @@ -257,9 +255,13 @@ rcl_subscription_get_options(const rcl_subscription_t * subscription); * - subscription is NULL * - subscription is invalid (never called init, called fini, or invalid) * - * The returned handle is only valid as long as the rcl subscription is valid. - * - * \TODO(wjwwood) should the return value of this be const? + * The returned handle is made invalid if the subscription is finalized or if + * rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * subscription as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the subscription using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. * * This function is not thread-safe. * From 6ccbb35597a3bd53f91a5639a108ecd368ee492e Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 14:59:11 -0800 Subject: [PATCH 54/71] improve readability of *s <=> ns macros --- rcl/include/rcl/time.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index 3fa7144..c80c63e 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -23,12 +23,12 @@ extern "C" #include "rcl/types.h" #include "rcl/visibility_control.h" -#define RCL_S_TO_NS(seconds) (seconds * 1000000000) -#define RCL_MS_TO_NS(milliseconds) (milliseconds * 1000000) +#define RCL_S_TO_NS(seconds) (seconds * 1000 * 1000 * 1000) +#define RCL_MS_TO_NS(milliseconds) (milliseconds * 1000 * 1000) #define RCL_US_TO_NS(microseconds) (microseconds * 1000) -#define RCL_NS_TO_S(nanoseconds) (nanoseconds / 1000000000) -#define RCL_NS_TO_MS(nanoseconds) (nanoseconds / 1000000) +#define RCL_NS_TO_S(nanoseconds) (nanoseconds / 1000 * 1000 * 1000) +#define RCL_NS_TO_MS(nanoseconds) (nanoseconds / 1000 * 1000) #define RCL_NS_TO_US(nanoseconds) (nanoseconds / 1000) /// Struct which encapsulates a point in time according to a system clock. From 7bceed40712ba229c577909e27c8f33892fc8180 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 15:00:52 -0800 Subject: [PATCH 55/71] fix style --- rcl/src/rcl/common.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h index 2b2d5f7..e5c8027 100644 --- a/rcl/src/rcl/common.h +++ b/rcl/src/rcl/common.h @@ -26,10 +26,11 @@ extern "C" #define RCL_CHECK_ARGUMENT_FOR_NULL(argument, error_return_type) \ RCL_CHECK_FOR_NULL_WITH_MSG(argument, #argument " argument is null", return error_return_type) -#define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) if (!value) { \ +#define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) \ + if (!(value)) { \ RCL_SET_ERROR_MSG(msg); \ error_statement; \ -} + } /// Retrieve the value of the given environment variable if it exists, or "". /* The returned cstring is only valid until the next time this function is From b92148d75fdfd1c4a15f0996d5e0c4e1f8d19f3e Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 15:02:45 -0800 Subject: [PATCH 56/71] remove extra enable testing check --- rcl/test/CMakeLists.txt | 182 +++++++++++++++++++++------------------- 1 file changed, 96 insertions(+), 86 deletions(-) diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 83f96b6..5692dd9 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -1,94 +1,104 @@ -if(AMENT_ENABLE_TESTING) - set(extra_test_libraries) - set(extra_memory_tools_env PHONY=) # Use a phony env var so there is always at least one. - ament_find_gtest() # For GTEST_LIBRARIES - if(APPLE) - add_library(${PROJECT_NAME}_memory_tools_interpose SHARED memory_tools_osx_interpose.cpp) - target_link_libraries(${PROJECT_NAME}_memory_tools_interpose ${GTEST_LIBRARIES}) - set_target_properties(${PROJECT_NAME}_memory_tools_interpose - PROPERTIES COMPILE_FLAGS "-std=c++11") - list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools_interpose) - list(APPEND extra_memory_tools_env - DYLD_INSERT_LIBRARIES=$) - endif() - add_library(${PROJECT_NAME}_memory_tools SHARED memory_tools.cpp) - if(NOT WIN32) - set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") - endif() - if(UNIX AND NOT APPLE) - list(APPEND extra_test_libraries dl) - list(APPEND extra_memory_tools_env DL_PRELOAD=$) - endif() - target_link_libraries(${PROJECT_NAME}_memory_tools ${extra_test_libraries}) - target_compile_definitions(${PROJECT_NAME}_memory_tools - PRIVATE "RCL_MEMORY_TOOLS_BUILDING_DLL") - list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools) +set(extra_test_libraries) +set(extra_memory_tools_env PHONY=) # Use a phony env var so there is always at least one. +ament_find_gtest() # For GTEST_LIBRARIES +if(APPLE) + add_library(${PROJECT_NAME}_memory_tools_interpose SHARED memory_tools_osx_interpose.cpp) + target_link_libraries(${PROJECT_NAME}_memory_tools_interpose ${GTEST_LIBRARIES}) + set_target_properties(${PROJECT_NAME}_memory_tools_interpose + PROPERTIES COMPILE_FLAGS "-std=c++11") + list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools_interpose) + list(APPEND extra_memory_tools_env + DYLD_INSERT_LIBRARIES=$) +endif() +add_library(${PROJECT_NAME}_memory_tools SHARED memory_tools.cpp) +if(NOT WIN32) + set_target_properties(${PROJECT_NAME}_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") +endif() +if(UNIX AND NOT APPLE) + list(APPEND extra_test_libraries dl) + list(APPEND extra_memory_tools_env DL_PRELOAD=$) +endif() +target_link_libraries(${PROJECT_NAME}_memory_tools ${extra_test_libraries}) +target_compile_definitions(${PROJECT_NAME}_memory_tools + PRIVATE "RCL_MEMORY_TOOLS_BUILDING_DLL") +list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools) - if(NOT WIN32) - ament_add_gtest(test_memory_tools test_memory_tools.cpp ENV ${extra_memory_tools_env}) - if(TARGET test_memory_tools) - target_include_directories(test_memory_tools PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ) - if(NOT WIN32) - set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") - endif() - target_link_libraries(test_memory_tools ${PROJECT_NAME} ${extra_test_libraries}) - endif() - endif() - - ament_add_gtest(test_allocator rcl/test_allocator.cpp ENV ${extra_memory_tools_env}) - if(TARGET test_allocator) - target_include_directories(test_allocator PUBLIC +if(NOT WIN32) + ament_add_gtest(test_memory_tools test_memory_tools.cpp ENV ${extra_memory_tools_env}) + if(TARGET test_memory_tools) + target_include_directories(test_memory_tools PUBLIC ${rcl_interfaces_INCLUDE_DIRS} ${rmw_INCLUDE_DIRS} ) if(NOT WIN32) - set_target_properties(test_allocator PROPERTIES COMPILE_FLAGS "-std=c++11") + set_target_properties(test_memory_tools PROPERTIES COMPILE_FLAGS "-std=c++11") endif() - target_link_libraries(test_allocator ${PROJECT_NAME} ${extra_test_libraries}) - endif() - - ament_add_gtest(test_time rcl/test_time.cpp ENV ${extra_memory_tools_env}) - if(TARGET test_time) - target_include_directories(test_time PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ) - if(NOT WIN32) - set_target_properties(test_time PROPERTIES COMPILE_FLAGS "-std=c++11") - endif() - target_link_libraries(test_time ${PROJECT_NAME} ${extra_test_libraries}) - endif() - - ament_add_gtest(test_common - rcl/test_common.cpp - ENV - ${extra_memory_tools_env} - EMPTY_TEST= - NORMAL_TEST=foo - ) - if(TARGET test_common) - target_include_directories(test_common PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ) - if(NOT WIN32) - set_target_properties(test_common PROPERTIES COMPILE_FLAGS "-std=c++11") - endif() - target_link_libraries(test_common ${PROJECT_NAME} ${extra_test_libraries}) - endif() - - ament_add_gtest(test_rcl rcl/test_rcl.cpp ENV ${extra_memory_tools_env}) - if(TARGET test_rcl) - target_include_directories(test_rcl PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ) - if(NOT WIN32) - set_target_properties(test_rcl PROPERTIES COMPILE_FLAGS "-std=c++11") - endif() - target_link_libraries(test_rcl ${PROJECT_NAME} ${extra_test_libraries}) + target_link_libraries(test_memory_tools ${PROJECT_NAME} ${extra_test_libraries}) endif() endif() + +ament_add_gtest(test_allocator rcl/test_allocator.cpp ENV ${extra_memory_tools_env}) +if(TARGET test_allocator) + target_include_directories(test_allocator PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + if(NOT WIN32) + set_target_properties(test_allocator PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() + target_link_libraries(test_allocator ${PROJECT_NAME} ${extra_test_libraries}) +endif() + +ament_add_gtest(test_time rcl/test_time.cpp ENV ${extra_memory_tools_env}) +if(TARGET test_time) + target_include_directories(test_time PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + if(NOT WIN32) + set_target_properties(test_time PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() + target_link_libraries(test_time ${PROJECT_NAME} ${extra_test_libraries}) +endif() + +ament_add_gtest(test_common + rcl/test_common.cpp + ENV + ${extra_memory_tools_env} + EMPTY_TEST= + NORMAL_TEST=foo +) +if(TARGET test_common) + target_include_directories(test_common PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + if(NOT WIN32) + set_target_properties(test_common PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() + target_link_libraries(test_common ${PROJECT_NAME} ${extra_test_libraries}) +endif() + +ament_add_gtest(test_rcl rcl/test_rcl.cpp ENV ${extra_memory_tools_env}) +if(TARGET test_rcl) + target_include_directories(test_rcl PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + if(NOT WIN32) + set_target_properties(test_rcl PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() + target_link_libraries(test_rcl ${PROJECT_NAME} ${extra_test_libraries}) +endif() + +ament_add_gtest(test_node rcl/test_node.cpp ENV ${extra_memory_tools_env}) +if(TARGET test_node) + target_include_directories(test_node PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ) + if(NOT WIN32) + set_target_properties(test_node PROPERTIES COMPILE_FLAGS "-std=c++11") + endif() + target_link_libraries(test_node ${PROJECT_NAME} ${extra_test_libraries}) +endif() From f35c06e27cb2039ddfdc25e3f778873f1bc41f01 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 15:33:52 -0800 Subject: [PATCH 57/71] fix indention of preprocessor directives --- rcl/src/rcl/common.c | 2 +- rcl/src/rcl/time_unix.c | 8 ++++---- rcl/src/rcl/time_win32.c | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rcl/src/rcl/common.c b/rcl/src/rcl/common.c index ab9f523..e4db769 100644 --- a/rcl/src/rcl/common.c +++ b/rcl/src/rcl/common.c @@ -22,7 +22,7 @@ extern "C" #include #if defined(WIN32) -#define WINDOWS_ENV_BUFFER_SIZE 2048 +# define WINDOWS_ENV_BUFFER_SIZE 2048 static char __env_buffer[WINDOWS_ENV_BUFFER_SIZE]; #endif // defined(WIN32) diff --git a/rcl/src/rcl/time_unix.c b/rcl/src/rcl/time_unix.c index 765742c..bc7e452 100644 --- a/rcl/src/rcl/time_unix.c +++ b/rcl/src/rcl/time_unix.c @@ -13,7 +13,7 @@ // limitations under the License. #if defined(WIN32) -#error time_unix.c is not intended to be used with win32 based systems +# error time_unix.c is not intended to be used with win32 based systems #endif // defined(WIN32) #if __cplusplus @@ -37,9 +37,9 @@ extern "C" #if !defined(__MACH__) // Assume clock_get_time is available on OS X. // This id an appropriate check for clock_gettime() according to: // http://man7.org/linux/man-pages/man2/clock_gettime.2.html -#if !defined(_POSIX_TIMERS) || !_POSIX_TIMERS -#error no monotonic clock function available -#endif // !defined(_POSIX_TIMERS) || !_POSIX_TIMERS +# if !defined(_POSIX_TIMERS) || !_POSIX_TIMERS +# error no monotonic clock function available +# endif // !defined(_POSIX_TIMERS) || !_POSIX_TIMERS #endif // !defined(__MACH__) #define __WOULD_BE_NEGATIVE(seconds, subseconds) (seconds < 0 || (subseconds < 0 && seconds == 0)) diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c index 987ac68..919cf46 100644 --- a/rcl/src/rcl/time_win32.c +++ b/rcl/src/rcl/time_win32.c @@ -13,7 +13,7 @@ // limitations under the License. #ifndef WIN32 -#error time_win32.c is only intended to be used with win32 based systems +# error time_win32.c is only intended to be used with win32 based systems #endif #if __cplusplus From 64dc7ac2200074e1c463db79ecf486feb53d31a4 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 15:41:33 -0800 Subject: [PATCH 58/71] move find_package call to testing code --- rcl/CMakeLists.txt | 1 - rcl/test/CMakeLists.txt | 2 ++ 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index f79fe59..f0d54d5 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -60,7 +60,6 @@ ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) if(AMENT_ENABLE_TESTING) - find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 5692dd9..291fcd9 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -1,3 +1,5 @@ +find_package(ament_cmake_gtest REQUIRED) + set(extra_test_libraries) set(extra_memory_tools_env PHONY=) # Use a phony env var so there is always at least one. ament_find_gtest() # For GTEST_LIBRARIES From c6a903ffaf7255279d4fd12623b0a6d142fe550e Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 15:46:00 -0800 Subject: [PATCH 59/71] refactor the name of rmw handle getters --- rcl/include/rcl/guard_condition.h | 2 +- rcl/include/rcl/node.h | 2 +- rcl/include/rcl/publisher.h | 2 +- rcl/include/rcl/subscription.h | 2 +- rcl/src/rcl/guard_condition.c | 2 +- rcl/src/rcl/node.c | 2 +- rcl/src/rcl/publisher.c | 6 +++--- rcl/src/rcl/subscription.c | 6 +++--- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index bd45b95..79b4d1d 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/rcl/include/rcl/guard_condition.h @@ -158,7 +158,7 @@ rcl_guard_condition_trigger(const rcl_guard_condition_t * guard_condition); */ RCL_PUBLIC rmw_guard_condition_t * -rcl_guard_condition_get_rmw_guard_condition_handle(const rcl_guard_condition_t * guard_condition); +rcl_guard_condition_get_rmw_handle(const rcl_guard_condition_t * guard_condition); #if __cplusplus } diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 9e82f3d..6f6c54a 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -208,7 +208,7 @@ rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); */ RCL_PUBLIC rmw_node_t * -rcl_node_get_rmw_node_handle(const rcl_node_t * node); +rcl_node_get_rmw_handle(const rcl_node_t * node); /// Return the associated rcl instance id. /* This id is stored when rcl_node_init is called and can be compared with the diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index c67d842..2cc4583 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -263,7 +263,7 @@ rcl_publisher_get_options(const rcl_publisher_t * publisher); */ RCL_PUBLIC rmw_publisher_t * -rcl_publisher_get_rmw_publisher_handle(const rcl_publisher_t * publisher); +rcl_publisher_get_rmw_handle(const rcl_publisher_t * publisher); #if __cplusplus } diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 0c7a3c5..1c376dc 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -270,7 +270,7 @@ rcl_subscription_get_options(const rcl_subscription_t * subscription); */ RCL_PUBLIC rmw_subscription_t * -rcl_subscription_get_rmw_subscription_handle(const rcl_subscription_t * subscription); +rcl_subscription_get_rmw_handle(const rcl_subscription_t * subscription); #if __cplusplus } diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c index c2e48fd..0d93155 100644 --- a/rcl/src/rcl/guard_condition.c +++ b/rcl/src/rcl/guard_condition.c @@ -137,7 +137,7 @@ rcl_trigger_guard_condition(const rcl_guard_condition_t * guard_condition) } rmw_guard_condition_t * -rcl_guard_condition_get_rmw_guard_condition_handle(const rcl_guard_condition_t * guard_condition) +rcl_guard_condition_get_rmw_handle(const rcl_guard_condition_t * guard_condition) { RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, NULL); RCL_CHECK_FOR_NULL_WITH_MSG( diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 5ef8a87..e27c5e9 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -158,7 +158,7 @@ rcl_node_get_options(const rcl_node_t * node) } rmw_node_t * -rcl_node_get_rmw_node_handle(const rcl_node_t * node) +rcl_node_get_rmw_handle(const rcl_node_t * node) { RCL_CHECK_ARGUMENT_FOR_NULL(node, NULL); RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return NULL); diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index d0c2a67..808a2ca 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -69,7 +69,7 @@ rcl_publisher_init( // rmw handle (create rmw publisher) // TODO(wjwwood): pass along the allocator to rmw when it supports it publisher->impl->rmw_handle = rmw_create_publisher( - rcl_node_get_rmw_node_handle(node), + rcl_node_get_rmw_handle(node), type_support, topic_name, &rmw_qos_profile_default); @@ -95,7 +95,7 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); if (publisher->impl) { rmw_ret_t ret = - rmw_destroy_publisher(rcl_node_get_rmw_node_handle(node), publisher->impl->rmw_handle); + rmw_destroy_publisher(rcl_node_get_rmw_handle(node), publisher->impl->rmw_handle); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); result = RCL_RET_ERROR; @@ -151,7 +151,7 @@ rcl_publisher_get_options(const rcl_publisher_t * publisher) } rmw_publisher_t * -rcl_publisher_get_rmw_publisher_handle(const rcl_publisher_t * publisher) +rcl_publisher_get_rmw_handle(const rcl_publisher_t * publisher) { RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL); RCL_CHECK_FOR_NULL_WITH_MSG( diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 72d46c8..ce09153 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -67,7 +67,7 @@ rcl_subscription_init( // rmw_handle // TODO(wjwwood): pass allocator once supported in rmw api. subscription->impl->rmw_handle = rmw_create_subscription( - rcl_node_get_rmw_node_handle(node), + rcl_node_get_rmw_handle(node), type_support, topic_name, &rmw_qos_profile_default, @@ -94,7 +94,7 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); if (subscription->impl) { rmw_ret_t ret = - rmw_destroy_subscription(rcl_node_get_rmw_node_handle(node), subscription->impl->rmw_handle); + rmw_destroy_subscription(rcl_node_get_rmw_handle(node), subscription->impl->rmw_handle); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); result = RCL_RET_ERROR; @@ -167,7 +167,7 @@ rcl_subscription_get_options(const rcl_subscription_t * subscription) } rmw_subscription_t * -rcl_subscription_get_rmw_subscription_handle(const rcl_subscription_t * subscription) +rcl_subscription_get_rmw_handle(const rcl_subscription_t * subscription) { RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL); RCL_CHECK_FOR_NULL_WITH_MSG( From 5372c520922f633aea2bf4884b239ae49d3da6ef Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 15:56:17 -0800 Subject: [PATCH 60/71] change output of memory tools for readability --- rcl/test/memory_tools_common.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools_common.cpp index 8789a75..c7ea833 100644 --- a/rcl/test/memory_tools_common.cpp +++ b/rcl/test/memory_tools_common.cpp @@ -66,8 +66,8 @@ custom_malloc(size_t size) void * memory = malloc(size); uint64_t fw_size = size; MALLOC_PRINTF( - "malloc expected(%s): %p %" PRIu64 "\n", - malloc_expected ? "true " : "false", memory, fw_size); + " malloc (%s) %p %" PRIu64 "\n", + malloc_expected ? " expected" : "not expected", memory, fw_size); return memory; } @@ -108,8 +108,8 @@ custom_realloc(void * memory_in, size_t size) void * memory = realloc(memory_in, size); uint64_t fw_size = size; MALLOC_PRINTF( - "realloc expected(%s): %p %p %" PRIu64 "\n", - malloc_expected ? "true " : "false", memory_in, memory, fw_size); + "realloc (%s) %p %p %" PRIu64 "\n", + malloc_expected ? " expected" : "not expected", memory_in, memory, fw_size); return memory; } @@ -147,7 +147,8 @@ custom_free(void * memory) (*unexpected_free_callback)(); } } - MALLOC_PRINTF("free expected(%s): %p\n", malloc_expected ? "true " : "false", memory); + MALLOC_PRINTF( + " free (%s) %p\n", malloc_expected ? " expected" : "not expected", memory); free(memory); } From c816e64d0c8760e67c799dd163829c1da55a670a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 16:00:33 -0800 Subject: [PATCH 61/71] additional fix for renaming rmw handle functions --- rcl/src/rcl/wait.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 0fba416..a214673 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -177,7 +177,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al #define SET_ADD_RMW(Type, RMWStorage) \ /* Also place into rmw storage. */ \ - rmw_ ## Type ## _t * rmw_handle = rcl_ ## Type ## _get_rmw_ ## Type ## _handle(Type); \ + rmw_ ## Type ## _t * rmw_handle = rcl_ ## Type ## _get_rmw_handle(Type); \ RCL_CHECK_FOR_NULL_WITH_MSG( \ rmw_handle, rcl_get_error_string_safe(), return RCL_RET_ERROR); \ wait_set->impl->RMWStorage[current_index] = rmw_handle->data; From 8da408a3d84c841276705c1391a32d8ea9e0c59c Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 16 Dec 2015 16:10:47 -0800 Subject: [PATCH 62/71] make indentation the same - one space per pp condition --- rcl/src/rcl/time_unix.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/src/rcl/time_unix.c b/rcl/src/rcl/time_unix.c index bc7e452..fd6cbc4 100644 --- a/rcl/src/rcl/time_unix.c +++ b/rcl/src/rcl/time_unix.c @@ -38,7 +38,7 @@ extern "C" // This id an appropriate check for clock_gettime() according to: // http://man7.org/linux/man-pages/man2/clock_gettime.2.html # if !defined(_POSIX_TIMERS) || !_POSIX_TIMERS -# error no monotonic clock function available +# error no monotonic clock function available # endif // !defined(_POSIX_TIMERS) || !_POSIX_TIMERS #endif // !defined(__MACH__) From 41d123b5710c58a31263bb97e4a9b6d721e19d83 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 16:39:10 -0800 Subject: [PATCH 63/71] add warnings when return codes of API are ignored --- rcl/include/rcl/allocator.h | 3 +++ rcl/include/rcl/guard_condition.h | 7 ++++++ rcl/include/rcl/macros.h | 33 ++++++++++++++++++++++++++++ rcl/include/rcl/node.h | 9 ++++++++ rcl/include/rcl/publisher.h | 9 ++++++++ rcl/include/rcl/rcl.h | 5 +++++ rcl/include/rcl/subscription.h | 9 ++++++++ rcl/include/rcl/time.h | 3 +++ rcl/include/rcl/timer.h | 15 +++++++++++++ rcl/include/rcl/visibility_control.h | 2 +- rcl/include/rcl/wait.h | 17 +++++++++++++- rcl/src/rcl/wait.c | 17 +++++++++----- 12 files changed, 122 insertions(+), 7 deletions(-) create mode 100644 rcl/include/rcl/macros.h diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index e9014b6..ce32052 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -20,6 +20,7 @@ extern "C" { #endif +#include "rcl/macros.h" #include "rcl/types.h" #include "rcl/visibility_control.h" @@ -61,6 +62,7 @@ typedef struct rcl_allocator_t * This function is lock-free. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_allocator_t rcl_get_default_allocator(); @@ -69,6 +71,7 @@ rcl_get_default_allocator(); * function pointer fields. */ RCL_PUBLIC +RCL_WARN_UNUSED void * rcl_reallocf(void * pointer, size_t size, rcl_allocator_t * allocator); diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index 79b4d1d..c1f6c1a 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/rcl/include/rcl/guard_condition.h @@ -20,6 +20,7 @@ extern "C" { #endif +#include "rcl/macros.h" #include "rcl/node.h" #include "rcl/visibility_control.h" @@ -41,6 +42,7 @@ typedef struct rcl_guard_condition_options_t /// Return a rcl_guard_condition_t struct with members set to NULL. RCL_PUBLIC +RCL_WARN_UNUSED rcl_guard_condition_t rcl_get_zero_initialized_guard_condition(); @@ -84,6 +86,7 @@ rcl_get_zero_initialized_guard_condition(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_guard_condition_init( rcl_guard_condition_t * guard_condition, @@ -106,6 +109,7 @@ rcl_guard_condition_init( * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * node); @@ -115,6 +119,7 @@ rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * n * This function is lock-free. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_guard_condition_options_t rcl_guard_condition_get_default_options(); @@ -136,6 +141,7 @@ rcl_guard_condition_get_default_options(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_guard_condition_trigger(const rcl_guard_condition_t * guard_condition); @@ -157,6 +163,7 @@ rcl_guard_condition_trigger(const rcl_guard_condition_t * guard_condition); * \return rmw guard_condition handle if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED rmw_guard_condition_t * rcl_guard_condition_get_rmw_handle(const rcl_guard_condition_t * guard_condition); diff --git a/rcl/include/rcl/macros.h b/rcl/include/rcl/macros.h new file mode 100644 index 0000000..2399307 --- /dev/null +++ b/rcl/include/rcl/macros.h @@ -0,0 +1,33 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__MACROS_H_ +#define RCL__MACROS_H_ + +#if __cplusplus +extern "C" +{ +#endif + +#ifndef _WIN32 +# define RCL_WARN_UNUSED __attribute__((warn_unused_result)) +#else +# define RCL_WARN_UNUSED _Check_return_ +#endif + +#if __cplusplus +} +#endif + +#endif // RCL__MACROS_H_ diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 6f6c54a..9850810 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -23,6 +23,7 @@ extern "C" #include #include "rcl/allocator.h" +#include "rcl/macros.h" #include "rcl/types.h" #include "rcl/visibility_control.h" @@ -59,6 +60,7 @@ typedef struct rcl_node_options_t /// Return a rcl_node_t struct with members initialized to NULL. RCL_PUBLIC +RCL_WARN_UNUSED rcl_node_t rcl_get_zero_initialized_node(); @@ -106,6 +108,7 @@ rcl_get_zero_initialized_node(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * options); @@ -124,6 +127,7 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_node_fini(rcl_node_t * node); @@ -146,6 +150,7 @@ rcl_node_get_default_options(); * \return name string if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED const char * rcl_node_get_name(const rcl_node_t * node); @@ -163,6 +168,7 @@ rcl_node_get_name(const rcl_node_t * node); * \return options struct if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED const rcl_node_options_t * rcl_node_get_options(const rcl_node_t * node); @@ -185,6 +191,7 @@ rcl_node_get_options(const rcl_node_t * node); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); @@ -207,6 +214,7 @@ rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); * \return rmw node handle if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED rmw_node_t * rcl_node_get_rmw_handle(const rcl_node_t * node); @@ -226,6 +234,7 @@ rcl_node_get_rmw_handle(const rcl_node_t * node); * \return rcl instance id captured at node creation or 0 if there was an error */ RCL_PUBLIC +RCL_WARN_UNUSED uint64_t rcl_node_get_rcl_instance_id(const rcl_node_t * node); diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 2cc4583..6cc5d8b 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -22,6 +22,7 @@ extern "C" #include "rosidl_generator_c/message_type_support.h" +#include "rcl/macros.h" #include "rcl/node.h" #include "rcl/visibility_control.h" @@ -51,6 +52,7 @@ typedef struct rcl_publisher_options_t * being allocated on the heap. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_publisher_t rcl_get_zero_initialized_publisher(); @@ -126,6 +128,7 @@ rcl_get_zero_initialized_publisher(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_publisher_init( rcl_publisher_t * publisher, @@ -150,11 +153,13 @@ rcl_publisher_init( * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node); /// Return the default publisher options in a rcl_publisher_options_t. RCL_PUBLIC +RCL_WARN_UNUSED rcl_publisher_options_t rcl_publisher_get_default_options(); @@ -203,6 +208,7 @@ rcl_publisher_get_default_options(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_publish(const rcl_publisher_t * publisher, const void * ros_message); @@ -222,6 +228,7 @@ rcl_publish(const rcl_publisher_t * publisher, const void * ros_message); * \return name string if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED const char * rcl_publisher_get_topic_name(const rcl_publisher_t * publisher); @@ -241,6 +248,7 @@ rcl_publisher_get_topic_name(const rcl_publisher_t * publisher); * \return options struct if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED const rcl_publisher_options_t * rcl_publisher_get_options(const rcl_publisher_t * publisher); @@ -262,6 +270,7 @@ rcl_publisher_get_options(const rcl_publisher_t * publisher); * \return rmw publisher handle if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED rmw_publisher_t * rcl_publisher_get_rmw_handle(const rcl_publisher_t * publisher); diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 21d5f04..0257473 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -20,6 +20,7 @@ extern "C" { #endif +#include "rcl/macros.h" #include "rcl/node.h" #include "rcl/publisher.h" #include "rcl/subscription.h" @@ -60,6 +61,7 @@ extern "C" * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_init(int argc, char ** argv, rcl_allocator_t allocator); @@ -88,6 +90,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator); * RCL_RET_ERROR if an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_shutdown(); @@ -102,6 +105,7 @@ rcl_shutdown(); * \return a unique id specific to this rcl instance, or 0 if not initialized. */ RCL_PUBLIC +RCL_WARN_UNUSED uint64_t rcl_get_instance_id(); @@ -112,6 +116,7 @@ rcl_get_instance_id(); * atomic_is_lock_free() returns true for atomic_uint_least64_t. */ RCL_PUBLIC +RCL_WARN_UNUSED bool rcl_ok(); diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 1c376dc..a417248 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -22,6 +22,7 @@ extern "C" #include "rosidl_generator_c/message_type_support.h" +#include "rcl/macros.h" #include "rcl/node.h" #include "rcl/visibility_control.h" @@ -53,6 +54,7 @@ typedef struct rcl_subscription_options_t * is being allocated on the heap. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_subscription_t rcl_get_zero_initialized_subscription(); @@ -127,6 +129,7 @@ rcl_get_zero_initialized_subscription(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_subscription_init( rcl_subscription_t * subscription, @@ -153,11 +156,13 @@ rcl_subscription_init( * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node); /// Return the default subscription options in a rcl_subscription_options_t. RCL_PUBLIC +RCL_WARN_UNUSED rcl_subscription_options_t rcl_subscription_get_default_options(); @@ -204,6 +209,7 @@ rcl_subscription_get_default_options(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_take( const rcl_subscription_t * subscription, @@ -227,6 +233,7 @@ rcl_take( * \return name string if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED const char * rcl_subscription_get_topic_name(const rcl_subscription_t * subscription); @@ -246,6 +253,7 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription); * \return options struct if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED const rcl_subscription_options_t * rcl_subscription_get_options(const rcl_subscription_t * subscription); @@ -269,6 +277,7 @@ rcl_subscription_get_options(const rcl_subscription_t * subscription); * \return rmw subscription handle if successful, otherwise NULL */ RCL_PUBLIC +RCL_WARN_UNUSED rmw_subscription_t * rcl_subscription_get_rmw_handle(const rcl_subscription_t * subscription); diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index c80c63e..60ccf5d 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -20,6 +20,7 @@ extern "C" { #endif +#include "rcl/macros.h" #include "rcl/types.h" #include "rcl/visibility_control.h" @@ -93,6 +94,7 @@ typedef struct rcl_duration_t * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_system_time_point_now(rcl_system_time_point_t * now); @@ -117,6 +119,7 @@ rcl_system_time_point_now(rcl_system_time_point_t * now); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_steady_time_point_now(rcl_steady_time_point_t * now); diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index a8b72a0..8753356 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -23,6 +23,7 @@ extern "C" #include #include "rcl/allocator.h" +#include "rcl/macros.h" #include "rcl/time.h" #include "rcl/types.h" #include "rmw/rmw.h" @@ -51,6 +52,7 @@ typedef void (* rcl_timer_callback_t)(rcl_timer_t *, uint64_t); /// Return a zero initialized timer. RCL_PUBLIC +RCL_WARN_UNUSED rcl_timer_t rcl_get_zero_initialized_timer(); @@ -112,6 +114,7 @@ rcl_get_zero_initialized_timer(); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_init( rcl_timer_t * timer, @@ -136,6 +139,7 @@ rcl_timer_init( * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_fini(rcl_timer_t * timer); @@ -169,6 +173,7 @@ rcl_timer_fini(rcl_timer_t * timer); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_call(rcl_timer_t * timer); @@ -193,6 +198,7 @@ rcl_timer_call(rcl_timer_t * timer); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready); @@ -222,6 +228,7 @@ rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call); @@ -248,6 +255,7 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_since_last_call); @@ -268,6 +276,7 @@ rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, uint64_t * time_si * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period); @@ -292,6 +301,7 @@ rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period); @@ -308,6 +318,7 @@ rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64 * \return function pointer to the callback, or NULL if an error occurred */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_timer_callback_t rcl_timer_get_callback(const rcl_timer_t * timer); @@ -326,6 +337,7 @@ rcl_timer_get_callback(const rcl_timer_t * timer); * \return function pointer to the old callback, or NULL if an error occurred */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_timer_callback_t rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback); @@ -347,6 +359,7 @@ rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_ * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_cancel(rcl_timer_t * timer); @@ -368,6 +381,7 @@ rcl_timer_cancel(rcl_timer_t * timer); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled); @@ -387,6 +401,7 @@ rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_timer_reset(rcl_timer_t * timer); diff --git a/rcl/include/rcl/visibility_control.h b/rcl/include/rcl/visibility_control.h index 24b3a25..85e3b16 100644 --- a/rcl/include/rcl/visibility_control.h +++ b/rcl/include/rcl/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index ca67d19..c36300d 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -23,8 +23,9 @@ extern "C" #include #include -#include "rcl/subscription.h" #include "rcl/guard_condition.h" +#include "rcl/macros.h" +#include "rcl/subscription.h" #include "rcl/timer.h" #include "rcl/types.h" #include "rcl/visibility_control.h" @@ -49,6 +50,7 @@ typedef struct rcl_wait_set_t /// Return a rcl_wait_set_t struct with members set to NULL. RCL_PUBLIC +RCL_WARN_UNUSED rcl_wait_set_t rcl_get_zero_initialized_wait_set(); @@ -93,6 +95,7 @@ rcl_get_zero_initialized_wait_set(); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_init( rcl_wait_set_t * wait_set, @@ -121,6 +124,7 @@ rcl_wait_set_init( * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_fini(rcl_wait_set_t * wait_set); @@ -138,6 +142,7 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * allocator); @@ -157,6 +162,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_add_subscription( rcl_wait_set_t * wait_set, @@ -178,6 +184,7 @@ rcl_wait_set_add_subscription( * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); @@ -207,6 +214,7 @@ rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); * RCL_RET_ERROR if an unspecified error occurs. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size); @@ -215,6 +223,7 @@ rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size); * \see rcl_wait_set_add_subscription */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_add_guard_condition( rcl_wait_set_t * wait_set, @@ -225,6 +234,7 @@ rcl_wait_set_add_guard_condition( * \see rcl_wait_set_clear_subscriptions */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set); @@ -233,6 +243,7 @@ rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set); * \see rcl_wait_set_resize_subscriptions */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); @@ -241,6 +252,7 @@ rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); * \see rcl_wait_set_add_subscription */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_add_timer( rcl_wait_set_t * wait_set, @@ -251,6 +263,7 @@ rcl_wait_set_add_timer( * \see rcl_wait_set_clear_subscriptions */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set); @@ -259,6 +272,7 @@ rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set); * \see rcl_wait_set_resize_subscriptions */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size); @@ -352,6 +366,7 @@ rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size); * RCL_RET_ERROR an unspecified error occur. */ RCL_PUBLIC +RCL_WARN_UNUSED rcl_ret_t rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout); diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index a214673..0323963 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -55,14 +55,18 @@ __wait_set_is_valid(const rcl_wait_set_t * wait_set) static void __wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator) { + rcl_ret_t ret; if (wait_set->subscriptions) { - rcl_wait_set_resize_subscriptions(wait_set, 0); + ret = rcl_wait_set_resize_subscriptions(wait_set, 0); + assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. } if (wait_set->guard_conditions) { - rcl_wait_set_resize_guard_conditions(wait_set, 0); + ret = rcl_wait_set_resize_guard_conditions(wait_set, 0); + assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. } if (wait_set->timers) { - rcl_wait_set_resize_timers(wait_set, 0); + ret = rcl_wait_set_resize_timers(wait_set, 0); + assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. } if (wait_set->impl) { allocator.deallocate(wait_set->impl, allocator.state); @@ -416,8 +420,11 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) // Check for timeout. if (ret == RMW_RET_TIMEOUT) { // Assume none were set (because timeout was reached first), and clear all. - rcl_wait_set_clear_subscriptions(wait_set); - rcl_wait_set_clear_guard_conditions(wait_set); + rcl_ret_t rcl_ret; + rcl_ret = rcl_wait_set_clear_subscriptions(wait_set); + assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set. + rcl_ret = rcl_wait_set_clear_guard_conditions(wait_set); + assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set. return RCL_RET_TIMEOUT; } // Set corresponding rcl subscription handles NULL. From 1af1507baa346cbcb39600f39a96517ae1cd20d0 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 17:30:23 -0800 Subject: [PATCH 64/71] implement tests and fix problems with node.c --- rcl/include/rcl/node.h | 27 +++- rcl/include/rcl/rcl.h | 2 +- rcl/src/rcl/node.c | 53 +++++-- rcl/test/rcl/test_node.cpp | 307 +++++++++++++++++++++++++++++++++++++ 4 files changed, 373 insertions(+), 16 deletions(-) create mode 100644 rcl/test/rcl/test_node.cpp diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 9850810..60b75c1 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -43,7 +43,7 @@ typedef struct rcl_node_options_t // bool anonymous_name; // rmw_qos_profile_t parameter_qos; /// If true, no parameter infrastructure will be setup. - bool no_parameters; + // bool no_parameters; /// If set, then this value overrides the ROS_DOMAIN_ID environment variable. /* It defaults to RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID, which will cause the * node to use the ROS domain ID set in the ROS_DOMAIN_ID environment @@ -93,7 +93,10 @@ rcl_get_zero_initialized_node(); * ret = rcl_node_fini(&node); * // ... error handling for rcl_node_fini() * + * This function allocates heap memory. * This function is not thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_uint_least64_t. * * \pre the node handle must be allocated, zero initialized, and invalid * \post the node handle is valid and can be used to in other rcl_* functions @@ -119,7 +122,10 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o * Any middleware primitives created by the user, e.g. publishers, services, etc., * are invalid after deinitialization. * + * This function manipulates heap memory. * This function is not thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_uint_least64_t. * * \param[in] node handle to the node to be finalized * \return RCL_RET_OK if node was finalized successfully, or @@ -146,6 +152,10 @@ rcl_node_get_default_options(); * The value of the string may change if the value in the rcl_node_t changes, * and therefore copying the string is recommended if this is a concern. * + * This function does not manipulate heap memory. + * This function is thread-safe for different nodes. + * This function is lock-free. + * * \param[in] node pointer to the node * \return name string if successful, otherwise NULL */ @@ -164,6 +174,10 @@ rcl_node_get_name(const rcl_node_t * node); * The values in the struct may change if the options of the rcl_node_t changes, * and therefore copying the struct is recommended if this is a concern. * + * This function does not manipulate heap memory. + * This function is thread-safe for different nodes. + * This function is lock-free. + * * \param[in] node pointer to the node * \return options struct if successful, otherwise NULL */ @@ -184,6 +198,10 @@ rcl_node_get_options(const rcl_node_t * node); * The domain_id field must point to an allocated size_t object to which the * ROS domain ID will be written. * + * This function does not manipulate heap memory. + * This function is thread-safe for different nodes. + * This function is lock-free. + * * \param[in] node the handle to the node being queried * \return RCL_RET_OK if node the domain ID was retrieved successfully, or * RCL_RET_NODE_INVALID if the node is invalid, or @@ -209,6 +227,9 @@ rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); * this function each time it is needed and avoid use of the handle * concurrently with functions that might change it. * + * This function does not manipulate heap memory. + * This function is thread-safe for different nodes. + * This function is lock-free. * * \param[in] node pointer to the rcl node * \return rmw node handle if successful, otherwise NULL @@ -230,6 +251,10 @@ rcl_node_get_rmw_handle(const rcl_node_t * node); * This function will succeed, however, even if rcl_shutdown has been called * since the node was created. * + * This function does not manipulate heap memory. + * This function is thread-safe for different nodes. + * This function is lock-free. + * * \param[in] node pointer to the rcl node * \return rcl instance id captured at node creation or 0 if there was an error */ diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 0257473..4b42d39 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -80,7 +80,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator); * - No new work (executing callbacks) will be done in executors. * - Currently running work in executors will be finished. * - * This function does not allocate heap memory. + * This function manipulates heap memory. * This function is thread-safe, except with rcl_init(). * This function is lock-free so long as the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_uint_least64_t. diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index e27c5e9..ccb1201 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -33,10 +33,11 @@ typedef struct rcl_node_impl_t rcl_node_options_t options; rmw_node_t * rmw_node_handle; uint64_t rcl_instance_id; + size_t actual_domain_id; } rcl_node_impl_t; rcl_node_t -rcl_get_uninitialized_node() +rcl_get_zero_initialized_node() { static rcl_node_t null_node = {0}; return null_node; @@ -56,6 +57,11 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o RCL_SET_ERROR_MSG("node already initialized, or struct memory was unintialized"); return RCL_RET_ALREADY_INIT; } + // Make sure rcl has been initialized. + if (!rcl_ok()) { + RCL_SET_ERROR_MSG("rcl_init() has not been called"); + return RCL_RET_NOT_INIT; + } const rcl_allocator_t * allocator = &options->allocator; RCL_CHECK_FOR_NULL_WITH_MSG( allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); @@ -71,23 +77,27 @@ rcl_node_init(rcl_node_t * node, const char * name, const rcl_node_options_t * o RCL_SET_ERROR_MSG("node name cannot be empty string"); goto fail; } - // node options + // node options (assume it is trivially copyable) node->impl->options = *options; // node rmw_node_handle - // First determine the ROS_DOMAIN_ID. - // The result of rcl_impl_getenv on Windows is only valid until the next call to rcl_impl_getenv. - ret = rcl_impl_getenv("ROS_DOMAIN_ID", &ros_domain_id); - if (ret != RCL_RET_OK) { - goto fail; - } - if (ros_domain_id) { - unsigned long number = strtoul(ros_domain_id, NULL, 0); // NOLINT(runtime/int) - if (number == ULONG_MAX) { - RCL_SET_ERROR_MSG("failed to interpret ROS_DOMAIN_ID as integral number"); + if (node->impl->options.domain_id == RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID) { + // Find the domain ID set by the environment. + ret = rcl_impl_getenv("ROS_DOMAIN_ID", &ros_domain_id); + if (ret != RCL_RET_OK) { goto fail; } - domain_id = (size_t)number; + if (ros_domain_id) { + unsigned long number = strtoul(ros_domain_id, NULL, 0); // NOLINT(runtime/int) + if (number == ULONG_MAX) { + RCL_SET_ERROR_MSG("failed to interpret ROS_DOMAIN_ID as integral number"); + goto fail; + } + domain_id = (size_t)number; + } + } else { + domain_id = node->impl->options.domain_id; } + node->impl->actual_domain_id = domain_id; node->impl->rmw_node_handle = rmw_create_node(name, domain_id); RCL_CHECK_FOR_NULL_WITH_MSG( node->impl->rmw_node_handle, rmw_get_error_string_safe(), goto fail); @@ -118,6 +128,7 @@ rcl_node_fini(rcl_node_t * node) RCL_CHECK_FOR_NULL_WITH_MSG( allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); allocator.deallocate(node->impl, allocator.state); + node->impl = NULL; return result; } @@ -125,7 +136,6 @@ rcl_node_options_t rcl_node_get_default_options() { static rcl_node_options_t default_options = { - .no_parameters = false, .domain_id = RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID, }; // Must set the allocator after because it is not a compile time constant. @@ -157,6 +167,21 @@ rcl_node_get_options(const rcl_node_t * node) return &node->impl->options; } +rcl_ret_t +rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(domain_id, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + node->impl, "node implementation is invalid", return RCL_RET_NODE_INVALID); + if (node->impl->rcl_instance_id != rcl_get_instance_id()) { + RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match"); + return RCL_RET_NODE_INVALID; + } + *domain_id = node->impl->actual_domain_id; + return RCL_RET_OK; +} + rmw_node_t * rcl_node_get_rmw_handle(const rcl_node_t * node) { diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp new file mode 100644 index 0000000..2f2acdb --- /dev/null +++ b/rcl/test/rcl/test_node.cpp @@ -0,0 +1,307 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/rcl.h" +#include "rcl/node.h" + +#include "../memory_tools.hpp" +#include "../scope_exit.hpp" +#include "rcl/error_handling.h" + +class TestNodeFixture : public::testing::Test +{ +public: + void SetUp() + { + set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";}); + set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";}); + set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";}); + start_memory_checking(); + } + + void TearDown() + { + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + set_on_unexpected_malloc_callback(nullptr); + set_on_unexpected_realloc_callback(nullptr); + set_on_unexpected_free_callback(nullptr); + } +}; + +void * +failing_malloc(size_t size, void * state) +{ + (void)(size); + (void)(state); + return nullptr; +} + +void * +failing_realloc(void * pointer, size_t size, void * state) +{ + (void)(pointer); + (void)(size); + (void)(state); + return nullptr; +} + +void +failing_free(void * pointer, void * state) +{ + (void)pointer; + (void)state; +} + +/* Tests the node accessors, i.e. rcl_node_get_* functions. + */ +TEST_F(TestNodeFixture, test_rcl_node_accessors) { + stop_memory_checking(); + rcl_ret_t ret; + // Initialize rcl with rcl_init(). + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret); // Shutdown later after invalid node. + // Create an invalid node (rcl_shutdown). + rcl_node_t invalid_node = rcl_get_zero_initialized_node(); + const char * name = "node_name"; + rcl_node_options_t default_options = rcl_node_get_default_options(); + default_options.domain_id = 42; // Set the domain id to something explicit. + ret = rcl_node_init(&invalid_node, name, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + auto rcl_invalid_node_exit = make_scope_exit([&invalid_node]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_node_fini(&invalid_node); + EXPECT_EQ(RCL_RET_OK, ret); + }); + ret = rcl_shutdown(); // Shutdown to invalidate the node. + ASSERT_EQ(RCL_RET_OK, ret); + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret); + auto rcl_shutdown_exit = make_scope_exit([]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_shutdown(); + ASSERT_EQ(RCL_RET_OK, ret); + }); + // Create a zero init node. + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + // Create a normal node. + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, name, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + auto rcl_node_exit = make_scope_exit([&node]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + }); + // Test rcl_node_get_name(). + const char * actual_node_name; + actual_node_name = rcl_node_get_name(nullptr); + EXPECT_EQ(nullptr, actual_node_name); + rcl_reset_error(); + actual_node_name = rcl_node_get_name(&zero_node); + EXPECT_EQ(nullptr, actual_node_name); + rcl_reset_error(); + actual_node_name = rcl_node_get_name(&invalid_node); + EXPECT_EQ(nullptr, actual_node_name); + rcl_reset_error(); + start_memory_checking(); + assert_no_malloc_begin(); + assert_no_realloc_begin(); + assert_no_free_begin(); + actual_node_name = rcl_node_get_name(&node); + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + EXPECT_TRUE(actual_node_name); + if (actual_node_name) { + EXPECT_EQ(std::string(name), std::string(actual_node_name)); + } + // Test rcl_node_get_options(). + const rcl_node_options_t * actual_options; + actual_options = rcl_node_get_options(nullptr); + EXPECT_EQ(nullptr, actual_options); + rcl_reset_error(); + actual_options = rcl_node_get_options(&zero_node); + EXPECT_EQ(nullptr, actual_options); + rcl_reset_error(); + actual_options = rcl_node_get_options(&invalid_node); + EXPECT_EQ(nullptr, actual_options); + rcl_reset_error(); + start_memory_checking(); + assert_no_malloc_begin(); + assert_no_realloc_begin(); + assert_no_free_begin(); + actual_options = rcl_node_get_options(&node); + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + EXPECT_NE(nullptr, actual_options); + if (actual_options) { + EXPECT_EQ(default_options.allocator.allocate, actual_options->allocator.allocate); + EXPECT_EQ(default_options.domain_id, actual_options->domain_id); + } + // Test rcl_node_get_domain_id(). + size_t actual_domain_id; + ret = rcl_node_get_domain_id(nullptr, &actual_domain_id); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ret = rcl_node_get_domain_id(&zero_node, &actual_domain_id); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + ret = rcl_node_get_domain_id(&invalid_node, &actual_domain_id); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + start_memory_checking(); + assert_no_malloc_begin(); + assert_no_realloc_begin(); + assert_no_free_begin(); + ret = rcl_node_get_domain_id(&node, &actual_domain_id); + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + EXPECT_EQ(RCL_RET_OK, ret); + if (RCL_RET_OK == ret) { + EXPECT_EQ(42, actual_domain_id); + } + // Test rcl_node_get_rmw_handle(). + rmw_node_t * node_handle; + node_handle = rcl_node_get_rmw_handle(nullptr); + EXPECT_EQ(nullptr, node_handle); + rcl_reset_error(); + node_handle = rcl_node_get_rmw_handle(&zero_node); + EXPECT_EQ(nullptr, node_handle); + rcl_reset_error(); + node_handle = rcl_node_get_rmw_handle(&invalid_node); + EXPECT_EQ(nullptr, node_handle); + rcl_reset_error(); + start_memory_checking(); + assert_no_malloc_begin(); + assert_no_realloc_begin(); + assert_no_free_begin(); + node_handle = rcl_node_get_rmw_handle(&node); + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + EXPECT_NE(nullptr, node_handle); + // Test rcl_node_get_rcl_instance_id(). + uint64_t instance_id; + instance_id = rcl_node_get_rcl_instance_id(nullptr); + EXPECT_EQ(0, instance_id); + rcl_reset_error(); + instance_id = rcl_node_get_rcl_instance_id(&zero_node); + EXPECT_EQ(0, instance_id); + rcl_reset_error(); + instance_id = rcl_node_get_rcl_instance_id(&invalid_node); + EXPECT_NE(0, instance_id); + EXPECT_NE(42, instance_id); + rcl_reset_error(); + start_memory_checking(); + assert_no_malloc_begin(); + assert_no_realloc_begin(); + assert_no_free_begin(); + instance_id = rcl_node_get_rcl_instance_id(&node); + assert_no_malloc_end(); + assert_no_realloc_end(); + assert_no_free_end(); + stop_memory_checking(); + EXPECT_NE(0, instance_id); +} + +/* Tests the node life cycle, including rcl_node_init() and rcl_node_fini(). + */ +TEST_F(TestNodeFixture, test_rcl_node_life_cycle) { + stop_memory_checking(); + rcl_ret_t ret; + rcl_node_t node = rcl_get_zero_initialized_node(); + const char * name = "node_name"; + rcl_node_options_t default_options = rcl_node_get_default_options(); + // Trying to init before rcl_init() should fail. + ret = rcl_node_init(&node, name, &default_options); + ASSERT_EQ(RCL_RET_NOT_INIT, ret) << "Expected RCL_RET_NOT_INIT"; + rcl_reset_error(); + // Initialize rcl with rcl_init(). + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret); + auto rcl_shutdown_exit = make_scope_exit([]() { + rcl_ret_t ret = rcl_shutdown(); + ASSERT_EQ(RCL_RET_OK, ret); + }); + // Try invalid arguments. + ret = rcl_node_init(nullptr, name, &default_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ret = rcl_node_init(&node, nullptr, &default_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ret = rcl_node_init(&node, name, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + // Try with invalid allocator. + rcl_node_options_t options_with_invalid_allocator = rcl_node_get_default_options(); + options_with_invalid_allocator.allocator.allocate = nullptr; + options_with_invalid_allocator.allocator.deallocate = nullptr; + options_with_invalid_allocator.allocator.reallocate = nullptr; + ret = rcl_node_init(&node, name, &options_with_invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT"; + rcl_reset_error(); + node = rcl_get_zero_initialized_node(); + // Try with failing allocator. + rcl_node_options_t options_with_failing_allocator = rcl_node_get_default_options(); + options_with_failing_allocator.allocator.allocate = failing_malloc; + options_with_failing_allocator.allocator.deallocate = failing_free; + options_with_failing_allocator.allocator.reallocate = failing_realloc; + ret = rcl_node_init(&node, name, &options_with_failing_allocator); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << "Expected RCL_RET_BAD_ALLOC"; + rcl_reset_error(); + node = rcl_get_zero_initialized_node(); + // Try fini with invalid arguments. + ret = rcl_node_fini(nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT"; + rcl_reset_error(); + // Try fini with an uninitialized node. + ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + // Try a normal init and fini. + ret = rcl_node_init(&node, name, &default_options); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + node = rcl_get_zero_initialized_node(); + // Try repeated init and fini calls. + ret = rcl_node_init(&node, name, &default_options); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_node_init(&node, name, &default_options); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << "Expected RCL_RET_ALREADY_INIT"; + ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + node = rcl_get_zero_initialized_node(); + // Try with a specific domain id. + rcl_node_options_t options_with_custom_domain_id = rcl_node_get_default_options(); + options_with_custom_domain_id.domain_id = 42; + ret = rcl_node_init(&node, name, &options_with_custom_domain_id); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + node = rcl_get_zero_initialized_node(); +} From 655577bad28ed51748076f5bc5c859c1b788f9c0 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 17:36:38 -0800 Subject: [PATCH 65/71] fix for cpplint warning --- rcl/test/rcl/test_node.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp index 2f2acdb..31464ca 100644 --- a/rcl/test/rcl/test_node.cpp +++ b/rcl/test/rcl/test_node.cpp @@ -17,6 +17,8 @@ #include "rcl/rcl.h" #include "rcl/node.h" +#include + #include "../memory_tools.hpp" #include "../scope_exit.hpp" #include "rcl/error_handling.h" From fe37e8231156e632988f9ed9de013a1e98aa8cd6 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Dec 2015 23:00:43 -0800 Subject: [PATCH 66/71] fix warning on Windows --- rcl/test/rcl/test_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp index 31464ca..8e057a1 100644 --- a/rcl/test/rcl/test_node.cpp +++ b/rcl/test/rcl/test_node.cpp @@ -130,7 +130,7 @@ TEST_F(TestNodeFixture, test_rcl_node_accessors) { assert_no_realloc_end(); assert_no_free_end(); stop_memory_checking(); - EXPECT_TRUE(actual_node_name); + EXPECT_TRUE(actual_node_name ? true : false); if (actual_node_name) { EXPECT_EQ(std::string(name), std::string(actual_node_name)); } From 1adcc057a72d28c07030e22e74fee6a31cdfd771 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 17 Dec 2015 16:09:49 -0800 Subject: [PATCH 67/71] fixup cpplint --- rcl/test/rcl/test_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp index 8e057a1..dc93b84 100644 --- a/rcl/test/rcl/test_node.cpp +++ b/rcl/test/rcl/test_node.cpp @@ -14,11 +14,11 @@ #include +#include + #include "rcl/rcl.h" #include "rcl/node.h" -#include - #include "../memory_tools.hpp" #include "../scope_exit.hpp" #include "rcl/error_handling.h" From a14caefdd1ddf9398000243471f2c540c5c6ea3a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 17 Dec 2015 17:30:18 -0800 Subject: [PATCH 68/71] enable more warnings for C compiling and fix them --- rcl/CMakeLists.txt | 1 + rcl/src/rcl/guard_condition.c | 3 +-- rcl/src/rcl/rcl.c | 13 +++++++++---- rcl/src/rcl/subscription.c | 2 +- rcl/src/rcl/wait.c | 10 +++++++++- 5 files changed, 21 insertions(+), 8 deletions(-) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index f0d54d5..a1658d5 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -13,6 +13,7 @@ include_directories(include) if(NOT WIN32) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra") endif() set(${PROJECT_NAME}_sources diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c index 0d93155..b4d30b3 100644 --- a/rcl/src/rcl/guard_condition.c +++ b/rcl/src/rcl/guard_condition.c @@ -113,8 +113,7 @@ rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition, rcl_node_t * n rcl_guard_condition_options_t rcl_guard_condition_get_default_options() { - static rcl_guard_condition_options_t default_options = {0}; - // Must set the allocator after because it is not a compile time constant. + static rcl_guard_condition_options_t default_options; default_options.allocator = rcl_get_default_allocator(); return default_options; } diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index 13ec6a8..6809407 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -26,7 +26,7 @@ extern "C" #include "rcl/error_handling.h" static atomic_bool __rcl_is_initialized = ATOMIC_VAR_INIT(false); -static rcl_allocator_t __rcl_allocator = {0}; +static rcl_allocator_t __rcl_allocator; static int __rcl_argc = 0; static char ** __rcl_argv = NULL; static atomic_uint_least64_t __rcl_instance_id = ATOMIC_VAR_INIT(0); @@ -36,7 +36,7 @@ static void __clean_up_init() { if (__rcl_argv) { - size_t i; + int i; for (i = 0; i < __rcl_argc; ++i) { if (__rcl_argv[i]) { // Use the old allocator. @@ -69,9 +69,14 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator) RCL_SET_ERROR_MSG("rcl_init called while already initialized"); return RCL_RET_ALREADY_INIT; } + // There is a race condition between the time __rcl_is_initialized is set true, + // and when the allocator is set, in which rcl_shutdown() could get rcl_ok() as + // true and try to use the allocator, but it isn't set yet... + // A very unlikely race condition, but it is possile I think. + // I've documented that rcl_init() and rcl_shutdown() are not thread-safe with each other. + __rcl_allocator = allocator; // Set the new allocator. // TODO(wjwwood): Remove rcl specific command line arguments. // For now just copy the argc and argv. - __rcl_allocator = allocator; // Set the new allocator. __rcl_argc = argc; __rcl_argv = (char **)__rcl_allocator.allocate(sizeof(char *) * argc, __rcl_allocator.state); if (!__rcl_argv) { @@ -80,7 +85,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator) goto fail; } memset(__rcl_argv, 0, sizeof(char **) * argc); - size_t i; + int i; for (i = 0; i < argc; ++i) { __rcl_argv[i] = (char *)__rcl_allocator.allocate(strlen(argv[i]), __rcl_allocator.state); memcpy(__rcl_argv[i], argv[i], strlen(argv[i])); diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index ce09153..2e58be0 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -137,7 +137,7 @@ rcl_take( rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info; // Call rmw_take_with_info. rmw_ret_t ret = - rmw_take_with_info(subscription->impl->rmw_handle, ros_message, taken, message_info); + rmw_take_with_info(subscription->impl->rmw_handle, ros_message, taken, message_info_local); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); return RCL_RET_ERROR; diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 0323963..eb87cdc 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -42,7 +42,15 @@ typedef struct rcl_wait_set_impl_t rcl_wait_set_t rcl_get_zero_initialized_wait_set() { - static rcl_wait_set_t null_wait_set = {0}; + static rcl_wait_set_t null_wait_set = { + .subscriptions = NULL, + .size_of_subscriptions = 0, + .guard_conditions = NULL, + .size_of_guard_conditions = 0, + .timers = NULL, + .size_of_timers = 0, + .impl = NULL, + }; return null_wait_set; } From 1030d18c59109572ef28bf693ffb9f7b981d6e9b Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Thu, 17 Dec 2015 18:42:24 -0800 Subject: [PATCH 69/71] use ASSERT_FALSE and ASSERT_TRUE --- rcl/test/rcl/test_rcl.cpp | 40 +++++++++++++++++++-------------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/rcl/test/rcl/test_rcl.cpp b/rcl/test/rcl/test_rcl.cpp index 42b634c..f618bad 100644 --- a/rcl/test/rcl/test_rcl.cpp +++ b/rcl/test/rcl/test_rcl.cpp @@ -107,25 +107,25 @@ TEST_F(TestRCLFixture, test_rcl_init_and_ok_and_shutdown) { ret = rcl_shutdown(); EXPECT_EQ(RCL_RET_NOT_INIT, ret); rcl_reset_error(); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); // If argc is not 0, but argv is, it should be an invalid argument. ret = rcl_init(42, nullptr, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); rcl_reset_error(); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); // If either the allocate or deallocate function pointers are not set, it should be invalid arg. rcl_allocator_t invalid_allocator = rcl_get_default_allocator(); invalid_allocator.allocate = nullptr; ret = rcl_init(0, nullptr, invalid_allocator); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); rcl_reset_error(); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); invalid_allocator.allocate = rcl_get_default_allocator().allocate; invalid_allocator.deallocate = nullptr; ret = rcl_init(0, nullptr, invalid_allocator); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); rcl_reset_error(); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); // If the malloc call fails (with some valid arguments to copy), it should be a bad alloc. { FakeTestArgv test_args; @@ -136,47 +136,47 @@ TEST_F(TestRCLFixture, test_rcl_init_and_ok_and_shutdown) { ret = rcl_init(test_args.argc, test_args.argv, failing_allocator); EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); rcl_reset_error(); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); } // If argc is 0 and argv is nullptr and the allocator is valid, it should succeed. ret = rcl_init(0, nullptr, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret); - ASSERT_EQ(true, rcl_ok()); + ASSERT_TRUE(rcl_ok()); // Then shutdown should work. ret = rcl_shutdown(); EXPECT_EQ(ret, RCL_RET_OK); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); // Valid argc/argv values and a valid allocator should succeed. { FakeTestArgv test_args; ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret); - ASSERT_EQ(true, rcl_ok()); + ASSERT_TRUE(rcl_ok()); } // Then shutdown should work. ret = rcl_shutdown(); EXPECT_EQ(RCL_RET_OK, ret); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); // A repeat call to shutdown should not work. ret = rcl_shutdown(); EXPECT_EQ(RCL_RET_NOT_INIT, ret); rcl_reset_error(); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); // Repeat, but valid, calls to rcl_init() should fail. { FakeTestArgv test_args; ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret); - ASSERT_EQ(true, rcl_ok()); + rcl_ok()); ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_ALREADY_INIT, ret); rcl_reset_error(); - ASSERT_EQ(true, rcl_ok()); + ASSERT_TRUE(rcl_ok()); } // But shutdown should still work. ret = rcl_shutdown(); EXPECT_EQ(ret, RCL_RET_OK); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); } /* Tests the rcl_get_instance_id() and rcl_ok() functions. @@ -185,18 +185,18 @@ TEST_F(TestRCLFixture, test_rcl_get_instance_id_and_ok) { rcl_ret_t ret; // Instance id should be 0 before rcl_init(). EXPECT_EQ(0, rcl_get_instance_id()); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); // It should still return 0 after an invalid init. ret = rcl_init(1, nullptr, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); EXPECT_EQ(0, rcl_get_instance_id()); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); // A non-zero instance id should be returned after a valid init. { FakeTestArgv test_args; ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret); - ASSERT_EQ(true, rcl_ok()); + ASSERT_TRUE(rcl_ok()); } // And it should be allocation free. uint64_t first_instance_id; @@ -214,20 +214,20 @@ TEST_F(TestRCLFixture, test_rcl_get_instance_id_and_ok) { ret = rcl_shutdown(); EXPECT_EQ(ret, RCL_RET_OK); EXPECT_EQ(0, rcl_get_instance_id()); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); // It should return a different value after another valid init. { FakeTestArgv test_args; ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret); - ASSERT_EQ(true, rcl_ok()); + ASSERT_TRUE(rcl_ok()); } EXPECT_NE(0, rcl_get_instance_id()); EXPECT_NE(first_instance_id, rcl_get_instance_id()); - ASSERT_EQ(true, rcl_ok()); + ASSERT_TRUE(rcl_ok()); // Shutting down a second time should result in 0 again. ret = rcl_shutdown(); EXPECT_EQ(ret, RCL_RET_OK); EXPECT_EQ(0, rcl_get_instance_id()); - ASSERT_EQ(false, rcl_ok()); + ASSERT_FALSE(rcl_ok()); } From 5054744c6d7e8cb0a05f543abdd9a72656935af6 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Thu, 17 Dec 2015 19:03:11 -0800 Subject: [PATCH 70/71] that happens when you "program" in the git web ui --- rcl/test/rcl/test_rcl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/test/rcl/test_rcl.cpp b/rcl/test/rcl/test_rcl.cpp index f618bad..7b8d0af 100644 --- a/rcl/test/rcl/test_rcl.cpp +++ b/rcl/test/rcl/test_rcl.cpp @@ -167,7 +167,7 @@ TEST_F(TestRCLFixture, test_rcl_init_and_ok_and_shutdown) { FakeTestArgv test_args; ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret); - rcl_ok()); + ASSERT_TRUE(rcl_ok()); ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_ALREADY_INIT, ret); rcl_reset_error(); From b358e986b2c5d41bb6426e6684677d374fc6233f Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Thu, 17 Dec 2015 19:11:01 -0800 Subject: [PATCH 71/71] add rosidl_default_runtime dependency --- rcl/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rcl/package.xml b/rcl/package.xml index 493e2c1..5f921a3 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -18,6 +18,8 @@ rcl_interfaces rosidl_generator_c + rosidl_default_runtime + rmw_implementation ament_cmake_gtest