diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
index 7a1b40f9..feb46c4e 100644
--- a/.github/workflows/ci.yaml
+++ b/.github/workflows/ci.yaml
@@ -28,11 +28,6 @@ jobs:
         cd /home/ros2_ws/src/
         git clone https://github.com/ros-controls/ros2_control
         git clone https://github.com/ros-controls/ros2_controllers
-        git clone https://github.com/ddengster/ros2_control/ -b coffeebot_deps ros2_control_ddengster
-        git clone https://github.com/ros-controls/control_toolbox/ -b ros2-master
-        touch ros2_control_ddengster/COLCON_IGNORE
-        cp -r ros2_control_ddengster/joint_limits_interface ros2_control_ddengster/transmission_interface  ros2_control
-        sed -i '/rclcpp/d' ros2_control/joint_limits_interface/CMakeLists.txt
         rosdep update
         rosdep install --from-paths ./ -i -y --rosdistro foxy \
           --ignore-src
diff --git a/Docker/Dockerfile b/Docker/Dockerfile
index 3769c83b..2b079289 100644
--- a/Docker/Dockerfile
+++ b/Docker/Dockerfile
@@ -16,13 +16,7 @@ RUN mkdir -p /home/ros2_ws/src \
     && cd /home/ros2_ws/src/ \
     && git clone https://github.com/ros-simulation/gazebo_ros2_control \
     && git clone https://github.com/ros-controls/ros2_control \
-    && git clone https://github.com/ros-controls/control_toolbox -b ros2-master \
     && git clone https://github.com/ros-controls/ros2_controllers \
-    && git clone https://github.com/ddengster/ros2_control/ -b coffeebot_deps ros2_control_ddengster \
-    && touch ros2_control_ddengster/COLCON_IGNORE \
-    && cp -r ros2_control_ddengster/joint_limits_interface ros2_control_ddengster/transmission_interface ros2_control \
-    && sed -i '/rclcpp/d' ros2_control/joint_limits_interface/CMakeLists.txt \
-    && sed -i '/rclcpp/d' ros2_control/joint_limits_interface/package.xml \
     && rosdep update \
     && rosdep install --from-paths ./ -i -y --rosdistro foxy \
       --ignore-src
diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp
index d0359a64..a1023cac 100644
--- a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp
+++ b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp
@@ -35,15 +35,12 @@
 #define GAZEBO_ROS2_CONTROL__DEFAULT_ROBOT_HW_SIM_HPP_
 
 #include <algorithm>
+#include <memory>
 #include <string>
 #include <vector>
 
 // ros_control
 #include "control_toolbox/pid_ros.hpp"
-#if 0  // @todo
-#include <hardware_interface/joint_command_interface.hpp>
-#include <hardware_interface/robot_hw.hpp>
-#endif
 
 #include "joint_limits_interface/joint_limits.hpp"
 #include "joint_limits_interface/joint_limits_interface.hpp"
@@ -59,12 +56,12 @@
 #include "pluginlib/class_list_macros.hpp"
 #include "rclcpp/rclcpp.hpp"
 
-// gazebo_ros_control
-#include "gazebo_ros2_control/robot_hw_sim.hpp"
-
 // URDF
 #include "urdf/model.h"
 
+// gazebo_ros_control
+#include "gazebo_ros2_control/robot_hw_sim.hpp"
+
 namespace gazebo_ros2_control
 {
 
@@ -87,36 +84,32 @@ class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim
   // Methods used to control a joint.
   enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID};
 
+private:
+  rclcpp::Node::SharedPtr nh_;
+
 protected:
-  // Register the limits of the joint specified by joint_name and joint_handle. The limits are
-  // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also.
-  // Return the joint's type, lower position limit, upper position limit, and effort limit.
+  /// \brief Register the limits of the joint specified by joint_name and joint_handle.
+  /// The limits are retrieved from joint_limit_nh. If urdf_model is not NULL, limits
+  /// are retrieved from it also. Return the joint's type, lower position limit, upper
+  /// position limit, and effort limit.
   void registerJointLimits(
-    const std::string & joint_name,
-    const hardware_interface::JointStateHandle & joint_handle,
-    const ControlMethod ctrl_method,
-    const rclcpp::Node::SharedPtr & joint_limit_nh,
+    size_t joint_nr,
     const urdf::Model * const urdf_model,
     int * const joint_type, double * const lower_limit,
     double * const upper_limit, double * const effort_limit,
     double * const vel_limit);
 
-  unsigned int n_dof_;
+  /// \brief Refreshes all valid handle references in a collection.
+  /// Requests from the RobotHardware updated handle references. This is required after
+  /// any call to RobotHardware::register_joint() and before a handle is used or bound
+  /// to a controller since the internal implementation of register_joint binds to doubles
+  /// inside a std::vector and thus growth of that vector invalidates all existing
+  /// iterators (i.e. handles).
+  /// See https://github.com/ros-controls/ros2_control/issues/212
+  /// If a handle in the collection is unset (no joint name or interface name) then it is skipped.
+  void bindJointHandles(std::vector<hardware_interface::JointHandle> & joint_iface_handles);
 
-#if 0  // @todo
-  hardware_interface::JointStateInterface js_interface_;
-  hardware_interface::EffortJointInterface ej_interface_;
-  hardware_interface::VelocityJointInterface vj_interface_;
-  hardware_interface::PositionJointInterface pj_interface_;
-#endif
-#if 0  // @todo
-  joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_;
-  joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_;
-  joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_;
-  joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_;
-  joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_;
-  joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_;
-#endif
+  unsigned int n_dof_;
   std::vector<std::string> joint_names_;
   std::vector<int> joint_types_;
   std::vector<double> joint_lower_limits_;
@@ -126,33 +119,43 @@ class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim
   std::vector<ControlMethod> joint_control_methods_;
   std::vector<control_toolbox::PidROS> pid_controllers_;
   std::vector<double> joint_position_;
-  std::vector<double> joint_velocity_;
-  std::vector<double> joint_effort_;
-  std::vector<double> joint_effort_command_;
-  std::vector<double> joint_position_command_;
+
+  /// \brief stores the joint positions on ESTOP activation
+  /// During ESTOP these positions will override the output position command value.
   std::vector<double> last_joint_position_command_;
-  std::vector<double> joint_velocity_command_;
-  std::vector<hardware_interface::OperationMode> joint_opmodes_;
 
+  /// \brief handles to the joints from within Gazebo
   std::vector<gazebo::physics::JointPtr> sim_joints_;
 
-  std::vector<hardware_interface::JointStateHandle> joint_states_;
-  std::vector<hardware_interface::JointCommandHandle> joint_cmds_;
-  std::vector<hardware_interface::JointCommandHandle> joint_eff_cmdhandle_;
-  std::vector<hardware_interface::JointCommandHandle> joint_vel_cmdhandle_;
+  /// \brief The current positions of the joints
+  std::vector<hardware_interface::JointHandle> joint_pos_stateh_;
+  /// \brief The current velocities of the joints
+  std::vector<hardware_interface::JointHandle> joint_vel_stateh_;
+  /// \brief The current effort forces applied to the joints
+  std::vector<hardware_interface::JointHandle> joint_eff_stateh_;
+
+  /// \brief The current position command value (if control mode is POSITION) of the joints
+  std::vector<hardware_interface::JointHandle> joint_pos_cmdh_;
+  /// \brief The current effort command value (if control mode is EFFORT) of the joints
+  std::vector<hardware_interface::JointHandle> joint_eff_cmdh_;
+  /// \brief The current velocity command value (if control mode is VELOCITY) of the joints
+  std::vector<hardware_interface::JointHandle> joint_vel_cmdh_;
+
+  /// \brief The operational mode (active/inactive) state of the joints
+  std::vector<hardware_interface::OperationMode> joint_opmodes_;
+
+  /// \brief operational mode handles of the joints pointing to values in the joint_opmodes_
+  /// collection
   std::vector<hardware_interface::OperationModeHandle> joint_opmodehandles_;
 
