Joint to cartesian controller

[InigoMoreno]

Fixes #223 Working joint to cartesian controller.
I decided to go with the new controller chaining instead of the previous way of instantiating a controller manager from a controller. Basically the new joint_to_cartesian_controller creates new state and command interfaces "joint_to_cartesian_controller/jointx" from the interfaces of "jointx". A joint trajectory controller can then control these fake interfaces and instead of moving the robot, they move the target frame.

If you want to test this, you can't use "rqt_joint_trajectory_controller", as that plugin assumes the joints controlled are the ones in the urdf, but that's not the case here, so I wrote a small script to test it. To test everyhting, just turn on the joint_cartesian_trajectory_controller, run the script changing the trajectory point and watch the target move. You can also activate the cartesian_motion_control to make the robot follow the target.

Before merging, please be aware that this depends on chaining state interfaces, something that was introduced in ros-controls/ros2_control#1021 and is only available on the jazzy binaries of ros2_control. You can make it work in humble by compiling ros2_control from source. Maybe this should be merged onto a new jazzy branch. I also haven't updated the README, and the copyright header now mentions PAL Robotics as I started from the passthrough controller on ros2_control_demos/example_12.

Here is the script I used for testing:

import rclpy
from control_msgs.action import FollowJointTrajectory
from rclpy.action import ActionClient
from rclpy.node import Node
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from math import pi


class FollowJointTrajectoryTestNode(Node):
    def __init__(self):
        super().__init__('follow_joint_trajectory_test_node')
        self._action_client = ActionClient(
            self,
            FollowJointTrajectory,
            '/joint_cartesian_trajectory_controller/follow_joint_trajectory')
        self.send_trajectory()

    def send_trajectory(self):
        goal_msg = FollowJointTrajectory.Goal()

        # Define the joint names
        goal_msg.trajectory.joint_names = [f'joint_to_cartesian_controller/joint{i}' for i in range(1, 7)]

        # Define the trajectory points
        point = JointTrajectoryPoint()
        point.positions = [
            0.0008580626611980172,
            0.3575830628720197,
            -0.5730281342118568,
            -0.3947451669167576,
            0.04257727938199307,
            0.49471162399555446,]
        # point.positions[0] -= pi/8
        # point.positions[3] += pi/8
        point.time_from_start.sec = 2
        goal_msg.trajectory.points.append(point)

        # Wait for the action server to be available
        self._action_client.wait_for_server()

        # Send the goal
        self.get_logger().info('Sending trajectory goal...')
        self._action_client.send_goal_async(
            goal_msg, feedback_callback=self.feedback_callback).add_done_callback(
            self.goal_response_callback)

    def goal_response_callback(self, future):
        self.get_logger().info('Goal response received. Waiting for result...')
        future.result().get_result_async().add_done_callback(self.result_callback)

    def result_callback(self, future):
        result = future.result()
        self.get_logger().info(f'Action completed with result: {result.result}')
        self.get_logger().info('Shutting down node...')
        exit(0)

    def feedback_callback(self, feedback_msg):
        self.get_logger().info(f'Received feedback: {feedback_msg.feedback}')


def main(args=None):
    rclpy.init(args=args)
    node = FollowJointTrajectoryTestNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()