Added initial demos in gazebo_ros2_control_demos

.github/workflows/ci.yaml

@@ -42,11 +42,11 @@ jobs:
         cd /home/ros2_ws/
         . /opt/ros/foxy/local_setup.sh
         export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH
-        colcon build --packages-up-to gazebo_ros2_control
+        colcon build --packages-up-to gazebo_ros2_control_demos
     - name: Run tests
       id: test
       run: |
         cd /home/ros2_ws/
         . /opt/ros/foxy/local_setup.sh
-        colcon test --event-handlers console_direct+ --packages-select gazebo_ros2_control
+        colcon test --event-handlers console_direct+ --packages-select gazebo_ros2_control gazebo_ros2_control_demos
         colcon test-result

gazebo_ros2_control_demos/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.5.0)
+project(gazebo_ros2_control_demos)
+
+# Default to C11
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 11)
+endif()
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(NOT WIN32)
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+
+install(DIRECTORY
+  launch
+  config
+  urdf
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+add_executable(example_position examples/example_position.cpp)
+ament_target_dependencies(example_position
+  rclcpp
+  rclcpp_action
+  control_msgs
+)
+
+add_executable(example_velocity examples/example_velocity.cpp)
+ament_target_dependencies(example_velocity
+  rclcpp
+  rclcpp_action
+  control_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+## Install
+install(TARGETS example_position example_velocity
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

gazebo_ros2_control_demos/config/cartpole_controller.yaml

@@ -0,0 +1,17 @@
+cart_pole_controller:
+  ros__parameters:
+    type: joint_trajectory_controller/JointTrajectoryController
+    joints:
+       - slider_to_cart
+    write_op_modes:
+       - slider_to_cart
+    state_publish_rate: 25
+    action_monitor_rate: 20
+    constraints:
+      stopped_velocity_tolerance: 0.05
+      goal_time: 5
+
+joint_state_controller:
+  ros__parameters:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50

gazebo_ros2_control_demos/examples/example_position.cpp

@@ -0,0 +1,179 @@
+// Copyright 2020 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;
+
+void common_goal_response(
+  std::shared_future<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->desired.positions) {
+    std::cout << x << "\t";
+  }
+  std::cout << std::endl;
+  std::cout << "feedback->desired.velocities :";
+  for (auto & x : feedback->desired.velocities) {
+    std::cout << x << "\t";
+  }
+  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_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(),
+    "/cart_pole_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 :(");
+    return 1;
+  }
+  RCLCPP_ERROR(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");
+    return 1;
+  }
+  RCLCPP_ERROR(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 :(");
+    return 1;
+  }
+
+  std::cout << "async_send_goal" << std::endl;
+  rclcpp::shutdown();
+
+  return 0;
+}