Minor cleanup to ros_control_interface and trajectory execution (#2208)

moveit_plugins/moveit_ros_control_interface/README.md

@@ -2,7 +2,7 @@
 
 This package provides plugins of base class `moveit_controller_manager::MoveItControllerManager` and a new plugin base class for `moveit_controller_manager::MoveItControllerHandle` allocators.
 The allocator class is necessary because `moveit_controller_manager::MoveItControllerHandle` needs a name passed to the constructor.
-Two variantes are provided, `moveit_ros_control_interface::Ros2ControlManager` for interfacing a single ros_control node and `moveit_ros_control_interface::Ros2ControlMultiManager` for seamless integration with any number of ros_control nodes.
+Two variants are provided, `moveit_ros_control_interface::Ros2ControlManager` for interfacing a single ros_control node and `moveit_ros_control_interface::Ros2ControlMultiManager` for seamless integration with any number of ros_control nodes.
 
 
 ## moveit_ros_control_interface::Ros2ControlManager
@@ -17,13 +17,13 @@ These plugins should be registered with lookup names that match the correspondin
 Currently plugins for `position_controllers/JointTrajectoryController`, `velocity_controllers/JointTrajectoryController` and `effort_controllers/JointTrajectoryController` are available, which simply wrap `moveit_simple_controller_manager::FollowJointTrajectoryControllerHandle` instances.
 
 ### Setup
-In your MoveIt launch file (e.g. `ROBOT_moveit_config/launch/ROBOT_moveit_controller_manager.launch.xml`) set the `moveit_controller_manager` parameter:
+In your MoveIt launch file (e.g. `ROBOT_moveit_config/launch/ROBOT_moveit_controller_manager.launch.xml`), set the `moveit_controller_manager` parameter:
 ```
 <param name="moveit_controller_manager" value="moveit_ros_control_interface::Ros2ControlManager" />
 ```
 
-And make sure to set the `ros_control_namespace` parameter to the namespace (without the /controller_manager/ part) of the ros_control-based node you like to interface.
-If you are using the `moveit_setup_assistent` you can add it to `ROBOT_moveit_config/config/moveit_controllers.yaml`, e.g.:
+Make sure to set the `ros_control_namespace` parameter to the namespace (without the /controller_manager/ part) of the ros_control-based node you like to interface.
+If you are using `moveit_setup_assistant`, you can add it to `ROBOT_moveit_config/config/moveit_controllers.yaml`, e.g.:
 ```
 ros_control_namespace: /ROS_CONTROL_NODE
 controller_list:

moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp

@@ -208,7 +208,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
       const std::string& type = controller.type;
       AllocatorsMap::iterator alloc_it = allocators_.find(type);
       if (alloc_it == allocators_.end())
-      {  // create allocator is needed
+      {  // create allocator if needed
         alloc_it = allocators_.insert(std::make_pair(type, loader_.createUniqueInstance(type))).first;
       }
 
@@ -227,7 +227,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
   }
 
   /**
-   * \brief get fully qualified name
+   * \brief Get fully qualified name
    * @param name name to be resolved to an absolute name
    * @return resolved name
    */
@@ -243,7 +243,6 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
   Ros2ControlManager()
     : loader_("moveit_ros_control_interface", "moveit_ros_control_interface::ControllerHandleAllocator")
   {
-    RCLCPP_INFO_STREAM(LOGGER, "Started moveit_ros_control_interface::Ros2ControlManager for namespace " << ns_);
   }
 
   /**
@@ -253,6 +252,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
   Ros2ControlManager(const std::string& ns)
     : ns_(ns), loader_("moveit_ros_control_interface", "moveit_ros_control_interface::ControllerHandleAllocator")
   {
+    RCLCPP_INFO_STREAM(LOGGER, "Started moveit_ros_control_interface::Ros2ControlManager for namespace " << ns_);
   }
 
   void initialize(const rclcpp::Node::SharedPtr& node) override
@@ -293,7 +293,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
     std::scoped_lock<std::mutex> lock(controllers_mutex_);
     HandleMap::iterator it = handles_.find(name);
     if (it != handles_.end())
-    {  // controller is is manager by this interface
+    {  // controller is manager by this interface
       return it->second;
     }
     return moveit_controller_manager::MoveItControllerHandlePtr();
@@ -372,8 +372,8 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
   /**
    * \brief Filter lists for managed controller and computes switching set.
    * Stopped list might be extended by unsupported controllers that claim needed resources
-   * @param activate
-   * @param deactivate
+   * @param activate vector of controllers to be activated
+   * @param deactivate vector of controllers to be deactivated
    * @return true if switching succeeded
    */
   bool switchControllers(const std::vector<std::string>& activate_base,
@@ -673,10 +673,10 @@ class Ros2ControlMultiManager : public moveit_controller_manager::MoveItControll
   }
 
   /**
-   * \brief delegates switch  to all known interfaces. Stops on first failing switch.
-   * @param activate
-   * @param deactivate
-   * @return
+   * \brief delegates switch to all known interfaces. Stops on first failing switch.
+   * @param activate vector of controllers to be activated
+   * @param deactivate vector of controllers to be deactivated
+   * @return true if switching succeeded
    */
   bool switchControllers(const std::vector<std::string>& activate, const std::vector<std::string>& deactivate) override
   {

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

@@ -1668,8 +1668,8 @@ bool TrajectoryExecutionManager::ensureActiveControllers(const std::vector<std::
           ci.last_update_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
         }
         // reset the state update cache
-        for (const std::string& controller_to_activate : controllers_to_deactivate)
-          known_controllers_[controller_to_activate].last_update_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
+        for (const std::string& controller_to_deactivate : controllers_to_deactivate)
+          known_controllers_[controller_to_deactivate].last_update_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
         return controller_manager_->switchControllers(controllers_to_activate, controllers_to_deactivate);
       }
       else