Leverage forward declarations (#233)

dependencies_unstable.rosinstall

@@ -17,7 +17,7 @@
 - git:
     local-name: tesseract_qt
     uri: https://github.com/tesseract-robotics/tesseract_qt.git
-    version: main
+    version: master
 - git:
     local-name: descartes_light
     uri: https://github.com/swri-robotics/descartes_light.git

tesseract_monitoring/CMakeLists.txt

@@ -1,70 +1,37 @@
-cmake_minimum_required(VERSION 3.5.0)
+cmake_minimum_required(VERSION 3.15.0)
 
 # Extract package name and version
 find_package(ros_industrial_cmake_boilerplate REQUIRED)
 extract_package_metadata(pkg)
 project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES CXX)
 
 find_package(tesseract_environment REQUIRED)
-find_package(orocos_kdl REQUIRED)
 find_package(tesseract_common REQUIRED)
+find_package(tesseract_rosutils REQUIRED)
+find_package(orocos_kdl REQUIRED)
 find_package(Eigen3 REQUIRED)
 
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   tesseract_msgs
-  tesseract_rosutils
   dynamic_reconfigure
   pluginlib
   tf2_ros
   tf2_eigen
   visualization_msgs
 )
 
-generate_dynamic_reconfigure_options(
-  cfg/EnvironmentMonitorDynamicReconfigure.cfg
-)
-
-catkin_package(
-  INCLUDE_DIRS
-    include
-  LIBRARIES
-    ${PROJECT_NAME}_environment
-    ${PROJECT_NAME}_environment_interface
-    ${PROJECT_NAME}_contacts
-  CATKIN_DEPENDS
-    roscpp
-    tesseract_msgs
-    tesseract_rosutils
-    pluginlib
-    dynamic_reconfigure
-    tf2_ros
-    tf2_eigen
-    visualization_msgs
-  DEPENDS
-    EIGEN3
-    orocos_kdl
-    tesseract_common
-    tesseract_environment
-)
-
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
-
 # Load variable for clang tidy args, compiler options and cxx version
 tesseract_variables()
 
 # Tesseract ROS Nodes
 add_library(${PROJECT_NAME}_environment SHARED src/environment_monitor.cpp src/current_state_monitor.cpp)