-  // limits
-  std::vector<joint_limits_interface::PositionJointSaturationHandle> joint_pos_limit_handles_;
-  std::vector<joint_limits_interface::PositionJointSoftLimitsHandle>
-  joint_pos_soft_limit_handles_;
-  std::vector<joint_limits_interface::EffortJointSaturationHandle> joint_eff_limit_handles_;
-  std::vector<joint_limits_interface::EffortJointSoftLimitsHandle>
-  joint_eff_soft_limit_handles_;
-  std::vector<joint_limits_interface::VelocityJointSaturationHandle> joint_vel_limit_handles_;
-  std::vector<joint_limits_interface::VelocityJointSoftLimitsHandle> joint_vel_soft_limit_handles_;
+  /// \brief Limits for the joints if defined in the URDF or Node parameters
+  /// The implementation of the joint limit will be chosen based on the URDF parameters and could be
+  /// one of the hard (saturation) or soft limits based on the control mode (effort, position or
+  /// velocity)
+  std::vector<std::unique_ptr<joint_limits_interface::JointLimitHandle>> joint_limit_handles_;
 
   std::string physics_type_;
+  bool usingODE;
 
   // e_stop_active_ is true if the emergency stop is active.
   bool e_stop_active_, last_e_stop_active_;
diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp
index 0f92bd15..32c76367 100644
--- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp
+++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp
@@ -32,6 +32,7 @@
 */
 
 #include <limits>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -40,8 +41,6 @@
 
 namespace gazebo_ros2_control
 {
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("default_robot_hw_sim");
-
 bool DefaultRobotHWSim::initSim(
   const std::string & robot_namespace,
   rclcpp::Node::SharedPtr & model_nh,
@@ -51,7 +50,7 @@ bool DefaultRobotHWSim::initSim(
 {
   // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each
   // parameter's name is "joint_limits/<joint name>". An example is "joint_limits/axle_joint".
-  rclcpp::Node::SharedPtr & joint_limit_nh = model_nh;
+  nh_ = model_nh;
 
   // Resize vectors to our DOF
   n_dof_ = transmissions.size();
@@ -62,292 +61,262 @@ bool DefaultRobotHWSim::initSim(
   joint_effort_limits_.resize(n_dof_);
   joint_vel_limits_.resize(n_dof_);
   joint_control_methods_.resize(n_dof_);
-#if 0
-  pid_controllers_.resize(n_dof_);
-#endif
-  joint_position_.resize(n_dof_);
-  joint_velocity_.resize(n_dof_);
-  joint_effort_.resize(n_dof_);
-  joint_effort_command_.resize(n_dof_);
-  joint_position_command_.resize(n_dof_);
-  joint_velocity_command_.resize(n_dof_);
   joint_opmodes_.resize(n_dof_);
-
-  joint_states_.resize(n_dof_);
-  joint_cmds_.resize(n_dof_);
   joint_opmodehandles_.resize(n_dof_);
-  joint_eff_cmdhandle_.resize(n_dof_);
-  joint_vel_cmdhandle_.resize(n_dof_);
 
-  joint_pos_limit_handles_.resize(n_dof_);
-  joint_pos_soft_limit_handles_.resize(n_dof_);
-  joint_eff_limit_handles_.resize(n_dof_);
-  joint_eff_soft_limit_handles_.resize(n_dof_);
-  joint_vel_limit_handles_.resize(n_dof_);
+  joint_pos_stateh_.resize(n_dof_, hardware_interface::JointHandle("position"));
+  joint_vel_stateh_.resize(n_dof_, hardware_interface::JointHandle("velocity"));
+  joint_eff_stateh_.resize(n_dof_, hardware_interface::JointHandle("effort"));
+  joint_pos_cmdh_.resize(n_dof_, hardware_interface::JointHandle("position_command"));
+  joint_eff_cmdh_.resize(n_dof_, hardware_interface::JointHandle("effort_command"));
+  joint_vel_cmdh_.resize(n_dof_, hardware_interface::JointHandle("velocity_command"));
+
+  joint_limit_handles_.resize(n_dof_);
+
+  // get physics engine type
+#if GAZEBO_MAJOR_VERSION >= 8
+  gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics();
+#else
+  gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->GetPhysicsEngine();
+#endif
+  physics_type_ = physics->GetType();
+  if (physics_type_.empty()) {
+    RCLCPP_ERROR(nh_->get_logger(), "No physics engine configured in Gazebo.");
+    return false;
+  }
+
+  // special handing since ODE uses a different set/get interface then the other engines
+  usingODE = (physics_type_.compare("ode") == 0);
 
   // Initialize values
   for (unsigned int j = 0; j < n_dof_; j++) {
+    //
+    // Perform some validation on the URDF joint and actuator spec
+    //
+
     // Check that this transmission has one joint
-    if (transmissions[j].joints_.empty()) {
-      std::cerr << "Transmission " << transmissions[j].name_ <<
-        " has no associated joints.";
+    if (transmissions[j].joints.empty()) {
+      RCLCPP_WARN_STREAM(
+        nh_->get_logger(), "Transmission " << transmissions[j].name <<
+          " has no associated joints.");
       continue;
-    } else if (transmissions[j].joints_.size() > 1) {
-      std::cerr << "Transmission " << transmissions[j].name_ <<
-        " has more than one joint. Currently the default robot hardware simulation " <<
-        " interface only supports one.";
+    } else if (transmissions[j].joints.size() > 1) {
+      RCLCPP_WARN_STREAM(
+        nh_->get_logger(), "Transmission " << transmissions[j].name <<
+          " has more than one joint. Currently the default robot hardware simulation " <<
+          " interface only supports one.");
       continue;
     }
 
-    std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
+    std::string joint_name = joint_names_[j] = transmissions[j].joints[0].name;
+
+    std::vector<std::string> joint_interfaces = transmissions[j].joints[0].interfaces;
     if (joint_interfaces.empty() &&
-      !(transmissions[j].actuators_.empty()) &&
-      !(transmissions[j].actuators_[0].hardware_interfaces_.empty()))
+      !(transmissions[j].actuators.empty()) &&
+      !(transmissions[j].actuators[0].interfaces.empty()))
     {
       // TODO(anyone): Deprecate HW interface specification in actuators in ROS J
-      joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_;
-      std::cerr << "The <hardware_interface> element of tranmission " <<
-        transmissions[j].name_ <<
-        " should be nested inside the <joint> element, not <actuator>. " <<
-        "The transmission will be properly loaded, but please update " <<
-        "your robot model to remain compatible with future versions of the plugin.";
+      joint_interfaces = transmissions[j].actuators[0].interfaces;
+      RCLCPP_WARN_STREAM(
+        nh_->get_logger(), "The <hardware_interface> element of transmission " <<
+          transmissions[j].name << " should be nested inside the <joint> element," <<
+          " not <actuator>. The transmission will be properly loaded, but please update " <<
+          "your robot model to remain compatible with future versions of the plugin.");
     }
     if (joint_interfaces.empty()) {
-      std::cerr << "Joint " << transmissions[j].name_ <<
-        " of transmission " << transmissions[j].name_ <<
-        " does not specify any hardware interface. " <<
-        "Not adding it to the robot hardware simulation.";
+      RCLCPP_WARN_STREAM(
+        nh_->get_logger(), "Joint " << transmissions[j].name <<
+          " of transmission " << transmissions[j].name <<
+          " does not specify any hardware interface. " <<
+          "Not adding it to the robot hardware simulation.");
       continue;
     } else if (joint_interfaces.size() > 1) {
-      std::cerr << "Joint " << transmissions[j].name_ <<
-        " of transmission " << transmissions[j].name_ <<
-        " specifies multiple hardware interfaces. " <<
-        "Currently the default robot hardware simulation interface only supports one. "
-        "Using the first entry";
-      // continue;
+      RCLCPP_WARN_STREAM(
+        nh_->get_logger(), "Joint " << transmissions[j].name <<
+          " of transmission " << transmissions[j].name <<
+          " specifies multiple hardware interfaces. " <<
+          "Currently the default robot hardware simulation interface only supports one." <<
+          "Using the first entry");
+      // only a warning, allow joint to continue
     }
 
-    // Add data from transmission
-    joint_names_[j] = transmissions[j].joints_[0].name_;
-    joint_position_[j] = 1.0;
-    joint_velocity_[j] = 0.0;
-    joint_effort_[j] = 1.0;  // N/m for continuous joints
-    joint_effort_command_[j] = 0.0;
-    joint_position_command_[j] = 0.0;
-    joint_velocity_command_[j] = 0.0;
-
-    const std::string & hardware_interface = joint_interfaces.front();
-
-    // Debug
-    std::cerr << "Loading joint '" << joint_names_[j] <<
-      "' of type '" << hardware_interface << "'" << std::endl;
-
-    // Create joint state interface for all joints
-#if 0  // @todo
-    js_interface_.registerHandle(
-      hardware_interface::JointStateHandle(
-        joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]));
-#endif
-
+    std::string hardware_interface = joint_interfaces.front();
     // Decide what kind of command interface this actuator/joint has
-    hardware_interface::JointStateHandle joint_handle;
-    if (hardware_interface == "EffortJointInterface" ||
-      hardware_interface == "hardware_interface/EffortJointInterface")
-    {
-      RCLCPP_INFO(
-        joint_limit_nh->get_logger(), "joint %s is configured in EFFORT mode",
-        transmissions[j].joints_[0].name_.c_str());
-      // Create effort joint interface
+    if (hardware_interface == "hardware_interface/EffortJointInterface") {
       joint_control_methods_[j] = EFFORT;
-#if 0  // @todo
-      joint_handle = hardware_interface::JointHandle(
-        js_interface_.getHandle(joint_names_[j]),
-        &joint_effort_command_[j]);
-
-      ej_interface_.registerHandle(joint_handle);
-#endif
+    } else if (hardware_interface == "hardware_interface/PositionJointInterface") {
+      joint_control_methods_[j] = POSITION;
+    } else if (hardware_interface == "hardware_interface/VelocityJointInterface") {
+      joint_control_methods_[j] = VELOCITY;
     } else {
-      if (hardware_interface == "PositionJointInterface" ||
-        hardware_interface == "hardware_interface/PositionJointInterface")
+      RCLCPP_WARN_STREAM(
+        nh_->get_logger(), "No matching joint interface '" <<
+          hardware_interface << "' for joint " << joint_name);
+      RCLCPP_INFO(
+        nh_->get_logger(),
+        "    Expecting one of 'hardware_interface/{EffortJointInterface |"
+        " PositionJointInterface | VelocityJointInterface}'");
+      return false;
+    }
+
+    //
+    // Accept this URDF joint as valid and link with the gazebo joint of the same name
+    //
+
+    // I think it's safe to only skip this joint and not abort if there is no match found
+    gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name);
+    if (!simjoint) {
+      RCLCPP_WARN_STREAM(
+        nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name <<
+          "' which is not in the gazebo model.");
+      continue;
+    }
+    sim_joints_.push_back(simjoint);
+
+    // Accept this joint and continue configuration
+    RCLCPP_INFO_STREAM(
+      nh_->get_logger(), "Loading joint '" << joint_name << "' of type '" <<
+        hardware_interface << "'");
+
+    ///
+    /// \brief a helper function for registering joint state or command handles
+    ///
+    auto register_joint_interface = [this, &joint_name, &j](
+      const char * interface_name,
+      std::vector<hardware_interface::JointHandle> & joint_iface_handles)
       {
-        // Create position joint interface
-        joint_control_methods_[j] = POSITION;
-        RCLCPP_INFO(
-          joint_limit_nh->get_logger(), "joint %s is configured in POSITION mode",
-          transmissions[j].joints_[0].name_.c_str());
-#if 0  // @todo
-        joint_handle = hardware_interface::JointHandle(
-          js_interface_.getHandle(joint_names_[j]),
-          &joint_position_command_[j]);
-        pj_interface_.registerHandle(joint_handle);
-#endif
-      } else {
-        if (hardware_interface == "VelocityJointInterface" ||
-          hardware_interface == "hardware_interface/VelocityJointInterface")
+        // set joint name and interface on our stored handle
+        joint_iface_handles[j] = hardware_interface::JointHandle(joint_name, interface_name);
+
+        // register the joint with the underlying RobotHardware implementation
+        if (register_joint(
+            joint_name, interface_name,
+            0.0) != hardware_interface::return_type::OK)
         {
-          // Create velocity joint interface
-          joint_control_methods_[j] = VELOCITY;
-          RCLCPP_INFO(
-            joint_limit_nh->get_logger(), "joint %s is configured in VELOCITY mode",
-            transmissions[j].joints_[0].name_.c_str());
-#if 0  // @todo
-          joint_handle = hardware_interface::JointHandle(
-            js_interface_.getHandle(joint_names_[j]),
-            &joint_velocity_command_[j]);
-          vj_interface_.registerHandle(joint_handle);
-#endif
-        } else {
-          std::cerr << "No matching hardware interface found for '" <<
-            hardware_interface << "' while loading interfaces for " << joint_names_[j] << std::endl;
+          RCLCPP_ERROR_STREAM(
+            nh_->get_logger(), "cant register " << interface_name <<
+              " state handle for joint " << joint_name);
+          return false;
+        }
+
+        // now retrieve the handle with the bound value reference (bound directly to the
+        // DynamicJointState msg in RobotHardware)
+        if (get_joint_handle(joint_iface_handles[j]) != hardware_interface::return_type::OK) {
+          RCLCPP_ERROR_STREAM(
+            nh_->get_logger(), "state handle " << interface_name << " failure for joint " <<
+              joint_name);
           return false;
         }
-      }
-    }
-    if (hardware_interface == "EffortJointInterface" ||
-      hardware_interface == "PositionJointInterface" ||
-      hardware_interface == "VelocityJointInterface")
-    {
-      std::cerr << "Deprecated syntax, please prepend 'hardware_interface/' to '" <<
-        hardware_interface << "' within the <hardwareInterface> tag in joint '" <<
-        joint_names_[j] << "'." << std::endl;
-    }
 
-    // Get the gazebo joint that corresponds to the robot joint.
-    // RCLCPP_DEBUG_STREAM(LOGGER, "Getting pointer to gazebo joint: "
-    //  << joint_names_[j]);
-    gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]);
-    if (!joint) {
-      std::cerr << "This robot has a joint named \"" << joint_names_[j] <<
-        "\" which is not in the gazebo model." << std::endl;
+        // verify handle references a target value
+        if (!joint_iface_handles[j]) {
+          RCLCPP_ERROR_STREAM(
+            nh_->get_logger(), interface_name << " handle for joint " << joint_name <<
+              " is null");
+          return false;
+        }
+        return true;
+      };
+
+    // register the state handles
+    if (
+      !register_joint_interface("position", joint_pos_stateh_) ||
+      !register_joint_interface("velocity", joint_vel_stateh_) ||
+      !register_joint_interface("effort", joint_eff_stateh_))
+    {
+      RCLCPP_ERROR_STREAM(
+        nh_->get_logger(),
+        "plugin aborting due to previous " << joint_name << " joint registration errors");
       return false;
     }
-    sim_joints_.push_back(joint);
-    joint_position_[j] = joint->Position(0);
-    joint_velocity_[j] = joint->GetVelocity(0);
 
-    // get physics engine type
-    gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics();
+    // Register the command handle
+    switch (joint_control_methods_[j]) {
+      case EFFORT: register_joint_interface("effort_command", joint_eff_cmdh_); break;
+      case POSITION: register_joint_interface("position_command", joint_pos_cmdh_); break;
+      case VELOCITY: register_joint_interface("velocity_command", joint_vel_cmdh_); break;
+    }
 
-    physics_type_ = physics->GetType();
-    if (physics_type_.empty()) {
-      std::cerr << "No physics type found." << std::endl;
+    // set joints operation mode to ACTIVE and register handle for controlling opmode
+    joint_opmodes_[j] = hardware_interface::OperationMode::ACTIVE;
+    joint_opmodehandles_[j] = hardware_interface::OperationModeHandle(
+      joint_name, &joint_opmodes_[j]);
+    if (register_operation_mode_handle(&joint_opmodehandles_[j]) !=
+      hardware_interface::return_type::OK)
+    {
+      RCLCPP_WARN_STREAM(nh_->get_logger(), "cant register opmode handle for joint" << joint_name);
     }
+  }
 
+  // since handles references may have changed due to underlying DynamicJointState msg
+  // vectors resizing and reallocating we need to get these handles again
+  // any handles not registered are skipped, such as the command handles if they arent
+  // involved in the control method
+  bindJointHandles(joint_pos_stateh_);
+  bindJointHandles(joint_vel_stateh_);
+  bindJointHandles(joint_eff_stateh_);
+  bindJointHandles(joint_pos_cmdh_);
+  bindJointHandles(joint_vel_cmdh_);
+  bindJointHandles(joint_eff_cmdh_);
+
+
+  //
+  // Complete initialization of limits, PID controllers, etc now that registered handles are bound
+  //
+  for (size_t j = 0; j < joint_names_.size(); j++) {
+    auto simjoint = sim_joints_[j];
+    assert(simjoint);
+
+    // setup joint limits
     registerJointLimits(
-      joint_names_[j], joint_handle, joint_control_methods_[j],
-      joint_limit_nh, urdf_model,
+      j,
+      urdf_model,
       &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j],
       &joint_effort_limits_[j], &joint_vel_limits_[j]);
     if (joint_control_methods_[j] != EFFORT) {
-      // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or
-      // joint->SetParam("vel") to control the joint.
       try {
-        pid_controllers_.push_back(
-          control_toolbox::PidROS(
-            joint_limit_nh,
-            transmissions[j].joints_[0].name_));
-        joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".p");
-        joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".i");
-        joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".d");
-        joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".i_clamp_max");
-        joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".i_clamp_min");
-        joint_limit_nh->declare_parameter(transmissions[j].joints_[0].name_ + ".antiwindup");
-        if (pid_controllers_[j].initPid()) {
-          switch (joint_control_methods_[j]) {
-            case POSITION:
-              joint_control_methods_[j] = POSITION_PID;
-              RCLCPP_INFO(
-                joint_limit_nh->get_logger(), "joint %s is configured in POSITION_PID mode",
-                transmissions[j].joints_[0].name_.c_str());
-              break;
-            case VELOCITY:
-              joint_control_methods_[j] = VELOCITY_PID;
-              RCLCPP_INFO(
-                joint_limit_nh->get_logger(), "joint %s is configured in VELOCITY_PID mode",
-                transmissions[j].joints_[0].name_.c_str());
-              break;
+        nh_->declare_parameter(transmissions[j].joints[0].name + ".p");
+        nh_->declare_parameter(transmissions[j].joints[0].name + ".i");
+        nh_->declare_parameter(transmissions[j].joints[0].name + ".d");
+        nh_->declare_parameter(transmissions[j].joints[0].name + ".i_clamp_max");
+        nh_->declare_parameter(transmissions[j].joints[0].name + ".i_clamp_min");
+        nh_->declare_parameter(transmissions[j].joints[0].name + ".antiwindup", false);
+
+        if (nh_->get_parameter(transmissions[j].joints[0].name + ".p").get_type() ==
+          rclcpp::PARAMETER_DOUBLE &&
+          nh_->get_parameter(transmissions[j].joints[0].name + ".i").get_type() ==
+          rclcpp::PARAMETER_DOUBLE &&
+          nh_->get_parameter(transmissions[j].joints[0].name + ".d").get_type() ==
+          rclcpp::PARAMETER_DOUBLE)
+        {
+          pid_controllers_.push_back(
+            control_toolbox::PidROS(nh_, transmissions[j].joints[0].name));
+          if (pid_controllers_[j].initPid()) {
+            switch (joint_control_methods_[j]) {
+              case POSITION: joint_control_methods_[j] = POSITION_PID;
+                RCLCPP_INFO(
+                  nh_->get_logger(), "joint %s is configured in POSITION_PID mode",
+                  transmissions[j].joints[0].name.c_str());
+                break;
+              case VELOCITY: joint_control_methods_[j] = VELOCITY_PID;
+                RCLCPP_INFO(
+                  nh_->get_logger(), "joint %s is configured in VELOCITY_PID mode",
+                  transmissions[j].joints[0].name.c_str());
+                break;
+            }
           }
         }
       } catch (const std::exception & e) {
         RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "%s", e.what());
       }
