From 4a10958947d1716765c1c88e1d42e092a450d63c Mon Sep 17 00:00:00 2001 From: franklinselva Date: Mon, 9 Sep 2024 11:02:55 +0200 Subject: [PATCH] Add curiosity_common package for bridging Gazebo and IsaacSim Related to #49 --- .../curiosity_common/CMakeLists.txt | 31 ++++ .../curiosity_common/__init__.py | 0 .../curiosity_common/sim/__init__.py | 0 .../curiosity_common/sim/arm_control.py | 125 ++++++++++++++++ .../curiosity_common/sim/wheel_control.py | 133 ++++++++++++++++++ curiosity_rover/curiosity_common/package.xml | 27 ++++ 6 files changed, 316 insertions(+) create mode 100644 curiosity_rover/curiosity_common/CMakeLists.txt create mode 100644 curiosity_rover/curiosity_common/curiosity_common/__init__.py create mode 100644 curiosity_rover/curiosity_common/curiosity_common/sim/__init__.py create mode 100644 curiosity_rover/curiosity_common/curiosity_common/sim/arm_control.py create mode 100644 curiosity_rover/curiosity_common/curiosity_common/sim/wheel_control.py create mode 100644 curiosity_rover/curiosity_common/package.xml diff --git a/curiosity_rover/curiosity_common/CMakeLists.txt b/curiosity_rover/curiosity_common/CMakeLists.txt new file mode 100644 index 00000000..07b3ecf5 --- /dev/null +++ b/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() diff --git a/curiosity_rover/curiosity_common/curiosity_common/__init__.py b/curiosity_rover/curiosity_common/curiosity_common/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/curiosity_rover/curiosity_common/curiosity_common/sim/__init__.py b/curiosity_rover/curiosity_common/curiosity_common/sim/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/curiosity_rover/curiosity_common/curiosity_common/sim/arm_control.py b/curiosity_rover/curiosity_common/curiosity_common/sim/arm_control.py new file mode 100644 index 00000000..d4c05a48 --- /dev/null +++ b/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) diff --git a/curiosity_rover/curiosity_common/curiosity_common/sim/wheel_control.py b/curiosity_rover/curiosity_common/curiosity_common/sim/wheel_control.py new file mode 100644 index 00000000..8e06cc76 --- /dev/null +++ b/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 diff --git a/curiosity_rover/curiosity_common/package.xml b/curiosity_rover/curiosity_common/package.xml new file mode 100644 index 00000000..942953c4 --- /dev/null +++ b/curiosity_rover/curiosity_common/package.xml @@ -0,0 +1,27 @@ + + + + curiosity_common + 0.0.0 + A common package for interaction with gazebo and isaacsim + franklinselva + TODO: License declaration + + ament_cmake + + action_msgs + sensor_msgs + geometry_msgs + rclcpp + rclpy + + trajectory_msgs + ament_index_python + + ament_lint_auto + ament_lint_common + + + ament_cmake + +