Use files from demos for testing (#485)

(cherry picked from commit a12ef5a)

gz_ros2_control_demos/examples/example_position.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <chrono>
 #include <memory>
 #include <string>
 #include <vector>
@@ -26,6 +27,9 @@ 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;
 
+std::vector<double> desired_goals = {0, -1, 1, 0.0};
+unsigned int ct_goals_reached = 0;
+
 void common_goal_response(
   rclcpp_action::ClientGoalHandle
   <control_msgs::action::FollowJointTrajectory>::SharedPtr future)
@@ -47,21 +51,21 @@ 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());
+  printf("common_result_response time: %f\n", rclcpp::Clock(RCL_ROS_TIME).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");
+      printf("Action goal succeeded\n");
       break;
     case rclcpp_action::ResultCode::ABORTED:
-      printf("Goal was aborted\n");
+      printf("Action goal was aborted\n");
       return;
     case rclcpp_action::ResultCode::CANCELED:
-      printf("Goal was canceled\n");
+      printf("Action goal was canceled\n");
       return;
     default:
-      printf("Unknown result code\n");
+      printf("Action goal: Unknown result code\n");
       return;
   }
 }
@@ -70,25 +74,32 @@ 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 << "feedback->desired: positions: " << feedback->desired.positions.at(0);
+  std::cout << ", velocities: " << feedback->desired.velocities.at(0) << std::endl;
+
+  std::cout << "feedback->actual: positions: " << feedback->actual.positions.at(0);
   std::cout << std::endl;
-  std::cout << "feedback->desired.velocities :";
-  for (auto & x : feedback->desired.velocities) {
-    std::cout << x << "\t";
+
+  if (ct_goals_reached < desired_goals.size()) {
+    if (fabs(feedback->actual.positions[0] - desired_goals.at(ct_goals_reached)) < 0.1) {
+      std::cout << "Goal # " << ct_goals_reached << ": " << desired_goals.at(ct_goals_reached) <<
+        " reached" << std::endl;
+      ct_goals_reached++;
+      if (ct_goals_reached < desired_goals.size()) {
+        std::cout << "next goal # " << ct_goals_reached << ": " <<
+          desired_goals.at(ct_goals_reached) <<
+          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_DEBUG(node->get_logger(), "node created");
 
   rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
   action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
@@ -98,36 +109,43 @@ int main(int argc, char * argv[])
     node->get_node_waitables_interface(),
     "/joint_trajectory_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");
+  while (true) {
+    bool response =
+      action_client->wait_for_action_server(std::chrono::seconds(1));
+    if (!response) {
+      using namespace std::chrono_literals;
+      std::this_thread::sleep_for(2000ms);
+      RCLCPP_WARN(node->get_logger(), "Trying to connect to the server again");
+      continue;
+    } else {
+      break;
+    }
   }
-  std::cout << "Created action server" << std::endl;
+
+  RCLCPP_DEBUG(node->get_logger(), "Created action server");
 
   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.time_from_start = rclcpp::Duration::from_seconds(1.0);
   point.positions.resize(joint_names.size());
-
-  point.positions[0] = 5.0;
+  point.positions[0] = desired_goals[0];
 
   trajectory_msgs::msg::JointTrajectoryPoint point2;
-  point2.time_from_start = rclcpp::Duration::from_seconds(1.0);
+  point2.time_from_start = rclcpp::Duration::from_seconds(2.0);
   point2.positions.resize(joint_names.size());
-  point2.positions[0] = -1.0;
+  point2.positions[0] = desired_goals[1];
 
   trajectory_msgs::msg::JointTrajectoryPoint point3;
-  point3.time_from_start = rclcpp::Duration::from_seconds(2.0);
+  point3.time_from_start = rclcpp::Duration::from_seconds(3.0);
   point3.positions.resize(joint_names.size());
-  point3.positions[0] = 1.0;
+  point3.positions[0] = desired_goals[2];
 
   trajectory_msgs::msg::JointTrajectoryPoint point4;
-  point4.time_from_start = rclcpp::Duration::from_seconds(3.0);
+  point4.time_from_start = rclcpp::Duration::from_seconds(4.0);
   point4.positions.resize(joint_names.size());
-  point4.positions[0] = 0.0;
+  point4.positions[0] = desired_goals[3];
 
   points.push_back(point);
   points.push_back(point2);
@@ -144,6 +162,7 @@ int main(int argc, char * argv[])
   goal_msg.trajectory.joint_names = joint_names;
   goal_msg.trajectory.points = points;
 
+  RCLCPP_DEBUG(node->get_logger(), "async_send_goal");
   auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);
 
   if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=
@@ -173,11 +192,20 @@ int main(int argc, char * argv[])
     rclcpp::FutureReturnCode::SUCCESS)
   {
     RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
+    action_client.reset();
+    node.reset();
     return 1;
   }
 
   action_client.reset();
   node.reset();
+
+  if (desired_goals.size() != ct_goals_reached) {
+    RCLCPP_ERROR(node->get_logger(), "Not all the goals were reached");
+    rclcpp::shutdown();
+    return -1;
+  }
+
   rclcpp::shutdown();
 
   return 0;

gz_ros2_control_demos/launch/cart_example_effort.launch.py

@@ -26,6 +26,7 @@
 def generate_launch_description():
     # Launch Arguments
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
+    gz_args = LaunchConfiguration('gz_args', default='')
 
     # Get URDF via xacro
     robot_description_content = Command(
@@ -92,7 +93,7 @@ def generate_launch_description():
                 [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
                                        'launch',
                                        'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 3 empty.sdf'])]),
