Update canadarm_demo to manage gazebosim and isaacsim environments

Related to space-ros#48

canadarm2/canadarm_demo/CMakeLists.txt

@@ -9,12 +9,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(rclpy REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-
-rosidl_generate_interfaces(${PROJECT_NAME}
-  "action/MoveJoint.action"
-)
-
+find_package(canadarm_common REQUIRED)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -30,7 +25,6 @@ endif()
 
 
 install(DIRECTORY
-  action
   launch
   DESTINATION share/${PROJECT_NAME}
 )

canadarm2/canadarm_demo/launch/canadarm.launch.py

@@ -1,24 +1,30 @@
 """Canadarm2 demo launch file."""
 
 from launch import LaunchDescription  # type: ignore
-
+from launch.actions import DeclareLaunchArgument  # type: ignore
+from launch.substitutions import LaunchConfiguration  # type: ignore
 from launch_ros.actions import Node  # type: ignore
 
 
 def generate_launch_description():
     """Generate launch description with multiple components."""
 
-    # run_node = Node(
-    #    package="canadarm",
-    #    executable="move_joint_server",
-    #    output='screen'
-    # )
+    environment_arg = DeclareLaunchArgument(
+        "environment",
+        default_value="gazebosim",
+        description="Environment to run the node in",
+    )
 
-    run_move_arm = Node(package="canadarm_demo", executable="move_arm", output="screen")
+    run_move_arm = Node(
+        package="canadarm_demo",
+        executable="move_arm",
+        output="screen",
+        parameters=[{"environment": LaunchConfiguration("environment")}],
+    )
 
     return LaunchDescription(
         [
-            # run_node,
+            environment_arg,
             run_move_arm,
         ]
     )

canadarm2/canadarm_demo/nodes/move_arm

@@ -3,22 +3,16 @@
 
 import rclpy  # type: ignore
 from rclpy.node import Node  # type: ignore
-from builtin_interfaces.msg import Duration  # type: ignore
-
-from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint  # type: ignore
 from std_srvs.srv import Empty  # type: ignore
 
+from canadarm_common.sim.joint_control import GazeboJointController, IsaacJointController  # type: ignore
+
 
 class MoveArm(Node):
     """Move the arm to a specific position."""
 
     def __init__(self):
         super().__init__("arm_node")
-        self.arm_publisher_ = self.create_publisher(
-            JointTrajectory,
-            "/canadarm_joint_trajectory_controller/joint_trajectory",
-            10,
-        )
         self.open_srv = self.create_service(Empty, "open_arm", self.open_arm_callback)
         self.close_srv = self.create_service(
             Empty, "close_arm", self.close_arm_callback
@@ -27,10 +21,23 @@ class MoveArm(Node):
             Empty, "random_arm", self.random_arm_callback
         )
 
-    def open_arm_callback(self, request, response):  # pylint: disable=unused-argument
-        """Open the arm."""
-        traj = JointTrajectory()
-        traj.joint_names = [
+        # Set sim parameter
+        self.declare_parameter(
+            "environment",
+            rclpy.Parameter.Type.STRING,
+        )
+
+        self._environment = (
+            self.get_parameter("environment").get_parameter_value().string_value
+        )
+
+        if self._environment == "gazebosim":
+            self._controller = GazeboJointController(self)
+        elif self._environment == "isaacsim":
+            self._controller = IsaacJointController(self)
+        self._logger.info(f"Using {self._environment} environment")
+
+        self._joint_names = [
             "Base_Joint",
             "Shoulder_Roll",
             "Shoulder_Yaw",
@@ -39,57 +46,25 @@ class MoveArm(Node):
             "Wrist_Yaw",
             "Wrist_Roll",
         ]
+        self._open_positions = [0.0, 0.0, 0.0, -3.1416, 0.0, 0.0, 0.0]
+        self._close_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        self._random_positions = [1.0, -1.5, 2.0, -3.2, 0.8, 0.5, -1.0]
 
-        point1 = JointTrajectoryPoint()
-        point1.positions = [0.0, 0.0, 0.0, -3.1416, 0.0, 0.0, 0.0]
-        point1.time_from_start = Duration(sec=4)
-
-        traj.points.append(point1)
-        self.arm_publisher_.publish(traj)
+    def open_arm_callback(self, request, response):  # pylint: disable=unused-argument
+        """Open the arm."""
+        self._controller.set_joint_states(self._joint_names, self._open_positions)
 
         return response
 
     def close_arm_callback(self, request, response):  # pylint: disable=unused-argument
         """Close the arm."""
-        traj = JointTrajectory()
-        traj.joint_names = [
-            "Base_Joint",
-            "Shoulder_Roll",
-            "Shoulder_Yaw",
-            "Elbow_Pitch",
-            "Wrist_Pitch",
-            "Wrist_Yaw",
-            "Wrist_Roll",
-        ]
-
-        point1 = JointTrajectoryPoint()
-        point1.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-        point1.time_from_start = Duration(sec=4)
-
-        traj.points.append(point1)
-        self.arm_publisher_.publish(traj)
+        self._controller.set_joint_states(self._joint_names, self._close_positions)
 
         return response
 
     def random_arm_callback(self, request, response):  # pylint: disable=unused-argument
         """Move the arm to a random position."""
-        traj = JointTrajectory()
-        traj.joint_names = [
-            "Base_Joint",
-            "Shoulder_Roll",
-            "Shoulder_Yaw",
-            "Elbow_Pitch",
-            "Wrist_Pitch",
-            "Wrist_Yaw",
-            "Wrist_Roll",
-        ]
-
-        point1 = JointTrajectoryPoint()
-        point1.positions = [1.0, -1.5, 2.0, -3.2, 0.8, 0.5, -1.0]
-        point1.time_from_start = Duration(sec=4)
-
-        traj.points.append(point1)
-        self.arm_publisher_.publish(traj)
+        self._controller.set_joint_states(self._joint_names, self._random_positions)
 
         return response
 

canadarm2/canadarm_demo/nodes/move_joint_server

@@ -7,7 +7,7 @@ from rclpy.node import Node  # type: ignore
 
 from std_msgs.msg import Float64MultiArray  # type: ignore
 
-from canadarm_demo.action import MoveJoint
+from canadarm_common.action import MoveJoint
 
 
 class MoveJointActionServer(Node):

canadarm2/canadarm_demo/package.xml

@@ -9,20 +9,16 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_python</buildtool_depend>
-  <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <depend>rclcpp</depend>
   <depend>rclpy</depend>
-  <depend>action_msgs</depend>
+  <depend>canadarm_common</depend>
 
   <exec_depend>ament_index_python</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>std_msgs</exec_depend>
 
-  <member_of_group>rosidl_interface_packages</member_of_group>
-
   <export>
     <test_depend>ament_lint_auto</test_depend>
     <test_depend>ament_lint_common</test_depend>