Added velocity and effort joint testing

.gitignore

@@ -0,0 +1 @@
+test/__pycache__/

.pre-commit-config.yaml

@@ -1,17 +1,3 @@
-# To use:
-#
-#     pre-commit run -a
-#
-# Or:
-#
-#     pre-commit install  # (runs every time you commit in git)
-#
-# To update this file:
-#
-#     pre-commit autoupdate
-#
-# See https://github.com/pre-commit/pre-commit
-
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks
@@ -43,10 +29,12 @@ repos:
       - id: requirements-txt-fixer
       - id: sort-simple-yaml
       - id: trailing-whitespace
-  - repo: https://github.com/psf/black
-    rev: 22.12.0
+  - repo: https://github.com/astral-sh/ruff-pre-commit
+    rev: v0.6.9
     hooks:
-      - id: black
+      - id: ruff
+        args: ['--output-format=full', '--fix', '--config', 'pyproject.toml']
+      - id: ruff-format
   - repo: https://github.com/codespell-project/codespell
     rev: v2.0.0
     hooks:
@@ -71,3 +59,11 @@ repos:
     rev: v2.10.0
     hooks:
       - id: hadolint-docker
+  - repo: https://github.com/pre-commit/mirrors-prettier
+    rev: "v3.1.0"
+    hooks:
+      - id: prettier
+        additional_dependencies:
+          - "[email protected]"
+          - "@prettier/[email protected]"
+        files: \.(xml|xacro|srdf)$

.prettierrc.cjs

@@ -0,0 +1,5 @@
+module.exports = {
+  plugins: [require.resolve("@prettier/plugin-xml")],
+  xmlWhitespaceSensitivity: "ignore",
+  xmlQuoteAttributes: "double"
+}

CMakeLists.txt

@@ -56,9 +56,12 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_cmake_pytest REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)
+  find_package(ros_testing REQUIRED)
 
   # ROS2 linters, but disable copyright test. PickNik's copyright's may not conform
   # to this test
