Skip to content

Port message_filters C++ part #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
gaoethan wants to merge 2 commits into from
Closed

Port message_filters C++ part #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a283ff8
Port message_filters C++ part:
gaoethan Jul 12, 2018
3289857
port C++ tests of message_filters
gaoethan Jul 12, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
137 changes: 78 additions & 59 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,90 +1,109 @@
cmake_minimum_required(VERSION 2.8.3)
cmake_minimum_required(VERSION 3.5)

project(message_filters)

if(NOT WIN32)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Wextra")
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(catkin REQUIRED COMPONENTS roscpp rosconsole)
catkin_package(
INCLUDE_DIRS include
LIBRARIES message_filters
CATKIN_DEPENDS roscpp rosconsole
find_package(ament_cmake_python REQUIRED)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rcutils)
find_package(builtin_interfaces REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)

include_directories(include)
add_library(${PROJECT_NAME} SHARED src/connection.cpp)
ament_target_dependencies(${PROJECT_NAME}
"rclcpp"
"rcutils"
"std_msgs"
"builtin_interfaces"
"sensor_msgs")

install(
TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
catkin_python_setup()

find_package(Boost REQUIRED COMPONENTS signals thread)
install(
DIRECTORY "include/"
DESTINATION include
)

include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
link_directories(${catkin_LIBRARY_DIRS})
find_package(python_cmake_module REQUIRED)
_ament_cmake_python_register_environment_hook()
install(
DIRECTORY "src/${PROJECT_NAME}/"
DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
PATTERN "*.pyc" EXCLUDE
PATTERN "__pycache__" EXCLUDE
)

add_library(${PROJECT_NAME} src/connection.cpp)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})

install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()

install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h")
find_package(ament_cmake_gtest)

if(CATKIN_ENABLE_TESTING)
# Ugly workaround for check_test_ran macro issue
#add_subdirectory(test)
find_package(catkin COMPONENTS rostest rosunit)
ament_add_gtest(${PROJECT_NAME}-test_simple test/test_simple.cpp)
if(TARGET ${PROJECT_NAME}-test_simple)
target_link_libraries(${PROJECT_NAME}-test_simple ${PROJECT_NAME})
endif()

include_directories(${GTEST_INCLUDE_DIRS})

# ********** Tests **********
catkin_add_gtest(${PROJECT_NAME}-msg_cache_unittest test/msg_cache_unittest.cpp)
ament_add_gtest(${PROJECT_NAME}-msg_cache_unittest test/msg_cache_unittest.cpp)
if(TARGET ${PROJECT_NAME}-msg_cache_unittest)
target_link_libraries(${PROJECT_NAME}-msg_cache_unittest message_filters ${GTEST_LIBRARIES})
endif()

catkin_add_gtest(${PROJECT_NAME}-time_synchronizer_unittest test/time_synchronizer_unittest.cpp)
if(TARGET ${PROJECT_NAME}-time_synchronizer_unittest)
target_link_libraries(${PROJECT_NAME}-time_synchronizer_unittest message_filters ${GTEST_LIBRARIES})
target_link_libraries(${PROJECT_NAME}-msg_cache_unittest ${PROJECT_NAME})
endif()

catkin_add_gtest(${PROJECT_NAME}-test_synchronizer test/test_synchronizer.cpp)
if(TARGET ${PROJECT_NAME}-test_synchronizer)
target_link_libraries(${PROJECT_NAME}-test_synchronizer message_filters ${GTEST_LIBRARIES})
ament_add_gtest(${PROJECT_NAME}-test_chain test/test_chain.cpp)
if(TARGET ${PROJECT_NAME}-test_chain)
target_link_libraries(${PROJECT_NAME}-test_chain ${PROJECT_NAME})
endif()

catkin_add_gtest(${PROJECT_NAME}-test_exact_time_policy test/test_exact_time_policy.cpp)
if(TARGET ${PROJECT_NAME}-test_exact_time_policy)
target_link_libraries(${PROJECT_NAME}-test_exact_time_policy message_filters ${GTEST_LIBRARIES})
ament_add_gtest(${PROJECT_NAME}-test_subscriber test/test_subscriber.cpp)
if(TARGET ${PROJECT_NAME}-test_subscriber)
target_link_libraries(${PROJECT_NAME}-test_subscriber ${PROJECT_NAME})
endif()

catkin_add_gtest(${PROJECT_NAME}-test_approximate_time_policy test/test_approximate_time_policy.cpp)
if(TARGET ${PROJECT_NAME}-test_approximate_time_policy)
target_link_libraries(${PROJECT_NAME}-test_approximate_time_policy message_filters ${GTEST_LIBRARIES})
ament_add_gtest(${PROJECT_NAME}-test_synchronizer test/test_synchronizer.cpp)
if(TARGET ${PROJECT_NAME}-test_synchronizer)
target_link_libraries(${PROJECT_NAME}-test_synchronizer ${PROJECT_NAME})
endif()

