diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index a1658d5..8256aa8 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -12,8 +12,8 @@ find_package(rosidl_generator_c REQUIRED) include_directories(include) if(NOT WIN32) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra") endif() set(${PROJECT_NAME}_sources @@ -47,10 +47,17 @@ macro(target) ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) + + # rcl_lib_dir is passed as APPEND_LIBRARY_DIRS for each ament_add_gtest call so + # the librcl that they link against is on the library path. + # This is especially important on Windows. + # This is overwritten each loop, but which one it points to doesn't really matter. + set(rcl_lib_dir "$") endmacro() call_for_each_rmw_implementation(target GENERATE_DEFAULT) +ament_export_dependencies(ament_cmake) ament_export_dependencies(rcl_interfaces) ament_export_dependencies(rmw) ament_export_dependencies(rmw_implementation) @@ -67,7 +74,14 @@ if(AMENT_ENABLE_TESTING) add_subdirectory(test) endif() -ament_package() +ament_package( + CONFIG_EXTRAS rcl-extras.cmake +) + +install( + DIRECTORY cmake + DESTINATION share/${PROJECT_NAME} +) install( DIRECTORY include/ diff --git a/rcl/cmake/get_rcl_information.cmake b/rcl/cmake/get_rcl_information.cmake new file mode 100644 index 0000000..af89c33 --- /dev/null +++ b/rcl/cmake/get_rcl_information.cmake @@ -0,0 +1,90 @@ +# Copyright 2016 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. + +# +# Get all information about rcl for a specific RMW implementation. +# +# It sets the common variables _DEFINITIONS, _INCLUDE_DIRS and _LIBRARIES +# with the given prefix. +# +# :param rmw_implementation: the RMW implementation name +# :type target: string +# :param var_prefix: the prefix of all output variable names +# :type var_prefix: string +# +# Copy/pasted from rclcpp/cmake/get_rclcpp_information.cmake. +# Code duplication is evil. Don't do this at home, kids. + +macro(get_rcl_information rmw_implementation var_prefix) + # pretend to be a "package" + # so that the variables can be used by various functions / macros + set(${var_prefix}_FOUND TRUE) + + # include directories + normalize_path(${var_prefix}_INCLUDE_DIRS "${rcl_DIR}/../../../include") + + # libraries + set(_libs) + # search for library relative to this CMake file + set(_library_target "rcl") + get_available_rmw_implementations(_rmw_impls) + list(LENGTH _rmw_impls _rmw_impls_length) + if(_rmw_impls_length GREATER 1) + set(_library_target "${_library_target}__${rmw_implementation}") + endif() + set(_lib "NOTFOUND") + find_library( + _lib NAMES "${_library_target}" + PATHS "${rcl_DIR}/../../../lib" + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH + ) + if(NOT _lib) + # warn about not existing library and ignore it + message(WARNING "Package 'rcl' doesn't contain the library '${_library_target}'") + elseif(NOT IS_ABSOLUTE "${_lib}") + # the found library must be an absolute path + message(FATAL_ERROR "Package 'rcl' found the library '${_library_target}' at '${_lib}' which is not an absolute path") + elseif(NOT EXISTS "${_lib}") + # the found library must exist + message(FATAL_ERROR "Package 'rcl' found the library '${_lib}' which doesn't exist") + else() + list(APPEND _libs "${_lib}") + endif() + + # dependencies + set(_exported_dependencies + "rcl_interfaces" + "rmw" + "${rmw_implementation}" + "rosidl_generator_c") + set(${var_prefix}_DEFINITIONS) + foreach(_dep ${_exported_dependencies}) + if(NOT ${_dep}_FOUND) + find_package("${_dep}" QUIET REQUIRED) + endif() + if(${_dep}_DEFINITIONS) + list_append_unique(${var_prefix}_DEFINITIONS "${${_dep}_DEFINITIONS}") + endif() + if(${_dep}_INCLUDE_DIRS) + list_append_unique(${var_prefix}_INCLUDE_DIRS "${${_dep}_INCLUDE_DIRS}") + endif() + if(${_dep}_LIBRARIES) + list(APPEND _libs "${${_dep}_LIBRARIES}") + endif() + endforeach() + if(_libs) + ament_libraries_deduplicate(_libs "${_libs}") + endif() + set(${var_prefix}_LIBRARIES "${_libs}") +endmacro() diff --git a/rcl/include/rcl/rcl.h b/rcl/include/rcl/rcl.h index 6258d02..e742e77 100644 --- a/rcl/include/rcl/rcl.h +++ b/rcl/include/rcl/rcl.h @@ -25,6 +25,7 @@ extern "C" #include "rcl/publisher.h" #include "rcl/subscription.h" #include "rcl/types.h" +#include "rcl/wait.h" #include "rcl/visibility_control.h" /// Global initialization of rcl. diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index 0f6220d..edf990e 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -24,15 +24,14 @@ extern "C" #include "rcl/types.h" #include "rcl/visibility_control.h" -#define RCL_S_TO_NS(seconds) (seconds * 1000 * 1000 * 1000) -#define RCL_MS_TO_NS(milliseconds) (milliseconds * 1000 * 1000) +#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 / (1000 * 1000 * 1000)) #define RCL_NS_TO_MS(nanoseconds) (nanoseconds / (1000 * 1000)) #define RCL_NS_TO_US(nanoseconds) (nanoseconds / 1000) - enum rcl_time_source_type_t { RCL_TIME_SOURCE_UNINITIALIZED = 0, @@ -77,324 +76,344 @@ typedef struct rcl_duration_t /// Check if the time_source has valid values. /* This function returns true if the time source appears to be valid. -* It will check that the type is not uninitialized, and that pointers -* are not invalid. Note that if data is uninitialized it may give a -* false positive. -* -* \param[in] time_source the handle to the time_source which is being queried -* \return true if the source is believed to be valid, otherwise return false. -*/ + * It will check that the type is not uninitialized, and that pointers + * are not invalid. + * Note that if data is uninitialized it may give a false positive. + * + * \param[in] time_source the handle to the time_source which is being queried + * \return true if the source is believed to be valid, otherwise return false. + */ RCL_PUBLIC RCL_WARN_UNUSED -bool rcl_time_source_valid(rcl_time_source_t * time_source); +bool +rcl_time_source_valid(rcl_time_source_t * time_source); /// Initialize a timesource as a RCL_ROS_TIME time source. /* This will allocate all necessary internal structures, and initialize variables. -* It is specifically setting up a RCL_ROS_TIME time source. -* -* \param[in] time_source the handle to the time_source which is being initialized -* \return RCL_RET_OK if the time source was successfully initialized, or -* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or -* RCL_RET_ERROR an unspecified error occur. -*/ + * It is specifically setting up a RCL_ROS_TIME time source. + * + * \param[in] time_source the handle to the time_source which is being initialized + * \return RCL_RET_OK if the time source was successfully initialized, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_init_ros_time_source(rcl_time_source_t * time_source); +rcl_ret_t +rcl_init_ros_time_source(rcl_time_source_t * time_source); /// Finalize a timesource as a RCL_ROS_TIME time source. /* This will deallocate all necessary internal structures, and clean up any variables. -* It is specifically setting up a RCL_ROS_TIME time source. It is expected to be paired with -* the init fuction. -* -* \param[in] time_source the handle to the time_source which is being initialized -* \return RCL_RET_OK if the time source was successfully finalized, or -* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or -* RCL_RET_ERROR an unspecified error occur. -*/ + * It is specifically setting up a RCL_ROS_TIME time source. It is expected to be paired with + * the init fuction. + * + * \param[in] time_source the handle to the time_source which is being initialized + * \return RCL_RET_OK if the time source was successfully finalized, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_fini_ros_time_source(rcl_time_source_t * time_source); +rcl_ret_t +rcl_fini_ros_time_source(rcl_time_source_t * time_source); /// Initialize a timesource as a RCL_STEADY_TIME time source. /* This will allocate all necessary internal structures, and initialize variables. -* It is specifically setting up a RCL_STEADY_TIME time source. -* -* \param[in] time_source the handle to the time_source which is being initialized -* \return RCL_RET_OK if the time source was successfully initialized, or -* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or -* RCL_RET_ERROR an unspecified error occur. -*/ + * It is specifically setting up a RCL_STEADY_TIME time source. + * + * \param[in] time_source the handle to the time_source which is being initialized + * \return RCL_RET_OK if the time source was successfully initialized, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_init_steady_time_source(rcl_time_source_t * time_source); +rcl_ret_t +rcl_init_steady_time_source(rcl_time_source_t * time_source); /// Finalize a timesource as a RCL_STEADY_TIME time source. /* Finalize the timesource as a RCL_STEADY_TIME time source. -* -* This will deallocate all necessary internal structures, and clean up any variables. -* It is specifically setting up a steady time source. It is expected to be paired with -* the init fuction. -* -* \param[in] time_source the handle to the time_source which is being initialized -* \return RCL_RET_OK if the time source was successfully finalized, or -* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or -* RCL_RET_ERROR an unspecified error occur. -*/ + * + * This will deallocate all necessary internal structures, and clean up any variables. + * It is specifically setting up a steady time source. It is expected to be paired with + * the init fuction. + * + * \param[in] time_source the handle to the time_source which is being initialized + * \return RCL_RET_OK if the time source was successfully finalized, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_fini_steady_time_source(rcl_time_source_t * time_source); +rcl_ret_t +rcl_fini_steady_time_source(rcl_time_source_t * time_source); /// Initialize a timesource as a RCL_SYSTEM_TIME time source. /* Initialize the timesource as a RCL_SYSTEM_TIME time source. -* -* This will allocate all necessary internal structures, and initialize variables. -* It is specifically setting up a system time source. -* -* \param[in] time_source the handle to the time_source which is being initialized -* \return RCL_RET_OK if the time source was successfully initialized, or -* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or -* RCL_RET_ERROR an unspecified error occur. -*/ + * + * This will allocate all necessary internal structures, and initialize variables. + * It is specifically setting up a system time source. + * + * \param[in] time_source the handle to the time_source which is being initialized + * \return RCL_RET_OK if the time source was successfully initialized, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_init_system_time_source(rcl_time_source_t * time_source); +rcl_ret_t +rcl_init_system_time_source(rcl_time_source_t * time_source); /// Finalize a timesource as a RCL_SYSTEM_TIME time source. /* Finalize the timesource as a RCL_SYSTEM_TIME time source. -* -* This will deallocate all necessary internal structures, and clean up any variables. -* It is specifically setting up a system time source. It is expected to be paired with -* the init fuction. -* -* \param[in] time_source the handle to the time_source which is being initialized. -* \return RCL_RET_OK if the time source was successfully finalized, or -* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or -* RCL_RET_ERROR an unspecified error occur. -*/ + * + * This will deallocate all necessary internal structures, and clean up any variables. + * It is specifically setting up a system time source. It is expected to be paired with + * the init fuction. + * + * \param[in] time_source the handle to the time_source which is being initialized. + * \return RCL_RET_OK if the time source was successfully finalized, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_fini_system_time_source(rcl_time_source_t * time_source); +rcl_ret_t +rcl_fini_system_time_source(rcl_time_source_t * time_source); /// Initialize a time point using the time_source. /* This function will initialize the time_point using the time_source -* as a reference. -* If the time_source is null it will use the system default time_source. -* -* This will allocate all necessary internal structures, and initialize variables. -* The time_source may be of types RCL_ROS_TIME, RCL_STEADY_TIME, or RCL_SYSTEM_TIME. -* -* \param[in] time_point the handle to the time_source which is being initialized. -* \param[in] time_source the handle to the time_source will be used for reference. -* \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_ERROR an unspecified error occur. -*/ + * as a reference. + * If the time_source is null it will use the system default time_source. + * + * This will allocate all necessary internal structures, and initialize variables. + * The time_source may be of types RCL_ROS_TIME, RCL_STEADY_TIME, or RCL_SYSTEM_TIME. + * + * \param[in] time_point the handle to the time_source which is being initialized. + * \param[in] time_source the handle to the time_source will be used for reference. + * \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_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_init_time_point(rcl_time_point_t * time_point, rcl_time_source_t * time_source); +rcl_ret_t +rcl_init_time_point(rcl_time_point_t * time_point, rcl_time_source_t * time_source); /// Finalize a time_point /* Finalize the time_point such that it is ready for deallocation. -* -* This will deallocate all necessary internal structures, and clean up any variables. -* * -* \param[in] time_point the handle to the time_source which is being finalized. -* \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_ERROR an unspecified error occur. -*/ + * + * This will deallocate all necessary internal structures, and clean up any variables. + * + * \param[in] time_point the handle to the time_source which is being finalized. + * \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_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_fini_time_point(rcl_time_point_t * time_point); +rcl_ret_t +rcl_fini_time_point(rcl_time_point_t * time_point); /// Initialize a duration using the time_source. -/* This function will initialize the duration using the time_source -* as a reference. -* If the time_source is null it will use the system default time_source. -* -* This will allocate all necessary internal structures, and initialize variables. -* The time_source may be of types ros, steady, or system. -* -* \param[in] duration the handle to the duration which is being initialized. -* \param[in] time_source the handle to the time_source will be used for reference. -* \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_ERROR an unspecified error occur. -*/ +/* This function will initialize the duration using the time_source as a reference. + * If the time_source is null it will use the system default time_source. + * + * This will allocate all necessary internal structures, and initialize variables. + * The time_source may be of types ros, steady, or system. + * + * \param[in] duration the handle to the duration which is being initialized. + * \param[in] time_source the handle to the time_source will be used for reference. + * \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_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_init_duration(rcl_duration_t * duration, rcl_time_source_t * time_source); +rcl_ret_t +rcl_init_duration(rcl_duration_t * duration, rcl_time_source_t * time_source); /// Finalize a duration /* Finalize the duration such that it is ready for deallocation. -* -* This will deallocate all necessary internal structures, and clean up any variables. -* * -* \param[in] duration the handle to the duration which is being finalized. -* \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_ERROR an unspecified error occur. -*/ + * + * This will deallocate all necessary internal structures, and clean up any variables. + * + * \param[in] duration the handle to the duration which is being finalized. + * \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_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_fini_duration(rcl_duration_t * duration); +rcl_ret_t +rcl_fini_duration(rcl_duration_t * duration); /// Get the default RCL_ROS_TIME time source /* This function will get the process default time source. -* This time source is specifically of the ROS time abstraction, -* and may be overridden by updates. -* -* If the default has not yet been used it will allocate -* and initialize the time source. -* -* \return rcl_time_source_t if it successfully found or allocated a - time source. If an error occurred it will return NULL. -*/ + * This time source is specifically of the ROS time abstraction, + * and may be overridden by updates. + * + * If the default has not yet been used it will allocate + * and initialize the time source. + * + * \return rcl_time_source_t if it successfully found or allocated a + * time source. If an error occurred it will return NULL. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_time_source_t * rcl_get_default_ros_time_source(void); +rcl_time_source_t * +rcl_get_default_ros_time_source(void); /// Get the default RCL_STEADY_TIME time source /* This function will get the process default time source. -* This time source is specifically of the steady time abstraction, -* it should not be able to be overridden.. -* -* If the default has not yet been used it will allocate -* and initialize the time source. -* -* \return rcl_time_source_t if it successfully found or allocated a - time source. If an error occurred it will return NULL. -*/ + * This time source is specifically of the steady time abstraction, + * it should not be able to be overridden.. + * + * If the default has not yet been used it will allocate + * and initialize the time source. + * + * \return rcl_time_source_t if it successfully found or allocated a + * time source. If an error occurred it will return NULL. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_time_source_t * rcl_get_default_steady_time_source(void); +rcl_time_source_t * +rcl_get_default_steady_time_source(void); /// Get the default RCL_SYSTEM_TIME time source /* This function will get the process default time source. -* This time source is specifically of the system time abstraction, -* and may be overridden by updates to the system clock. -* -* If the default has not yet been used it will allocate -* and initialize the time source. -* -* \return rcl_time_source_t if it successfully found or allocated a - time source. If an error occurred it will return NULL. -*/ + * This time source is specifically of the system time abstraction, + * and may be overridden by updates to the system clock. + * + * If the default has not yet been used it will allocate + * and initialize the time source. + * + * \return rcl_time_source_t if it successfully found or allocated a + * time source. If an error occurred it will return NULL. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_time_source_t * rcl_get_default_system_time_source(void); +rcl_time_source_t * +rcl_get_default_system_time_source(void); /// Set the current time on the RCL_ROS_TIME time source /* This function is used to set the time on a ros time source. -* It will error if passed a differnt time source. -* -* This should not block, except on Windows. One caveat is that -* if the ros time abstraction is active, it will invoke the user -* defined callbacks, for pre and post update notifications. The -* calbacks are supposed to be short running and non-blocking. -* -* \param[in] process_time_source The time source on which to set the value. -* \return RCL_RET_OK if the value was set successfully, or -* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or -* RCL_RET_ERROR an unspecified error occur. -*/ + * It will error if passed a differnt time source. + * + * This should not block, except on Windows. One caveat is that + * if the ros time abstraction is active, it will invoke the user + * defined callbacks, for pre and post update notifications. The + * calbacks are supposed to be short running and non-blocking. + * + * \param[in] process_time_source The time source on which to set the value. + * \return RCL_RET_OK if the value was set successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_set_default_ros_time_source(rcl_time_source_t * process_time_source); +rcl_ret_t +rcl_set_default_ros_time_source(rcl_time_source_t * process_time_source); /// Compute the difference between two time points /* This function takes two time points and computes the duration between them. -* The two time points must be using the same time abstraction.. -* And the resultant duration will also be of the same abstraction. -* -* The value will be computed as duration = finish - start. If start is after -* finish the duration will be negative. -* -* \param[in] start The time point for the start of the duration. -* \param[in] finish The time point for the end of the duration. -* \param[out] delta The duration between the start and finish. -* \return RCL_RET_OK if the difference was computed successfully, or -* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or -* RCL_RET_ERROR an unspecified error occur. -*/ + * The two time points must be using the same time abstraction, and the + * resultant duration will also be of the same abstraction. + * + * The value will be computed as duration = finish - start. If start is after + * finish the duration will be negative. + * + * \param[in] start The time point for the start of the duration. + * \param[in] finish The time point for the end of the duration. + * \param[out] delta The duration between the start and finish. + * \return RCL_RET_OK if the difference was computed successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_difference_times(rcl_time_point_t * start, rcl_time_point_t * finish, +rcl_ret_t +rcl_difference_times(rcl_time_point_t * start, rcl_time_point_t * finish, rcl_duration_t * delta); /// Fill the time point with the current value of the associated clock. /* This function will populate the data of the time_point object with the -* current value from it's associated time abstraction. -* -* \param[out] time_point The time_point to populate. -* \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_ERROR an unspecified error occur. -*/ + * current value from it's associated time abstraction. + * + * \param[out] time_point The time_point to populate. + * \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_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_get_time_point_now(rcl_time_point_t * time_point); +rcl_ret_t +rcl_get_time_point_now(rcl_time_point_t * time_point); /// Enable the ros time abstraction override. /* This method will enable the ros time abstraction override values, -* such that the time source will report the set value instead of falling -* back to system time. -* -* \param[in] time_source The time_source to enable. -* \return RCL_RET_OK if the time source was enabled successfully, or -* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or -* RCL_RET_ERROR an unspecified error occur. -*/ + * such that the time source will report the set value instead of falling + * back to system time. + * + * \param[in] time_source The time_source to enable. + * \return RCL_RET_OK if the time source was enabled successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_enable_ros_time_override(rcl_time_source_t * time_source); +rcl_ret_t +rcl_enable_ros_time_override(rcl_time_source_t * time_source); /// Disable the ros time abstraction override. /* This method will disable the RCL_ROS_TIME time abstraction override values, -* such that the time source will report the system time even if a custom -* value has been set. -* -* \param[in] time_source The time_source to disable. -* \return RCL_RET_OK if the time source was disabled successfully, or -* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or -* RCL_RET_ERROR an unspecified error occur. -*/ + * such that the time source will report the system time even if a custom + * value has been set. + * + * \param[in] time_source The time_source to disable. + * \return RCL_RET_OK if the time source was disabled successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_disable_ros_time_override(rcl_time_source_t * time_source); +rcl_ret_t +rcl_disable_ros_time_override(rcl_time_source_t * time_source); /// Check if the RCL_ROS_TIME time source has the override enabled. /* This will populate the is_enabled object to indicate if the -* time overide is enabled. If it is enabled, the set value will be returned. -* Otherwise this time source will return the equivalent to system time abstraction. -* -* \param[in] time_source The time_source to query. -* \param[out] is_enabled Whether the override is enabled.. -* \return RCL_RET_OK if the time source was queried successfully, or -* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or -* RCL_RET_ERROR an unspecified error occur. -*/ + * time overide is enabled. If it is enabled, the set value will be returned. + * Otherwise this time source will return the equivalent to system time abstraction. + * + * \param[in] time_source The time_source to query. + * \param[out] is_enabled Whether the override is enabled.. + * \return RCL_RET_OK if the time source was queried successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_is_enabled_ros_time_override(rcl_time_source_t * time_source, +rcl_ret_t +rcl_is_enabled_ros_time_override(rcl_time_source_t * time_source, bool * is_enabled); /// Set the current time for this RCL_ROS_TIME time source. /* This function will update the internal storage for the RCL_ROS_TIME time source. -* If queried and override enabled the time source will return this value, -* otherwise it will return the system time. -* -* \param[in] time_source The time_source to update. -* \param[in] time_value The new current time. -* \return RCL_RET_OK if the time source was set successfully, or -* RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or -* RCL_RET_ERROR an unspecified error occur. -*/ + * If queried and override enabled the time source will return this value, + * otherwise it will return the system time. + * + * \param[in] time_source The time_source to update. + * \param[in] time_value The new current time. + * \return RCL_RET_OK if the time source was set successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_ret_t rcl_set_ros_time_override(rcl_time_source_t * time_source, +rcl_ret_t +rcl_set_ros_time_override(rcl_time_source_t * time_source, rcl_time_point_value_t time_value); /// Retrieve the current time as a rcl_time_point_value_t (an alias for unint64_t). @@ -412,8 +431,8 @@ rcl_ret_t rcl_set_ros_time_override(rcl_time_source_t * time_source, * On Windows this is lock-free if the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_int_least64_t. * - * TODO(tfoote) I talked with @wjwwood about possibly promoting this - * method into rmw for more reuse. + * TODO(tfoote): I talked with @wjwwood about possibly promoting this + * method into rmw for more reuse. * * \param[out] now a datafield in which the current time is stored * \return RCL_RET_OK if the current time was successfully obtained, or diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index a3a2c36..f1608c8 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -79,6 +79,8 @@ rcl_get_zero_initialized_wait_set(void); * ret = rcl_wait_set_fini(&wait_set); * // ... error handling * + * \TODO(wjwwood): consider the "fixed guard conditions", a la rmw's wait set. + * * 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. @@ -297,7 +299,7 @@ rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size); * 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()); + * rcl_ret_t ret = rcl_wait_set_init(&wait_set, 2, 1, 0, rcl_get_default_allocator()); * // ... error handling * do { * ret = rcl_wait_set_clear_subscriptions(&wait_set); diff --git a/rcl/package.xml b/rcl/package.xml index 5f921a3..d263184 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -18,6 +18,7 @@ rcl_interfaces rosidl_generator_c + ament_cmake rosidl_default_runtime rmw_implementation @@ -26,6 +27,7 @@ ament_lint_auto ament_lint_common rmw + std_msgs ament_cmake diff --git a/rcl/rcl-extras.cmake b/rcl/rcl-extras.cmake new file mode 100644 index 0000000..64610af --- /dev/null +++ b/rcl/rcl-extras.cmake @@ -0,0 +1,17 @@ +# Copyright 2016 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. + +# copied from rcl/rcl-extras.cmake + +include("${rcl_DIR}/get_rcl_information.cmake") diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 808a2ca..3316419 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -48,6 +48,10 @@ rcl_publisher_init( 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); + if (!node->impl) { + RCL_SET_ERROR_MSG("invalid node"); + return RCL_RET_NODE_INVALID; + } 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); diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index e233562..90b5dd1 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #if defined(WIN32) @@ -27,9 +28,8 @@ static rcl_time_source_t * rcl_default_ros_time_source; static rcl_time_source_t * rcl_default_steady_time_source; static rcl_time_source_t * rcl_default_system_time_source; - // Internal storage for RCL_ROS_TIME implementation -typedef struct +typedef struct rcl_ros_time_source_storage_t { atomic_uint_least64_t current_time; bool active; @@ -37,21 +37,24 @@ typedef struct } rcl_ros_time_source_storage_t; // Implementation only -rcl_ret_t rcl_get_steady_time(void * data, rcl_time_point_value_t * current_time) +rcl_ret_t +rcl_get_steady_time(void * data, rcl_time_point_value_t * current_time) { - (void) data; // unused + (void)data; // unused return rcl_steady_time_now(current_time); } // Implementation only -rcl_ret_t rcl_get_system_time(void * data, rcl_time_point_value_t * current_time) +rcl_ret_t +rcl_get_system_time(void * data, rcl_time_point_value_t * current_time) { - (void) data; // unused + (void)data; // unused return rcl_system_time_now(current_time); } // Internal method for zeroing values on init, assumes time_source is valid -void rcl_init_generic_time_source(rcl_time_source_t * time_source) +void +rcl_init_generic_time_source(rcl_time_source_t * time_source) { time_source->type = RCL_TIME_SOURCE_UNINITIALIZED; time_source->pre_update = NULL; @@ -62,7 +65,8 @@ void rcl_init_generic_time_source(rcl_time_source_t * time_source) // The function used to get the current ros time. // This is in the implementation only -rcl_ret_t rcl_get_ros_time(void * data, rcl_time_point_value_t * current_time) +rcl_ret_t +rcl_get_ros_time(void * data, rcl_time_point_value_t * current_time) { rcl_ros_time_source_storage_t * t = (rcl_ros_time_source_storage_t *)data; if (!t->active) { @@ -72,7 +76,8 @@ rcl_ret_t rcl_get_ros_time(void * data, rcl_time_point_value_t * current_time) return RCL_RET_OK; } -bool rcl_time_source_valid(rcl_time_source_t * time_source) +bool +rcl_time_source_valid(rcl_time_source_t * time_source) { if (time_source == NULL || time_source->type == RCL_TIME_SOURCE_UNINITIALIZED || @@ -83,7 +88,8 @@ bool rcl_time_source_valid(rcl_time_source_t * time_source) return true; } -rcl_ret_t rcl_init_ros_time_source(rcl_time_source_t * time_source) +rcl_ret_t +rcl_init_ros_time_source(rcl_time_source_t * time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); rcl_init_generic_time_source(time_source); @@ -93,7 +99,8 @@ rcl_ret_t rcl_init_ros_time_source(rcl_time_source_t * time_source) return RCL_RET_OK; } -rcl_ret_t rcl_fini_ros_time_source(rcl_time_source_t * time_source) +rcl_ret_t +rcl_fini_ros_time_source(rcl_time_source_t * time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); if (time_source->type != RCL_ROS_TIME) { @@ -104,7 +111,8 @@ rcl_ret_t rcl_fini_ros_time_source(rcl_time_source_t * time_source) return RCL_RET_OK; } -rcl_ret_t rcl_init_steady_time_source(rcl_time_source_t * time_source) +rcl_ret_t +rcl_init_steady_time_source(rcl_time_source_t * time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); rcl_init_generic_time_source(time_source); @@ -113,7 +121,8 @@ rcl_ret_t rcl_init_steady_time_source(rcl_time_source_t * time_source) return RCL_RET_OK; } -rcl_ret_t rcl_fini_steady_time_source(rcl_time_source_t * time_source) +rcl_ret_t +rcl_fini_steady_time_source(rcl_time_source_t * time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); if (time_source->type != RCL_STEADY_TIME) { @@ -123,19 +132,18 @@ rcl_ret_t rcl_fini_steady_time_source(rcl_time_source_t * time_source) return RCL_RET_OK; } -rcl_ret_t rcl_init_system_time_source(rcl_time_source_t * time_source) +rcl_ret_t +rcl_init_system_time_source(rcl_time_source_t * time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); rcl_init_generic_time_source(time_source); time_source->get_now = rcl_get_system_time; time_source->type = RCL_SYSTEM_TIME; - if (!time_source) { - return RCL_RET_ERROR; - } return RCL_RET_OK; } -rcl_ret_t rcl_fini_system_time_source(rcl_time_source_t * time_source) +rcl_ret_t +rcl_fini_system_time_source(rcl_time_source_t * time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); if (time_source->type != RCL_SYSTEM_TIME) { @@ -145,7 +153,8 @@ rcl_ret_t rcl_fini_system_time_source(rcl_time_source_t * time_source) return RCL_RET_OK; } -rcl_ret_t rcl_init_time_point(rcl_time_point_t * time_point, rcl_time_source_t * time_source) +rcl_ret_t +rcl_init_time_point(rcl_time_point_t * time_point, rcl_time_source_t * time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT); if (!time_source) { @@ -157,14 +166,16 @@ rcl_ret_t rcl_init_time_point(rcl_time_point_t * time_point, rcl_time_source_t * return RCL_RET_OK; } -rcl_ret_t rcl_fini_time_point(rcl_time_point_t * time_point) +rcl_ret_t +rcl_fini_time_point(rcl_time_point_t * time_point) { RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT); - (void) time_point; + (void)time_point; return RCL_RET_OK; } -rcl_ret_t rcl_init_duration(rcl_duration_t * duration, rcl_time_source_t * time_source) +rcl_ret_t +rcl_init_duration(rcl_duration_t * duration, rcl_time_source_t * time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(duration, RCL_RET_INVALID_ARGUMENT); if (!time_source) { @@ -176,17 +187,19 @@ rcl_ret_t rcl_init_duration(rcl_duration_t * duration, rcl_time_source_t * time_ return RCL_RET_OK; } -rcl_ret_t rcl_fini_duration(rcl_duration_t * duration) +rcl_ret_t +rcl_fini_duration(rcl_duration_t * duration) { RCL_CHECK_ARGUMENT_FOR_NULL(duration, RCL_RET_INVALID_ARGUMENT); - (void) duration; + (void)duration; return RCL_RET_OK; } -rcl_time_source_t * rcl_get_default_ros_time_source(void) +rcl_time_source_t * +rcl_get_default_ros_time_source(void) { if (!rcl_default_ros_time_source) { - rcl_default_ros_time_source = calloc(1, sizeof(rcl_time_source_t)); + rcl_default_ros_time_source = (rcl_time_source_t *)calloc(1, sizeof(rcl_time_source_t)); rcl_ret_t retval = rcl_init_ros_time_source(rcl_default_ros_time_source); if (retval != RCL_RET_OK) { return NULL; @@ -195,10 +208,11 @@ rcl_time_source_t * rcl_get_default_ros_time_source(void) return rcl_default_ros_time_source; } -rcl_time_source_t * rcl_get_default_steady_time_source(void) +rcl_time_source_t * +rcl_get_default_steady_time_source(void) { if (!rcl_default_steady_time_source) { - rcl_default_steady_time_source = calloc(1, sizeof(rcl_time_source_t)); + rcl_default_steady_time_source = (rcl_time_source_t *)calloc(1, sizeof(rcl_time_source_t)); rcl_ret_t retval = rcl_init_steady_time_source(rcl_default_steady_time_source); if (retval != RCL_RET_OK) { return NULL; @@ -207,10 +221,11 @@ rcl_time_source_t * rcl_get_default_steady_time_source(void) return rcl_default_steady_time_source; } -rcl_time_source_t * rcl_get_default_system_time_source(void) +rcl_time_source_t * +rcl_get_default_system_time_source(void) { if (!rcl_default_system_time_source) { - rcl_default_system_time_source = calloc(1, sizeof(rcl_time_source_t)); + rcl_default_system_time_source = (rcl_time_source_t *)calloc(1, sizeof(rcl_time_source_t)); rcl_ret_t retval = rcl_init_system_time_source(rcl_default_system_time_source); if (retval != RCL_RET_OK) { return NULL; @@ -219,7 +234,8 @@ rcl_time_source_t * rcl_get_default_system_time_source(void) return rcl_default_system_time_source; } -rcl_ret_t rcl_set_default_ros_time_source(rcl_time_source_t * process_time_source) +rcl_ret_t +rcl_set_default_ros_time_source(rcl_time_source_t * process_time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(process_time_source, RCL_RET_INVALID_ARGUMENT); if (rcl_default_ros_time_source) { @@ -229,7 +245,8 @@ rcl_ret_t rcl_set_default_ros_time_source(rcl_time_source_t * process_time_sourc return RCL_RET_OK; } -rcl_ret_t rcl_difference_times(rcl_time_point_t * start, rcl_time_point_t * finish, +rcl_ret_t +rcl_difference_times(rcl_time_point_t * start, rcl_time_point_t * finish, rcl_duration_t * delta) { if (start->time_source->type != finish->time_source->type) { @@ -244,7 +261,8 @@ rcl_ret_t rcl_difference_times(rcl_time_point_t * start, rcl_time_point_t * fini return RCL_RET_OK; } -rcl_ret_t rcl_get_time_point_now(rcl_time_point_t * time_point) +rcl_ret_t +rcl_get_time_point_now(rcl_time_point_t * time_point) { RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT); if (time_point->time_source && time_point->time_source->get_now) { @@ -255,7 +273,8 @@ rcl_ret_t rcl_get_time_point_now(rcl_time_point_t * time_point) return RCL_RET_ERROR; } -rcl_ret_t rcl_enable_ros_time_override(rcl_time_source_t * time_source) +rcl_ret_t +rcl_enable_ros_time_override(rcl_time_source_t * time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); if (time_source->type != RCL_ROS_TIME) { @@ -272,7 +291,8 @@ rcl_ret_t rcl_enable_ros_time_override(rcl_time_source_t * time_source) return RCL_RET_OK; } -rcl_ret_t rcl_disable_ros_time_override(rcl_time_source_t * time_source) +rcl_ret_t +rcl_disable_ros_time_override(rcl_time_source_t * time_source) { RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); if (time_source->type != RCL_ROS_TIME) { @@ -288,7 +308,9 @@ rcl_ret_t rcl_disable_ros_time_override(rcl_time_source_t * time_source) return RCL_RET_OK; } -rcl_ret_t rcl_is_enabled_ros_time_override(rcl_time_source_t * time_source, +rcl_ret_t +rcl_is_enabled_ros_time_override( + rcl_time_source_t * time_source, bool * is_enabled) { RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); @@ -306,7 +328,9 @@ rcl_ret_t rcl_is_enabled_ros_time_override(rcl_time_source_t * time_source, return RCL_RET_OK; } -rcl_ret_t rcl_set_ros_time_override(rcl_time_source_t * time_source, +rcl_ret_t +rcl_set_ros_time_override( + rcl_time_source_t * time_source, rcl_time_point_value_t time_value) { RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT); diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index f01a92b..41c877e 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -110,16 +110,22 @@ rcl_wait_set_init( 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); + memset(wait_set->impl, 0, sizeof(rcl_wait_set_impl_t)); 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; + static rmw_guard_conditions_t fixed_guard_conditions; + fixed_guard_conditions.guard_conditions = NULL; + fixed_guard_conditions.guard_condition_count = 0; wait_set->impl->rmw_waitset = rmw_create_waitset( - NULL, 2 * number_of_subscriptions + number_of_guard_conditions); + &fixed_guard_conditions, 2 * number_of_subscriptions + number_of_guard_conditions); if (!wait_set->impl->rmw_waitset) { goto fail; } + // Set allocator. + wait_set->impl->allocator = allocator; // Initialize subscription space. rcl_ret_t ret; if ((ret = rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions)) != RCL_RET_OK) { @@ -150,8 +156,6 @@ rcl_wait_set_init( fail_ret = ret; goto fail; } - // Set allocator. - wait_set->impl->allocator = allocator; return RCL_RET_OK; fail: if (__wait_set_is_valid(wait_set)) { @@ -395,7 +399,9 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) 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. + // If min_timeout is > 0, then compare it to the time until each timer. + // Take the lowest and use that for the wait timeout. + if (min_timeout > 0) { size_t i; for (i = 0; i < wait_set->size_of_timers; ++i) { if (!wait_set->timers[i]) { @@ -430,6 +436,18 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) &dummy_clients, wait_set->impl->rmw_waitset, timeout_argument); + // Check for timeout. + if (ret == RMW_RET_TIMEOUT) { + // Assume none were set (because timeout was reached first), and clear all. + rcl_ret_t rcl_ret; + // This next line prevents "assigned but never used" warnings in Release mode. + (void)rcl_ret; // NO LINT + 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; + } // Check for error. if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); @@ -447,17 +465,6 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) 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. - rcl_ret_t rcl_ret; - (void)rcl_ret; // NO LINT - 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. for (i = 0; i < wait_set->size_of_subscriptions; ++i) { assert(i < wait_set->impl->rmw_subscriptions.subscriber_count); // Defensive. diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index a3d8edc..f586b27 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -1,117 +1,91 @@ find_package(ament_cmake_gtest REQUIRED) +find_package(std_msgs REQUIRED) + +include(rcl_add_custom_gtest.cmake) set(extra_test_libraries) -set(extra_memory_tools_env PHONY=) # Use a phony env var so there is always at least one. -# rcl_lib_dir is passed as APPEND_LIBRARY_DIRS for each ament_add_gtest call so -# the librcl that they link against is on the library path. -# This is especially improtant on Windows. -set(rcl_lib_dir $) -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_env) -if(NOT WIN32) - ament_add_gtest(test_memory_tools test_memory_tools.cpp - ENV ${extra_memory_tools_env} APPEND_LIBRARY_DIRS ${rcl_lib_dir}) - 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}) +# This subdirectory extends both extra_test_libraries and extra_test_env. +add_subdirectory(memory_tools) + +macro(test_target) + find_package(${rmw_implementation} REQUIRED) + test_target_function() +endmacro() + +function(test_target_function) + # TODO(wjwwood): remove these when C type support is supported by typesupport_introspection. + # after removing, we can also recombine the function and macro + if("${rmw_implementation} " STREQUAL "rmw_connext_dynamic_cpp ") + message(STATUS "Skipping tests for '${rmw_implementation}'") + return() + endif() + if("${rmw_implementation} " STREQUAL "rmw_fastrtps_cpp ") + message(STATUS "Skipping tests for '${rmw_implementation}'") + return() endif() -endif() -ament_add_gtest(test_allocator rcl/test_allocator.cpp - ENV ${extra_memory_tools_env} APPEND_LIBRARY_DIRS ${rcl_lib_dir}) -if(TARGET test_allocator) - target_include_directories(test_allocator PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} + message(STATUS "Creating tests for '${rmw_implementation}'") + + rcl_add_custom_gtest(test_allocator${target_suffix} + SRCS rcl/test_allocator.cpp + ENV ${extra_test_env} + APPEND_LIBRARY_DIRS "${rcl_lib_dir}" + LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries} + AMENT_DEPENDENCIES ${rmw_implementation} ) - 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} APPEND_LIBRARY_DIRS ${rcl_lib_dir}) -if(TARGET test_time) - target_include_directories(test_time PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} + rcl_add_custom_gtest(test_time${target_suffix} + SRCS rcl/test_time.cpp + ENV ${extra_test_env} + APPEND_LIBRARY_DIRS "${rcl_lib_dir}" + LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries} + AMENT_DEPENDENCIES ${rmw_implementation} ) - 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 - APPEND_LIBRARY_DIRS - ${rcl_lib_dir} -) -if(TARGET test_common) - target_include_directories(test_common PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} + rcl_add_custom_gtest(test_common${target_suffix} + SRCS rcl/test_common.cpp + ENV + ${extra_test_env} + EMPTY_TEST= + NORMAL_TEST=foo + APPEND_LIBRARY_DIRS "${rcl_lib_dir}" + LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries} + AMENT_DEPENDENCIES ${rmw_implementation} ) - 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} APPEND_LIBRARY_DIRS ${rcl_lib_dir}) -if(TARGET test_rcl) - target_include_directories(test_rcl PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} + rcl_add_custom_gtest(test_rcl${target_suffix} + SRCS rcl/test_rcl.cpp + ENV ${extra_test_env} + APPEND_LIBRARY_DIRS "${rcl_lib_dir}" + LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries} + AMENT_DEPENDENCIES ${rmw_implementation} ) - 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} APPEND_LIBRARY_DIRS ${rcl_lib_dir}) -if(TARGET test_node) - target_include_directories(test_node PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} + rcl_add_custom_gtest(test_node${target_suffix} + SRCS rcl/test_node.cpp + ENV ${extra_test_env} + APPEND_LIBRARY_DIRS "${rcl_lib_dir}" + LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries} + AMENT_DEPENDENCIES ${rmw_implementation} ) - 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() + + rcl_add_custom_gtest(test_publisher${target_suffix} + SRCS rcl/test_publisher.cpp + ENV ${extra_test_env} + APPEND_LIBRARY_DIRS "${rcl_lib_dir}" + LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries} + AMENT_DEPENDENCIES ${rmw_implementation} "std_msgs" + ) + + rcl_add_custom_gtest(test_subscription${target_suffix} + SRCS rcl/test_subscription.cpp + ENV ${extra_test_env} + APPEND_LIBRARY_DIRS "${rcl_lib_dir}" + LIBRARIES ${PROJECT_NAME}${target_suffix} ${extra_test_libraries} + AMENT_DEPENDENCIES ${rmw_implementation} "std_msgs" + ) +endfunction() + +call_for_each_rmw_implementation(test_target) diff --git a/rcl/test/memory_tools/CMakeLists.txt b/rcl/test/memory_tools/CMakeLists.txt new file mode 100644 index 0000000..1f394ea --- /dev/null +++ b/rcl/test/memory_tools/CMakeLists.txt @@ -0,0 +1,36 @@ +ament_find_gtest() # For GTEST_LIBRARIES + +# Create the memory_tools library which is used by the tests. (rmw implementation agnostic) +add_library(${PROJECT_NAME}_memory_tools SHARED memory_tools.cpp) +if(APPLE) + # Create an OS X specific version of the memory tools that does interposing. + # See: http://toves.freeshell.org/interpose/ + add_library(${PROJECT_NAME}_memory_tools_interpose SHARED memory_tools_osx_interpose.cpp) + target_link_libraries(${PROJECT_NAME}_memory_tools_interpose ${GTEST_LIBRARIES}) + list(APPEND extra_test_libraries ${PROJECT_NAME}_memory_tools_interpose) + list(APPEND extra_test_env + DYLD_INSERT_LIBRARIES=$) +endif() +if(UNIX AND NOT APPLE) + # On Linux like systems, add dl and use the normal library and DL_PRELOAD. + list(APPEND extra_test_libraries dl) + list(APPEND extra_test_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) + +# Create tests for the memory tools library. +if(NOT WIN32) # (memory tools doesn't do anything on Windows) + include(../rcl_add_custom_gtest.cmake) + + rcl_add_custom_gtest(test_memory_tools + SRCS test_memory_tools.cpp + ENV ${extra_test_env} + LIBRARIES ${extra_test_libraries} + ) +endif() + +set(extra_test_libraries ${extra_test_libraries} PARENT_SCOPE) +set(extra_test_env ${extra_test_env} PARENT_SCOPE) diff --git a/rcl/test/memory_tools.cpp b/rcl/test/memory_tools/memory_tools.cpp similarity index 95% rename from rcl/test/memory_tools.cpp rename to rcl/test/memory_tools/memory_tools.cpp index 17fd508..37c7b82 100644 --- a/rcl/test/memory_tools.cpp +++ b/rcl/test/memory_tools/memory_tools.cpp @@ -55,14 +55,16 @@ free(void * pointer) void start_memory_checking() { - printf("starting memory checking...\n"); - enabled.store(true); + if (!enabled.exchange(true)) { + printf("starting memory checking...\n"); + } } void stop_memory_checking() { - printf("stopping memory checking...\n"); - enabled.store(false); + if (enabled.exchange(false)) { + printf("stopping memory checking...\n"); + } } /****************************************************************************** diff --git a/rcl/test/memory_tools.hpp b/rcl/test/memory_tools/memory_tools.hpp similarity index 85% rename from rcl/test/memory_tools.hpp rename to rcl/test/memory_tools/memory_tools.hpp index 001c110..ef92e4a 100644 --- a/rcl/test/memory_tools.hpp +++ b/rcl/test/memory_tools/memory_tools.hpp @@ -16,8 +16,8 @@ // https://dxr.mozilla.org/mozilla-central/rev/ // cc9c6cd756cb744596ba039dcc5ad3065a7cc3ea/memory/build/replace_malloc.c -#ifndef MEMORY_TOOLS_HPP_ -#define MEMORY_TOOLS_HPP_ +#ifndef RCL__TEST__MEMORY_TOOLS__MEMORY_TOOLS_HPP_ +#define RCL__TEST__MEMORY_TOOLS__MEMORY_TOOLS_HPP_ #include @@ -104,4 +104,29 @@ RCL_MEMORY_TOOLS_PUBLIC void memory_checking_thread_init(); -#endif // MEMORY_TOOLS_HPP_ +// What follows is a set of failing allocator functions, used for testing. +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; +} + +#endif // RCL__TEST__MEMORY_TOOLS__MEMORY_TOOLS_HPP_ diff --git a/rcl/test/memory_tools_common.cpp b/rcl/test/memory_tools/memory_tools_common.cpp similarity index 90% rename from rcl/test/memory_tools_common.cpp rename to rcl/test/memory_tools/memory_tools_common.cpp index c7ea833..8f8d51f 100644 --- a/rcl/test/memory_tools_common.cpp +++ b/rcl/test/memory_tools/memory_tools_common.cpp @@ -25,7 +25,7 @@ #endif // defined(__APPLE__) #include "./memory_tools.hpp" -#include "./scope_exit.hpp" +#include "../scope_exit.hpp" static std::atomic enabled(false); @@ -65,9 +65,11 @@ custom_malloc(size_t size) } void * memory = malloc(size); uint64_t fw_size = size; - MALLOC_PRINTF( - " malloc (%s) %p %" PRIu64 "\n", - malloc_expected ? " expected" : "not expected", memory, fw_size); + if (!malloc_expected) { + MALLOC_PRINTF( + " malloc (%s) %p %" PRIu64 "\n", + malloc_expected ? " expected" : "not expected", memory, fw_size); + } return memory; } @@ -107,9 +109,11 @@ custom_realloc(void * memory_in, size_t size) } void * memory = realloc(memory_in, size); uint64_t fw_size = size; - MALLOC_PRINTF( - "realloc (%s) %p %p %" PRIu64 "\n", - malloc_expected ? " expected" : "not expected", memory_in, memory, fw_size); + if (!realloc_expected) { + MALLOC_PRINTF( + "realloc (%s) %p %p %" PRIu64 "\n", + realloc_expected ? " expected" : "not expected", memory_in, memory, fw_size); + } return memory; } @@ -147,8 +151,10 @@ custom_free(void * memory) (*unexpected_free_callback)(); } } - MALLOC_PRINTF( - " free (%s) %p\n", malloc_expected ? " expected" : "not expected", memory); + if (!free_expected) { + MALLOC_PRINTF( + " free (%s) %p\n", free_expected ? " expected" : "not expected", memory); + } free(memory); } diff --git a/rcl/test/memory_tools_osx_interpose.cpp b/rcl/test/memory_tools/memory_tools_osx_interpose.cpp similarity index 89% rename from rcl/test/memory_tools_osx_interpose.cpp rename to rcl/test/memory_tools/memory_tools_osx_interpose.cpp index 91579cf..d9397df 100644 --- a/rcl/test/memory_tools_osx_interpose.cpp +++ b/rcl/test/memory_tools/memory_tools_osx_interpose.cpp @@ -40,14 +40,16 @@ typedef struct interpose_s 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); + if (!enabled.exchange(true)) { + MALLOC_PRINTF("starting memory checking...\n"); + } } void osx_stop_memory_checking() { - MALLOC_PRINTF("stopping memory checking...\n"); - enabled.store(false); + if (enabled.exchange(false)) { + MALLOC_PRINTF("stopping memory checking...\n"); + } } OSX_INTERPOSE(custom_malloc, malloc); diff --git a/rcl/test/test_memory_tools.cpp b/rcl/test/memory_tools/test_memory_tools.cpp similarity index 100% rename from rcl/test/test_memory_tools.cpp rename to rcl/test/memory_tools/test_memory_tools.cpp diff --git a/rcl/test/rcl/test_allocator.cpp b/rcl/test/rcl/test_allocator.cpp index dfe41e6..d32fbf6 100644 --- a/rcl/test/rcl/test_allocator.cpp +++ b/rcl/test/rcl/test_allocator.cpp @@ -16,7 +16,7 @@ #include "rcl/allocator.h" -#include "../memory_tools.hpp" +#include "../memory_tools/memory_tools.hpp" class TestAllocatorFixture : public ::testing::Test { diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp index c8befcd..7632ece 100644 --- a/rcl/test/rcl/test_node.cpp +++ b/rcl/test/rcl/test_node.cpp @@ -20,7 +20,7 @@ #include "rcl/node.h" #include "rmw/rmw.h" // For rmw_get_implementation_identifier. -#include "../memory_tools.hpp" +#include "../memory_tools/memory_tools.hpp" #include "../scope_exit.hpp" #include "rcl/error_handling.h" @@ -47,30 +47,6 @@ public: } }; -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; -} - bool is_opensplice = std::string(rmw_get_implementation_identifier()).find("opensplice") != std::string::npos; #if defined(WIN32) diff --git a/rcl/test/rcl/test_publisher.cpp b/rcl/test/rcl/test_publisher.cpp new file mode 100644 index 0000000..412452f --- /dev/null +++ b/rcl/test/rcl/test_publisher.cpp @@ -0,0 +1,207 @@ +// 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/publisher.h" + +#include "rcl/rcl.h" +#include "std_msgs/msg/int64.h" +#include "std_msgs/msg/string.h" +#include "rosidl_generator_c/string_functions.h" + +#include "../memory_tools/memory_tools.hpp" +#include "../scope_exit.hpp" +#include "rcl/error_handling.h" + +class TestPublisherFixture : public ::testing::Test +{ +public: + rcl_node_t * node_ptr; + void SetUp() + { + stop_memory_checking(); + rcl_ret_t ret; + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "node_name"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + 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); + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_shutdown(); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } +}; + +/* Basic nominal test of a publisher. + */ +TEST_F(TestPublisherFixture, test_publisher_nominal) { + stop_memory_checking(); + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(std_msgs, msg, Int64); + // TODO(wjwwood): Change this back to just chatter when this OpenSplice problem is resolved: + // ======================================================================================== + // Report : WARNING + // Date : Wed Feb 10 18:17:03 PST 2016 + // Description : Create Topic "chatter" failed: typename + // differs exiting definition . + // Node : farl + // Process : test_subscription__rmw_opensplice_cpp <23524> + // Thread : main thread 7fff7342d000 + // Internals : V6.4.140407OSS///v_topicNew/v_topic.c/448/21/1455157023.781423000 + const char * topic_name = "chatter_int64"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + auto publisher_exit = make_scope_exit([&publisher, this]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + }); + std_msgs__msg__Int64 msg; + std_msgs__msg__Int64__init(&msg); + msg.data = 42; + ret = rcl_publish(&publisher, &msg); + std_msgs__msg__Int64__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); +} + +/* Basic nominal test of a publisher with a string. + */ +TEST_F(TestPublisherFixture, test_publisher_nominal_string) { + stop_memory_checking(); + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(std_msgs, msg, String); + const char * topic_name = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + auto publisher_exit = make_scope_exit([&publisher, this]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + }); + std_msgs__msg__String msg; + std_msgs__msg__String__init(&msg); + ASSERT_TRUE(rosidl_generator_c__String__assign(&msg.data, "testing")); + ret = rcl_publish(&publisher, &msg); + // TODO(wjwwood): re-enable this fini when ownership of the string is resolved. + // currently with Connext we will spuriously get a double free here. + // std_msgs__msg__String__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); +} + +/* Testing the publisher init and fini functions. + */ +TEST_F(TestPublisherFixture, test_publisher_init_fini) { + stop_memory_checking(); + rcl_ret_t ret; + // Setup valid inputs. + rcl_publisher_t publisher; + const rosidl_message_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(std_msgs, msg, Int64); + const char * topic_name = "chatter"; + rcl_publisher_options_t default_publisher_options = rcl_publisher_get_default_options(); + + // Try passing null for publisher in init. + ret = rcl_publisher_init(nullptr, this->node_ptr, ts, topic_name, &default_publisher_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing null for a node pointer in init. + publisher = rcl_get_zero_initialized_publisher(); + ret = rcl_publisher_init(&publisher, nullptr, ts, topic_name, &default_publisher_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing an invalid (uninitialized) node in init. + publisher = rcl_get_zero_initialized_publisher(); + rcl_node_t invalid_node = rcl_get_zero_initialized_node(); + ret = rcl_publisher_init(&publisher, &invalid_node, ts, topic_name, &default_publisher_options); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing null for the type support in init. + publisher = rcl_get_zero_initialized_publisher(); + ret = rcl_publisher_init( + &publisher, this->node_ptr, nullptr, topic_name, &default_publisher_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing null for the topic name in init. + publisher = rcl_get_zero_initialized_publisher(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, nullptr, &default_publisher_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing null for the options in init. + publisher = rcl_get_zero_initialized_publisher(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing options with an invalid allocate in allocator with init. + publisher = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t publisher_options_with_invalid_allocator; + publisher_options_with_invalid_allocator = rcl_publisher_get_default_options(); + publisher_options_with_invalid_allocator.allocator.allocate = nullptr; + ret = rcl_publisher_init( + &publisher, this->node_ptr, ts, topic_name, &publisher_options_with_invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // Try passing options with an invalid deallocate in allocator with init. + publisher = rcl_get_zero_initialized_publisher(); + publisher_options_with_invalid_allocator = rcl_publisher_get_default_options(); + publisher_options_with_invalid_allocator.allocator.deallocate = nullptr; + ret = rcl_publisher_init( + &publisher, this->node_ptr, ts, topic_name, &publisher_options_with_invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); + + // An allocator with an invalid realloc will probably work (so we will not test it). + + // Try passing options with a failing allocator with init. + publisher = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t publisher_options_with_failing_allocator; + publisher_options_with_failing_allocator = rcl_publisher_get_default_options(); + publisher_options_with_failing_allocator.allocator.allocate = failing_malloc; + publisher_options_with_failing_allocator.allocator.deallocate = failing_free; + publisher_options_with_failing_allocator.allocator.reallocate = failing_realloc; + ret = rcl_publisher_init( + &publisher, this->node_ptr, ts, topic_name, &publisher_options_with_failing_allocator); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string_safe(); + rcl_reset_error(); +} diff --git a/rcl/test/rcl/test_rcl.cpp b/rcl/test/rcl/test_rcl.cpp index c88f601..e0c4c7b 100644 --- a/rcl/test/rcl/test_rcl.cpp +++ b/rcl/test/rcl/test_rcl.cpp @@ -16,7 +16,7 @@ #include "rcl/rcl.h" -#include "../memory_tools.hpp" +#include "../memory_tools/memory_tools.hpp" #include "rcl/error_handling.h" class TestRCLFixture : public ::testing::Test @@ -42,30 +42,6 @@ public: } }; -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; -} - struct FakeTestArgv { FakeTestArgv() @@ -97,6 +73,9 @@ struct FakeTestArgv int argc; char ** argv; + +private: + FakeTestArgv(const FakeTestArgv &) = delete; }; /* Tests the rcl_init(), rcl_ok(), and rcl_shutdown() functions. diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp new file mode 100644 index 0000000..9c70cdd --- /dev/null +++ b/rcl/test/rcl/test_subscription.cpp @@ -0,0 +1,229 @@ +// 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 +#include + +#include "rcl/subscription.h" + +#include "rcl/rcl.h" +#include "std_msgs/msg/int64.h" +#include "std_msgs/msg/string.h" +#include "rosidl_generator_c/string_functions.h" + +#include "../memory_tools/memory_tools.hpp" +#include "../scope_exit.hpp" +#include "rcl/error_handling.h" + +class TestSubscriptionFixture : public ::testing::Test +{ +public: + rcl_node_t * node_ptr; + void SetUp() + { + stop_memory_checking(); + rcl_ret_t ret; + ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "node_name"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + 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); + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_shutdown(); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } +}; + +void +wait_for_subscription_to_be_ready( + rcl_subscription_t * subscription, + size_t max_tries, + int64_t period_ms, + bool & success) +{ + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = rcl_wait_set_init(&wait_set, 1, 0, 0, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + auto wait_set_exit = make_scope_exit([&wait_set]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + }); + size_t iteration = 0; + do { + ++iteration; + ret = rcl_wait_set_clear_subscriptions(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_wait_set_add_subscription(&wait_set, subscription); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); + if (ret == RCL_RET_TIMEOUT) { + continue; + } + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) { + if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) { + success = true; + return; + } + } + } while (iteration < max_tries); + success = false; +} + +/* Basic nominal test of a subscription. + */ +TEST_F(TestSubscriptionFixture, test_subscription_nominal) { + stop_memory_checking(); + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(std_msgs, msg, Int64); + // TODO(wjwwood): Change this back to just chatter when this OpenSplice problem is resolved: + // ======================================================================================== + // Report : WARNING + // Date : Wed Feb 10 18:17:03 PST 2016 + // Description : Create Topic "chatter" failed: typename + // differs exiting definition . + // Node : farl + // Process : test_subscription__rmw_opensplice_cpp <23524> + // Thread : main thread 7fff7342d000 + // Internals : V6.4.140407OSS///v_topicNew/v_topic.c/448/21/1455157023.781423000 + const char * topic = "chatter_int64"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + auto publisher_exit = make_scope_exit([&publisher, this]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + }); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + auto subscription_exit = make_scope_exit([&subscription, this]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + }); + // TODO(wjwwood): add logic to wait for the connection to be established + // probably using the count_subscriptions busy wait mechanism + // until then we will sleep for a short period of time + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + { + std_msgs__msg__Int64 msg; + std_msgs__msg__Int64__init(&msg); + msg.data = 42; + ret = rcl_publish(&publisher, &msg); + std_msgs__msg__Int64__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } + bool success; + wait_for_subscription_to_be_ready(&subscription, 10, 100, success); + ASSERT_TRUE(success); + { + std_msgs__msg__Int64 msg; + std_msgs__msg__Int64__init(&msg); + auto msg_exit = make_scope_exit([&msg]() { + stop_memory_checking(); + std_msgs__msg__Int64__fini(&msg); + }); + bool taken = false; + ret = rcl_take(&subscription, &msg, &taken, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ASSERT_TRUE(taken) << "failed to take a message, even though the subscription was ready"; + ASSERT_EQ(42, msg.data); + } +} + +/* Basic nominal test of a publisher with a string. + */ +TEST_F(TestSubscriptionFixture, test_subscription_nominal_string) { + stop_memory_checking(); + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(std_msgs, msg, String); + const char * topic = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + auto publisher_exit = make_scope_exit([&publisher, this]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + }); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + auto subscription_exit = make_scope_exit([&subscription, this]() { + stop_memory_checking(); + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + }); + // TODO(wjwwood): add logic to wait for the connection to be established + // probably using the count_subscriptions busy wait mechanism + // until then we will sleep for a short period of time + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + const char * test_string = "testing"; + { + std_msgs__msg__String msg; + std_msgs__msg__String__init(&msg); + ASSERT_TRUE(rosidl_generator_c__String__assign(&msg.data, test_string)); + ret = rcl_publish(&publisher, &msg); + // TODO(wjwwood): re-enable this fini when ownership of the string is resolved. + // currently with Connext we will spuriously get a double free here. + // std_msgs__msg__String__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } + bool success; + wait_for_subscription_to_be_ready(&subscription, 10, 100, success); + ASSERT_TRUE(success); + { + std_msgs__msg__String msg; + std_msgs__msg__String__init(&msg); + auto msg_exit = make_scope_exit([&msg]() { + stop_memory_checking(); + std_msgs__msg__String__fini(&msg); + }); + bool taken = false; + ret = rcl_take(&subscription, &msg, &taken, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ASSERT_TRUE(taken) << "failed to take a message, even though the subscription was ready"; + ASSERT_EQ(std::string(test_string), std::string(msg.data.data, msg.data.size)); + } +} diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index 1fda410..18c5a61 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -22,7 +22,7 @@ #include "rcl/error_handling.h" #include "rcl/time.h" -#include "../memory_tools.hpp" +#include "../memory_tools/memory_tools.hpp" class TestTimeFixture : public ::testing::Test { diff --git a/rcl/test/rcl_add_custom_gtest.cmake b/rcl/test/rcl_add_custom_gtest.cmake new file mode 100644 index 0000000..895aad3 --- /dev/null +++ b/rcl/test/rcl_add_custom_gtest.cmake @@ -0,0 +1,100 @@ +# Copyright 2016 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(rcl_add_custom_gtest_INCLUDED) + return() +endif() +set(rcl_add_custom_gtest_INCLUDED TRUE) + +# include CMake functions +include(CMakeParseArguments) + +# +# Custom macro for adding a gtest in rcl. +# +# It also takes some of the arguments of ament_add_gtest as well as +# INCLUDE_DIRS, LIBRARIES, and AMENT_DEPENDENCIES which are passed to +# target_include_directories(), target_link_libraries(), and +# ament_target_dependencies() respectively. +# +# :param target: the target name which will also be used as the test name +# :type target: string +# :param SRCS: list of source files used to create the gtest +# :type SRCS: list of strings +# :param ENV: list of env vars to set; listed as ``VAR=value`` +# :type ENV: list of strings +# :param APPEND_ENV: list of env vars to append if already set, otherwise set; +# listed as ``VAR=value`` +# :type APPEND_ENV: list of strings +# :param APPEND_LIBRARY_DIRS: list of library dirs to append to the appropriate +# OS specific env var, a la LD_LIBRARY_PATH +# :type APPEND_LIBRARY_DIRS: list of strings +# :param INCLUDE_DIRS: list of include directories to add to the target +# :type INCLUDE_DIRS: list of strings +# :param LIBRARIES: list of libraries to link to the target +# :type LIBRARIES: list of strings +# :param AMENT_DEPENDENCIES: list of depends to pass ament_target_dependencies +# :type AMENT_DEPENDENCIES: list of strings +# +# @public +# +macro(rcl_add_custom_gtest target) + cmake_parse_arguments(_ARG + "TRACE" + "" + "SRCS;ENV;APPEND_ENV;APPEND_LIBRARY_DIRS;INCLUDE_DIRS;LIBRARIES;AMENT_DEPENDENCIES" + ${ARGN}) + if(_ARG_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "rcl_add_custom_gtest() called with unused arguments: ${_ARG_UNPARSED_ARGUMENTS}") + endif() + if(_ARG_ENV) + set(_ARG_ENV "ENV" ${_ARG_ENV}) + endif() + if(_ARG_APPEND_ENV) + set(_ARG_APPEND_ENV "APPEND_ENV" ${_ARG_APPEND_ENV}) + endif() + if(_ARG_APPEND_LIBRARY_DIRS) + set(_ARG_APPEND_LIBRARY_DIRS "APPEND_LIBRARY_DIRS" ${_ARG_APPEND_LIBRARY_DIRS}) + endif() + + # Pass args along to ament_add_gtest(). + ament_add_gtest(${target} ${_ARG_SRCS} ${_ARG_ENV} ${_ARG_APPEND_ENV} ${_ARG_APPEND_LIBRARY_DIRS}) + # Check if the target was actually created. + if(TARGET ${target}) + if(_ARG_TRACE) + message(STATUS "rcl_add_custom_gtest() Target '${target}':") + endif() + # Add extra include directories, if any. + if(_ARG_INCLUDE_DIRS) + if(_ARG_TRACE) + message(STATUS " rcl_add_custom_gtest() INCLUDE_DIRS: ${_ARG_INCLUDE_DIRS}") + endif() + target_include_directories(${target} PUBLIC ${_ARG_INCLUDE_DIRS}) + endif() + # Add extra link libraries, if any. + if(_ARG_LIBRARIES) + if(_ARG_TRACE) + message(STATUS " rcl_add_custom_gtest() LIBRARIES: ${_ARG_LIBRARIES}") + endif() + target_link_libraries(${target} ${_ARG_LIBRARIES}) + endif() + # Add extra ament dependencies, if any. + if(_ARG_AMENT_DEPENDENCIES) + if(_ARG_TRACE) + message(STATUS " rcl_add_custom_gtest() AMENT_DEPENDENCIES: ${_ARG_AMENT_DEPENDENCIES}") + endif() + ament_target_dependencies(${target} ${_ARG_AMENT_DEPENDENCIES}) + endif() + endif() +endmacro()