+  set(ament_cmake_flake8_FOUND TRUE)
+  set(ament_cmake_pep257_FOUND TRUE)
   set(ament_cmake_cpplint_FOUND TRUE)
   set(ament_cmake_uncrustify_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
@@ -68,6 +71,14 @@ if(BUILD_TESTING)
   target_link_libraries(topic_based_system_test
     ${PROJECT_NAME})
   ament_target_dependencies(topic_based_system_test ${THIS_PACKAGE_INCLUDE_DEPENDS} ros2_control_test_assets)
+
+  # Integration tests
+  add_ros_test(
+    test/ros2_control.test.py
+    TIMEOUT
+    120
+    ARGS
+    test_file:=${CMAKE_CURRENT_SOURCE_DIR}/test/test_topic_based_robot.py)
 endif()
 
 pluginlib_export_plugin_description_file(hardware_interface topic_based_ros2_control_plugin_description.xml)

include/topic_based_ros2_control/topic_based_system.hpp

@@ -80,6 +80,8 @@ class TopicBasedSystem : public hardware_interface::SystemInterface
 
   struct MimicJoint
   {
+    std::string joint_name;
+    std::string mimicked_joint_name;
     std::size_t joint_index;
     std::size_t mimicked_joint_index;
     double multiplier = 1.0;

package.xml

@@ -1,32 +1,48 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <package format="3">
-    <name>topic_based_ros2_control</name>
-    <version>0.2.0</version>
-    <description>ros2 control hardware interface for topic_based sim</description>
+  <name>topic_based_ros2_control</name>
+  <version>0.2.0</version>
+  <description>ros2 control hardware interface for topic_based sim</description>
 
-    <maintainer email="[email protected]">Marq Rasmussen</maintainer>
-    <maintainer email="[email protected]">Jafar</maintainer>
-    <author email="[email protected]">Jafar</author>
+  <maintainer email="[email protected]">Marq Rasmussen</maintainer>
+  <maintainer email="[email protected]">Jafar</maintainer>
+  <author email="[email protected]">Jafar</author>
 
-    <license>BSD</license>
+  <license>BSD</license>
 
-    <url type="website">https://github.com/PickNikRobotics/topic_based_ros2_control</url>
-    <url type="bugtracker">https://github.com/PickNikRobotics/topic_based_ros2_control/issues</url>
-    <url type="repository">https://github.com/PickNikRobotics/topic_based_ros2_control/</url>
+  <url type="website">
+    https://github.com/PickNikRobotics/topic_based_ros2_control
+  </url>
+  <url type="bugtracker">
+    https://github.com/PickNikRobotics/topic_based_ros2_control/issues
+  </url>
+  <url type="repository">
+    https://github.com/PickNikRobotics/topic_based_ros2_control/
+  </url>
 
-    <buildtool_depend>ament_cmake</buildtool_depend>s
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-    <depend>angles</depend>
-    <depend>rclcpp</depend>
-    <depend>hardware_interface</depend>
-    <depend>sensor_msgs</depend>
+  <depend>angles</depend>
+  <depend>rclcpp</depend>
+  <depend>hardware_interface</depend>
+  <depend>sensor_msgs</depend>
 
-    <test_depend>ament_lint_auto</test_depend>
-    <test_depend>picknik_ament_copyright</test_depend>
-    <test_depend>ros2_control_test_assets</test_depend>
-    <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>joint_state_broadcaster</test_depend>
+  <test_depend>joint_trajectory_controller</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>picknik_ament_copyright</test_depend>
+  <test_depend>rclpy</test_depend>
+  <test_depend>robot_state_publisher</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+  <test_depend>ros_testing</test_depend>
+  <test_depend>xacro</test_depend>
 
-    <export>
-        <build_type>ament_cmake</build_type>
-    </export>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>

pyproject.toml

@@ -0,0 +1,20 @@
+[tool.ruff]
+extend-select = [
+  # Enabled by default
+  # pyflakes
+  # "F",
+  # pycodestyle
+  # "E",
+  "W",
+  # isort
+  "I",
+  # NumPy-specific rules
+  "NPY",
+  # Ruff-specific rules
+  "RUF",
+]
+# line-length = 88
+ignore = ["E501"]
+target-version = "py310"
+[tool.ruff.pydocstyle]
+convention = "google"

src/topic_based_system.cpp

@@ -91,6 +91,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
         // Check the initial_value param is used
         if (!interface.initial_value.empty())
         {
+          joint_states_[index][i] = std::stod(interface.initial_value);
           joint_commands_[index][i] = std::stod(interface.initial_value);
         }
       }
@@ -113,6 +114,8 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
         throw std::runtime_error(std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found");
       }
       MimicJoint mimic_joint;
+      mimic_joint.joint_name = joint.name;
+      mimic_joint.mimicked_joint_name = mimicked_joint_it->name;
       mimic_joint.joint_index = i;
       mimic_joint.mimicked_joint_index =
           static_cast<std::size_t>(std::distance(info_.joints.begin(), mimicked_joint_it));
@@ -270,64 +273,146 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
       joint_states_[POSITION_INTERFACE_INDEX].cbegin(), joint_states_[POSITION_INTERFACE_INDEX].cend(),
       joint_commands_[POSITION_INTERFACE_INDEX].cbegin(), 0.0,
       [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus<double>{});
-  if (diff <= trigger_joint_command_threshold_)
+
+  bool exist_velocity_command = false;
+  static bool exist_velocity_command_old = false;  // Use old state to publish zero velocities
+  for (std::size_t i = 0; i < info_.joints.size(); ++i)
+  {
+    if (fabs(joint_commands_[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_)
+    {
+      exist_velocity_command = true;
+    }
+  }
+
+  if (diff <= trigger_joint_command_threshold_ &&
+      (exist_velocity_command == false && exist_velocity_command_old == false))
   {
     return hardware_interface::return_type::OK;
   }
 
-  sensor_msgs::msg::JointState joint_state;
-  for (std::size_t i = 0; i < info_.joints.size(); ++i)
+  exist_velocity_command_old = exist_velocity_command;
+
+  // For Position Joint
   {
-    joint_state.name.push_back(info_.joints[i].name);
+    sensor_msgs::msg::JointState joint_state;
     joint_state.header.stamp = node_->now();
-    // only send commands to the interfaces that are defined for this joint
-    for (const auto& interface : info_.joints[i].command_interfaces)
+    for (std::size_t i = 0; i < info_.joints.size(); ++i)
     {
-      if (interface.name == hardware_interface::HW_IF_POSITION)
+      // only send commands to the interfaces that are defined for this joint
+      for (const auto& interface : info_.joints[i].command_interfaces)
       {
-        joint_state.position.push_back(joint_commands_[POSITION_INTERFACE_INDEX][i]);
+        if (interface.name == hardware_interface::HW_IF_POSITION)
+        {
+          joint_state.name.push_back(info_.joints[i].name);
+          joint_state.position.push_back(joint_commands_[POSITION_INTERFACE_INDEX][i]);
+        }
       }
-      else if (interface.name == hardware_interface::HW_IF_VELOCITY)
+    }
+
+    for (const auto& mimic_joint : mimic_joints_)
+    {
+      for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces)
       {
-        joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]);
+        if (interface.name == hardware_interface::HW_IF_POSITION)
+        {
+          for (size_t index = 0; index < joint_state.name.size(); index++)
+          {
+            if (joint_state.name[index] == mimic_joint.mimicked_joint_name)
+            {
+              joint_state.name.push_back(mimic_joint.joint_name);
+              joint_state.position.push_back(mimic_joint.multiplier * joint_state.position[index]);
+            }
+          }
+        }
       }
-      else if (interface.name == hardware_interface::HW_IF_EFFORT)
+    }
+
+    if (rclcpp::ok() && joint_state.name.size() != 0)
+    {
+      topic_based_joint_commands_publisher_->publish(joint_state);
+    }
+  }
+
+  // For Velocity Joint
+  {
+    sensor_msgs::msg::JointState joint_state;
+    joint_state.header.stamp = node_->now();
+    for (std::size_t i = 0; i < info_.joints.size(); ++i)
+    {
+      // only send commands to the interfaces that are defined for this joint
+      for (const auto& interface : info_.joints[i].command_interfaces)
       {
-        joint_state.effort.push_back(joint_commands_[EFFORT_INTERFACE_INDEX][i]);
+        if (interface.name == hardware_interface::HW_IF_VELOCITY)
+        {
+          joint_state.name.push_back(info_.joints[i].name);
+          joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]);
+        }
       }
