Added Omnimove external control

.gitignore

@@ -1 +1,4 @@
+build*
+CMake*.user
+.idea
 __pycache__/

examples/kuka_rsi_robot_tests/config/iontec.yaml

@@ -0,0 +1,22 @@
+ robot_manager_client:
+    ros__parameters:
+       home_pos: [0.0, -2.3911, 2.6529, 0.0, 1.308, 0.0]
+       times: [6, 12, 6, 10, 10, 10, 10, 5, 5, 5, 5, 10, 5, 5, 10, 5]
+       position_names: ["joint1_plus", "joint1_minus", "home", "joint2_plus", "home", "joint3_minus", "home", "joint4_plus", "joint4_minus", "home", "joint5_plus", "joint5_minus", "home", "joint6_plus", "joint6_minus", "home"]
+       joint_positions:
+          home: [0.0, -2.3911, 2.6529, 0.0, 1.308, 0.0]
+          joint1_plus: [0.4, -2.3911, 2.6529, 0.0, 1.308, 0.0]
+          joint1_minus: [-0.4, -2.3911, 2.6529, 0.0, 1.308, 0.0]
+          # joint2 only moving in one direction since we are at the limit
+          joint2_plus: [0.0, -1.9911, 2.6529, 0.0, 1.308, 0.0]
+              # joint3 only moving in one direction since we are at the limit
+          joint3_minus: [0.0, -2.3911, 2.2529, 0.0, 1.308, 0.0]
+              # joint4
+          joint4_plus: [0.0, -2.3911, 2.6529, 0.4, 1.308, 0.0]
+          joint4_minus: [0.0, -2.3911, 2.6529, -0.4, 1.308, 0.0]
+              # joint5
+          joint5_plus: [0.0, -2.3911, 2.6529, 0.0, 1.708, 0.0]
+          joint5_minus: [0.0, -2.3911, 2.6529, 0.0, 0.908, 0.0]
+              # joint6
+          joint6_plus: [0.0, -2.3911, 2.6529, 0.0, 1.308, 0.4]
+          joint6_minus: [0.0, -2.3911, 2.6529, 0.0, 1.308, -0.4]

examples/kuka_rsi_robot_tests/config/quantec.yaml

@@ -0,0 +1,21 @@
+ robot_manager_client:
+    ros__parameters:
+       times: [6, 12, 6, 10, 10, 10, 10, 5, 5, 5, 5, 10, 5, 5, 10, 5]
+       position_names: ["joint1_plus", "joint1_minus", "home", "joint2_plus", "home", "joint3_minus", "home", "joint4_plus", "joint4_minus", "home", "joint5_plus", "joint5_minus", "home", "joint6_plus", "joint6_minus", "home"]
+       joint_positions:
+          home: [0.0299, -2.38422, 2.3471, 0.0, 1.5943, -2.83578]
+          joint1_plus: [0.4299, -2.38422, 2.3471, 0.0, 1.5943, -2.83578]
+          joint1_minus: [-0.4299, -2.38422, 2.3471, 0.0, 1.5943, -2.83578]
+              # joint2 only moving in one direction since we are at the limit
+          joint2_plus: [0.0299, -1.98422, 2.3471, 0.0, 1.5943, -2.83578]
+              # joint3 only moving in one direction since we are at the limit
+          joint3_minus: [0.0299, -2.38422, 1.9471, 0.0, 1.5943, -2.83578]
+              # joint4
+          joint4_plus: [0.0299, -2.38422, 2.3471, 0.4, 1.5943, -2.83578]
+          joint4_minus: [0.0299, -2.38422, 2.3471, -0.4, 1.5943, -2.83578]
+              # joint5
+          joint5_plus: [0.0299, -2.38422, 2.3471, 0.0, 1.9943, -2.83578]
+          joint5_minus: [0.0299, -2.38422, 2.3471, 0.0, 1.1943, -2.83578]
+              # joint6
+          joint6_plus: [0.0299, -2.38422, 2.3471, 0.0, 1.5943, -2.43578]
+          joint6_minus: [0.0299, -2.38422, 2.3471, 0.0, 1.5943, -3.23578]

examples/kuka_rsi_robot_tests/kuka_rsi_robot_tests/robot_movement_test.py

