From 3cb5f0935f1efc7046ba8ad4fde017ba7ebb609d Mon Sep 17 00:00:00 2001
From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Date: Mon, 12 Aug 2024 11:30:53 -0400
Subject: [PATCH] Correctly load smoothing plugins in Servo integration tests
 (#2965)

* Load smoothing plugins in Servo integration tests

* Use actual constant names instead of raw ints
---
 .../tests/launch/servo_cpp_integration.test.py        |  8 +++++---
 .../tests/launch/servo_ros_integration.test.py        | 11 +++++++----
 .../moveit_servo/tests/test_ros_integration.cpp       |  6 +++---
 3 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py b/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py
index 6cb56bfa5b..fd14c8bb9d 100644
--- a/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py
+++ b/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py
@@ -22,8 +22,9 @@ def generate_test_description():
         .to_dict()
     }
 
-    # This filter parameter should be >1. Increase it for greater smoothing but slower motion.
-    low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5}
+    # This sets the update rate and planning group name for the acceleration limiting filter.
+    acceleration_filter_update_period = {"update_period": 0.01}
+    planning_group_name = {"planning_group_name": "panda_arm"}
 
     # ros2_control using FakeSystem as hardware
     ros2_controllers_path = os.path.join(
@@ -91,7 +92,8 @@ def generate_test_description():
         ),
         parameters=[
             servo_params,
-            low_pass_filter_coeff,
+            acceleration_filter_update_period,
+            planning_group_name,
             moveit_config.robot_description,
             moveit_config.robot_description_semantic,
             moveit_config.robot_description_kinematics,
diff --git a/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py b/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py
index b5123ba530..86848fd696 100644
--- a/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py
+++ b/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py
@@ -29,8 +29,9 @@ def generate_test_description():
         .to_dict()
     }
 
-    # This filter parameter should be >1. Increase it for greater smoothing but slower motion.
-    low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5}
+    # This sets the update rate and planning group name for the acceleration limiting filter.
+    acceleration_filter_update_period = {"update_period": 0.01}
+    planning_group_name = {"planning_group_name": "panda_arm"}
 
     # ros2_control using FakeSystem as hardware
     ros2_controllers_path = os.path.join(
@@ -82,7 +83,8 @@ def generate_test_description():
                 name="servo_node",
                 parameters=[
                     servo_params,
-                    low_pass_filter_coeff,
+                    acceleration_filter_update_period,
+                    planning_group_name,
                     moveit_config.robot_description,
                     moveit_config.robot_description_semantic,
                     moveit_config.robot_description_kinematics,
@@ -112,7 +114,8 @@ def generate_test_description():
         name="servo_node",
         parameters=[
             servo_params,
-            low_pass_filter_coeff,
+            acceleration_filter_update_period,
+            planning_group_name,
             moveit_config.robot_description,
             moveit_config.robot_description_semantic,
             moveit_config.robot_description_kinematics,
diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp
index ed23f4d69e..3022b8a278 100644
--- a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp
+++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp
@@ -74,7 +74,7 @@ TEST_F(ServoRosFixture, testJointJog)
 
   // Call input type service
   auto request = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
-  request->command_type = 0;
+  request->command_type = moveit_msgs::srv::ServoCommandType::Request::JOINT_JOG;
   auto response = switch_input_client_->async_send_request(request);
   ASSERT_EQ(response.get()->success, true);
 
@@ -116,7 +116,7 @@ TEST_F(ServoRosFixture, testTwist)
       "/servo_node/delta_twist_cmds", rclcpp::SystemDefaultsQoS());
 
   auto request = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
-  request->command_type = 1;
+  request->command_type = moveit_msgs::srv::ServoCommandType::Request::TWIST;
   auto response = switch_input_client_->async_send_request(request);
   ASSERT_EQ(response.get()->success, true);
 
@@ -155,7 +155,7 @@ TEST_F(ServoRosFixture, testPose)
       "/servo_node/pose_target_cmds", rclcpp::SystemDefaultsQoS());
 
   auto request = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
-  request->command_type = 2;
+  request->command_type = moveit_msgs::srv::ServoCommandType::Request::POSE;
   auto response = switch_input_client_->async_send_request(request);
   ASSERT_EQ(response.get()->success, true);