Update curiosity_rover_demo package for managing different simulation…

… environments

Related to #49

curiosity_rover/curiosity_rover_demo/launch/mars_rover.launch.py

@@ -1,22 +1,39 @@
 """Launch the curiosity rover demo."""
 
 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, SetParameter  # type: ignore
 
 
 def generate_launch_description():
     """Launch the curiosity rover demo."""
 
+    environment_arg = DeclareLaunchArgument(
+        "environment",
+        default_value="gazebosim",
+        description="Environment to run the demo in",
+    )
+
     arm_node = Node(
-        package="curiosity_rover_demo", executable="move_arm", output="screen"
+        package="curiosity_rover_demo",
+        executable="move_arm",
+        output="screen",
+        parameters=[{"environment": LaunchConfiguration("environment")}],
     )
 
     mast_node = Node(
-        package="curiosity_rover_demo", executable="move_mast", output="screen"
+        package="curiosity_rover_demo",
+        executable="move_mast",
+        output="screen",
+        parameters=[{"environment": LaunchConfiguration("environment")}],
     )
 
     wheel_node = Node(
-        package="curiosity_rover_demo", executable="move_wheel", output="screen"
+        package="curiosity_rover_demo",
+        executable="move_wheel",
+        output="screen",
+        parameters=[{"environment": LaunchConfiguration("environment")}],
     )
 
     run_node = Node(
@@ -26,6 +43,7 @@ def generate_launch_description():
     return LaunchDescription(
         [
             SetParameter(name="use_sim_time", value=True),
+            environment_arg,
             arm_node,
             mast_node,
             wheel_node,

curiosity_rover/curiosity_rover_demo/nodes/move_arm

@@ -3,66 +3,60 @@
 
 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 curiosity_common.sim.arm_control import GazeboJointController, IsaacJointController  # type: ignore
+
 
 class MoveArm(Node):
     """Move the arm of the curiosity rover."""
 
     def __init__(self):
         super().__init__("arm_node")
-        self.arm_publisher_ = self.create_publisher(
-            JointTrajectory, "/arm_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
         )
 
-        self.open = True
+        self.declare_parameter(
+            "environment",
+            rclpy.Parameter.Type.STRING,
+        )
 
-    def open_arm_callback(self, request, response):  # pylint: disable=unused-argument
-        """Open the arm of the curiosity rover."""
-        self.open = True
-        traj = JointTrajectory()
-        traj.joint_names = [
+        self._environment = (
+            self.get_parameter("environment").get_parameter_value().string_value
+        )
+
+        if self._environment == "gazebosim":
+            self._controller = GazeboJointController(
+                self, "/arm_joint_trajectory_controller/joint_trajectory"
+            )
+        elif self._environment == "isaacsim":
+            self._controller = IsaacJointController(
+                self, "/curiosity/arm/joint_command"
+            )
+
+        self._joint_names = [
             "arm_01_joint",
             "arm_02_joint",
             "arm_03_joint",
             "arm_04_joint",
             "arm_tools_joint",
         ]
+        self._open_positions = [0.0, -0.5, 3.0, 1.0, 0.0]
+        self._close_positions = [-1.57, -0.4, -1.1, -1.57, -1.57]
 
-        point1 = JointTrajectoryPoint()
-        point1.positions = [0.0, -0.5, 3.0, 1.0, 0.0]
-        point1.time_from_start = Duration(sec=4)
+        self.open = True
 
-        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 of the curiosity rover."""
-        self.open = False
-        traj = JointTrajectory()
-        traj.joint_names = [
-            "arm_01_joint",
-            "arm_02_joint",
-            "arm_03_joint",
-            "arm_04_joint",
-            "arm_tools_joint",
-        ]
-
-        point1 = JointTrajectoryPoint()
-        point1.positions = [-1.57, -0.4, -1.1, -1.57, -1.57]
-        point1.time_from_start = Duration(sec=4)
-
-        traj.points.append(point1)
-        self.arm_publisher_.publish(traj)
+        """Close the arm."""
+        self._controller.set_joint_states(self._joint_names, self._close_positions)
 
         return response
 

curiosity_rover/curiosity_rover_demo/nodes/move_mast

@@ -5,18 +5,17 @@ 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 trajectory_msgs.msg import JointTrajectoryPoint  # type: ignore
 from std_srvs.srv import Empty  # type: ignore
 
+from curiosity_common.sim.arm_control import GazeboJointController, IsaacJointController  # type: ignore
+
 
 class MastArm(Node):
     """Move the mast arm of the curiosity rover."""
 
     def __init__(self):
         super().__init__("mast_node")
-        self.mast_publisher_ = self.create_publisher(
-            JointTrajectory, "/mast_joint_trajectory_controller/joint_trajectory", 10
-        )
         self.mast_open_srv = self.create_service(
             Empty, "mast_open", self.mast_open_callback
         )
@@ -27,56 +26,61 @@ class MastArm(Node):
             Empty, "mast_rotate", self.mast_rotate_callback
         )
 
+        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, "/mast_joint_trajectory_controller/joint_trajectory"
+            )
+        elif self._environment == "isaacsim":
+            self._controller = IsaacJointController(
+                self, "/curiosity/mast_arm/joint_command"
+            )
+
+        self._joint_names = ["mast_p_joint", "mast_02_joint", "mast_cameras_joint"]
+        self._open_positions = [0.0, -0.5, 0.0]
+        self._close_positions = [1.57, -1.57, 0.0]
+
     def mast_open_callback(self, request, response):  # pylint: disable=unused-argument
         """Move the mast arm to open position."""
-        traj = JointTrajectory()
-        traj.joint_names = ["mast_p_joint", "mast_02_joint", "mast_cameras_joint"]
-
-        point = JointTrajectoryPoint()
-        point.positions = [0.0, -0.5, 0.0]
-        point.time_from_start = Duration(sec=1)
+        self._controller.set_joint_states(self._joint_names, self._open_positions)
 
-        traj.points.append(point)
-        self.mast_publisher_.publish(traj)
         return response
 
     def mast_close_callback(self, request, response):  # pylint: disable=unused-argument
         """Move the mast arm to close position."""
-        traj = JointTrajectory()
-        traj.joint_names = ["mast_p_joint", "mast_02_joint", "mast_cameras_joint"]
-
-        point = JointTrajectoryPoint()
-        point.positions = [1.57, -1.57, 0.0]
-        point.time_from_start = Duration(sec=1)
+        self._controller.set_joint_states(self._joint_names, self._close_positions)
 
-        traj.points.append(point)
-        self.mast_publisher_.publish(traj)
         return response
 
     def mast_rotate_callback(
         self, request, response
     ):  # pylint: disable=unused-argument
         """Rotate the mast arm."""
 
-        traj = JointTrajectory()
-        traj.joint_names = ["mast_p_joint", "mast_02_joint", "mast_cameras_joint"]
-
         point1 = JointTrajectoryPoint()
         point1.positions = [0.0, -1.57, 0.0]
         point1.time_from_start = Duration(sec=2)
-        traj.points.append(point1)
 
         point2 = JointTrajectoryPoint()
         point2.positions = [0.0, -3.14, 0.0]
         point2.time_from_start = Duration(sec=4)
-        traj.points.append(point2)
 
         point3 = JointTrajectoryPoint()
         point3.positions = [0.0, -6.28, 0.0]
         point3.time_from_start = Duration(sec=6)
-        traj.points.append(point3)
 
-        self.mast_publisher_.publish(traj)
+        self._controller.set_joint_trajectory(
+            self._joint_names, [point1, point2, point3]
+        )
+
         return response
 
 

curiosity_rover/curiosity_rover_demo/nodes/move_wheel

@@ -4,27 +4,31 @@ import math
 
 import rclpy  # type: ignore
 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
+from curiosity_common.sim.wheel_control import GazeboMobileController, IsaacMobileController  # type: ignore
 
 
 class MoveWheel(Node):
     """Move the wheels of the curiosity rover."""
 
     def __init__(self):
         super().__init__("wheel_node")
-        self.wheel_publisher_ = self.create_publisher(
-            Float64MultiArray, "/wheel_velocity_controller/commands", 10
-        )
-        self.steer_publisher_ = self.create_publisher(
-            JointTrajectory, "/steer_position_controller/joint_trajectory", 10
+
+        self.declare_parameter(
+            "environment",
+            rclpy.Parameter.Type.STRING,
         )
-        self.suspension_publisher_ = self.create_publisher(
-            Float64MultiArray, "/wheel_tree_position_controller/commands", 10
+
+        self._environment = (
+            self.get_parameter("environment").get_parameter_value().string_value
         )
+
+        if self._environment == "gazebosim":
+            self._mobile_controller = GazeboMobileController(self)
+        else:
+            self._mobile_controller = IsaacMobileController(self)
+
         timer_period = 0.1  # seconds
         self.timer = self.create_timer(timer_period, self.timer_callback)
 
@@ -37,6 +41,11 @@ class MoveWheel(Node):
 
     def vel_callback(self, msg):
         """Velocity callback."""
+
+        if self._environment == "isaacsim":
+            self._mobile_controller.set_cmd_vel(msg)
+            return
+
         if (
             abs(self.last_vel.angular.z - self.curr_vel.angular.z) > 0.01
             and self.should_steer is False
@@ -51,11 +60,10 @@ class MoveWheel(Node):
     def set_wheel_common_speed(self, vel):
         """Set the wheel common speed."""
 
-        target_vel = Float64MultiArray()
-        target_vel.data = [vel, vel * 1.5, vel, -vel, -vel * 1.5, -vel]
+        target_vel = [vel, vel * 1.5, vel, -vel, -vel * 1.5, -vel]
 
         # self.get_logger().info(f'Publishing: "{target_vel.data}"')
-        self.wheel_publisher_.publish(target_vel)
+        self._mobile_controller.set_wheel_speed(target_vel)
 
     def map_angular_to_steering(self, angular_speed) -> float:
         """Map angular speed to steering angle."""
@@ -69,18 +77,16 @@ class MoveWheel(Node):
     def set_steering(self, turn_rad):
         """Set the steering angle."""
         # Ideal Ackerman Steering
-        target_steer = JointTrajectory()
-        # target_steer.header.stamp = self.get_clock().now().to_msg()
-        target_steer.joint_names = [
+        joint_names = [
             "suspension_steer_F_L_joint",
             "suspension_steer_B_L_joint",
             "suspension_steer_F_R_joint",
             "suspension_steer_B_R_joint",
         ]
-        steer_point = JointTrajectoryPoint()
+        steer_point = []
 
         if abs(turn_rad) < 1e-3:
-            steer_point.positions = [0.0, 0.0, 0.0, 0.0]
+            steer_point = [0.0, 0.0, 0.0, 0.0]
         else:
             turning_radius = abs(turn_rad)  # R
 
@@ -100,23 +106,16 @@ class MoveWheel(Node):
             alpha_o = round(alpha_o, 2)
 
             if turn_rad > 0.0:
-                steer_point.positions = [alpha_i, -alpha_i, alpha_o, -alpha_o]
+                steer_point = [alpha_i, -alpha_i, alpha_o, -alpha_o]
             else:
-                steer_point.positions = [-alpha_o, alpha_o, -alpha_i, alpha_i]
-
-        steer_point.time_from_start = Duration(sec=1)
+                steer_point = [-alpha_o, alpha_o, -alpha_i, alpha_i]
 
-        target_steer.points.append(steer_point)
+        self._mobile_controller.set_steering(joint_names, steer_point)
 
-        self.steer_publisher_.publish(target_steer)
-
-    def set_suspension(self, sus_val=None):
+    def set_suspension(self):
         """Set the suspension."""
-
-        target_val = Float64MultiArray()
-        if sus_val is None:
-            target_val.data = [0.3, -0.1, 0.3, -0.1]
-        self.suspension_publisher_.publish(target_val)
+        target_val = [0.3, -0.1, 0.3, -0.1]
+        self._mobile_controller.set_suspension(target_val)
 
     def timer_callback(self):
         """Timer callback."""

curiosity_rover/curiosity_rover_demo/package.xml

@@ -28,6 +28,7 @@
   <exec_depend>ros2controlcli</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>velocity_controllers</exec_depend>
+  <exec_depend>curiosity_common</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>