Created test for single joint, commands via subscriber not working

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
 ) 

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;
     }

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",

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

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)