@@ -0,0 +1,208 @@
+import copy
+
+import rclpy
+import std_srvs.srv
+from builtin_interfaces.msg import Duration
+from rclpy.node import Node
+from lifecycle_msgs.srv import ChangeState, GetState
+from lifecycle_msgs.msg import Transition, State
+from control_msgs.action import FollowJointTrajectory
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from rclpy.action import ActionClient
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+from threading import Event
+
+
+class RobotManagerClient(Node):
+    def __init__(self, service_group):
+        super().__init__("robot_manager_client")
+        self.moves = None
+        self.current_index = None
+        self.actionFinished = None
+        self.declare_parameter("start_service", False)
+        self.start_service = self.get_parameter("start_service").get_parameter_value().bool_value
+        self.action_client = None
+        self.start_movement_service = None
+        self.movement_done = Event()
+        self.declare_parameter("position_names", [""])
+        position_names = (
+            self.get_parameter("position_names").get_parameter_value().string_array_value
+        )
+        self.declare_parameter("times", [int()])
+        self.times = self.get_parameter("times").get_parameter_value().integer_array_value
+        self.get_logger().info(f"position names is {position_names}")
+        self.get_logger().info(f"times is {self.times}")
+        if self.start_service is True:
+            self.service_group = service_group
+            self.start_movement_service = self.create_service(
+                std_srvs.srv.Empty,
+                "start_robot_movements",
+                self.start_robot_movements_callback,
+                callback_group=service_group,
+            )
+            self.stop_movement_service = self.create_service(
+                std_srvs.srv.Empty,
+                "stop_robot_movements",
+                self.stop_robot_movements_callback,
+                callback_group=service_group,
+            )
+        state_client_group = MutuallyExclusiveCallbackGroup()
+        self.cli = self.create_client(
+            ChangeState, "/robot_manager/change_state", callback_group=state_client_group
+        )
+        self.state_client = self.create_client(
+            GetState, "/robot_manager/get_state", callback_group=state_client_group
+        )
+        while not self.cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("service not available, waiting again...")
+        self.req = ChangeState.Request()
+        action_client_group = MutuallyExclusiveCallbackGroup()
+        self.action_client = ActionClient(
+            self,
+            FollowJointTrajectory,
+            "/joint_trajectory_controller/follow_joint_trajectory",
+            callback_group=action_client_group,
+        )
+        self.create_movements(position_names)
+
+    def create_movements(self, position_names):
+        self.moves = []
+        for position_name in position_names:
+            param_name = "joint_positions." + position_name
+            if not self.has_parameter(param_name):
+                self.declare_parameter(param_name, [float()])
+            self.moves.append(
+                self.get_parameter(param_name).get_parameter_value().double_array_value
+            )
+
+        self.get_logger().info(f"moves are {self.moves}")
+
+    def send_configure(self):
+        self.req.transition.id = Transition.TRANSITION_CONFIGURE
+        while not self.cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("service not available, waiting again...")
+        self.get_logger().info("calling  configure")
+        future = self.cli.call_async(self.req)
+        future.add_done_callback(self.configure_done_callback)
+
+    def configure_done_callback(self, future):
+        self.get_logger().info("configure done. Calling activate")
+        self.send_activate()
+
+    def send_activate(self):
+        self.req.transition.id = Transition.TRANSITION_ACTIVATE
+        future = self.cli.call_async(self.req)
+        future.add_done_callback(self.activate_done_callback)
+
+    def send_deactivate(self):
+        self.req.transition.id = Transition.TRANSITION_DEACTIVATE
+        future = self.cli.call_async(self.req)
+        future.add_done_callback(self.deactivate_done_callback)
+
+    def activate_done_callback(self, future):
+        self.get_logger().info("activate done")
+        self.get_state()
+
+    def deactivate_done_callback(self, future):
+        self.get_logger().info(
+            "deactivate call done. If it failed, probably manual intervention is required"
+        )
+        self.movement_done.set()
+
+    def activate_robot(self):
+        self.send_configure()  # this will in callback send the activate
+
+    def get_state(self):
+        state_future = self.state_client.call_async(GetState.Request())
+        state_future.add_done_callback(self.get_state_callback)
+        self.get_logger().info("waiting for getting state")
+
+    def get_state_callback(self, future):
+        self.get_logger().info("got current state")
+        state_resp = future.result()
+        if state_resp.current_state.id == State.PRIMARY_STATE_ACTIVE:
+            self.get_logger().info("Robot is active")
+            self.current_index = 0
+            self.start_next_movement()
+        else:
+            self.get_logger().error("could not activate robot")
+            if self.start_service:
+                self.movement_done.set()
+            else:
+                self.executor.shutdown()
+
+    def start_next_movement(self):
+        if self.current_index < len(self.times):
+            self.get_logger().info("going to submit movement")
+            position = self.moves[self.current_index]
+            duration = self.times[self.current_index]
+            self.current_index = self.current_index + 1
+            self.start_robot_movement(position, duration)
+        else:
+            self.get_logger().info("All movements done")
+            if self.start_service:
+                self.movement_done.set()
+            else:
+                self.executor.shutdown()
+
+    def start_robot_movement(self, position, duration):
+        goal = FollowJointTrajectory.Goal()
+        trajectory = JointTrajectory()
+        trajectory.joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"]
+        goal_point = JointTrajectoryPoint()
+        goal_point.time_from_start = Duration(sec=duration)
+        goal_point.positions = position
+        trajectory.points.append(copy.deepcopy(goal_point))
+        goal.trajectory = trajectory
+        self.action_client.wait_for_server()
+        cur_action_submit = self.action_client.send_goal_async(goal)
+        self.get_logger().info("waiting for action to be submitted")
+        # self.executor.spin_until_future_complete(cur_action_submit)
+        cur_action_submit.add_done_callback(self.action_submit_callback)
+
+    def action_submit_callback(self, future):
+        self.get_logger().info("action submitted, waiting for action to be done")
+        result_action = future.result().get_result_async()
+        result_action.add_done_callback(self.action_done_callback)
+
+    def action_done_callback(self, future):
+        self.get_logger().info("action finished in callback")
+        self.start_next_movement()
+
+    def do_robot_movements(self):
+        self.activate_robot()
+
+    def start_robot_movements_callback(self, request, response):
+        self.get_logger().info("Processing start movement service call")
+        self.movement_done.clear()
+        self.do_robot_movements()
+        self.get_logger().info("waiting for all actions to be done")
+        self.movement_done.wait()
+        self.get_logger().info("service returning")
+        return response
+
+    def stop_robot_movements_callback(self, request, response):
+        self.get_logger().info("Processing start movement service call")
+        self.movement_done.clear()
+        self.send_deactivate()
+        self.movement_done.wait()
+        return response
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    service_group = rclpy.callback_groups.MutuallyExclusiveCallbackGroup()
+    robot_manager_client = RobotManagerClient(service_group)
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(robot_manager_client)
+    if robot_manager_client.start_service is False:
+        robot_manager_client.do_robot_movements()
+        executor.spin()
+        robot_manager_client.destroy_node()
+    else:
+        executor.spin()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

