diff --git a/curiosity_rover/curiosity_rover_demo/launch/mars_rover.launch.py b/curiosity_rover/curiosity_rover_demo/launch/mars_rover.launch.py
index f7456ea1..76d8c647 100644
--- a/curiosity_rover/curiosity_rover_demo/launch/mars_rover.launch.py
+++ b/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,
diff --git a/curiosity_rover/curiosity_rover_demo/nodes/move_arm b/curiosity_rover/curiosity_rover_demo/nodes/move_arm
index 7386d6ea..bfa17f80 100755
--- a/curiosity_rover/curiosity_rover_demo/nodes/move_arm
+++ b/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
diff --git a/curiosity_rover/curiosity_rover_demo/nodes/move_mast b/curiosity_rover/curiosity_rover_demo/nodes/move_mast
index e60e16d0..9c02e08a 100755
--- a/curiosity_rover/curiosity_rover_demo/nodes/move_mast
+++ b/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,30 +26,38 @@ 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(
@@ -58,25 +65,22 @@ class MastArm(Node):
): # 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
diff --git a/curiosity_rover/curiosity_rover_demo/nodes/move_wheel b/curiosity_rover/curiosity_rover_demo/nodes/move_wheel
index 376c828a..95c6b811 100755
--- a/curiosity_rover/curiosity_rover_demo/nodes/move_wheel
+++ b/curiosity_rover/curiosity_rover_demo/nodes/move_wheel
@@ -4,11 +4,9 @@ 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):
@@ -16,15 +14,21 @@ class MoveWheel(Node):
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."""
diff --git a/curiosity_rover/curiosity_rover_demo/package.xml b/curiosity_rover/curiosity_rover_demo/package.xml
index a0f0d92a..48272c9f 100644
--- a/curiosity_rover/curiosity_rover_demo/package.xml
+++ b/curiosity_rover/curiosity_rover_demo/package.xml
@@ -28,6 +28,7 @@
ros2controlcli
std_msgs
velocity_controllers
+ curiosity_common
ament_lint_auto
ament_lint_common