Skip to content

Commit

Permalink
fuse -> ROS 2 fuse_core : Nodes and Waitables (#284)
Browse files Browse the repository at this point in the history 
Co-authored-by: Brett Downing <[email protected]>
Co-authored-by: Shane Loretz <[email protected]>
  • Loading branch information
@methylDragon @BrettRD @sloretz
3 people authored Nov 12, 2022
1 parent f27a0b4 commit 9f9395e
Show file tree
Hide file tree
Showing 16 changed files with 467 additions and 417 deletions.
326 changes: 68 additions & 258 deletions fuse_core/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,21 +29,22 @@ include(boost-extras.cmake)

## fuse_core library
add_library(${PROJECT_NAME} SHARED
# src/async_motion_model.cpp
# src/async_publisher.cpp
# src/async_sensor_model.cpp
# src/ceres_options.cpp
src/constraint.cpp
src/graph.cpp
src/graph_deserializer.cpp
src/loss.cpp
src/serialization.cpp
src/time.cpp
src/timestamp_manager.cpp
src/transaction.cpp
src/transaction_deserializer.cpp
src/uuid.cpp
src/variable.cpp
src/async_motion_model.cpp
src/async_publisher.cpp
src/async_sensor_model.cpp
src/callback_wrapper.cpp
# src/ceres_options.cpp
src/constraint.cpp
src/graph.cpp
src/graph_deserializer.cpp
src/loss.cpp
src/serialization.cpp
src/time.cpp
src/timestamp_manager.cpp
src/transaction.cpp
src/transaction_deserializer.cpp
src/uuid.cpp
src/variable.cpp
)
target_include_directories(${PROJECT_NAME} PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
Expand Down Expand Up @@ -81,8 +82,58 @@ if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()

# find_package(ament_cmake_gtest REQUIRED)
# add_subdirectory(test)
ament_add_gtest(test_constraint
test/test_constraint.cpp)
target_link_libraries(test_constraint ${PROJECT_NAME})
target_include_directories(test_constraint PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})

ament_add_gtest(test_eigen
test/test_eigen.cpp)
target_link_libraries(test_eigen ${PROJECT_NAME})
target_include_directories(test_eigen PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})

# TODO(sloretz) fails to build: can't include ros/node_handle.h
# ament_add_gtest(test_local_parameterization
# test/test_local_parameterization.cpp)
# target_link_libraries(test_local_parameterization ${PROJECT_NAME})
# target_include_directories(test_local_parameterization PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})

ament_add_gtest(test_loss
test/test_loss.cpp)
target_link_libraries(test_loss ${PROJECT_NAME})
target_include_directories(test_loss PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})

# TODO(sloretz) fails to build: invalid use of rclcpp::Time::get_clock_type()
# ament_add_gtest(test_message_buffer
# test/test_message_buffer.cpp)
# target_link_libraries(test_message_buffer ${PROJECT_NAME})
# target_include_directories(test_message_buffer PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})

ament_add_gtest(test_timestamp_manager
test/test_timestamp_manager.cpp)
target_link_libraries(test_timestamp_manager ${PROJECT_NAME})
target_include_directories(test_timestamp_manager PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})

ament_add_gtest(test_transaction
test/test_transaction.cpp)
target_link_libraries(test_transaction ${PROJECT_NAME})
target_include_directories(test_transaction PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})

# TODO(sloretz) fails to build: can't include ros/node_handle.h
# ament_add_gtest(test_util
# test/test_util.cpp)
# target_link_libraries(test_util ${PROJECT_NAME})
# target_include_directories(test_util PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})

ament_add_gtest(test_uuid
test/test_uuid.cpp)
target_link_libraries(test_uuid ${PROJECT_NAME})
target_include_directories(test_uuid PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})

