Tests for CurrentStateMonitor using dependency injection

moveit_core/robot_state/src/robot_state.cpp

@@ -60,10 +60,16 @@ RobotState::RobotState(const RobotModelConstPtr& robot_model)
   , has_velocity_(false)
   , has_acceleration_(false)
   , has_effort_(false)
-  , dirty_link_transforms_(robot_model_->getRootJoint())
+  , dirty_link_transforms_(nullptr)
   , dirty_collision_body_transforms_(nullptr)
   , rng_(nullptr)
 {
+  if (robot_model == nullptr)
+  {
+    throw std::invalid_argument("RobotState cannot be constructed with nullptr RobotModelConstPtr");
+  }
+
+  dirty_link_transforms_ = robot_model_->getRootJoint();
   allocMemory();
   initTransforms();
 }

moveit_ros/planning/package.xml

@@ -41,6 +41,8 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
 
   <test_depend>moveit_resources_panda_moveit_config</test_depend>
 

moveit_ros/planning/planning_scene_monitor/CMakeLists.txt

@@ -3,6 +3,7 @@ set(MOVEIT_LIB_NAME moveit_planning_scene_monitor)
 add_library(${MOVEIT_LIB_NAME} SHARED
   src/planning_scene_monitor.cpp
   src/current_state_monitor.cpp
+  src/current_state_monitor_middleware_handle.cpp
   src/trajectory_monitor.cpp
 )
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
@@ -36,3 +37,15 @@ install(TARGETS demo_scene
 )
 
 install(DIRECTORY include/ DESTINATION include)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_gmock REQUIRED)
+
+  ament_add_gmock(current_state_monitor_tests
+    test/current_state_monitor_tests.cpp
+  )
+  target_link_libraries(current_state_monitor_tests
+    ${MOVEIT_LIB_NAME}
+  )
+endif()

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h

@@ -36,18 +36,19 @@
 
 #pragma once
 
-#include <tf2_ros/buffer.h>
-#include <moveit/robot_state/robot_state.h>
-#include <sensor_msgs/msg/joint_state.hpp>
-#include <boost/function.hpp>
-#include <boost/thread/mutex.hpp>
-#include <boost/thread/condition_variable.hpp>
 #include <boost/signals2.hpp>
+#include <boost/thread/condition_variable.hpp>
+#include <boost/thread/mutex.hpp>
+#include <functional>
+#include <moveit/robot_state/robot_state.h>
 #include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <string>
