diff --git a/joint_controller_ros2_control/CMakeLists.txt b/joint_controller_ros2_control/CMakeLists.txt
index 8fd7897..3f1e224 100644
--- a/joint_controller_ros2_control/CMakeLists.txt
+++ b/joint_controller_ros2_control/CMakeLists.txt
@@ -99,6 +99,14 @@ install(TARGETS
   LIBRARY DESTINATION lib
 )
 
+install(DIRECTORY 
+  test/urdf
+  test/bringup/launch
+  test/bringup/config
+  test/worlds 
+  DESTINATION share/${PROJECT_NAME}
+)
+
 ament_export_targets(
   export_${PROJECT_NAME} HAS_LIBRARY_TARGET
 ) 
diff --git a/joint_controller_ros2_control/src/joint_controller.cpp b/joint_controller_ros2_control/src/joint_controller.cpp
index 5f0bbd3..13af49c 100644
--- a/joint_controller_ros2_control/src/joint_controller.cpp
+++ b/joint_controller_ros2_control/src/joint_controller.cpp
@@ -62,12 +62,15 @@ controller_interface::CallbackReturn JointController::on_cleanup(
 controller_interface::CallbackReturn JointController::on_configure(
       const rclcpp_lifecycle::State & previous_state)
 {
+    
     auto ret = configure_joints();
     if (ret != CallbackReturn::SUCCESS)
     {
         return ret;
     }
 
+    //reference_interfaces_.resize(joint_num_ * 5, std::numeric_limits<double>::quiet_NaN());
+
     auto subscribers_qos = rclcpp::SystemDefaultsQoS();
     subscribers_qos.keep_last(1);
     subscribers_qos.best_effort();
@@ -296,7 +299,7 @@ std::vector<hardware_interface::CommandInterface> JointController::on_export_ref
 
     for (const auto & interface : params_.reference_interfaces)
     {
-        for (size_t i; i < joint_num_; ++i)
+        for (size_t i = 0; i < joint_num_; ++i)
         {
             const auto& joint_name = joint_names_[i];
             if(interface == hardware_interface::HW_IF_POSITION)
@@ -331,6 +334,7 @@ std::vector<hardware_interface::CommandInterface> JointController::on_export_ref
             }
         }
     }
+    reference_interfaces_.resize(joint_names_.size() * params_.reference_interfaces.size(), std::numeric_limits<double>::quiet_NaN());
     return reference_interfaces;
 }
 #ifdef ROS2_CONTROL_VER_3
@@ -372,6 +376,7 @@ controller_interface::CallbackReturn JointController::get_state_interfaces()
             const auto& joint_name = state_interface_iterator->get_prefix_name();
             auto interface_name = state_interface_iterator->get_interface_name();
             size_t index = std::distance(state_interfaces_.begin(), state_interface_iterator);
+            std::cout << joint_name << "/" << interface_name << ": " << index << std::endl;
             if(joint_names_[i] == joint_name)
             {
                 if(interface_name == hardware_interface::HW_IF_POSITION)
@@ -380,7 +385,7 @@ controller_interface::CallbackReturn JointController::get_state_interfaces()
                 }
                 else if(interface_name == hardware_interface::HW_IF_VELOCITY)
                 {
-                    position_interface_indexes.push_back(index);
+                    velocity_interface_indexes.push_back(index);
                 }
             }
         }
@@ -398,14 +403,14 @@ controller_interface::CallbackReturn JointController::get_state_interfaces()
     if(position_state_interfaces_.size() != joint_num_)
     {
         RCLCPP_WARN(get_node()->get_logger(),
-            "Size of position state inetrface map is (%zu), but should be (%zu)",
+            "Size of position state inetrface vector is (%zu), but should be (%zu)",
             effort_command_interfaces_.size(), joint_num_);
         return controller_interface::CallbackReturn::ERROR;
     }
     if(velocity_state_interfaces_.size() != joint_num_)
     {
         RCLCPP_WARN(get_node()->get_logger(),
-            "Size of position state inetrface map is (%zu), but should be (%zu)",
+            "Size of position state inetrface vector is (%zu), but should be (%zu)",
             effort_command_interfaces_.size(), joint_num_);
         return controller_interface::CallbackReturn::ERROR;
     }
diff --git a/joint_controller_ros2_control/src/joint_controller_parameters.yaml b/joint_controller_ros2_control/src/joint_controller_parameters.yaml
index f3617e8..ad5bb82 100644
--- a/joint_controller_ros2_control/src/joint_controller_parameters.yaml
+++ b/joint_controller_ros2_control/src/joint_controller_parameters.yaml
@@ -1,4 +1,12 @@
 joint_controller:
+  frequency: {
+    type: double,
+    default_value: 1.0,
+    description: "Frequency of pid controller.",
+    validation: {
+      gt<>: 0.0,
+    }
+  }
   joint_names: {
     type: string_array,
     default_value: [],
@@ -42,15 +50,6 @@ joint_controller:
           gt_eq<>: 0.0,
         }
       }
-    
-  frequency: {
-    type: double,
-    default_value: 1.0,
-    description: "Frequency of pid controller.",
-    validation: {
-      gt<>: 0.0,
-    }
-  }
   command_interface: {
     type: string,
     default_value: "effort",
diff --git a/joint_controller_ros2_control/test/bringup/config/single_joint_test.yaml b/joint_controller_ros2_control/test/bringup/config/single_joint_test.yaml
new file mode 100644
index 0000000..ec0475a
--- /dev/null
+++ b/joint_controller_ros2_control/test/bringup/config/single_joint_test.yaml
@@ -0,0 +1,32 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    joint_controller:
+      type: joint_controller/JointController
+
+
+joint_controller:
+  ros__parameters:
+    joint_names:
+      - body_1_joint
+
+    joint_params:
+      body_1_joint: 
+        position_max: 3.14
+        position_min: -3.14
+        position_offset: 0.0
+        velocity_max: 10.0
+        effort_max: 10.0
+       
+    frequency: 500.0
+
+    pid_gains:
+      body_1_joint:
+        p: 1.0
+        d: 0.0
+        i: 0.0
+        ilimit: 1.0
diff --git a/joint_controller_ros2_control/test/bringup/launch/rviz.launch.py b/joint_controller_ros2_control/test/bringup/launch/rviz.launch.py
new file mode 100644
index 0000000..d830fde
--- /dev/null
+++ b/joint_controller_ros2_control/test/bringup/launch/rviz.launch.py
@@ -0,0 +1,96 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gui",
+            default_value="true",
+            description="Start RViz2 automatically with this launch file.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "package",
+            default_value="joint_controller",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "urdf_file",
+            default_value="single_joint_test.urdf.xacro",
+        )
+    )
+
+
+    # Initialize Arguments
+    package_name = LaunchConfiguration("package")
+    urdf_file = LaunchConfiguration("urdf_file")
+
+    # Get URDF via xacro
+    robot_description_content = ParameterValue(
+        Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare(package_name),
+                    "urdf",
+                    urdf_file,
+                ]
+            ),
+        ]
+    ), value_type=str)
+    robot_description = {"robot_description": robot_description_content}
+
+    # rviz_config_file = PathJoinSubstitution(
+    #     [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"]
+    # )
+
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    joint_state_publisher_gui = Node(
+        package = "joint_state_publisher_gui",
+        executable = "joint_state_publisher_gui",
+    )
+
+    # Initialize Arguments
+    gui = LaunchConfiguration("gui")
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        # arguments=["-d", rviz_config_file],
+        condition=IfCondition(gui),
+    )
+
+
+
+    nodes = [
+        robot_state_pub_node,
+        joint_state_publisher_gui, 
+        rviz_node 
+    ]
+
+    return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
diff --git a/joint_controller_ros2_control/test/bringup/launch/single_joint_test.launch.py b/joint_controller_ros2_control/test/bringup/launch/single_joint_test.launch.py
new file mode 100644
index 0000000..034f8b5
--- /dev/null
+++ b/joint_controller_ros2_control/test/bringup/launch/single_joint_test.launch.py
@@ -0,0 +1,162 @@
+# Copyright 2020 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, ExecuteProcess
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit, OnProcessStart
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch_ros.parameter_descriptions import ParameterValue
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    # Declare arguments
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "package",
+            default_value="joint_controller",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "urdf_file",
+            default_value="single_joint_test.urdf.xacro",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controller_type_file",
+            default_value="single_joint_test.yaml",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controller_type",
+            default_value="joint_controller",
+        )
+    )
+
+    # Initialize Arguments
+    package_name = LaunchConfiguration("package")
+    urdf_file = LaunchConfiguration("urdf_file")
+    controller_file = LaunchConfiguration("controller_type_file")
+    controller_type = LaunchConfiguration("controller_type")
+
+
+    # Get URDF via xacro
+    robot_description_content = ParameterValue(
+        Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare(package_name),
+                    "urdf",
+                    urdf_file,
+                ]
+            ),
+        ]
+    ), value_type=str)
+
+    # Set up dictionary parameters:
+    robot_description = {"robot_description": robot_description_content}
+    use_sim_time = {"use_sim_time": True}
+
+    # Load world for gazebo sim
+    world = PathJoinSubstitution(
+        [
+            FindPackageShare(package_name),
+            "worlds",
+            'empty.world'
+        ]
+    )
+
+    # robot_state_publisher node:
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description, use_sim_time],
+    )
+
+    # Spawn Meldog:
+    spawn_entity = Node(package='ros_gz_sim', executable='create',
+                    arguments=['-topic', 'robot_description',
+                                '-name', 'Meldog'],
+                    output='screen')
+
+    # Load joint_state_broadcaster
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    # Load joint_trajectory_controller 
+    load_joint_effort_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', controller_type],
+        output='screen'
+    )
+
+    # Bridge between ros2 and Ignition Gazebo
+    # ros2_gazebo_sim_bridge =  IncludeLaunchDescription(
+    #         PythonLaunchDescriptionSource(
+    #             [os.path.join(get_package_share_directory('ros_ign_gazebo'),
+    #                           'launch', 'ign_gazebo.launch.py')]),
+    #         launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])])
+    gazebo_sim_bridge = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+            get_package_share_directory('ros_gz_sim'), 'launch'), '/gz_sim.launch.py']),
+            launch_arguments={'gz_args': ['-r -v -v4 ', world], 'on_exit_shutdown': 'true'}.items()
+        )
+    
+    nodes = [
+        gazebo_sim_bridge,
+
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=spawn_entity,
+                on_exit=[load_joint_state_broadcaster],
+            )
+        ),
+
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action= load_joint_state_broadcaster,
+                on_exit=[load_joint_effort_controller],
+            )
+        ),
+
+        robot_state_pub_node,
+        spawn_entity,
+    ]
+    return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
diff --git a/joint_controller_ros2_control/test/urdf/joint_hardware_macros.urdf.xacro b/joint_controller_ros2_control/test/urdf/joint_hardware_macros.urdf.xacro
new file mode 100644
index 0000000..2f3162a
--- /dev/null
+++ b/joint_controller_ros2_control/test/urdf/joint_hardware_macros.urdf.xacro
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name ="joint_hardware_macros">
+
+    <!-- JMotor and actuator macro for ros2_control hardware interface -->
+    <xacro:macro name="joint_hardware_macro" params="name pos_min pos_max vel_max effort_max">
+        <joint name="${name}_joint">
+            <command_interface name="position">
+                <param name="min">${pos_min}</param>
+                <param name="max">${pos_max}</param>
+            </command_interface>
+            <command_interface name="velocity">
+                <param name="min">-${vel_max}</param>
+                <param name="max">${vel_max}</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-${effort_max}</param>
+                <param name="max">${effort_max}</param>
+            </command_interface>                  
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+    </xacro:macro>
+
+</robot>
\ No newline at end of file
diff --git a/joint_controller_ros2_control/test/urdf/single_joint.urdf.xacro b/joint_controller_ros2_control/test/urdf/single_joint.urdf.xacro
new file mode 100644
index 0000000..6b43b6a
--- /dev/null
+++ b/joint_controller_ros2_control/test/urdf/single_joint.urdf.xacro
@@ -0,0 +1,80 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="single_joint_urdf">
+
+<xacro:property name="stator_length" value="0.5"/>
+<xacro:property name="rotor_radius" value="0.2"/>
+<xacro:property name="rotor_size" value="0.1"/>
+<xacro:property name="rotor_length" value="0.5"/>
+
+
+<xacro:macro name="single_joint_macro" params="name parent_name x y inertia pos_max pos_min vel_max effort_max">
+    
+    <link name="${name}_stator">
+        <inertial>
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <mass value="10.0"/> 
+            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> 
+        </inertial>
+
+        <visual>
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <box size="${stator_length} ${stator_length} ${stator_length}"/>
+            </geometry>
+            <material name="">
+                <color rgba="1.0 0.0 0.0 1.0"/>
+            </material>
+        </visual>
+    </link>
+
+    <gazebo reference="${name}_stator">
+        <material>Gazebo/Red</material>
+    </gazebo>
+
+
+    <link name="${name}_rotor">
+        <inertial>
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <mass value="1.0"/> 
+            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="${inertia}"/> 
+        </inertial>
+
+        <visual>
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <cylinder radius="${rotor_radius}" length="${rotor_size}"/>
+            </geometry>
+           <material name="">
+                <color rgba="0.0 1.0 0.0 1.0"/>
+            </material>
+        </visual>
+        <visual>
+            <origin xyz="${-rotor_length/2} 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <box size="${rotor_length} ${rotor_size} ${rotor_size}"/>
+            </geometry>
+            <material name="">
+                <color rgba="0.0 1.0 0.0 1.0"/>
+            </material>
+        </visual>
+    </link>
+
+    <gazebo reference="${name}_rotor">
+        <material>Gazebo/Green</material>
+    </gazebo>
+
+    <joint name="${parent_name}_${name}" type="fixed">
+        <origin xyz="${x} ${y} ${stator_length/2}" rpy="0.0 0.0 0.0"/>
+        <parent link="${parent_name}"/>
+        <child link="${name}_stator"/>
+    </joint>
+
+    <joint name="${name}_joint" type="revolute">
+        <origin xyz="${stator_length/2 + rotor_size/2} 0.0 0.0" rpy="0.0 ${PI/2} 0.0"/>
+        <parent link="${name}_stator"/>
+        <child link="${name}_rotor"/>
+        <axis xyz="0.0 0.0 1.0"/>
+        <limit lower="${pos_min}" upper="${pos_max}" effort="${effort_max}" velocity="${vel_max}"/>
+    </joint>
+</xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/joint_controller_ros2_control/test/urdf/single_joint_test.urdf.xacro b/joint_controller_ros2_control/test/urdf/single_joint_test.urdf.xacro
new file mode 100644
index 0000000..f805af9
--- /dev/null
+++ b/joint_controller_ros2_control/test/urdf/single_joint_test.urdf.xacro
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="single_joint_test">
+
+    <xacro:property name="PI" value="3.141592"/>
+
+    <xacro:property name="rotor_inertia" value="1.0"/>
+    <xacro:property name="rotor_position_max" value="${PI}"/>
+    <xacro:property name="rotor_position_min" value="${-PI}"/>
+    <xacro:property name="rotor_velocity_max" value="10.0"/>
+    <xacro:property name="rotor_effort_max" value="10.0"/>
+    
+    <link name="world"/>
+    
+    <!-- Single joint macro -->
+    <xacro:include filename="single_joint.urdf.xacro"/>
+
+    <xacro:single_joint_macro name="body_1" parent_name="world" x="0.0" y="0.0" inertia="${rotor_inertia}" pos_max="${rotor_position_max}" pos_min="${rotor_position_min}" vel_max="${rotor_velocity_max}" effort_max="${rotor_effort_max}"/>
+    
+
+    <ros2_control name="single_joint_test" type="system">
+        <hardware>  
+            <plugin>ign_ros2_control/IgnitionSystem</plugin>
+        </hardware>
+        <!-- Add joint hardware interfaces -->
+        <xacro:include filename="joint_hardware_macros.urdf.xacro"/>   
+        <xacro:joint_hardware_macro name="body_1" pos_min="rotor_position_min" pos_max="rotor_position_max" vel_max="rotor_velocity_max" effort_max="rotor_effort_max"/>
+    </ros2_control>
+
+    <gazebo>
+        <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+            <parameters>$(find joint_controller)/config/single_joint_test.yaml</parameters>
+        </plugin>
+    </gazebo>
+
+</robot>
\ No newline at end of file
diff --git a/joint_controller_ros2_control/test/worlds/empty.world b/joint_controller_ros2_control/test/worlds/empty.world
new file mode 100644
index 0000000..4abcd71
--- /dev/null
+++ b/joint_controller_ros2_control/test/worlds/empty.world
@@ -0,0 +1,73 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="default">
+    <!-- <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin
+      filename="gz-sim-imu-system"
+      name="gz::sim::systems::Imu">
+    </plugin> -->
+
+    <include>
+      <uri>
+        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane
+      </uri>
+    </include>
+
+    <include>
+      <uri>
+        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun
+      </uri>
+    </include>
+
+    <scene>
+      <shadows>false</shadows>
+    </scene>
+
+
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+
+    <physics type="ode">
+      <real_time_update_rate>2500.0</real_time_update_rate>
+      <max_step_size>0.0005</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>200</iters>
+          <precon_iters>0</precon_iters>
+          <sor>1.400000</sor>
+          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
+        </solver>
+        <constraints>
+          <cfm>0.00001</cfm>
+          <erp>0.2</erp>
+          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
+          <contact_surface_layer>0.01000</contact_surface_layer>
+        </constraints>
+      </ode>
+    </physics>
+
+  </world>
+</sdf>
\ No newline at end of file