diff --git a/embr_phys/ros2_ws/src/embr_core/embr/node_capstan_can.py b/embr_phys/ros2_ws/src/embr_core/embr/node_capstan_can.py new file mode 100644 index 0000000..e69de29 diff --git a/embr_phys/ros2_ws/src/embr_core/embr/node_capstan_helper.py b/embr_phys/ros2_ws/src/embr_core/embr/node_capstan_helper.py new file mode 100644 index 0000000..2f559de --- /dev/null +++ b/embr_phys/ros2_ws/src/embr_core/embr/node_capstan_helper.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 +"""Route normalized teleoperation commands to a capstan motor or RViz.""" + +import math +import sys +import time + +import rclpy +from embr_interfaces.msg import TeleCmd +from rclpy.node import Node +from std_msgs.msg import Float32, Float64MultiArray + + +class CapstanTeleopControlSystem(Node): + """A transport-independent bridge from ``tele_cmd`` to one motor.""" + + def __init__(self, simulation=False): + super().__init__("capstan_teleop_control_system") + self.declare_parameter("simulation", simulation) + self.declare_parameter("max_velocity", 10.0) + self.declare_parameter("command_timeout", 0.5) + self.declare_parameter("publish_period", 0.05) + + self._simulation = bool(self.get_parameter("simulation").value) + self._max_velocity = self._positive_parameter("max_velocity") + self._timeout = self._positive_parameter("command_timeout") + period = self._positive_parameter("publish_period") + if period >= self._timeout: + raise ValueError("publish_period must be less than command_timeout") + + self._velocity = 0.0 + self._last_command = None + message_type = Float64MultiArray if self._simulation else Float32 + topic = ( + "planetary_eagle_controller/commands" + if self._simulation + else "capstan_velocity_level" + ) + self._publisher = self.create_publisher(message_type, topic, 10) + self._subscriber = self.create_subscription( + TeleCmd, "tele_cmd", self.teleop_callback, 10 + ) + self._timer = self.create_timer(period, self._publish_command) + self.get_logger().info( + f"Capstan {'simulation' if self._simulation else 'real'} mode: {topic}" + ) + + def _positive_parameter(self, name): + value = float(self.get_parameter(name).value) + if not math.isfinite(value) or value <= 0.0: + raise ValueError(f"{name} must be finite and positive") + return value + + def teleop_callback(self, message): + # A single test-stand axis uses the forward channel. Turn is intentionally + # ignored so this node can share the same TeleCmd source as the drivetrain. + if math.isfinite(message.velocity): + self._velocity = max(-1.0, min(1.0, message.velocity)) + else: + self.get_logger().warn("Invalid tele_cmd: stopping capstan") + self._velocity = 0.0 + self._last_command = time.monotonic() + self._publish_command() + + def _publish_command(self): + if ( + self._last_command is None + or time.monotonic() - self._last_command > self._timeout + ): + self._velocity = 0.0 + + if self._simulation: + command = Float64MultiArray() + command.data = [self._velocity * self._max_velocity] + else: + command = Float32() + command.data = self._velocity + self._publisher.publish(command) + + +def main(args=None): + cli_args = list(sys.argv[1:] if args is None else args) + simulation = "--sim" in cli_args or "-sim" in cli_args + cli_args = [arg for arg in cli_args if arg not in ("--sim", "-sim")] + rclpy.init(args=cli_args) + node = None + try: + node = CapstanTeleopControlSystem(simulation=simulation) + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/embr_phys/ros2_ws/src/embr_core/embr/node_canopen_handler.py b/embr_phys/ros2_ws/src/embr_core/embr/node_maxon_canopen.py similarity index 100% rename from embr_phys/ros2_ws/src/embr_core/embr/node_canopen_handler.py rename to embr_phys/ros2_ws/src/embr_core/embr/node_maxon_canopen.py diff --git a/embr_phys/ros2_ws/src/embr_core/embr/node_maxon_drivetrain.py b/embr_phys/ros2_ws/src/embr_core/embr/node_maxon_helper.py similarity index 100% rename from embr_phys/ros2_ws/src/embr_core/embr/node_maxon_drivetrain.py rename to embr_phys/ros2_ws/src/embr_core/embr/node_maxon_helper.py diff --git a/embr_phys/ros2_ws/src/embr_core/setup.py b/embr_phys/ros2_ws/src/embr_core/setup.py index 4ee64f9..bd5e55a 100644 --- a/embr_phys/ros2_ws/src/embr_core/setup.py +++ b/embr_phys/ros2_ws/src/embr_core/setup.py @@ -23,9 +23,11 @@ license="Apache-2.0", entry_points={ "console_scripts": [ - "CANopen = embr.node_canopen_handler:main", - "drivetrain = embr.node_maxon_drivetrain:main", - "showcase = embr.node_maxon_single_showcase:main", + "dt_can = embr.node_maxon_canopen:main", + "dt_helper = embr.node_maxon_helper:main", + "dt_showcase = embr.node_maxon_single_showcase:main", + "cp_helper = embr.node_capstan_helper:main", + "cp_can = embr.node_capstan_can:main", "teleoperation = embr.node_teleoperation:main", ], }, diff --git a/embr_phys/ros2_ws/src/embr_core/test/test_canopen_handler.py b/embr_phys/ros2_ws/src/embr_core/test/test_canopen_handler.py index dd2867c..946874f 100644 --- a/embr_phys/ros2_ws/src/embr_core/test/test_canopen_handler.py +++ b/embr_phys/ros2_ws/src/embr_core/test/test_canopen_handler.py @@ -17,7 +17,7 @@ def handler(monkeypatch): sys.modules['rclpy.qos'].QoSProfile = Mock() sys.modules['embr_interfaces.msg'].OperationStatus = SimpleNamespace sys.modules['std_msgs.msg'].Float32MultiArray = SimpleNamespace - path = Path(__file__).parents[1] / 'embr/node_canopen_handler.py' + path = Path(__file__).parents[1] / 'embr/node_maxon_handler.py' spec = importlib.util.spec_from_file_location('handler_under_test', path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) diff --git a/embr_phys/ros2_ws/src/embr_description/config/planetary_eagle_controllers.yaml b/embr_phys/ros2_ws/src/embr_description/config/planetary_eagle_controllers.yaml new file mode 100644 index 0000000..00b93b7 --- /dev/null +++ b/embr_phys/ros2_ws/src/embr_description/config/planetary_eagle_controllers.yaml @@ -0,0 +1,14 @@ +controller_manager: + ros__parameters: + update_rate: 50 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + planetary_eagle_controller: + type: forward_command_controller/ForwardCommandController + +planetary_eagle_controller: + ros__parameters: + joints: [revolute_1] + interface_name: velocity diff --git a/embr_phys/ros2_ws/src/embr_description/launch/view_planetary_eagle.launch.py b/embr_phys/ros2_ws/src/embr_description/launch/view_planetary_eagle.launch.py new file mode 100644 index 0000000..c24f889 --- /dev/null +++ b/embr_phys/ros2_ws/src/embr_description/launch/view_planetary_eagle.launch.py @@ -0,0 +1,62 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + package_share = FindPackageShare("embr_description") + model_path = PathJoinSubstitution( + [package_share, "urdf", "planetary_eagle", "urdf", "eagle_power_test_stand_assembly.xacro"] + ) + controllers_file = PathJoinSubstitution( + [package_share, "config", "planetary_eagle_controllers.yaml"] + ) + rviz_config = PathJoinSubstitution([package_share, "rviz", "planetary_eagle.rviz"]) + robot_description = ParameterValue( + Command([FindExecutable(name="xacro"), " ", model_path]), value_type=str + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + "teleop", + default_value="true", + description="Start the TeleCmd-to-simulated-joint command bridge.", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[{"robot_description": robot_description, "publish_frequency": 50.0}], + output="screen", + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[{"robot_description": robot_description}, controllers_file], + output="screen", + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "planetary_eagle_controller"], + output="screen", + ), + Node( + package="embr_core", + executable="cp_helper", + arguments=["--sim"], + condition=IfCondition(LaunchConfiguration("teleop")), + output="screen", + ), + Node( + package="rviz2", + executable="rviz2", + arguments=["-d", rviz_config], + output="screen", + ), + ] + ) diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Encoder_Magnet_Holder.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Encoder_Magnet_Holder.stl new file mode 100644 index 0000000..e155c20 Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Encoder_Magnet_Holder.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/ODrive_S1.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/ODrive_S1.stl new file mode 100644 index 0000000..4061986 Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/ODrive_S1.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/ODrive_S1_1.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/ODrive_S1_1.stl new file mode 100644 index 0000000..e55c118 Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/ODrive_S1_1.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/ODrive_S1_Cover.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/ODrive_S1_Cover.stl new file mode 100644 index 0000000..c83609d Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/ODrive_S1_Cover.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Part_1.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Part_1.stl new file mode 100644 index 0000000..9415cfb Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Part_1.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor.stl new file mode 100644 index 0000000..2f330ae Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor_1.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor_1.stl new file mode 100644 index 0000000..bad87b3 Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor_1.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor_2.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor_2.stl new file mode 100644 index 0000000..e070038 Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor_2.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor_3.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor_3.stl new file mode 100644 index 0000000..1c383f8 Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor_3.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor_4.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor_4.stl new file mode 100644 index 0000000..35ec2e5 Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Rotor_4.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator.stl new file mode 100644 index 0000000..1209620 Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator_1.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator_1.stl new file mode 100644 index 0000000..9871e91 Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator_1.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator_2.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator_2.stl new file mode 100644 index 0000000..9ff88f6 Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator_2.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator_3.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator_3.stl new file mode 100644 index 0000000..51c0c28 Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator_3.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator_4.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator_4.stl new file mode 100644 index 0000000..3fd9ace Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Stator_4.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Test_Stand.stl b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Test_Stand.stl new file mode 100644 index 0000000..54015d0 Binary files /dev/null and b/embr_phys/ros2_ws/src/embr_description/meshes/meshes_eagle/meshes/Test_Stand.stl differ diff --git a/embr_phys/ros2_ws/src/embr_description/package.xml b/embr_phys/ros2_ws/src/embr_description/package.xml index 806102f..bae2ef0 100644 --- a/embr_phys/ros2_ws/src/embr_description/package.xml +++ b/embr_phys/ros2_ws/src/embr_description/package.xml @@ -14,10 +14,12 @@ rviz2 controller_manager diff_drive_controller + forward_command_controller joint_state_broadcaster hardware_interface teleop_twist_keyboard xacro + embr_core ament_cmake diff --git a/embr_phys/ros2_ws/src/embr_description/rviz/planetary_eagle.rviz b/embr_phys/ros2_ws/src/embr_description/rviz/planetary_eagle.rviz new file mode 100644 index 0000000..2e086d5 --- /dev/null +++ b/embr_phys/ros2_ws/src/embr_description/rviz/planetary_eagle.rviz @@ -0,0 +1,62 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 0.01 + Class: rviz_default_plugins/Grid + Name: Grid + Plane: XY + Plane Cell Count: 20 + Reference Frame: root + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Name: Planetary Eagle + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: root + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.18 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Pitch: 0.45 + Target Frame: root + Value: Orbit (rviz) + Yaw: 0.785 + Saved: ~ +Window Geometry: + Height: 900 + Width: 1440 diff --git a/embr_phys/ros2_ws/src/embr_description/urdf/planetary_eagle/urdf/eagle_power_test_stand_assembly.xacro b/embr_phys/ros2_ws/src/embr_description/urdf/planetary_eagle/urdf/eagle_power_test_stand_assembly.xacro new file mode 100644 index 0000000..cb52d4b --- /dev/null +++ b/embr_phys/ros2_ws/src/embr_description/urdf/planetary_eagle/urdf/eagle_power_test_stand_assembly.xacro @@ -0,0 +1,359 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/embr_phys/ros2_ws/src/embr_description/urdf/planetary_eagle/urdf/planetary_eagle.ros2_control.xacro b/embr_phys/ros2_ws/src/embr_description/urdf/planetary_eagle/urdf/planetary_eagle.ros2_control.xacro new file mode 100644 index 0000000..381eb95 --- /dev/null +++ b/embr_phys/ros2_ws/src/embr_description/urdf/planetary_eagle/urdf/planetary_eagle.ros2_control.xacro @@ -0,0 +1,23 @@ + + + + + mock_components/GenericSystem + true + + + + + + + + + + + + + + +