-      joint->SetParam("fmax", 0, joint_effort_limits_[j]);
-    }
-    joint_states_[j] = hardware_interface::JointStateHandle(
-      joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]);
-    if (register_joint_state_handle(&joint_states_[j]) != hardware_interface::return_type::OK) {
-      RCLCPP_WARN_ONCE(LOGGER, "Failed to register joint state handle.");
-    }
-
-    joint_cmds_[j] = hardware_interface::JointCommandHandle(
-      joint_names_[j], &joint_position_command_[j]);
-    if (hardware_interface == "PositionJointInterface" ||
-      hardware_interface == "hardware_interface/PositionJointInterface")
-    {
-      if (register_joint_command_handle(&joint_cmds_[j]) != hardware_interface::return_type::OK) {
-        RCLCPP_WARN_ONCE(LOGGER, "Failed to register joint commands.");
-      }
-    }
-
-    joint_opmodehandles_[j] = hardware_interface::OperationModeHandle(
-      joint_names_[j], &joint_opmodes_[j]);
-    if (register_operation_mode_handle(&joint_opmodehandles_[j]) !=
-      hardware_interface::return_type::OK)
-    {
-      RCLCPP_WARN_ONCE(LOGGER, "Failed to register joint_opm ode handles.");
-    }
-
-    joint_limits_interface::JointLimits limits;  // hack, refactor registerjointhandle
-    limits.has_position_limits = true;
-    limits.min_position = joint_lower_limits_[j];
-    limits.max_position = joint_upper_limits_[j];
-    limits.max_velocity = joint_vel_limits_[j];
-    limits.has_velocity_limits = true;
-    limits.max_effort = joint_effort_limits_[j];
-    limits.has_effort_limits = true;
-
-    joint_pos_limit_handles_[j] = joint_limits_interface::PositionJointSaturationHandle(
-      joint_states_[j], joint_cmds_[j], limits);
-
-    joint_eff_cmdhandle_[j] = hardware_interface::JointCommandHandle(
-      joint_names_[j], &joint_effort_command_[j]);
-    // should be register_joint_effort_command_handle, but there's only 1 buffer for now
-    if (hardware_interface == "EffortJointInterface" ||
-      hardware_interface == "hardware_interface/EffortJointInterface")
-    {
-      if (register_joint_command_handle(&joint_eff_cmdhandle_[j]) !=
-        hardware_interface::return_type::OK)
-      {
-        RCLCPP_WARN_ONCE(LOGGER, "Failed to register joint effort command handle.");
-      }
+      simjoint->SetParam("fmax", 0, joint_effort_limits_[j]);
     }
