Working iisy with ros2_control and actor in same world

src/controller_test/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.8)
+project(controller_test)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(control_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_executable(move src/test.cpp)
+set_property(TARGET move PROPERTY CXX_STANDARD 17)
+ament_target_dependencies(move rclcpp moveit_ros_planning_interface control_msgs)
+
+install(TARGETS
+        move
+        DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/controller_test/package.xml

@@ -0,0 +1,23 @@
+<?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>controller_test</name>
+  <version>0.1.0</version>
+  <description>
+     Test for Moveit on IISY
+  </description>
+  <author email="bastian.hofmann@xitaso.com">bastian</author>
+  <maintainer email="bastian.hofmann@xitaso.com">bastian</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>moveit_ros_planning_interface</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

src/controller_test/src/test.cpp

@@ -0,0 +1,196 @@
+#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(
+  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(),
+    "/arm_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 = {
+    "base_rot",
+    "base_link1_joint",
+    "link1_link2_joint",
+    "link2_rot",
+    "link2_link3_joint",
+    "link3_rot"
+    };
+
+  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;
+  point.positions[1] = 0.0;
+  point.positions[2] = 0.0;
+  point.positions[3] = 0.0;
+  point.positions[4] = 0.0;
+  point.positions[5] = 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;
+  point2.positions[1] = 0.0;
+  point2.positions[2] = 0.0;
+  point2.positions[3] = 0.0;
+  point2.positions[4] = 0.0;
+  point2.positions[5] = 0.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;
+  point3.positions[1] = 0.0;
+  point3.positions[2] = 0.0;
+  point3.positions[3] = 0.0;
+  point3.positions[4] = 0.0;
+  point3.positions[5] = 0.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;
+  point4.positions[1] = 0.0;
+  point4.positions[2] = 0.0;
+  point4.positions[3] = 0.0;
+  point4.positions[4] = 0.0;
+  point4.positions[5] = 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;
+}