+            launch_arguments=[('gz_args', [gz_args, ' -r -v 3 empty.sdf'])]),
         RegisterEventHandler(
             event_handler=OnProcessExit(
                 target_action=gz_spawn_entity,

gz_ros2_control_demos/launch/cart_example_position.launch.py

@@ -26,6 +26,7 @@
 def generate_launch_description():
     # Launch Arguments
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
+    gz_args = LaunchConfiguration('gz_args', default='')
 
     # Get URDF via xacro
     robot_description_content = Command(
@@ -93,7 +94,7 @@ def generate_launch_description():
                 [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
                                        'launch',
                                        'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
+            launch_arguments=[('gz_args', [gz_args, ' -r -v 1 empty.sdf'])]),
         RegisterEventHandler(
             event_handler=OnProcessExit(
                 target_action=gz_spawn_entity,

gz_ros2_control_demos/launch/cart_example_velocity.launch.py

@@ -26,6 +26,7 @@
 def generate_launch_description():
     # Launch Arguments
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
+    gz_args = LaunchConfiguration('gz_args', default='')
 
     # Get URDF via xacro
     robot_description_content = Command(
@@ -101,7 +102,7 @@ def generate_launch_description():
                 [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
                                        'launch',
                                        'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 3 empty.sdf'])]),
+            launch_arguments=[('gz_args', [gz_args, ' -r -v 3 empty.sdf'])]),
         RegisterEventHandler(
             event_handler=OnProcessExit(
                 target_action=gz_spawn_entity,

gz_ros2_control_demos/urdf/test_cart_position.xacro.urdf

@@ -61,7 +61,7 @@
         <param name="max">15</param>
       </command_interface>
       <state_interface name="position">
-        <param name="initial_value">5.0</param>
+        <param name="initial_value">1.0</param>
       </state_interface>
       <state_interface name="velocity"/>
       <state_interface name="effort"/>

gz_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf

@@ -121,7 +121,9 @@
         <param name="min">-15</param>
         <param name="max">15</param>
       </command_interface>
-      <state_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">1.0</param>
+      </state_interface>
       <state_interface name="velocity"/>
       <state_interface name="effort"/>
     </joint>

gz_ros2_control_tests/CMakeLists.txt

@@ -7,31 +7,6 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-  find_package(control_msgs REQUIRED)
-  find_package(rclcpp REQUIRED)
-  find_package(rclcpp_action REQUIRED)
-
-  add_executable(test_position src/test_position.cpp)
-  ament_target_dependencies(test_position
-    rclcpp
-    rclcpp_action
-    control_msgs
-  )
-
-  ## Install
-  install(
-    TARGETS
-      test_position
-    DESTINATION
-      lib/${PROJECT_NAME}
-  )
-
-  install(DIRECTORY
-    urdf
-    config
-    DESTINATION share/${PROJECT_NAME}/
-  )
-
   add_subdirectory(tests)
 endif()
 

gz_ros2_control_tests/package.xml

@@ -9,32 +9,20 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <build_depend>control_msgs</build_depend>
-  <build_depend>rclcpp</build_depend>
-  <build_depend>rclcpp_action</build_depend>
-
-  <exec_depend>ament_index_python</exec_depend>
-  <exec_depend>control_msgs</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>hardware_interface</exec_depend>
-  <exec_depend>gz_ros2_control</exec_depend>
-  <exec_depend>joint_state_broadcaster</exec_depend>
-  <exec_depend>joint_trajectory_controller</exec_depend>
-  <exec_depend>launch</exec_depend>
-  <exec_depend>launch_ros</exec_depend>
-  <exec_depend>launch_testing_ament_cmake</exec_depend>
-  <exec_depend>python3-psutil</exec_depend>
-  <exec_depend>python3-pytest</exec_depend>
-  <exec_depend>ros2launch</exec_depend>
-  <exec_depend>rclcpp</exec_depend>
-  <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>ros_gz_sim</exec_depend>
-  <exec_depend>ros2controlcli</exec_depend>
-  <exec_depend>xacro</exec_depend>
-
-  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_index_python</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>gz_ros2_control_demos</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>launch_testing_ros</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>python3-psutil</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  <test_depend>rclpy</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>ros2launch</test_depend>
+  <test_depend>rosgraph_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>