+  }
 
-    joint_eff_limit_handles_[j] = joint_limits_interface::EffortJointSaturationHandle(
-      joint_states_[j], joint_eff_cmdhandle_[j], limits);
+  // get the current state of the sim joint to initialize our ROS control joints
+  // @note(guru-florida): perhaps we dont need this if readSim() is called after init anyway
+  readSim(rclcpp::Time(), rclcpp::Duration(0, 0));
 
-    joint_vel_cmdhandle_[j] = hardware_interface::JointCommandHandle(
-      joint_names_[j], &joint_velocity_command_[j]);
-    if (hardware_interface == "VelocityJointInterface" ||
-      hardware_interface == "hardware_interface/VelocityJointInterface")
-    {
-      if (register_joint_command_handle(&joint_vel_cmdhandle_[j]) !=
-        hardware_interface::return_type::OK)
-      {
-        std::cerr << "Failed to register joint velocity command handle." << std::endl;
-      }
-    }
-
-    joint_vel_limit_handles_[j] = joint_limits_interface::VelocityJointSaturationHandle(
-      joint_states_[j], joint_vel_cmdhandle_[j], limits);
-    // register not implemented
-  }
-#if 0  // @todo
-  // Register interfaces
-  registerInterface(&js_interface_);
-  registerInterface(&ej_interface_);
-  registerInterface(&pj_interface_);
-  registerInterface(&vj_interface_);
-#endif
   // Initialize the emergency stop code.
   e_stop_active_ = false;
   last_e_stop_active_ = false;