-      else
+    }
+
+    for (const auto& mimic_joint : mimic_joints_)
+    {
+      for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces)
       {
-        RCLCPP_WARN_ONCE(node_->get_logger(), "Joint '%s' has unsupported command interfaces found: %s.",
-                         info_.joints[i].name.c_str(), interface.name.c_str());
+        if (interface.name == hardware_interface::HW_IF_VELOCITY)
+        {
+          for (size_t index = 0; index < joint_state.name.size(); index++)
+          {
+            if (joint_state.name[index] == mimic_joint.mimicked_joint_name)
+            {
+              joint_state.name.push_back(mimic_joint.joint_name);
+              joint_state.velocity.push_back(mimic_joint.multiplier * joint_state.velocity[index]);
+            }
+          }
+        }
       }
     }
+
+    if (rclcpp::ok() && joint_state.name.size() != 0)
+    {
+      topic_based_joint_commands_publisher_->publish(joint_state);
+    }
   }
 
-  for (const auto& mimic_joint : mimic_joints_)
+  // For Effort Joint
   {
-    for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces)
+    sensor_msgs::msg::JointState joint_state;
+    joint_state.header.stamp = node_->now();
+    for (std::size_t i = 0; i < info_.joints.size(); ++i)
     {
-      if (interface.name == hardware_interface::HW_IF_POSITION)
-      {
-        joint_state.position[mimic_joint.joint_index] =
-            mimic_joint.multiplier * joint_state.position[mimic_joint.mimicked_joint_index];
-      }
-      else if (interface.name == hardware_interface::HW_IF_VELOCITY)
+      // only send commands to the interfaces that are defined for this joint
+      for (const auto& interface : info_.joints[i].command_interfaces)
       {
-        joint_state.velocity[mimic_joint.joint_index] =
-            mimic_joint.multiplier * joint_state.velocity[mimic_joint.mimicked_joint_index];
+        if (interface.name == hardware_interface::HW_IF_EFFORT)
+        {
+          joint_state.name.push_back(info_.joints[i].name);
+          joint_state.effort.push_back(joint_commands_[EFFORT_INTERFACE_INDEX][i]);
+        }
       }
-      else if (interface.name == hardware_interface::HW_IF_EFFORT)
+    }
+
+    for (const auto& mimic_joint : mimic_joints_)
+    {
+      for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces)
       {
-        joint_state.effort[mimic_joint.joint_index] =
-            mimic_joint.multiplier * joint_state.effort[mimic_joint.mimicked_joint_index];
+        if (interface.name == hardware_interface::HW_IF_EFFORT)
+        {
+          for (size_t index = 0; index < joint_state.name.size(); index++)
+          {
+            if (joint_state.name[index] == mimic_joint.mimicked_joint_name)
+            {
+              joint_state.name.push_back(mimic_joint.joint_name);
+              joint_state.effort.push_back(mimic_joint.multiplier * joint_state.effort[index]);
+            }
+          }
+        }
       }
     }
-  }
 
-  if (rclcpp::ok())
-  {
-    topic_based_joint_commands_publisher_->publish(joint_state);
+    if (rclcpp::ok() && joint_state.name.size() != 0)
+    {
+      topic_based_joint_commands_publisher_->publish(joint_state);
+    }
   }
 
   return hardware_interface::return_type::OK;