+#include <tf2_ros/buffer.h>
 
 namespace planning_scene_monitor
 {
-using JointStateUpdateCallback = boost::function<void(const sensor_msgs::msg::JointState::ConstSharedPtr&)>;
+using JointStateUpdateCallback = std::function<void(sensor_msgs::msg::JointState::ConstSharedPtr)>;
 
 /** @class CurrentStateMonitor
     @brief Monitors the joint_states topic and tf to maintain the current state of the robot. */
@@ -56,10 +57,58 @@ class CurrentStateMonitor
   using TFConnection = boost::signals2::connection;
 
 public:
+  /**
+   * @brief      Dependency injection class for testing the CurrentStateMonitor
+   */
+  class MiddlewareHandle
+  {
+  public:
+    /**
+     * @brief      Destroys the object.
+     */
+    virtual ~MiddlewareHandle() = default;
+
+    /**
+     * @brief      Get the current time
+     *
+     * @return     Time object representing the time when this is called
+     */
+    virtual rclcpp::Time now() const = 0;
+
+    /**
+     * @brief      Creates a joint state subscription
+     *
+     * @param[in]  topic     The topic
+     * @param[in]  callback  The callback
+     */
+    virtual void createJointStateSubscription(const std::string& topic, JointStateUpdateCallback callback) = 0;
+
+    /**
+     * @brief      Reset the joint state subscription
+     */
+    virtual void resetJointStateSubscription() = 0;
+
+    /**
+     * @brief      Get the joint state topic name
+     *
+     * @return     The joint state topic name.
+     */
+    virtual std::string getJointStateTopicName() const = 0;
+  };
+
+  /** @brief Constructor.
+   *  @param middleware_handle   The ros middleware handle
+   *  @param robot_model         The current kinematic model to build on
+   *  @param tf_buffer           A pointer to the tf2_ros Buffer to use
+   */
+  CurrentStateMonitor(std::unique_ptr<MiddlewareHandle> middleware_handle,
+                      const moveit::core::RobotModelConstPtr& robot_model,
+                      const std::shared_ptr<tf2_ros::Buffer>& tf_buffer);
+
   /** @brief Constructor.
-   *  @param node A shared_ptr to a node used for subscription to joint_states_topic
-   *  @param robot_model The current kinematic model to build on
-   *  @param tf_buffer A pointer to the tf2_ros Buffer to use
+   *  @param node          A shared_ptr to a node used for subscription to joint_states_topic
+   *  @param robot_model   The current kinematic model to build on
+   *  @param tf_buffer     A pointer to the tf2_ros Buffer to use
    */
   CurrentStateMonitor(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model,
                       const std::shared_ptr<tf2_ros::Buffer>& tf_buffer);
@@ -111,7 +160,7 @@ class CurrentStateMonitor
    */
   inline bool haveCompleteState(const rclcpp::Duration& age) const
   {
-    return haveCompleteStateHelper(node_->now() - age, nullptr);
+    return haveCompleteStateHelper(middleware_handle_->now() - age, nullptr);
   }
 
   /** @brief Query whether we have joint state information for all DOFs in the kinematic model
@@ -140,7 +189,7 @@ class CurrentStateMonitor
    */
   inline bool haveCompleteState(const rclcpp::Duration& age, std::vector<std::string>& missing_joints) const
   {
-    return haveCompleteStateHelper(node_->now() - age, &missing_joints);
+    return haveCompleteStateHelper(middleware_handle_->now() - age, &missing_joints);
   }
 
   /** @brief Get the current state
@@ -216,10 +265,10 @@ class CurrentStateMonitor
   bool haveCompleteStateHelper(const rclcpp::Time& oldest_allowed_update_time,
                                std::vector<std::string>* missing_joints) const;
 
-  void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr joint_state);
+  void jointStateCallback(sensor_msgs::msg::JointState::ConstSharedPtr joint_state);
   void tfCallback();
 
-  std::shared_ptr<rclcpp::Node> node_;
+  std::unique_ptr<MiddlewareHandle> middleware_handle_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   moveit::core::RobotModelConstPtr robot_model_;
   moveit::core::RobotState robot_state_;
@@ -228,7 +277,6 @@ class CurrentStateMonitor
   bool copy_dynamics_;  // Copy velocity and effort from joint_state
   rclcpp::Time monitor_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
   double error_;
-  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber_;
   rclcpp::Time current_state_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
 
   mutable std::mutex state_update_lock_;

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor_middleware_handle.hpp

@@ -0,0 +1,91 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, PickNik, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Tyler Weaver */
+
+#pragma once
+
+#include <moveit/planning_scene_monitor/current_state_monitor.h>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <string>
+
+namespace planning_scene_monitor
+{
+/**
+ * @brief      This class contains the ros interfaces for CurrentStateMontior
+ */
+class CurrentStateMonitorMiddlewareHandle : public CurrentStateMonitor::MiddlewareHandle
+{
+public:
+  /**
+   * @brief      Constructor
+   *
+   * @param[in]  node  The ros node
+   */
+  CurrentStateMonitorMiddlewareHandle(const rclcpp::Node::SharedPtr& node);
+
+  /**
+   * @brief      Get the current time
+   *
+   * @return     Time object representing the time when this is called
+   */
+  rclcpp::Time now() const override;
+
+  /**
+   * @brief      Creates a joint state subscription
+   *
+   * @param[in]  topic     The topic
+   * @param[in]  callback  The callback
+   */
+  void createJointStateSubscription(const std::string& topic, JointStateUpdateCallback callback) override;
+
+  /**
+   * @brief      Reset the joint state subscription
+   */
+  void resetJointStateSubscription() override;
+
+  /**
+   * @brief      Get the joint state topic name
+   *
+   * @return     The joint state topic name.
+   */
+  std::string getJointStateTopicName() const override;
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscription_;
+};
+
+}  // namespace planning_scene_monitor

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

@@ -35,20 +35,25 @@
 /* Author: Ioan Sucan */
 
 #include <moveit/planning_scene_monitor/current_state_monitor.h>
+#include <moveit/planning_scene_monitor/current_state_monitor_middleware_handle.hpp>
 
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
+#include <memory>
 #include <limits>
 
 namespace planning_scene_monitor
 {
+namespace
+{
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.current_state_monitor");
+}
 
-planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const rclcpp::Node::SharedPtr& node,
-                                                                 const moveit::core::RobotModelConstPtr& robot_model,
-                                                                 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
-  : node_(node)
+CurrentStateMonitor::CurrentStateMonitor(std::unique_ptr<CurrentStateMonitor::MiddlewareHandle> middleware_handle,
+                                         const moveit::core::RobotModelConstPtr& robot_model,
+                                         const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
+  : middleware_handle_(std::move(middleware_handle))
   , tf_buffer_(tf_buffer)
   , robot_model_(robot_model)
   , robot_state_(robot_model)
@@ -59,6 +64,13 @@ planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const rclcpp::N
   robot_state_.setToDefaultValues();
 }
 
+CurrentStateMonitor::CurrentStateMonitor(const rclcpp::Node::SharedPtr& node,
+                                         const moveit::core::RobotModelConstPtr& robot_model,
+                                         const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
+  : CurrentStateMonitor(std::make_unique<CurrentStateMonitorMiddlewareHandle>(node), robot_model, tf_buffer)
+{
+}
+
 CurrentStateMonitor::~CurrentStateMonitor()
 {
   stopStateMonitor();
@@ -137,10 +149,14 @@ void CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topi
   {
     joint_time_.clear();
     if (joint_states_topic.empty())
+    {
       RCLCPP_ERROR(LOGGER, "The joint states topic cannot be an empty string");
+    }
     else
-      joint_state_subscriber_ = node_->create_subscription<sensor_msgs::msg::JointState>(
-          joint_states_topic, 25, std::bind(&CurrentStateMonitor::jointStateCallback, this, std::placeholders::_1));
+    {
+      middleware_handle_->createJointStateSubscription(
+          joint_states_topic, std::bind(&CurrentStateMonitor::jointStateCallback, this, std::placeholders::_1));
+    }
     if (tf_buffer_ && !robot_model_->getMultiDOFJointModels().empty())
     {
       // TODO (anasarrak): replace this for the appropiate function, there is no similar
@@ -149,7 +165,7 @@ void CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topi
       //     tf_buffer_->_addTransformsChangedListener(std::bind(&CurrentStateMonitor::tfCallback, this))));
     }
     state_monitor_started_ = true;
-    monitor_start_time_ = node_->now();
+    monitor_start_time_ = middleware_handle_->now();
     RCLCPP_INFO(LOGGER, "Listening to joint states on topic '%s'", joint_states_topic.c_str());
   }
 }
@@ -163,7 +179,7 @@ void CurrentStateMonitor::stopStateMonitor()
 {
   if (state_monitor_started_)
   {
-    joint_state_subscriber_.reset();
+    middleware_handle_->resetJointStateSubscription();
     if (tf_buffer_ && tf_connection_)
     {
       // TODO (anasarrak): replace this for the appropiate function, there is no similar
@@ -178,10 +194,7 @@ void CurrentStateMonitor::stopStateMonitor()
 
 std::string CurrentStateMonitor::getMonitoredTopic() const
 {
-  if (joint_state_subscriber_)
-    return joint_state_subscriber_->get_topic_name();
-  else
-    return "";
+  return middleware_handle_->getJointStateTopicName();
 }
 
 bool CurrentStateMonitor::haveCompleteStateHelper(const rclcpp::Time& oldest_allowed_update_time,
@@ -214,15 +227,15 @@ bool CurrentStateMonitor::haveCompleteStateHelper(const rclcpp::Time& oldest_all
 
 bool CurrentStateMonitor::waitForCurrentState(const rclcpp::Time& t, double wait_time) const
 {
-  rclcpp::Time start = node_->now();
+  rclcpp::Time start = middleware_handle_->now();
   rclcpp::Duration elapsed(0, 0);
   rclcpp::Duration timeout = rclcpp::Duration::from_seconds(wait_time);
 
   std::unique_lock<std::mutex> lock(state_update_lock_);
   while (current_state_time_ < t)
   {
     state_update_condition_.wait_for(lock, (timeout - elapsed).to_chrono<std::chrono::duration<double>>());
-    elapsed = node_->now() - start;
+    elapsed = middleware_handle_->now() - start;
     if (elapsed > timeout)
     {
       RCLCPP_INFO(LOGGER,
@@ -276,7 +289,7 @@ bool CurrentStateMonitor::waitForCompleteState(const std::string& group, double
   return ok;
 }
 
-void CurrentStateMonitor::jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr joint_state)
+void CurrentStateMonitor::jointStateCallback(sensor_msgs::msg::JointState::ConstSharedPtr joint_state)
 {
   if (joint_state->name.size() != joint_state->position.size())
   {
@@ -416,7 +429,7 @@ void CurrentStateMonitor::tfCallback()
   {
     // stub joint state: multi-dof joints are not modelled in the message,
     // but we should still trigger the update callbacks
-    sensor_msgs::msg::JointState::ConstSharedPtr joint_state(new sensor_msgs::msg::JointState);
+    auto joint_state = std::make_shared<sensor_msgs::msg::JointState>();
     for (JointStateUpdateCallback& update_callback : update_callbacks_)
       update_callback(joint_state);
   }