@@ -355,20 +324,33 @@ bool DefaultRobotHWSim::initSim(
   return true;
 }
 
-void DefaultRobotHWSim::readSim(rclcpp::Time time, rclcpp::Duration period)
+void DefaultRobotHWSim::readSim(rclcpp::Time, rclcpp::Duration)
 {
-  for (unsigned int j = 0; j < n_dof_; j++) {
-    // Gazebo has an interesting API...
-    double position = sim_joints_[j]->Position(0);
-    if (joint_types_[j] == urdf::Joint::PRISMATIC) {
-      joint_position_[j] = position;
-    } else {
-      joint_position_[j] += angles::shortest_angular_distance(
-        joint_position_[j],
-        position);
+  for (unsigned int j = 0; j < joint_names_.size(); j++) {
+    auto joint_handle = std::make_shared<hardware_interface::JointHandle>(
+      joint_names_[j], "position");
+    if (get_joint_handle(*joint_handle) == hardware_interface::return_type::OK) {
+      double position = sim_joints_[j]->Position(0);
+      if (joint_types_[j] == urdf::Joint::PRISMATIC) {
+        joint_handle->set_value(position);
+      } else {
+        double prev_position = joint_handle->get_value();
+        joint_handle->set_value(
+          prev_position +
+          angles::shortest_angular_distance(prev_position, position));
+      }
+    }
+    joint_handle = std::make_shared<hardware_interface::JointHandle>(
+      joint_names_[j], "velocity");
+    if (get_joint_handle(*joint_handle) == hardware_interface::return_type::OK) {
+      joint_handle->set_value(sim_joints_[j]->GetVelocity(0));
+    }
+
+    joint_handle = std::make_shared<hardware_interface::JointHandle>(
+      joint_names_[j], "effort");
+    if (get_joint_handle(*joint_handle) == hardware_interface::return_type::OK) {
+      joint_handle->set_value(sim_joints_[j]->GetForce((unsigned int)(0)));
     }
-    joint_velocity_[j] = sim_joints_[j]->GetVelocity(0);
-    joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0));
   }
 }
 
@@ -377,43 +359,35 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period)
   // If the E-stop is active, joints controlled by position commands will maintain their positions.
   if (e_stop_active_) {
     if (!last_e_stop_active_) {
-      last_joint_position_command_ = joint_position_;
+      last_joint_position_command_.clear();
+      std::transform(
+        joint_pos_stateh_.begin(), joint_pos_stateh_.end(),
+        std::back_inserter(last_joint_position_command_),
+        [](const hardware_interface::JointHandle & ph) {return ph.get_value();});
       last_e_stop_active_ = true;
     }
-    joint_position_command_ = last_joint_position_command_;
+    for (int i = 0; i < n_dof_; i++) {
+      joint_pos_cmdh_[i].set_value(last_joint_position_command_[i]);
+    }
   } else {
     last_e_stop_active_ = false;
   }
-#if 0  // @todo
-  ej_sat_interface_.enforceLimits(period);
-  ej_limits_interface_.enforceLimits(period);
-  pj_sat_interface_.enforceLimits(period);
-  pj_limits_interface_.enforceLimits(period);
-  vj_sat_interface_.enforceLimits(period);
-  vj_limits_interface_.enforceLimits(period);
-#else
-  for (auto eff_limit_handle : joint_eff_limit_handles_) {
-    eff_limit_handle.enforceLimits(period);
-  }
-  for (auto pos_limit_handle : joint_pos_limit_handles_) {
-    pos_limit_handle.enforceLimits(period);
-  }
-  for (auto vel_limit_handle : joint_vel_limit_handles_) {
-    vel_limit_handle.enforceLimits(period);
+  for (auto & limit_handle : joint_limit_handles_) {
+    if (limit_handle) {
+      limit_handle->enforce_limits(period);
+    }
   }
-
-#endif
   for (unsigned int j = 0; j < n_dof_; j++) {
     switch (joint_control_methods_[j]) {
       case EFFORT:
         {
-          const double effort = e_stop_active_ ? 0 : joint_effort_command_[j];
+          const double effort = e_stop_active_ ? 0 : joint_eff_cmdh_[j].get_value();
           sim_joints_[j]->SetForce(0, effort);
         }
         break;
 
       case POSITION:
-        sim_joints_[j]->SetPosition(0, joint_position_command_[j], true);
+        sim_joints_[j]->SetPosition(0, joint_pos_cmdh_[j].get_value(), true);
         break;
 
       case POSITION_PID:
@@ -422,47 +396,49 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period)
           switch (joint_types_[j]) {
             case urdf::Joint::REVOLUTE:
               angles::shortest_angular_distance_with_limits(
-                joint_position_[j],
-                joint_position_command_[j],
+                joint_pos_stateh_[j].get_value(),
+                joint_pos_cmdh_[j].get_value(),
                 joint_lower_limits_[j],
                 joint_upper_limits_[j],
                 error);
               break;
             case urdf::Joint::CONTINUOUS:
               error = angles::shortest_angular_distance(
-                joint_position_[j],
-                joint_position_command_[j]);
+                joint_pos_stateh_[j].get_value(),
+                joint_pos_cmdh_[j].get_value());
               break;
             default:
-              error = joint_position_command_[j] - joint_position_[j];
+              error = joint_pos_cmdh_[j].get_value() - joint_pos_stateh_[j].get_value();
           }
 
           const double effort_limit = joint_effort_limits_[j];
-          double effort = ignition::math::clamp(
+
+          const double effort = ignition::math::clamp(
             pid_controllers_[j].computeCommand(error, period),
             -effort_limit, effort_limit);
+
           sim_joints_[j]->SetForce(0, effort);
           sim_joints_[j]->SetParam("friction", 0, 0.0);
         }
         break;
 
       case VELOCITY:
-        if (physics_type_.compare("ode") == 0) {
-          sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
+        if (usingODE) {
+          sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_vel_cmdh_[j].get_value());
         } else {
-          sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
+          sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_vel_cmdh_[j].get_value());
         }
         break;
 
       case VELOCITY_PID:
         double error;
         if (e_stop_active_) {
-          error = -joint_velocity_[j];
+          error = -joint_vel_stateh_[j].get_value();
         } else {
-          error = joint_velocity_command_[j] - joint_velocity_[j];
+          error = joint_vel_cmdh_[j].get_value() - joint_vel_stateh_[j].get_value();
         }
         const double effort_limit = joint_effort_limits_[j];
