Multi switch

combined_robot_hw_tests/test/cm_test.cpp

@@ -472,7 +472,7 @@ TEST(CMTests, listControllers)
     EXPECT_EQ(state1.claimed_resources[0].resources[1], "hiDOF_joint2");
 
     EXPECT_EQ(state2.name, "vel_eff_controller");
-    EXPECT_EQ(state2.state, "stopped");
+    EXPECT_EQ(state2.state, "initialized");
     EXPECT_EQ(state2.type, "controller_manager_tests/VelEffController");
     EXPECT_EQ(state2.claimed_resources.size(), 2);
     EXPECT_EQ(state2.claimed_resources[0].hardware_interface, "hardware_interface::VelocityJointInterface");

controller_interface/CMakeLists.txt

@@ -10,6 +10,23 @@ catkin_package(
   INCLUDE_DIRS include
   )
 
+if(CATKIN_ENABLE_TESTING)
+
+  find_package(rosunit REQUIRED)
+
+  catkin_add_gmock(controller_base_test
+    test/controller_base_test.cpp
+  )
+
+  target_include_directories(controller_base_test PRIVATE include)
+  target_include_directories(controller_base_test SYSTEM PRIVATE ${catkin_INCLUDE_DIRS})
+
+  target_link_libraries(controller_base_test
+    ${catkin_LIBRARIES}
+  )
+
+endif()
+
 # Install
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})

controller_interface/include/controller_interface/controller_base.h

@@ -60,7 +60,7 @@ class ControllerBase
    *
    * \param time The current time
    */
-  virtual void starting(const ros::Time& /*time*/) {};
+  virtual void starting(const ros::Time& /*time*/) {}
 
   /** \brief This is called periodically by the realtime thread when the controller is running
    *
@@ -74,47 +74,137 @@ class ControllerBase
    *
    * \param time The current time
    */
-  virtual void stopping(const ros::Time& /*time*/) {};
+  virtual void stopping(const ros::Time& /*time*/) {}
+
+  /** \brief This is called from within the realtime thread while the controller is
+   * waiting to start
+   *
+   * \param time The current time
+   */
+  virtual void waiting(const ros::Time& /*time*/) {}
+
+  /** \brief This is called from within the realtime thread when the controller needs
+   * to be aborted
+   *
+   * \param time The current time
+   */
+  virtual void aborting(const ros::Time& /*time*/) {}
+
+  /** \brief Check if the controller is initialized
+   * \returns true if the controller is initialized
+   */
+  bool isInitialized() const
+  {
+    return state_ == INITIALIZED;
+  }
 
   /** \brief Check if the controller is running
    * \returns true if the controller is running
    */
-  bool isRunning()
+  bool isRunning() const
+  {
+    return state_ == RUNNING;
+  }
+
+  /** \brief Check if the controller is stopped
+   * \returns true if the controller is stopped
+   */
+  bool isStopped() const
   {
-    return (state_ == RUNNING);
+    return state_ == STOPPED;
+  }
+
+  /** \brief Check if the controller is waiting
+   * \returns true if the controller is waiting
+   */
+  bool isWaiting() const
+  {
+    return state_ == WAITING;
+  }
+
+  /** \brief Check if the controller is aborted
+   * \returns true if the controller is aborted
+   */
+  bool isAborted() const
+  {
+    return state_ == ABORTED;
   }
 
   /// Calls \ref update only if this controller is running.
   void updateRequest(const ros::Time& time, const ros::Duration& period)
   {
     if (state_ == RUNNING)
+    {
       update(time, period);
+    }
   }
 
-  /// Calls \ref starting only if this controller is initialized or already running
+  /// Calls \ref starting unless this controller is just constructed
   bool startRequest(const ros::Time& time)
   {
-    // start succeeds even if the controller was already started
-    if (state_ == RUNNING || state_ == INITIALIZED){
+    // start works from any state, except CONSTRUCTED
+    if (state_ != CONSTRUCTED)
+    {
       starting(time);
       state_ = RUNNING;
       return true;
     }
     else
+    {
+      ROS_FATAL("Failed to start controller. It is not initialized.");
       return false;
+    }
   }
 
-  /// Calls \ref stopping only if this controller is initialized or already running
+  /// Calls \ref stopping unless this controller is just constructed
   bool stopRequest(const ros::Time& time)
   {
-    // stop succeeds even if the controller was already stopped
-    if (state_ == RUNNING || state_ == INITIALIZED){
+    // stop works from any state, except CONSTRUCTED
+    if (state_ != CONSTRUCTED)
+    {
       stopping(time);
-      state_ = INITIALIZED;
+      state_ = STOPPED;
       return true;
     }
     else
+    {
+      ROS_FATAL("Failed to stop controller. It is not initialized.");
       return false;
+    }
+  }
+
+  /// Calls \ref waiting unless this controller is just constructed
+  bool waitRequest(const ros::Time& time)
+  {
+    // wait works from any state, except CONSTRUCTED
+    if (state_ != CONSTRUCTED)
+    {
+      waiting(time);
+      state_ = WAITING;
+      return true;
+    }
+    else
+    {
+      ROS_FATAL("Failed to wait controller. It is not initialized.");
+      return false;
+    }
+  }
+
+  /// Calls \ref abort unless this controller is just constructed
+  bool abortRequest(const ros::Time& time)
+  {
+    // abort works from any state, except CONSTRUCTED
+    if (state_ != CONSTRUCTED)
+    {
+      aborting(time);
+      state_ = ABORTED;
+      return true;
+    }
+    else
+    {
+      ROS_FATAL("Failed to abort controller. It is not initialized.");
+      return false;
+    }
   }
 
   /*\}*/
@@ -148,7 +238,7 @@ class ControllerBase
   /*\}*/
 
   /// The current execution state of the controller
-  enum {CONSTRUCTED, INITIALIZED, RUNNING} state_;
+  enum {CONSTRUCTED, INITIALIZED, RUNNING, STOPPED, WAITING, ABORTED} state_;
 
 
 private:

controller_interface/package.xml

@@ -19,4 +19,6 @@
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
   <depend>roscpp</depend>
+
+  <test_depend>rosunit</test_depend>
 </package>