Added simple logic of gearbox and rviz sim

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()

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",
         ],
     },

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)

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

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",
+            ),
+        ]
+    )

embr_phys/ros2_ws/src/embr_description/package.xml

@@ -14,10 +14,12 @@
   <exec_depend>rviz2</exec_depend>
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>diff_drive_controller</exec_depend>
+  <exec_depend>forward_command_controller</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>hardware_interface</exec_depend>
   <exec_depend>teleop_twist_keyboard</exec_depend>
   <exec_depend>xacro</exec_depend>
+  <exec_depend>embr_core</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

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