ament_add_gtest(test_variable
test/test_variable.cpp)
target_link_libraries(test_variable ${PROJECT_NAME})
target_include_directories(test_variable PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
endif()

# TODO(CH3): Move this to the test directory and port to ROS 2
Expand Down Expand Up @@ -198,173 +249,6 @@ endif()
# CXX_STANDARD_REQUIRED YES
# )

# # Constraint tests
# catkin_add_gtest(test_constraint
# test/test_constraint.cpp
# )
# add_dependencies(test_constraint
# ${catkin_EXPORTED_TARGETS}
# )
# target_include_directories(test_constraint
# PRIVATE
# include
# ${Boost_INCLUDE_DIRS}
# ${catkin_INCLUDE_DIRS}
# ${CERES_INCLUDE_DIRS}
# ${CMAKE_CURRENT_SOURCE_DIR}
# ${EIGEN3_INCLUDE_DIRS}
# )
# target_link_libraries(test_constraint
# ${PROJECT_NAME}
# )
# set_target_properties(test_constraint
# PROPERTIES
# CXX_STANDARD 14
# CXX_STANDARD_REQUIRED YES
# )

# # Eigen tests
# catkin_add_gtest(test_eigen
# test/test_eigen.cpp
# )
# add_dependencies(test_eigen
# ${catkin_EXPORTED_TARGETS}
# )
# target_include_directories(test_eigen
# PRIVATE
# include
# ${Boost_INCLUDE_DIRS}
# ${catkin_INCLUDE_DIRS}
# ${CERES_INCLUDE_DIRS}
# ${CMAKE_CURRENT_SOURCE_DIR}
# ${EIGEN3_INCLUDE_DIRS}
# )
# target_link_libraries(test_eigen
# ${PROJECT_NAME}
# )
# set_target_properties(test_eigen
# PROPERTIES
# CXX_STANDARD 14
# CXX_STANDARD_REQUIRED YES
# )

# # Local Parameterization tests
# catkin_add_gtest(test_local_parameterization
# test/test_local_parameterization.cpp
# )
# target_include_directories(test_local_parameterization
# PRIVATE
# include
# )
# target_link_libraries(test_local_parameterization
# ${PROJECT_NAME}
# )
# set_target_properties(test_local_parameterization
# PROPERTIES
# CXX_STANDARD 14
# CXX_STANDARD_REQUIRED YES
# )

# # Loss tests
# catkin_add_gtest(test_loss
# test/test_loss.cpp
# )
# add_dependencies(test_loss
# ${catkin_EXPORTED_TARGETS}
# )
# target_include_directories(test_loss
# PRIVATE
# include
# ${Boost_INCLUDE_DIRS}
# ${catkin_INCLUDE_DIRS}
# ${CERES_INCLUDE_DIRS}
# ${CMAKE_CURRENT_SOURCE_DIR}
# ${EIGEN3_INCLUDE_DIRS}
# )
# target_link_libraries(test_loss
# ${PROJECT_NAME}
# )
# set_target_properties(test_loss
# PROPERTIES
# CXX_STANDARD 14
# CXX_STANDARD_REQUIRED YES
# )

# # Message Buffer Tests
# catkin_add_gtest(test_message_buffer
# test/test_message_buffer.cpp
# )
# add_dependencies(test_message_buffer
# ${catkin_EXPORTED_TARGETS}
# )
# target_include_directories(test_message_buffer
# PRIVATE
# include
# ${Boost_INCLUDE_DIRS}
# ${catkin_INCLUDE_DIRS}
# ${CERES_INCLUDE_DIRS}
# ${EIGEN3_INCLUDE_DIRS}
# )
# target_link_libraries(test_message_buffer
# ${catkin_LIBRARIES}
# )
# set_target_properties(test_message_buffer
# PROPERTIES
# CXX_STANDARD 14
# CXX_STANDARD_REQUIRED YES
# )

# # Timestamp Manager Tests
# catkin_add_gtest(test_timestamp_manager
# test/test_timestamp_manager.cpp
# )
# add_dependencies(test_timestamp_manager
# ${catkin_EXPORTED_TARGETS}
# )
# target_include_directories(test_timestamp_manager
# PRIVATE
# include
# ${Boost_INCLUDE_DIRS}
# ${catkin_INCLUDE_DIRS}
# ${CERES_INCLUDE_DIRS}
# ${EIGEN3_INCLUDE_DIRS}
# )
# target_link_libraries(test_timestamp_manager
# ${PROJECT_NAME}
# ${catkin_LIBRARIES}
# )
# set_target_properties(test_timestamp_manager
# PROPERTIES
# CXX_STANDARD 14
# CXX_STANDARD_REQUIRED YES
# )

# # Transaction tests
# catkin_add_gtest(test_transaction
# test/test_transaction.cpp
# )
# add_dependencies(test_transaction
# ${catkin_EXPORTED_TARGETS}
# )
# target_include_directories(test_transaction
# PRIVATE
# include
# ${Boost_INCLUDE_DIRS}
# ${catkin_INCLUDE_DIRS}
# ${CERES_INCLUDE_DIRS}
# ${CMAKE_CURRENT_SOURCE_DIR}
# ${EIGEN3_INCLUDE_DIRS}
# )
# target_link_libraries(test_transaction
# ${PROJECT_NAME}
# ${catkin_LIBRARIES}
# )
# set_target_properties(test_transaction
# PROPERTIES
# CXX_STANDARD 14
# CXX_STANDARD_REQUIRED YES
# )

# # Parameter tests
# add_rostest_gtest(test_parameter
# test/parameter.test
Expand Down Expand Up @@ -405,80 +289,6 @@ endif()
# CXX_STANDARD 14
# CXX_STANDARD_REQUIRED YES
# )

# # Util tests
# catkin_add_gtest(test_util
# test/test_util.cpp
# )
# add_dependencies(test_util
# ${catkin_EXPORTED_TARGETS}
# )
# target_include_directories(test_util
# PRIVATE
# include
# ${Boost_INCLUDE_DIRS}
# ${catkin_INCLUDE_DIRS}
# ${CERES_INCLUDE_DIRS}
# ${EIGEN3_INCLUDE_DIRS}
# )
# target_link_libraries(test_util
# ${catkin_LIBRARIES}
# )
# set_target_properties(test_util
# PROPERTIES
# CXX_STANDARD 14
# CXX_STANDARD_REQUIRED YES
# )

# # UUID tests
# catkin_add_gtest(test_uuid
# test/test_uuid.cpp
# )
# add_dependencies(test_uuid
# ${catkin_EXPORTED_TARGETS}
# )
# target_include_directories(test_uuid
# PRIVATE
# include
# ${Boost_INCLUDE_DIRS}
# ${catkin_INCLUDE_DIRS}
# ${CERES_INCLUDE_DIRS}
# ${EIGEN3_INCLUDE_DIRS}
# )
# target_link_libraries(test_uuid
# ${PROJECT_NAME}
# ${catkin_LIBRARIES}
# )
# set_target_properties(test_uuid
# PROPERTIES
# CXX_STANDARD 14
# CXX_STANDARD_REQUIRED YES
# )

# # Variable tests
# catkin_add_gtest(test_variable
# test/test_variable.cpp
# )
# add_dependencies(test_variable
# ${catkin_EXPORTED_TARGETS}
# )
# target_include_directories(test_variable
# PRIVATE
# include
# ${Boost_INCLUDE_DIRS}
# ${catkin_INCLUDE_DIRS}
# ${CERES_INCLUDE_DIRS}
# ${CMAKE_CURRENT_SOURCE_DIR}
# ${EIGEN3_INCLUDE_DIRS}
# )
# target_link_libraries(test_variable
# ${PROJECT_NAME}
# )
# set_target_properties(test_variable
# PROPERTIES
# CXX_STANDARD 14
# CXX_STANDARD_REQUIRED YES
# )
# endif()

#############
Expand Down
22 changes: 10 additions & 12 deletions fuse_core/include/fuse_core/async_motion_model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,14 +34,13 @@
#ifndef FUSE_CORE_ASYNC_MOTION_MODEL_H
#define FUSE_CORE_ASYNC_MOTION_MODEL_H

#include <fuse_core/graph.h>
#include <fuse_core/fuse_macros.h>
#include <fuse_core/motion_model.h>

#include <fuse_core/transaction.h>
// #include <rclcpp/rclcpp.hpp> TODO(CH3): Uncomment when ready
#include <ros/callback_queue.h>
#include <ros/node_handle.h>
#include <ros/spinner.h>
#include <fuse_core/graph.h>
#include <fuse_core/callback_wrapper.h>

#include <rclcpp/rclcpp.hpp>

#include <string>

Expand Down Expand Up @@ -171,13 +170,12 @@ class AsyncMotionModel : public MotionModel
void stop() override;

protected:
ros::CallbackQueue callback_queue_; //!< The local callback queue used for all subscriptions
std::shared_ptr<fuse_core::CallbackAdapter> callback_queue_; //!< The callback queue used for fuse internal callbacks
std::string name_; //!< The unique name for this motion model instance
// rclcpp::Node::SharedPtr node_; //!< The node for this motion model (TODO(CH3): Uncomment when it's time)
ros::NodeHandle node_handle_; //!< A node handle in the global namespace using the local callback queue
ros::NodeHandle private_node_handle_; //!< A node handle in the private namespace using the local callback queue
ros::AsyncSpinner spinner_; //!< A single/multi-threaded spinner assigned to the local callback queue

rclcpp::Node::SharedPtr node_; //!< The node for this motion model
rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; //!< A single/multi-threaded spinner assigned to the local callback queue
rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr waitables_interface_;
size_t executor_thread_count_;
/**
* @brief Constructor
*
Expand Down
Loading

0 comments on commit 9f9395e

Please sign in to comment.