catkin_add_gtest(${PROJECT_NAME}-test_simple test/test_simple.cpp)
if(TARGET ${PROJECT_NAME}-test_simple)
target_link_libraries(${PROJECT_NAME}-test_simple message_filters ${GTEST_LIBRARIES})
ament_add_gtest(${PROJECT_NAME}-time_synchronizer_unittest test/time_synchronizer_unittest.cpp)
if(TARGET ${PROJECT_NAME}-time_synchronizer_unittest)
target_link_libraries(${PROJECT_NAME}-time_synchronizer_unittest ${PROJECT_NAME})
endif()

catkin_add_gtest(${PROJECT_NAME}-test_chain test/test_chain.cpp)
if(TARGET ${PROJECT_NAME}-test_chain)
target_link_libraries(${PROJECT_NAME}-test_chain message_filters ${GTEST_LIBRARIES})
ament_add_gtest(${PROJECT_NAME}-test_exact_time_policy test/test_exact_time_policy.cpp)
if(TARGET ${PROJECT_NAME}-test_exact_time_policy)
target_link_libraries(${PROJECT_NAME}-test_exact_time_policy ${PROJECT_NAME})
endif()

# Needs to be a rostest because it spins up a node, which blocks until it hears from the master (unfortunately)
add_rostest_gtest(${PROJECT_NAME}-time_sequencer_unittest test/time_sequencer_unittest.xml test/time_sequencer_unittest.cpp)
if(TARGET ${PROJECT_NAME}-time_sequencer_unittest)
target_link_libraries(${PROJECT_NAME}-time_sequencer_unittest message_filters)
ament_add_gtest(${PROJECT_NAME}-test_approximate_time_policy test/test_approximate_time_policy.cpp)
if(TARGET ${PROJECT_NAME}-test_approximate_time_policy)
target_link_libraries(${PROJECT_NAME}-test_approximate_time_policy ${PROJECT_NAME})
endif()

add_rostest_gtest(${PROJECT_NAME}-test_subscriber test/test_subscriber.xml test/test_subscriber.cpp)
if(TARGET ${PROJECT_NAME}-test_subscriber)
target_link_libraries(${PROJECT_NAME}-test_subscriber message_filters)
ament_add_gtest(${PROJECT_NAME}-test_fuzz test/test_fuzz.cpp SKIP_TEST)
if(TARGET ${PROJECT_NAME}-test_fuzz)
target_link_libraries(${PROJECT_NAME}-test_fuzz ${PROJECT_NAME})
endif()

# Unit test of the approximate synchronizer
catkin_add_nosetests(test/test_approxsync.py)
catkin_add_nosetests(test/test_message_filters_cache.py)
endif()

ament_package()
66 changes: 33 additions & 33 deletions include/message_filters/cache.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,17 +36,18 @@
#define MESSAGE_FILTERS_CACHE_H_

#include <deque>
#include "boost/thread.hpp"
#include "boost/shared_ptr.hpp"
#include <memory>
#include <functional>

#include "ros/time.h"
#include <rclcpp/rclcpp.hpp>

#include "connection.h"
#include "simple_filter.h"
#include "message_traits.h"