-        double effort = ignition::math::clamp(
+        const double effort = ignition::math::clamp(
           pid_controllers_[j].computeCommand(error, period),
           -effort_limit, effort_limit);
         sim_joints_[j]->SetForce(0, effort);
@@ -480,15 +456,15 @@ void DefaultRobotHWSim::eStopActive(const bool active)
 // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also.
 // Return the joint's type, lower position limit, upper position limit, and effort limit.
 void DefaultRobotHWSim::registerJointLimits(
-  const std::string & joint_name,
-  const hardware_interface::JointStateHandle & joint_handle,
-  const ControlMethod ctrl_method,
-  const rclcpp::Node::SharedPtr & joint_limit_nh,
+  size_t joint_nr,
   const urdf::Model * const urdf_model,
   int * const joint_type, double * const lower_limit,
   double * const upper_limit, double * const effort_limit,
   double * const vel_limit)
 {
+  const std::string & joint_name = joint_names_[joint_nr];
+  const ControlMethod ctrl_method = joint_control_methods_[joint_nr];
+
   *joint_type = urdf::Joint::UNKNOWN;
   *lower_limit = -std::numeric_limits<double>::max();
   *upper_limit = std::numeric_limits<double>::max();
@@ -505,17 +481,16 @@ void DefaultRobotHWSim::registerJointLimits(
     if (urdf_joint != NULL) {
       *joint_type = urdf_joint->type;
       // Get limits from the URDF file.
-      if (joint_limits_interface::getJointLimits(urdf_joint->name, joint_limit_nh, limits)) {
+      if (joint_limits_interface::getJointLimits(urdf_joint->name, nh_, limits)) {
         has_limits = true;
       }
-      if (joint_limits_interface::getSoftJointLimits(
-          urdf_joint->name, joint_limit_nh,
-          soft_limits))
-      {
+      if (joint_limits_interface::getSoftJointLimits(urdf_joint->name, nh_, soft_limits)) {
         has_soft_limits = true;
       }
 
       // @note (ddeng): these joint limits arent input into the node, so fetch them from the urdf
+      // TODO(guru-florida): These are now a part of the node as of 2020-09-03,
+      // so what one do we take?
       limits.min_position = urdf_joint->limits->lower;
       limits.max_position = urdf_joint->limits->upper;
       limits.has_position_limits = true;
@@ -531,14 +506,11 @@ void DefaultRobotHWSim::registerJointLimits(
       *effort_limit = limits.max_effort;
       *vel_limit = limits.max_velocity;
 
-      // urdf_joint->safety->k_position;
       has_limits = true;
     }
   }
 
-  // Get limits from the parameter server.
-  // @note: no longer using parameter servers
-  if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits)) {
+  if (joint_limits_interface::getJointLimits(joint_name, nh_, limits)) {
     has_limits = true;
   }
 
@@ -572,29 +544,24 @@ void DefaultRobotHWSim::registerJointLimits(
     switch (ctrl_method) {
       case EFFORT:
         {
-#if 0  // @todo
-          const joint_limits_interface::EffortJointSoftLimitsHandle
-            limits_handle(joint_handle, limits, soft_limits);
-          ej_limits_interface_.registerHandle(limits_handle);
-#endif
+          joint_limit_handles_[joint_nr] =
+            std::make_unique<joint_limits_interface::EffortJointSaturationHandle>(
+            joint_pos_cmdh_[joint_nr], joint_vel_cmdh_[joint_nr], joint_eff_cmdh_[joint_nr],
+            limits);
         }
         break;
       case POSITION:
         {
-#if 0  // @todo
-          const joint_limits_interface::PositionJointSoftLimitsHandle
-            limits_handle(joint_handle, limits, soft_limits);
-          pj_limits_interface_.registerHandle(limits_handle);
-#endif
+          joint_limit_handles_[joint_nr] =
+            std::make_unique<joint_limits_interface::PositionJointSoftLimitsHandle>(
+            joint_pos_stateh_[joint_nr], joint_pos_cmdh_[joint_nr], limits, soft_limits);
         }
         break;
       case VELOCITY:
         {
-#if 0  // @todo
-          const joint_limits_interface::VelocityJointSoftLimitsHandle
-            limits_handle(joint_handle, limits, soft_limits);
-          vj_limits_interface_.registerHandle(limits_handle);
-#endif
+          joint_limit_handles_[joint_nr] =
+            std::make_unique<joint_limits_interface::VelocityJointSaturationHandle>(
+            joint_vel_stateh_[joint_nr], joint_vel_cmdh_[joint_nr], limits);
         }
         break;
     }
@@ -602,37 +569,56 @@ void DefaultRobotHWSim::registerJointLimits(
     switch (ctrl_method) {
       case EFFORT:
         {
-#if 0
-          const joint_limits_interface::EffortJointSaturationHandle
-            sat_handle(joint_handle, limits);
-
-          ej_sat_interface_.registerHandle(sat_handle);
-#endif
+          joint_limit_handles_[joint_nr] =
+            std::make_unique<joint_limits_interface::EffortJointSaturationHandle>(
+            joint_pos_cmdh_[joint_nr], joint_vel_cmdh_[joint_nr], joint_eff_cmdh_[joint_nr],
+            limits);
         }
         break;
       case POSITION:
         {
-#if 0  // @todo
-          const joint_limits_interface::PositionJointSaturationHandle
-            sat_handle(joint_handle, limits);
-
-          pj_sat_interface_.registerHandle(sat_handle);
-#endif
+          joint_limit_handles_[joint_nr] =
+            std::make_unique<joint_limits_interface::PositionJointSaturationHandle>(
+            joint_pos_stateh_[joint_nr], joint_pos_cmdh_[joint_nr], limits);
         }
         break;
       case VELOCITY:
         {
-#if 0  // @todo
-          const joint_limits_interface::VelocityJointSaturationHandle
-            sat_handle(joint_handle, limits);
-          vj_sat_interface_.registerHandle(sat_handle);
-#endif
+          joint_limit_handles_[joint_nr] =
+            std::make_unique<joint_limits_interface::VelocityJointSaturationHandle>(
+            joint_vel_stateh_[joint_nr], joint_vel_cmdh_[joint_nr], limits);
         }
         break;
     }
   }
 }
 
+void DefaultRobotHWSim::bindJointHandles(
+  std::vector<hardware_interface::JointHandle> & joint_iface_handles)
+{
+  for (auto & jh : joint_iface_handles) {
+    // some handles, especially command handles, may not be registered so skip these
+    if (jh.get_name().empty() || jh.get_interface_name().empty()) {
+      continue;
+    }
+
+    // now retrieve the handle with the bound value reference (bound directly to the
+    // DynamicJointState msg in RobotHardware)
+    if (get_joint_handle(jh) != hardware_interface::return_type::OK) {
+      RCLCPP_ERROR_STREAM(
+        nh_->get_logger(), "state handle " << jh.get_interface_name() << " failure for joint " <<
+          jh.get_name());
+      continue;
+    }
+
+    // verify handle references a target value
+    if (!jh) {
+      RCLCPP_ERROR_STREAM(
+        nh_->get_logger(), jh.get_interface_name() << " handle for joint " << jh.get_name() <<
+          " is null");
+    }
+  }
+}
 }  // namespace gazebo_ros2_control
 
 PLUGINLIB_EXPORT_CLASS(gazebo_ros2_control::DefaultRobotHWSim, gazebo_ros2_control::RobotHWSim)
diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
index af535225..1c27e8b9 100644
--- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
+++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
@@ -60,9 +60,6 @@ class GazeboRosControlPrivate
   // Get the URDF XML from the parameter server
   std::string getURDF(std::string param_name) const;
 
-  // Get Transmissions from the URDF
-  bool parseTransmissionsFromURDF(const std::string & urdf_string);
-
   void eStopCB(const std::shared_ptr<std_msgs::msg::Bool> e_stop_active);
 
   // Node Handles
@@ -236,11 +233,14 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
   // Read urdf from ros parameter server then
   // setup actuators and mechanism control node.
   // This call will block if ROS is not properly initialized.
-  const std::string urdf_string = impl_->getURDF(impl_->robot_description_);
-  if (!impl_->parseTransmissionsFromURDF(urdf_string)) {
-    RCLCPP_ERROR(
+  std::string urdf_string;
+  try {
+    urdf_string = impl_->getURDF(impl_->robot_description_);
+    impl_->transmissions_ = transmission_interface::parse_transmissions_from_urdf(urdf_string);
+  } catch (const std::runtime_error & ex) {
+    RCLCPP_ERROR_STREAM(
       impl_->model_nh_->get_logger(),
-      "Error parsing URDF in gazebo_ros2_control plugin, plugin not active.\n");
+      "Error parsing URDF in gazebo_ros2_control plugin, plugin not active : " << ex.what());
     return;
   }
 
@@ -276,8 +276,9 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
       new controller_manager::ControllerManager(
         impl_->robot_hw_sim_, impl_->executor_,
         "gazebo_controller_manager"));
-#if 1
-    // @todo:Coded example here. should disable when spawn functionality of controller manager is up
+
+    // TODO(anyone): Coded example here. should disable when spawn functionality of
+    // controller manager is up
     auto load_params_from_yaml_node = [](rclcpp_lifecycle::LifecycleNode::SharedPtr lc_node,
         YAML::Node & yaml_node, const std::string & prefix)
       {
@@ -313,7 +314,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
               } else {
                 node->set_parameter(rclcpp::Parameter(key, val_str));
               }
-
               return;
             } else if (yaml_node.Type() == YAML::NodeType::Map) {
               for (auto yaml_node_it : yaml_node) {
@@ -339,7 +339,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
                     node->declare_parameter(key);
                   }
                   node->set_parameter({rclcpp::Parameter(key, val)});
-
                   if (key == "joints") {
                     if (!node->has_parameter("write_op_modes")) {
                       node->declare_parameter("write_op_modes");
@@ -370,7 +369,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
         if (yaml_config_file.empty()) {
           throw std::runtime_error("yaml config file path is empty");
         }
-
         YAML::Node root_node = YAML::LoadFile(yaml_config_file);
         for (auto yaml : root_node) {
           auto nodename = yaml.first.as<std::string>();
@@ -381,6 +379,10 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
         }
       };
 
+    // Start controller, will take effect at the end of the update function
+    std::vector<std::string> start_controllers = {};
+    std::vector<std::string> stop_controllers = {};
+
     YAML::Node root_node = YAML::LoadFile(impl_->param_file_);
     for (auto yaml : root_node) {
       auto controller_name = yaml.first.as<std::string>();
@@ -397,10 +399,23 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
               controller->get_lifecycle_node(),
               impl_->param_file_,
               controller_name);
+            if (controller_name != "joint_state_controller") {
+              controller->get_lifecycle_node()->configure();
+            }
+            start_controllers.push_back(controller_name);
           }
         }
       }
     }
