Add curiosity_common package for bridging Gazebo and IsaacSim

Related to space-ros#49

curiosity_rover/curiosity_common/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(curiosity_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 REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(action_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+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_package()

curiosity_rover/curiosity_common/curiosity_common/sim/arm_control.py

@@ -0,0 +1,125 @@
+"""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,
+        topic_name: str = "/arm_joint_trajectory_controller/joint_trajectory",
+    ):
+        super().__init__(node)
+
+        self.arm_publisher_ = self._node.create_publisher(
+            JointTrajectory,
+            topic_name,
+            10,
+        )
+
+    def set_joint_states(
+        self,
+        joint_names: List[str],
+        joint_positions: List[float],
+    ):
+        """Set joint states."""
+
+        if isinstance(joint_positions[0], JointTrajectoryPoint):
+            trajectory = JointTrajectory()
+            trajectory.joint_names = joint_names
+            trajectory.points = joint_positions
+            self.arm_publisher_.publish(trajectory)
+            return
+
+        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)
+
+    def set_joint_trajectory(
+        self, joint_names: List[str], trajectory: List[JointTrajectoryPoint]
+    ):
+        """Set joint trajectory."""
+
+        joint_trajectory = JointTrajectory()
+        joint_trajectory.joint_names = joint_names
+        joint_trajectory.points = trajectory
+
+        self.arm_publisher_.publish(joint_trajectory)
+
+
+class IsaacJointController(BaseJointControl):
+    """Joint control for IsaacSim."""
+
+    def __init__(self, node: Node, topic_name: str = "/canadarm2/arm/joint_command"):
+        super().__init__(node)
+
+        self.arm_publisher_ = self._node.create_publisher(
+            JointState,
+            topic_name,
+            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)
+
+    def set_joint_trajectory(
+        self, joint_names: List[str], trajectory: List[JointTrajectoryPoint]
+    ):
+        """Set joint trajectory."""
+
+        joint_state = JointState()
+        joint_state.name = joint_names
+        joint_state.position = trajectory[-1].positions
+
+        self.arm_publisher_.publish(joint_state)

curiosity_rover/curiosity_common/curiosity_common/sim/wheel_control.py

@@ -0,0 +1,133 @@
+"""This module allows the wheels 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 std_msgs.msg import Float64MultiArray  # type: ignore
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint  # type: ignore
+from geometry_msgs.msg import Twist  # type: ignore
+
+
+class BaseMobileControl:
+    """Base class for mobile 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 set_wheel_speed(self, mobile_positions: List[float]):
+        """Set mobile states."""
+        raise NotImplementedError
+
+
+class GazeboMobileController(BaseMobileControl):
+    """Mobile control for GazeboSim."""
+
+    def __init__(self, node: Node):
+        super().__init__(node)
+
+        self.wheel_publisher_ = self._node.create_publisher(
+            Float64MultiArray,
+            "/wheel_velocity_controller/commands",
+            10,
+        )
+
+        self.suspension_publisher_ = self._node.create_publisher(
+            Float64MultiArray,
+            "/wheel_tree_position_controller/commands",
+            10,
+        )
+
+        self.steer_publisher_ = self._node.create_publisher(
+            JointTrajectory,
+            "/steer_position_controller/joint_trajectory",
+            10,
+        )
+
+    def set_wheel_speed(
+        self,
+        mobile_positions: List[float],
+    ):
+        """Set mobile states."""
+
+        if len(mobile_positions) is not 6:
+            raise ValueError("Mobile positions should be 6 floats.")
+
+        msg = Float64MultiArray()
+        msg.data = mobile_positions
+        self.wheel_publisher_.publish(msg)
+
+    def set_suspension(
+        self,
+        mobile_positions: List[float],
+    ):
+        """Set mobile states."""
+
+        if len(mobile_positions) is not 4:
+            raise ValueError("Mobile positions should be 4 floats.")
+
+        msg = Float64MultiArray()
+        msg.data = mobile_positions
+        self.suspension_publisher_.publish(msg)
+
+    def set_steering(self, joint_names: List[str], joint_positions: List[float]):
+        """Set joint states."""
+        trajectory = JointTrajectory()
+        trajectory.joint_names = joint_names
+
+        point = JointTrajectoryPoint()
+        point.time_from_start = Duration(sec=1)
+        point.positions = joint_positions
+        trajectory.points.append(point)
+        self.steer_publisher_.publish(trajectory)
+
+
+class IsaacMobileController(BaseMobileControl):
+    """Mobile control for IsaacSim."""
+
+    def __init__(self, node: Node):
+        super().__init__(node)
+
+        self.wheel_publisher_ = self._node.create_publisher(
+            Twist,
+            "/curiosity/cmd_vel",
+            10,
+        )
+
+    def set_cmd_vel(self, msg: Twist):
+        """Forward Twist message to IsaacSim."""
+        self.wheel_publisher_.publish(msg)
+
+    def set_wheel_speed(
+        self,
+        mobile_positions: List[float],
+    ):
+        """Set mobile states."""
+        return
+
+    def set_suspension(
+        self,
+        _: List[float],
+    ):
+        """Set mobile states."""
+        return
+
+    def set_steer(self, _: List[str], __: List[float]):
+        """Set joint states."""
+        return

curiosity_rover/curiosity_common/package.xml

@@ -0,0 +1,27 @@
+<?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>curiosity_common</name>
+  <version>0.0.0</version>
+  <description>A common package for interaction with gazebo and isaacsim</description>
+  <maintainer email="[email protected]">franklinselva</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>action_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+
+  <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>ament_index_python</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>