-target_link_libraries(${PROJECT_NAME}_environment PUBLIC tesseract::tesseract_environment ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_environment PUBLIC tesseract::tesseract_common tesseract::tesseract_environment tesseract::tesseract_rosutils ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 target_compile_options(${PROJECT_NAME}_environment PRIVATE ${TESSERACT_COMPILE_OPTIONS})
 target_clang_tidy(${PROJECT_NAME}_environment ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
 target_cxx_version(${PROJECT_NAME}_environment PUBLIC VERSION 17)
 target_include_directories(${PROJECT_NAME}_environment PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>")
 target_include_directories(${PROJECT_NAME}_environment SYSTEM PUBLIC ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
-add_dependencies(${PROJECT_NAME}_environment ${PROJECT_NAME}_gencfg)
 
 add_executable(${PROJECT_NAME}_environment_node src/environment_monitor_node.cpp)
 target_link_libraries(${PROJECT_NAME}_environment_node PRIVATE ${PROJECT_NAME}_environment ${catkin_LIBRARIES})
@@ -75,24 +42,23 @@ target_include_directories(${PROJECT_NAME}_environment_node PUBLIC "$<BUILD_INTE
 target_include_directories(${PROJECT_NAME}_environment_node SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
 
 add_library(${PROJECT_NAME}_contacts src/contact_monitor.cpp)
-target_link_libraries(${PROJECT_NAME}_contacts PRIVATE tesseract::tesseract_environment ${PROJECT_NAME}_environment ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_contacts PUBLIC tesseract::tesseract_common tesseract::tesseract_environment tesseract::tesseract_rosutils ${PROJECT_NAME}_environment ${catkin_LIBRARIES})
 target_compile_options(${PROJECT_NAME}_contacts PRIVATE ${TESSERACT_COMPILE_OPTIONS})
 target_clang_tidy(${PROJECT_NAME}_contacts ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
 target_cxx_version(${PROJECT_NAME}_contacts PUBLIC VERSION 17)
 target_include_directories(${PROJECT_NAME}_contacts PRIVATE "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>")
 target_include_directories(${PROJECT_NAME}_contacts SYSTEM PRIVATE ${catkin_INCLUDE_DIRS})
-add_dependencies(${PROJECT_NAME}_contacts ${PROJECT_NAME}_gencfg)
 
 add_executable(${PROJECT_NAME}_contacts_node src/contact_monitor_node.cpp)
-target_link_libraries(${PROJECT_NAME}_contacts_node PRIVATE tesseract::tesseract_environment ${PROJECT_NAME}_contacts ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_contacts_node PRIVATE ${PROJECT_NAME}_contacts ${catkin_LIBRARIES})
 target_compile_options(${PROJECT_NAME}_contacts_node PRIVATE ${TESSERACT_COMPILE_OPTIONS})
 target_clang_tidy(${PROJECT_NAME}_contacts_node ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
 target_cxx_version(${PROJECT_NAME}_contacts_node PRIVATE VERSION 17)
 target_include_directories(${PROJECT_NAME}_contacts_node PRIVATE "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>")
 target_include_directories(${PROJECT_NAME}_contacts_node SYSTEM PRIVATE ${catkin_INCLUDE_DIRS})
 
 add_library(${PROJECT_NAME}_environment_interface SHARED src/environment_monitor_interface.cpp)
-target_link_libraries(${PROJECT_NAME}_environment_interface PUBLIC tesseract::tesseract_environment ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_environment_interface PUBLIC tesseract::tesseract_common tesseract::tesseract_environment tesseract::tesseract_rosutils ${catkin_LIBRARIES})
 target_compile_options(${PROJECT_NAME}_environment_interface PRIVATE ${TESSERACT_COMPILE_OPTIONS})
 target_clang_tidy(${PROJECT_NAME}_environment_interface ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
 target_cxx_version(${PROJECT_NAME}_environment_interface PUBLIC VERSION 17)
@@ -107,20 +73,40 @@ target_cxx_version(demo_scene PRIVATE VERSION 17)
 target_include_directories(demo_scene PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>")
 target_include_directories(demo_scene SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
 
-# Mark executables and/or libraries for installation
-install(TARGETS ${PROJECT_NAME}_environment ${PROJECT_NAME}_contacts ${PROJECT_NAME}_contacts_node ${PROJECT_NAME}_environment_node ${PROJECT_NAME}_environment_interface demo_scene
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+catkin_package(
+  SKIP_CMAKE_CONFIG_GENERATION
+  SKIP_PKG_CONFIG_GENERATION
+)
+
+configure_package(
+  NAMESPACE tesseract
+  TARGETS
+    ${PROJECT_NAME}_environment
+    ${PROJECT_NAME}_contacts
+    ${PROJECT_NAME}_contacts_node
+    ${PROJECT_NAME}_environment_node
+    ${PROJECT_NAME}_environment_interface
+    demo_scene
+  DEPENDENCIES
+    Eigen3
+    orocos_kdl
+    tesseract_common
+    tesseract_environment
+    tesseract_rosutils
+    "catkin REQUIRED COMPONENTS roscpp tesseract_msgs dynamic_reconfigure pluginlib tf2_ros tf2_eigen visualization_msgs"
 )
 
 # Mark cpp header files for installation
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-  FILES_MATCHING PATTERN "*.h"
-  PATTERN ".svn" EXCLUDE
- )
+install(
+  DIRECTORY include/${PROJECT_NAME}
+  DESTINATION include
+  FILES_MATCHING
+  PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE)
+
+if(NOT ${CATKIN_INSTALL_INTO_PREFIX_ROOT})
+  foreach(dir launch)
+    install(DIRECTORY ${dir}/ DESTINATION share/${PROJECT_NAME}/${dir})
+  endforeach()
+endif()
 
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)

tesseract_monitoring/include/tesseract_monitoring/contact_monitor.h

@@ -30,27 +30,26 @@
 
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
-#include <ros/ros.h>
-#include <sensor_msgs/JointState.h>
-#include <tesseract_msgs/ComputeContactResultVector.h>
-#include <tesseract_msgs/ModifyEnvironment.h>
-#include <tesseract_msgs/EnvironmentState.h>
-#include <mutex>
-#include <condition_variable>
+#include <memory>
+#include <vector>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_monitoring/constants.h>
-#include <tesseract_collision/core/discrete_contact_manager.h>
-#include <tesseract_environment/environment.h>
-#include <tesseract_environment/environment_monitor.h>
+#include <tesseract_collision/core/fwd.h>
+#include <tesseract_environment/fwd.h>
+
+namespace ros
+{
+class NodeHandle;
+}
 
 namespace tesseract_monitoring
 {
 class ContactMonitor
 {
 public:
   ContactMonitor(std::string monitor_namespace,
-                 tesseract_environment::Environment::UPtr env,
+                 std::unique_ptr<tesseract_environment::Environment> env,
                  ros::NodeHandle& nh,
                  ros::NodeHandle& pnh,
                  std::vector<std::string> monitored_link_names,
@@ -96,36 +95,9 @@ class ContactMonitor
    */
   void computeCollisionReportThread();
 
-  void callbackJointState(boost::shared_ptr<sensor_msgs::JointState> msg);
-
-  bool callbackModifyTesseractEnv(tesseract_msgs::ModifyEnvironment::Request& request,
-                                  tesseract_msgs::ModifyEnvironment::Response& response);
-
-  bool callbackComputeContactResultVector(tesseract_msgs::ComputeContactResultVector::Request& request,
-                                          tesseract_msgs::ComputeContactResultVector::Response& response);
-
-  void callbackTesseractEnvDiff(const tesseract_msgs::EnvironmentStatePtr& state);
-
 private:
-  std::string monitor_namespace_;
-  std::string monitored_namespace_;
-  int env_revision_{ 0 };
-  tesseract_environment::EnvironmentMonitor::UPtr monitor_;
-  ros::NodeHandle& nh_;
-  ros::NodeHandle& pnh_;
-  std::vector<std::string> monitored_link_names_;
-  std::vector<std::string> disabled_link_names_;
-  tesseract_collision::ContactTestType type_;
-  double contact_distance_;
-  tesseract_collision::DiscreteContactManager::UPtr manager_;
-  bool publish_contact_markers_{ false };
-  ros::Subscriber joint_states_sub_;
-  ros::Publisher contact_results_pub_;
-  ros::Publisher contact_marker_pub_;
-  ros::ServiceServer compute_contact_results_;
-  std::mutex modify_mutex_;
-  boost::shared_ptr<sensor_msgs::JointState> current_joint_states_;
-  std::condition_variable current_joint_states_evt_;
+  struct Implementation;
+  std::unique_ptr<Implementation> impl_;
 };
 
 }  // namespace tesseract_monitoring

tesseract_monitoring/include/tesseract_monitoring/current_state_monitor.h

@@ -39,18 +39,27 @@
 
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
-#include <ros/ros.h>
-#include <sensor_msgs/JointState.h>
-#include <mutex>
-#include <condition_variable>
 #include <memory>
 #include <functional>
+#include <vector>
 #include <unordered_map>
-#include <map>
-#include <tf2_ros/transform_broadcaster.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract_environment/environment.h>
+#include <tesseract_environment/fwd.h>
+#include <tesseract_scene_graph/fwd.h>
+
+namespace ros
+{
+class NodeHandle;
+class Duration;
+class Time;
+}  // namespace ros
+
+#include <ros/message_forward.h>
+namespace sensor_msgs
+{
+ROS_DECLARE_MESSAGE(JointState)
+}
 
 namespace tesseract_monitoring
 {
@@ -71,14 +80,14 @@ class CurrentStateMonitor
    * @param robot_model The current kinematic model to build on
    * @param tf A pointer to the tf transformer to use
    */
-  CurrentStateMonitor(const tesseract_environment::Environment::ConstPtr& env);
+  CurrentStateMonitor(const std::shared_ptr<const tesseract_environment::Environment>& env);
 
   /** @brief Constructor.
    *  @param robot_model The current kinematic model to build on
    *  @param tf A pointer to the tf transformer to use
    *  @param nh A ros::NodeHandle to pass node specific options
    */
-  CurrentStateMonitor(const tesseract_environment::Environment::ConstPtr& env, const ros::NodeHandle& nh);
+  CurrentStateMonitor(const std::shared_ptr<const tesseract_environment::Environment>& env, const ros::NodeHandle& nh);
 
   ~CurrentStateMonitor();
   CurrentStateMonitor(const CurrentStateMonitor&) = delete;
@@ -159,7 +168,8 @@ class CurrentStateMonitor
   bool waitForCompleteState(const std::string& manip, double wait_time) const;
 
   /** @brief Get the time point when the monitor was started */
-  const ros::Time& getMonitorStartTime() const { return monitor_start_time_; }
+  const ros::Time& getMonitorStartTime() const;
+
   /** @brief Add a function that will be called whenever the joint state is updated*/
   void addUpdateCallback(const JointStateUpdateCallback& fn);
 
@@ -170,39 +180,22 @@ class CurrentStateMonitor
    *  if the difference is less than a specified value (labeled the "allowed bounds error").
    *  This value can be set using this function.
    *  @param error The specified value for the "allowed bounds error". The default is machine precision. */
-  void setBoundsError(double error) { error_ = (error > 0) ? error : -error; }
+  void setBoundsError(double error);
+
   /** @brief When a joint value is received to be out of bounds, it is changed slightly to fit within bounds,
    *  if the difference is less than a specified value (labeled the "allowed bounds error").
    *  @return The stored value for the "allowed bounds error"
    */
-  double getBoundsError() const { return error_; }
+  double getBoundsError() const;
+
   /** @brief Allow the joint_state arrays velocity and effort to be copied into the robot state
    *  this is useful in some but not all applications
    */
-  void enableCopyDynamics(bool enabled) { copy_dynamics_ = enabled; }
+  void enableCopyDynamics(bool enabled);
 
 private:
-  void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
-  bool isPassiveOrMimicDOF(const std::string& dof) const;
-
-  ros::NodeHandle nh_;
-  tesseract_environment::Environment::ConstPtr env_;
-  tesseract_scene_graph::SceneState env_state_;
-  int last_environment_revision_;
-  std::map<std::string, ros::Time> joint_time_;
-  bool state_monitor_started_;
-  bool copy_dynamics_;  // Copy velocity and effort from joint_state
-  ros::Time monitor_start_time_;
-  double error_;
-  ros::Subscriber joint_state_subscriber_;
-  tf2_ros::TransformBroadcaster tf_broadcaster_;
-  ros::Time current_state_time_;
-  ros::Time last_tf_update_;
-  bool publish_tf_;
-
-  mutable std::mutex state_update_lock_;
-  mutable std::condition_variable state_update_condition_;
-  std::vector<JointStateUpdateCallback> update_callbacks_;
+  struct Implementation;
+  std::unique_ptr<Implementation> impl_;
 };
 
 }  // namespace tesseract_monitoring