namespace message_filters
{

using namespace std::placeholders;
/**
* \brief Stores a time history of messages
*
Expand All @@ -59,15 +60,15 @@ namespace message_filters
*
* Cache's input and output connections are both of the same signature as roscpp subscription callbacks, ie.
\verbatim
void callback(const boost::shared_ptr<M const>&);
void callback(const std::shared_ptr<M const>&);
\endverbatim
*/
template<class M>
class Cache : public SimpleFilter<M>
{
public:
typedef boost::shared_ptr<M const> MConstPtr;
typedef ros::MessageEvent<M const> EventType;
typedef std::shared_ptr<M const> MConstPtr;
typedef MessageEvent<M const> EventType;

template<class F>
Cache(F& f, unsigned int cache_size = 1)
Expand All @@ -89,7 +90,7 @@ class Cache : public SimpleFilter<M>
template<class F>
void connectInput(F& f)
{
incoming_connection_ = f.registerCallback(typename SimpleFilter<M>::EventCallback(boost::bind(&Cache::callback, this, _1)));
incoming_connection_ = f.registerCallback(typename SimpleFilter<M>::EventCallback(std::bind(&Cache::callback, this, _1)));
}

~Cache()
Expand Down Expand Up @@ -127,11 +128,11 @@ class Cache : public SimpleFilter<M>
*/
void add(const EventType& evt)
{
namespace mt = ros::message_traits;
namespace mt = message_filters::message_traits;

//printf(" Cache Size: %u\n", cache_.size()) ;
{
boost::mutex::scoped_lock lock(cache_lock_);
std::lock_guard<std::mutex> lock(cache_lock_);

while (cache_.size() >= cache_size_) // Keep popping off old data until we have space for a new msg
cache_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
Expand All @@ -143,7 +144,7 @@ class Cache : public SimpleFilter<M>

// Keep walking backwards along deque until we hit the beginning,
// or until we find a timestamp that's smaller than (or equal to) msg's timestamp
ros::Time evt_stamp = mt::TimeStamp<M>::value(*evt.getMessage());
rclcpp::Time evt_stamp = mt::TimeStamp<M>::value(*evt.getMessage());
while(rev_it != cache_.rend() && mt::TimeStamp<M>::value(*(*rev_it).getMessage()) > evt_stamp)
rev_it++;

Expand All @@ -163,11 +164,10 @@ class Cache : public SimpleFilter<M>
* \param start The start of the requested interval
* \param end The end of the requested interval
*/
std::vector<MConstPtr> getInterval(const ros::Time& start, const ros::Time& end) const
std::vector<MConstPtr> getInterval(const rclcpp::Time& start, const rclcpp::Time& end) const
{
namespace mt = ros::message_traits;

boost::mutex::scoped_lock lock(cache_lock_);
namespace mt = message_filters::message_traits;
std::lock_guard<std::mutex> lock(cache_lock_);

// Find the starting index. (Find the first index after [or at] the start of the interval)
unsigned int start_index = 0 ;
Expand Down Expand Up @@ -202,11 +202,11 @@ class Cache : public SimpleFilter<M>
* If the messages in the cache do not surround (start,end), then this will return the interval
* that gets closest to surrounding (start,end)
*/
std::vector<MConstPtr> getSurroundingInterval(const ros::Time& start, const ros::Time& end) const
std::vector<MConstPtr> getSurroundingInterval(const rclcpp::Time& start, const rclcpp::Time& end) const
{
namespace mt = ros::message_traits;
namespace mt = message_filters::message_traits;

boost::mutex::scoped_lock lock(cache_lock_);
std::lock_guard<std::mutex> lock(cache_lock_);
// Find the starting index. (Find the first index after [or at] the start of the interval)
unsigned int start_index = cache_.size()-1;
while(start_index > 0 &&
Expand Down Expand Up @@ -236,11 +236,11 @@ class Cache : public SimpleFilter<M>
* \param time Time that must occur right after the returned elem
* \returns shared_ptr to the newest elem that occurs before 'time'. NULL if doesn't exist
*/
MConstPtr getElemBeforeTime(const ros::Time& time) const
MConstPtr getElemBeforeTime(const rclcpp::Time& time) const
{
namespace mt = ros::message_traits;
namespace mt = message_filters::message_traits;

boost::mutex::scoped_lock lock(cache_lock_);
std::lock_guard<std::mutex> lock(cache_lock_);

MConstPtr out ;

Expand All @@ -264,11 +264,11 @@ class Cache : public SimpleFilter<M>
* \param time Time that must occur right before the returned elem
* \returns shared_ptr to the oldest elem that occurs after 'time'. NULL if doesn't exist
*/
MConstPtr getElemAfterTime(const ros::Time& time) const
MConstPtr getElemAfterTime(const rclcpp::Time& time) const
{
namespace mt = ros::message_traits;
namespace mt = message_filters::message_traits;

boost::mutex::scoped_lock lock(cache_lock_);
std::lock_guard<std::mutex> lock(cache_lock_);

MConstPtr out ;

Expand All @@ -292,13 +292,13 @@ class Cache : public SimpleFilter<M>
/**
* \brief Returns the timestamp associated with the newest packet cache
*/
ros::Time getLatestTime() const
rclcpp::Time getLatestTime() const
{
namespace mt = ros::message_traits;
namespace mt = message_filters::message_traits;

boost::mutex::scoped_lock lock(cache_lock_);
std::lock_guard<std::mutex> lock(cache_lock_);

ros::Time latest_time;
rclcpp::Time latest_time;

if (cache_.size() > 0)
latest_time = mt::TimeStamp<M>::value(*cache_.back().getMessage());
Expand All @@ -309,13 +309,13 @@ class Cache : public SimpleFilter<M>
/**
* \brief Returns the timestamp associated with the oldest packet cache
*/
ros::Time getOldestTime() const
rclcpp::Time getOldestTime() const
{
namespace mt = ros::message_traits;
namespace mt = message_filters::message_traits;

boost::mutex::scoped_lock lock(cache_lock_);
std::lock_guard<std::mutex> lock(cache_lock_);

ros::Time oldest_time;
rclcpp::Time oldest_time;

if (cache_.size() > 0)
oldest_time = mt::TimeStamp<M>::value(*cache_.front().getMessage());
Expand All @@ -330,7 +330,7 @@ class Cache : public SimpleFilter<M>
add(evt);
}

mutable boost::mutex cache_lock_ ; //!< Lock for cache_
mutable std::mutex cache_lock_ ; //!< Lock for cache_
std::deque<EventType> cache_ ; //!< Cache for the messages
unsigned int cache_size_ ; //!< Maximum number of elements allowed in the cache.

Expand Down
Loading