+    auto switch_future = std::async(
+      std::launch::async,
+      &controller_manager::ControllerManager::switch_controller, impl_->controller_manager_,
+      start_controllers, stop_controllers,
+      2, true, rclcpp::Duration(0, 0));  // STRICT_: 2
+    while (std::future_status::timeout == switch_future.wait_for(std::chrono::milliseconds(100))) {
+      impl_->controller_manager_->update();
+    }
+    switch_future.get();
 
     auto spin = [this]()
       {
@@ -410,17 +425,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
       };
     impl_->thread_executor_spin_ = std::thread(spin);
 
-    if (impl_->controller_manager_->configure() !=
-      controller_interface::return_type::SUCCESS)
-    {
-      RCLCPP_ERROR(impl_->model_nh_->get_logger(), "cm failed to configure");
-    }
-    if (impl_->controller_manager_->activate() !=
-      controller_interface::return_type::SUCCESS)
-    {
-      RCLCPP_ERROR(impl_->model_nh_->get_logger(), "cm failed to activate");
-    }
-#endif
     // Listen to the update event. This event is broadcast every simulation iteration.
     impl_->update_connection_ =
       gazebo::event::Events::ConnectWorldUpdateBegin(
@@ -543,13 +547,6 @@ std::string GazeboRosControlPrivate::getURDF(std::string param_name) const
   return urdf_string;
 }
 