examples/kuka_rsi_robot_tests/package.xml

@@ -0,0 +1,20 @@
+<?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>kuka_rsi_robot_tests</name>
+  <version>0.0.1</version>
+  <description>This package contains test nodes which connect to a kuka rsi robot and runs some basic movements</description>
+  <maintainer email="ravi.rathnam@kuka.com">ubuntu</maintainer>
+  <license>Apache License 2.0</license>
+  <depend>rclpy</depend>
+  <depend>lifecycle_msgs</depend>
+  <depend>joint_trajectory_controller</depend>
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

examples/kuka_rsi_robot_tests/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/kuka_rsi_robot_tests
+[install]
+install_scripts=$base/lib/kuka_rsi_robot_tests

examples/kuka_rsi_robot_tests/setup.py

@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = "kuka_rsi_robot_tests"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="KUKA Deutschland GmbH",
+    maintainer_email="ravi.rathnam@kuka.com",
+    description="This package contains test nodes which connect to a "
+    "kuka rsi robot and runs some basic movements",
+    license="Apache License 2.0",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "robot_movement_test = kuka_rsi_robot_tests.robot_movement_test:main",
+        ],
+    },
+)

kuka_utv3_ec_driver/CMakeLists.txt

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 3.8)
+project(kuka_utv3_ec_driver)
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(Boost REQUIRED COMPONENTS chrono)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+)
+
+include_directories(include)
+
+
+## COMPILE
+add_library(
+  kuka_utv3_ec_driver
+  SHARED
+  src/kuka_utv3_hardware_interface.cpp
+  src/external_control_message.cpp
+)
+
+add_executable(
+    platform_move_test
+    src/platform_move_test.cpp
+)
+ament_target_dependencies(
+    platform_move_test
+    rclcpp std_msgs std_srvs
+)
+
+install(TARGETS kuka_utv3_ec_driver
+  DESTINATION lib)
+
+install(TARGETS platform_move_test
+  DESTINATION lib/${PROJECT_NAME})
+
+
+ament_target_dependencies(kuka_utv3_ec_driver PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
+pluginlib_export_plugin_description_file(hardware_interface kuka_utv3_ec_driver.xml)
+
+if(BUILD_TESTING)
+  #find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  #ament_lint_auto_find_test_dependencies()
+endif()
+ament_export_libraries(
+  kuka_utv3_ec_driver
+  ${BOOST_LIBRARIES}
+)
+ament_package()