Add canadarm_common package for bridging simulation between Gazebo an…

…d IsaacSim

Related to space-ros#48

canadarm2/canadarm_common/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.8)
+project(canadarm_common)
+
+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_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(action_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# rosidl_generate_interfaces(${PROJECT_NAME}
+#   "action/MoveJoint.action"
+# )
+
+# Install Python modules
+ament_python_install_package(${PROJECT_NAME})
+
+install(DIRECTORY
+  action
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

canadarm2/canadarm_common/canadarm_common/sim/joint_control.py

@@ -0,0 +1,88 @@
+"""This module allows the joints control between two simulation environments.
+
+### Supoprted Simulation Environments
+ - GazeboSim
+    - GazeboSim uses ROS2 Control for joint control.
+ - IsaacSim
+    - IsaacSim expects joint states to be published on a topic.
+"""
+
+from typing import List
+
+from rclpy.node import Node  # type: ignore
+
+from builtin_interfaces.msg import Duration  # type: ignore
+from sensor_msgs.msg import JointState  # type: ignore
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint  # type: ignore
+
+
+class BaseJointControl:
+    """Base class for joint control."""
+
+    _instance = None
+    _node: Node = None
+
+    def __new__(cls, *args, **kwargs):
+        if cls._instance is None:
+            cls._instance = super().__new__(cls)
+        return cls._instance
+
+    def __init__(self, node: Node):
+        self._node = node
+
+    def get_joint_states(self):
+        """Get joint states."""
+        raise NotImplementedError
+
+    def set_joint_states(self, joint_names: List[str], joint_positions: List[float]):
+        """Set joint states."""
+        raise NotImplementedError
+
+
+class GazeboJointController(BaseJointControl):
+    """Joint control for GazeboSim."""
+
+    def __init__(self, node: Node):
+        super().__init__(node)
+
+        self.arm_publisher_ = self._node.create_publisher(
+            JointTrajectory,
+            "/canadarm_joint_trajectory_controller/joint_trajectory",
+            10,
+        )
+
+    def set_joint_states(self, joint_names: List[str], joint_positions: List[float]):
+        """Set joint states."""
+
+        trajectory = JointTrajectory()
+        trajectory.joint_names = joint_names
+
+        point = JointTrajectoryPoint()
+        point.positions = joint_positions
+        point.time_from_start = Duration(sec=4)
+
+        trajectory.points.append(point)
+
+        self.arm_publisher_.publish(trajectory)
+
+
+class IsaacJointController(BaseJointControl):
+    """Joint control for IsaacSim."""
+
+    def __init__(self, node: Node):
+        super().__init__(node)
+
+        self.arm_publisher_ = self._node.create_publisher(
+            JointState,
+            "/canadarm2/joint_command",
+            10,
+        )
+
+    def set_joint_states(self, joint_names: List[str], joint_positions: List[float]):
+        """Set joint states."""
+
+        joint_state = JointState()
+        joint_state.name = joint_names
+        joint_state.position = joint_positions
+
+        self.arm_publisher_.publish(joint_state)

canadarm2/canadarm_common/package.xml

@@ -0,0 +1,30 @@
+<?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>canadarm_common</name>
+  <version>0.0.1</version>
+  <description>Common modules, interfaces and integrations for canadarm2 demos</description>
+  <maintainer email="[email protected]">franklinselva</maintainer>
+  <license>GPL</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>rosidl_interface_packages</depend>
+  <depend>action_msgs</depend>
+
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>