Ros2

.gitmodules

@@ -0,0 +1,4 @@
+[submodule "kdl_parser"]
+	path = kdl_parser
+	url = https://github.com/AdrianZw/kdl_parser.git
+	branch = foxy

src/kdl_ik_service/ik.py → kdl_ik_service/kdl_ik_service/ik.py

@@ -1,5 +1,4 @@
 import PyKDL
-import rospy
 import urdf_parser_py.urdf  # from urdfdom_py
 import kdl_parser_py.urdf  # from kdl_parser_py
 import tf2_kdl
@@ -31,15 +30,14 @@ def hacky_urdf_parser_fix(urdf_str):
 
 
 # ================================= API ===================================================
-def calculate_ik(base_link, tip_link, seed_joint_state, goal_transform_geometry_msg, log_fun, solver="NR_JL"):
+def calculate_ik(base_link, tip_link, seed_joint_state, goal_transform_geometry_msg, log_fun, robot_urdf_string, solver="NR_JL"):
     """
     Calculates the Inverse Kinematics from base_link to tip_link according to the given
     goal_transform_geometry_msg. The initial joint states would be considered from seed_joint_state.
     Returns the result joint states and the success status.
     base_link eg. - "triangle_base_link" or "calib_left_arm_base_link" or "calib_right_arm_base_link"
     tip_link eg. - "left_arm_7_link" or "right_arm_7_link"
     """
-    robot_urdf_string = rospy.get_param('robot_description')
     robot_urdf_string_fixed = hacky_urdf_parser_fix(robot_urdf_string)
     urdf_obj = urdf_parser_py.urdf.URDF.from_xml_string(robot_urdf_string_fixed)
     _, kdl_tree = kdl_parser_py.urdf.treeFromUrdfModel(urdf_obj)

kdl_ik_service/launch/kdl_ik.py

@@ -0,0 +1,47 @@
+from doctest import debug
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler, LogInfo, EmitEvent
+from launch.event_handlers import OnShutdown, OnProcessExit
+from launch.events import Shutdown
+from launch.substitutions import LaunchConfiguration, LocalSubstitution, EnvironmentVariable
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # debug = LaunchConfiguration('debug')
+    debug_arg = DeclareLaunchArgument(
+        'debug',
+        default_value='200'
+    )
+    ik_node = Node(
+            package='kdl_ik_service',
+            executable='start_ros_server',
+            name='kdl_ik_service',
+            output='screen',
+            arguments=[],
+            # parameters=[{'ik_debug': debug_arg}],
+        )
+    return LaunchDescription([
+        debug_arg,
+        ik_node,
+        RegisterEventHandler(
+            OnProcessExit(
+                target_action=ik_node,
+                on_exit=[
+                    LogInfo(msg=(EnvironmentVariable(name='USER'),
+                                 ' closed the turtlesim window')),
+                    EmitEvent(event=Shutdown(
+                        reason='Window closed'))
+                ]
+            )
+        ),
+        RegisterEventHandler(
+            OnShutdown(
+                on_shutdown=[LogInfo(
+                    msg=['Launch was asked to shutdown: ',
+                         LocalSubstitution('event.reason')]
+                )]
+            )
+        ),
+    ])

kdl_ik_service/launch/kdl_ik.xml

@@ -0,0 +1,6 @@
+<launch>
+  <let name="ik_debug" value="false"/>
+  <node pkg="kdl_ik_service" exec="start_ros_server" name="kdl_ik_service" output="screen">
+    <param name="ik_debug" value="$(var ik_debug)"/>
+  </node>
+</launch>

package.xml → kdl_ik_service/package.xml

@@ -1,3 +1,5 @@
+<?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>kdl_ik_service</name>
   <version>0.7.0</version>
@@ -8,14 +10,16 @@
   <author>Amar Fayaz</author>
   <license>BSD</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <exec_depend>rospy</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 2">python_orocos_kdl</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-pykdl</exec_depend>
   <exec_depend>urdfdom_py</exec_depend>
   <exec_depend>kdl_parser_py</exec_depend>
   <exec_depend>moveit_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>1
