From 9cc984ba08151b7ea549667898118ad807f7e0e7 Mon Sep 17 00:00:00 2001 From: franklinselva Date: Mon, 9 Sep 2024 11:04:47 +0200 Subject: [PATCH] Update curiosity_rover_demo package for managing different simulation environments Related to #49 --- .../launch/mars_rover.launch.py | 24 ++++++- .../curiosity_rover_demo/nodes/move_arm | 62 +++++++++---------- .../curiosity_rover_demo/nodes/move_mast | 58 +++++++++-------- .../curiosity_rover_demo/nodes/move_wheel | 61 +++++++++--------- .../curiosity_rover_demo/package.xml | 1 + 5 files changed, 111 insertions(+), 95 deletions(-) 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