-// Get Transmissions from the URDF
-bool GazeboRosControlPrivate::parseTransmissionsFromURDF(const std::string & urdf_string)
-{
-  transmission_interface::TransmissionParser::parse(urdf_string, transmissions_);
-  return true;
-}
-
 // Emergency stop callback
 void GazeboRosControlPrivate::eStopCB(const std::shared_ptr<std_msgs::msg::Bool> e_stop_active)
 {
diff --git a/gazebo_ros2_control_demos/CMakeLists.txt b/gazebo_ros2_control_demos/CMakeLists.txt
index 34bfa83f..27a49626 100644
--- a/gazebo_ros2_control_demos/CMakeLists.txt
+++ b/gazebo_ros2_control_demos/CMakeLists.txt
@@ -18,6 +18,7 @@ find_package(ament_cmake REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
+find_package(std_msgs REQUIRED)
 
 install(DIRECTORY
   launch
@@ -36,8 +37,7 @@ ament_target_dependencies(example_position
 add_executable(example_velocity examples/example_velocity.cpp)
 ament_target_dependencies(example_velocity
   rclcpp
-  rclcpp_action
-  control_msgs
+  std_msgs
 )
 
 if(BUILD_TESTING)
diff --git a/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml b/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml
new file mode 100644
index 00000000..03f7ac24
--- /dev/null
+++ b/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml
@@ -0,0 +1,17 @@
+cart_pole_controller:
+  ros__parameters:
+    type: velocity_controllers/JointGroupVelocityController
+    joints:
+       - slider_to_cart
+    write_op_modes:
+       - slider_to_cart
+    state_publish_rate: 25
+    action_monitor_rate: 20
+    constraints:
+      stopped_velocity_tolerance: 0.05
+      goal_time: 5
+
+joint_state_controller:
+  ros__parameters:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50
diff --git a/gazebo_ros2_control_demos/examples/example_velocity.cpp b/gazebo_ros2_control_demos/examples/example_velocity.cpp
index 528af503..42f3e9f5 100644
--- a/gazebo_ros2_control_demos/examples/example_velocity.cpp
+++ b/gazebo_ros2_control_demos/examples/example_velocity.cpp
@@ -17,171 +17,40 @@
 #include <vector>
 
 #include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
 
-#include "control_msgs/action/follow_joint_trajectory.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
 
 std::shared_ptr<rclcpp::Node> node;
-bool common_goal_accepted = false;
-rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN;
-int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL;
-
-void common_goal_response(
-  std::shared_future<rclcpp_action::ClientGoalHandle
-  <control_msgs::action::FollowJointTrajectory>::SharedPtr> future)
-{
-  RCLCPP_DEBUG(
-    node->get_logger(), "common_goal_response time: %f",
-    rclcpp::Clock().now().seconds());
-  auto goal_handle = future.get();
-  if (!goal_handle) {
-    common_goal_accepted = false;
-    printf("Goal rejected\n");
-  } else {
-    common_goal_accepted = true;
-    printf("Goal accepted\n");
-  }
-}
-
-void common_result_response(
-  const rclcpp_action::ClientGoalHandle
-  <control_msgs::action::FollowJointTrajectory>::WrappedResult & result)
-{
-  printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds());
-  common_resultcode = result.code;
-  common_action_result_code = result.result->error_code;
-  switch (result.code) {
-    case rclcpp_action::ResultCode::SUCCEEDED:
-      printf("SUCCEEDED result code\n");
-      break;
-    case rclcpp_action::ResultCode::ABORTED:
-      printf("Goal was aborted\n");
-      return;
-    case rclcpp_action::ResultCode::CANCELED:
-      printf("Goal was canceled\n");
-      return;
-    default:
-      printf("Unknown result code\n");
-      return;
-  }
-}
-
-void common_feedback(
-  rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr,
-  const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback> feedback)
-{
-  std::cout << "feedback->desired.positions :";
-  for (auto & x : feedback->desired.positions) {
-    std::cout << x << "\t";
-  }
-  std::cout << std::endl;
-  std::cout << "feedback->desired.velocities :";
-  for (auto & x : feedback->desired.velocities) {
-    std::cout << x << "\t";
-  }
-  std::cout << std::endl;
-  std::cout << std::endl;
-}
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
 
-  node = std::make_shared<rclcpp::Node>("trajectory_test_node");
-
-  std::cout << "node created" << std::endl;
-
-  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
-  action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
-    node->get_node_base_interface(),
-    node->get_node_graph_interface(),
-    node->get_node_logging_interface(),
-    node->get_node_waitables_interface(),
-    "/cart_pole_controller/follow_joint_trajectory");
-
-  bool response =
-    action_client->wait_for_action_server(std::chrono::seconds(1));
-  if (!response) {
-    throw std::runtime_error("could not get action server");
-  }
-  std::cout << "Created action server" << std::endl;
-
-  std::vector<std::string> joint_names = {"slider_to_cart"};
-
-  std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
-  trajectory_msgs::msg::JointTrajectoryPoint point;
-  point.time_from_start = rclcpp::Duration::from_seconds(0.0);  // start asap
-  point.positions.resize(joint_names.size());
-  point.velocities.resize(joint_names.size());
-
-  point.positions[0] = 0.0;
-  point.velocities[0] = 0.0;
-
-  trajectory_msgs::msg::JointTrajectoryPoint point2;
-  point2.time_from_start = rclcpp::Duration::from_seconds(1.0);
-  point2.positions.resize(joint_names.size());
-  point2.velocities.resize(joint_names.size());
-  point2.positions[0] = -1.0;
-  point2.velocities[0] = -1.0;
-
-  trajectory_msgs::msg::JointTrajectoryPoint point3;
-  point3.time_from_start = rclcpp::Duration::from_seconds(2.0);
-  point3.positions.resize(joint_names.size());
-  point3.velocities.resize(joint_names.size());
-  point3.positions[0] = 1.0;
-  point3.velocities[0] = 1.0;
-
-  trajectory_msgs::msg::JointTrajectoryPoint point4;
-  point4.time_from_start = rclcpp::Duration::from_seconds(3.0);
-  point4.positions.resize(joint_names.size());
-  point4.velocities.resize(joint_names.size());
-  point4.positions[0] = 0.0;
-  point4.velocities[0] = 0.0;
+  node = std::make_shared<rclcpp::Node>("velocity_test_node");
 
-  points.push_back(point);
-  points.push_back(point2);
-  points.push_back(point3);
-  points.push_back(point4);
+  auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>("/commands", 10);
 
-  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt;
-  opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1);
-  opt.result_callback = std::bind(common_result_response, std::placeholders::_1);
-  opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2);
+  RCLCPP_INFO(node->get_logger(), "node created");
 
-  control_msgs::action::FollowJointTrajectory_Goal goal_msg;
-  goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
-  goal_msg.trajectory.joint_names = joint_names;
-  goal_msg.trajectory.points = points;
+  std_msgs::msg::Float64MultiArray commands;
 
-  auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);
+  using namespace std::chrono_literals;
 
-  if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=
-    rclcpp::FutureReturnCode::SUCCESS)
-  {
-    RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");
-    return 1;
-  }
-  RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");
+  commands.data.push_back(0);
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
 
-  rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr
-    goal_handle = goal_handle_future.get();
-  if (!goal_handle) {
-    RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
-    return 1;
-  }
-  RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server");
+  commands.data[0] = 1;
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
 
-  // Wait for the server to be done with the goal
-  auto result_future = action_client->async_get_result(goal_handle);
-  RCLCPP_INFO(node->get_logger(), "Waiting for result");
-  if (rclcpp::spin_until_future_complete(node, result_future) !=
-    rclcpp::FutureReturnCode::SUCCESS)
-  {
-    RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
-    return 1;
-  }
+  commands.data[0] = -1;
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
 
-  std::cout << "async_send_goal" << std::endl;
+  commands.data[0] = 0;
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
   rclcpp::shutdown();
 
   return 0;
diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml
index b74c7ed1..49a0e780 100644
--- a/gazebo_ros2_control_demos/package.xml
+++ b/gazebo_ros2_control_demos/package.xml
@@ -19,6 +19,7 @@
   <build_depend>control_msgs</build_depend>
   <build_depend>rclcpp</build_depend>
   <build_depend>rclcpp_action</build_depend>
+  <build_depend>std_msgs</build_depend>
 
   <exec_depend>ament_index_python</exec_depend>
   <exec_depend>control_msgs</exec_depend>
@@ -31,7 +32,9 @@
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
   <exec_depend>transmission_interface</exec_depend>
+  <exec_depend>velocity_controllers</exec_depend>
   <exec_depend>xacro</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
diff --git a/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf
index 490a1f7c..854cf6ba 100644
--- a/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf
+++ b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf
@@ -61,7 +61,7 @@
   <gazebo>
     <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
       <robot_sim_type>gazebo_ros2_control/DefaultRobotHWSim</robot_sim_type>
-      <parameters>$(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml</parameters>
+      <parameters>$(find gazebo_ros2_control_demos)/config/cartpole_controller_velocity.yaml</parameters>
     </plugin>
   </gazebo>
 </robot>