+  </export>
 </package>

kdl_ik_service/scripts/start_ros_server.py

@@ -0,0 +1,95 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import moveit_msgs.srv
+import geometry_msgs.msg
+import kdl_ik_service.ik
+import signal
+
+
+class IkService(Node):
+
+    def __init__(self):
+        super().__init__('kdl_ik_service')
+        self.srv = self.create_service(moveit_msgs.srv.GetPositionIK, 'get_ik', self.callback)
+        self.get_logger().info('IK server ready.')
+
+    def callback(self, request, response):
+        # print "Got request %s"%(request)
+        self.get_logger().debug("Got request %s" % (request))
+        # debug_mode = rospy.get_param("ik_debug", False)
+        debug_mode = self.get_parameter("ik_debug")
+        ik_logger = self.get_logger()
+
+        # response = moveit_msgs.srv.GetPositionIKResponse()
+
+        end_effector_link = request.ik_request.ik_link_name
+        ik_logger.log(end_effector_link)
+
+        pose_stamped = request.ik_request.pose_stamped
+        transform_stamped = geometry_msgs.msg.TransformStamped
+        transform_stamped.header = pose_stamped.header
+        transform_stamped.child_frame_id = end_effector_link
+        transform_stamped.transform = geometry_msgs.msg.Transform
+        transform_stamped.transform.translation = geometry_msgs.msg.Vector3
+        transform_stamped.transform.translation.x = pose_stamped.pose.position.x
+        transform_stamped.transform.translation.y = pose_stamped.pose.position.y
+        transform_stamped.transform.translation.z = pose_stamped.pose.position.z
+        transform_stamped.transform.rotation = pose_stamped.pose.orientation
+        ik_logger.log(transform_stamped)
+
+        base_link = transform_stamped.header.frame_id
+        ik_logger.log(base_link)
+
+        joint_state = request.ik_request.robot_state.joint_state
+        ik_logger.log(joint_state)
+
+        timeout = request.ik_request.timeout
+        ik_logger.log(timeout)
+
+        response.solution.joint_state = joint_state
+        ik_logger.log("Using NR_JL solver")
+        urdf_string = self.get_parameter("robot_description")
+        new_joint_state_vector, success = kdl_ik_service.ik.calculate_ik(base_link, end_effector_link,
+                                                                         joint_state.position, transform_stamped,
+                                                                         ik_logger.log, urdf_string, "NR_JL")
+        if not success:
+            ik_logger.log("NR_JL solver failed. Using LMA solver")
+            new_joint_state_vector, success = kdl_ik_service.ik.calculate_ik(base_link, end_effector_link,
+                                                                             joint_state.position, transform_stamped,
+                                                                             ik_logger.log, urdf_string, "LMA")
+
+        ik_logger.log(new_joint_state_vector)
+        response.solution.joint_state.position = new_joint_state_vector
+
+        if success:
+            response.error_code.val = response.error_code.SUCCESS
+        else:
+            response.error_code.val = response.error_code.NO_IK_SOLUTION
+        ik_logger.log(response)
+        return response
+
+
+def main():
+    signal.signal(signal.SIGINT, signal_handler)
+
+    rclpy.init()
+    ik_service = IkService()
+    rclpy.spin(ik_service)
+    rclpy.shutdown()
+
+def signal_handler(sig, frame):
+    exit(0)
+
+class Logger(object):
+    def __init__(self, display_output=False):
+        self.display_output = display_output
+
+    def log(self, line):
+        if self.display_output:
+            print(line)
+
+
+if __name__ == "__main__":
+    main()

kdl_ik_service/setup.cfg

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

kdl_ik_service/setup.py

@@ -0,0 +1,30 @@
+from setuptools import setup,find_packages
+
+from glob import glob
+import os
+
+package_name = 'kdl_ik_service'
+
+setup(
+    name=package_name,
+    version='0.0.3',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py'))
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='jonas',
+    maintainer_email='[email protected]',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'start_ros_server = scripts.start_ros_server:main',
+        ],
+    },
+)