Add test to check position controller (#134)

* Add test to check position controller

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

* make linters happy

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

---------

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
(cherry picked from commit 70fb797)

gz_ros2_control_tests/CMakeLists.txt

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 3.5)
+project(gz_ros2_control_tests)
+
+find_package(ament_cmake REQUIRED)
+
+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()
+
+ament_package()

gz_ros2_control_tests/config/cartpole_controller_position.yaml

@@ -0,0 +1,19 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # Hz
+
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - slider_to_cart
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity

gz_ros2_control_tests/package.xml

@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gz_ros2_control_tests</name>
+  <version>0.4.3</version>
+  <description>Gazebo ros2 control tests</description>
+  <maintainer email="alejandro@openrobotics.org">Alejandro Hernández</maintainer>
+  <license>Apache License 2.0</license>
+
+  <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>ros2launch</exec_depend>
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend condition="$GZ_VERSION == ''">ros_gz_sim</exec_depend>
+  <exec_depend condition="$GZ_VERSION == 'garden'">ros_gz_sim</exec_depend>
+  <exec_depend condition="$GZ_VERSION == fortress">ros_ign_gazebo</exec_depend>
+  <exec_depend>ros2controlcli</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

gz_ros2_control_tests/src/test_position.cpp

@@ -0,0 +1,196 @@
+// Copyright 2021 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.
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+#include <control_msgs/action/follow_joint_trajectory.hpp>
+
+std::shared_ptr<rclcpp::Node> node;
+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> derired_goals = {0, -1, 1, 0.0};
+unsigned int goal_reached = 0;
+
+void common_goal_response(
+  rclcpp_action::ClientGoalHandle
+  <control_msgs::action::FollowJointTrajectory>::SharedPtr future)
+{
+  RCLCPP_DEBUG(
+    node->get_logger(), "common_goal_response time: %f",
+    rclcpp::Clock().now().seconds());
+  auto goal_handle = future.get();
+  if (!goal_handle) {
+    common_goal_accepted = false;
+    printf("Goal rejected\n");
+  } else {
+    common_goal_accepted = true;
+    printf("Goal accepted\n");
+  }
+}
+
+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());
+  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");
+      break;
+    case rclcpp_action::ResultCode::ABORTED:
+      printf("Goal was aborted\n");
+      return;
+    case rclcpp_action::ResultCode::CANCELED:
+      printf("Goal was canceled\n");
+      return;
+    default:
+      printf("Unknown result code\n");
+      return;
+  }
+}
+
+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->actual.positions) {
+    std::cout << x << "\t";
+  }
+
+  std::cout << std::endl;
+  if (fabs(feedback->desired.positions[0] - derired_goals[goal_reached]) < 0.1) {
+    std::cout << "Goal " << derired_goals[goal_reached] << " reached" << '\n';
+    goal_reached++;
+    std::cout << "next goal " << goal_reached << '\n';
+  }
+}
+
+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_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
+  action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
+    node->get_node_base_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_logging_interface(),
+    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");
+  }
+  std::cout << "Created action server" << std::endl;
+
+  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.positions.resize(joint_names.size());
+
+  point.positions[0] = 0.0;
+
+  trajectory_msgs::msg::JointTrajectoryPoint point2;
+  point2.time_from_start = rclcpp::Duration::from_seconds(1.0);
+  point2.positions.resize(joint_names.size());
+  point2.positions[0] = -1.0;
+
+  trajectory_msgs::msg::JointTrajectoryPoint point3;
+  point3.time_from_start = rclcpp::Duration::from_seconds(2.0);
+  point3.positions.resize(joint_names.size());
+  point3.positions[0] = 1.0;
+
+  trajectory_msgs::msg::JointTrajectoryPoint point4;
+  point4.time_from_start = rclcpp::Duration::from_seconds(3.0);
+  point4.positions.resize(joint_names.size());
+  point4.positions[0] = 0.0;
+
+  // points.push_back(point);
+  points.push_back(point2);
+  points.push_back(point3);
+  points.push_back(point4);
+
+  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt;
+  opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1);
+  opt.result_callback = std::bind(common_result_response, std::placeholders::_1);
+  opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2);
+
+  control_msgs::action::FollowJointTrajectory_Goal goal_msg;
+  goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
+  goal_msg.trajectory.joint_names = joint_names;
+  goal_msg.trajectory.points = points;
+
+  auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);
+
+  if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");
+    action_client.reset();
+    node.reset();
+    return 1;
+  }
+  RCLCPP_INFO(node->get_logger(), "send goal call ok :)");
+
+  rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr
+    goal_handle = goal_handle_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
+    action_client.reset();
+    node.reset();
+    return 1;
+  }
+  RCLCPP_INFO(node->get_logger(), "Goal was accepted by server");
+
+  // Wait for the server to be done with the goal
+  auto result_future = action_client->async_get_result(goal_handle);
+  RCLCPP_INFO(node->get_logger(), "Waiting for result");
+  if (rclcpp::spin_until_future_complete(node, result_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
+    action_client.reset();
+    node.reset();
+    return 1;
+  }
+
+  action_client.reset();
+  node.reset();
+
+  std::cout << "async_send_goal" << std::endl;
+  rclcpp::shutdown();
+
+  if ((derired_goals.size() + 1) != goal_reached) {
+    return -1;
+  }
+
+  return 0;
+}

gz_ros2_control_tests/tests/CMakeLists.txt

@@ -0,0 +1,6 @@
+find_package(launch_testing_ament_cmake REQUIRED)
+
+add_launch_test(position_test.py
+  TARGET test_position_controller
+  TIMEOUT 120
+)