Arm+Core Integration

.gitignore

@@ -2,6 +2,8 @@
 devel/
 logs/
 build/
+install/
+log/
 bin/
 lib/
 msg_gen/

README.md

@@ -13,6 +13,8 @@ Contains URDF files, meshes, launch files, and configurations for Clucky and Tes
   - [Gazebo](#gazebo)
 - [Packages](#packages)
 - [Major To-Do Items](#major-to-do-items)
+- [Troubleshooting](#troubleshooting)
+  - [Log noise](#log-noise)
 - [Maintainer(s)](#maintainers)
 
 ## Software Prerequisites 
@@ -59,10 +61,19 @@ $ source install/setup.bash  # or if you are using zsh: install/setup.zsh
 Here are the main launch commands for these packages:
 
 ```bash
-$ ros2 launch core_description display.launch.py  # View Core URDF in RViz
-$ ros2 launch arm_description display.launch.py   # View Arm URDF in RViz
-$ ros2 launch core_gazebo core.gazebo.launch.py   # Simulate Clucky in Gazebo with diff_drive_controller
-$ ros2 launch arm_moveit_config demo.launch.py    # Run Moveit2 for Arm IK
+  # View a URDF in RViz (either interactively or passively with mode:=preview|live)
+$ ros2 launch core_description display.launch.py
+$ ros2 launch testbed_description display.launch.py
+$ ros2 launch arm_description display.launch.py
+$ ros2 launch rover_description display.launch.py
+
+  # Simulate in Gazebo
+$ ros2 launch core_gazebo core.gazebo.launch.py     # Core only with diff_drive_controller
+$ ros2 launch rover_description gazebo.launch.py    # Full rover; Core + Arm
+
+  # Bring up ros2_control for real hardware (interfaces with anchor over topic_based_ros2_control)
+$ ros2 launch core_description physical.launch.py   # Core only with diff_drive_controller
+$ ros2 launch rover_description physical.launch.py  # Full rover; normally launched via Anchor's rover.launch.py
 ```
 
 ### Gazebo
@@ -85,13 +96,42 @@ $ source install/setup.bash && ros2 launch core_gazebo core.gazebo.launch.py
 - **arm_moveit_config** - Includes configuration and launch files for using Moveit2 with the astra arm.
 - **core_description** - Includes URDF/Xacro files, launch files, and configurations for Core (Clucky, not Testbed).
 - **core_gazebo** - Configuration and launch files for simulating Clucky with Gazebo.
+- **rover_description** - Combines Core and Arm into the full rover; includes launch files for the Gazebo sim and real-hardware ros2_control bringup.
+- **testbed_description** - Like `core_description`, but for Testbed instead of Clucky.
 
 ## Major To-Do Items
 
 - Reduce STL file complexity
 - Add proper textures to Core and Arm
 - Convert Arm to fully Xacro
 
+## Troubleshooting
+
+- **`Switch controller timed out after 5.000000 seconds!`, then `Failed to activate controller`** - Gazebo is taking too long to start
+
+Controller activation only happens inside `controller_manager::update()`, which runs on the sim clock, and Humble hardcodes the 5 s timeout — so fix the sim, not the timeout. Nearly always the model spawned underground and the solver is taking a long time to figure it out. Raise `spawn_z` past the model's lowest collision point:
+
+```bash
+$ ros2 launch core_gazebo core.gazebo.launch.py spawn_z:=1.0
+```
+
+If that isn't it, good luck.
+
+- **`amdgpu: drmGetDevice2 failed.`** — no GPU in the container, so Mesa falls back to llvmpipe
+
+`run_container.sh` passes `/dev/dri` through when the host has it; check `ls /dev/dri` in the container and on the host. NVIDIA-only machines also need the nvidia-container-toolkit. Software rendering still runs, but will be slower and possibly create a fisheyed viewport.
+
+### Log noise
+
+| Message | Why |
+| ------- | --- |
+| `libEGL warning: egl: failed to create dri2 screen` | Only a problem if you have a single GPU. |
+| `amdgpu: os_same_file_description couldn't determine...` | Prints regardless of whether the GPU works. |
+| `groups: cannot find name for group ID 984` | The host's `render`/`video` GIDs, passed in for `/dev/dri` access. Applied fine, just unnamed inside the container. |
+| `kdl_parser: The root link base_link has an inertia specified` | KDL ignores root-link inertia when building TF. Gazebo still uses it. |
+| `IMU sensor 'core_emb_imu_sensor' not found in hardware_info` | The IMU reaches ROS via `gz-sim-imu-system` and `ros_gz_bridge` on `/core/imu/data`, not ros2_control. |
+| `Desired controller update period (0.02 s) is slower than the gazebo simulation period (0.001 s)` |  Controller `update_rate` is 50 Hz against a 1 ms Gazebo step. |
+
 ## Maintainer(s)
 
 | Name | Email | Discord |

arm_description/launch/display.launch.py

@@ -1,132 +1,130 @@
-# Author: Addison Sears-Collins
-# Date: September 14, 2021
-# Description: Launch a two-wheeled robot URDF file using Rviz.
-# https://automaticaddison.com
+# Visualize the Arm URDF in RViz.
+#
+# One `mode` argument controls everything:
+#   preview (default) - stand-alone URDF preview: local RSP + joint_state_publisher_gui
+#                       (drag joint sliders) + RViz. Runs in its own ROS_DOMAIN_ID
+#                       (preview_domain_id) so its fake /joint_states and /tf cannot
+#                       interfere with the real rover.
+#   live              - view a running rover/sim: RViz only, on the caller's domain,
+#                       using the rover's /robot_description, /tf and /joint_states.
+#                       Spawns no publishers, so it cannot interfere with the real rover.
+#
+# Unlike core/testbed/rover, arm_description has no nodes.launch.py (no controller/physical/
+# gazebo path of its own), so the RSP/JSP-GUI/RViz nodes are inlined here.
 
-import os
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition, UnlessCondition
-from launch.substitutions import Command, LaunchConfiguration
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.conditions import IfCondition
+
+# https://docs.ros.org/en/rolling/p/launch/launch.substitutions.html
+from launch.substitutions import (
+    Command,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    EqualsSubstitution,
+)
 from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
 from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
-
-    # Set the path to this package.
     pkg_share = FindPackageShare(package="arm_description").find("arm_description")
 
-    # Set the path to the RViz configuration settings
-    default_rviz_config_path = os.path.join(pkg_share, "config/display.rviz")
-
-    # Set the path to the URDF file
-    default_urdf_model_path = os.path.join(pkg_share, "urdf/ASTRA_Arm.urdf")
-
-    ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ##############
-    # Launch configuration variables specific to simulation
-    gui = LaunchConfiguration("gui")
-    urdf_model = LaunchConfiguration("urdf_model")
-    rviz_config_file = LaunchConfiguration("rviz_config_file")
-    use_robot_state_pub = LaunchConfiguration("use_robot_state_pub")
-    use_rviz = LaunchConfiguration("use_rviz")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-
-    # Declare the launch arguments
-    declare_urdf_model_path_cmd = DeclareLaunchArgument(
-        name="urdf_model",
-        default_value=default_urdf_model_path,
-        description="Absolute path to robot urdf file",
-    )
-
-    declare_rviz_config_file_cmd = DeclareLaunchArgument(
-        name="rviz_config_file",
-        default_value=default_rviz_config_path,
-        description="Full path to the RVIZ config file to use",
-    )
-
-    declare_use_joint_state_publisher_cmd = DeclareLaunchArgument(
-        name="gui",
-        default_value="True",
-        description="Flag to enable joint_state_publisher_gui",
-    )
+    ld = LaunchDescription()
 
-    declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
-        name="use_robot_state_pub",
-        default_value="True",
-        description="Whether to start the robot state publisher",
+    ####################################################################################
+    # Launch Arguments
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            name="mode",
+            default_value="preview",
+            description=(
+                "'preview' for a sandboxed local URDF preview with slider joint control; "
+                "'live' to view a running rover/sim (RViz only) on the current domain."
+            ),
+        )
     )
-
-    declare_use_rviz_cmd = DeclareLaunchArgument(
-        name="use_rviz", default_value="True", description="Whether to start RVIZ"
+    ld.add_action(
+        DeclareLaunchArgument(
+            name="preview_domain_id",
+            default_value="10",
+            description=(
+                "ROS_DOMAIN_ID used only in 'preview' mode to sandbox the preview from any "
+                "live rover topics. Must differ from the rover's live ROS_DOMAIN_ID."
+            ),
+        )
     )
 
-    declare_use_sim_time_cmd = DeclareLaunchArgument(
-        name="use_sim_time",
-        default_value="True",
-        description="Use simulation (Gazebo) clock if true",
-    )
+    is_preview = EqualsSubstitution(LaunchConfiguration("mode"), "preview")
 
-    # Specify the actions
+    ####################################################################################
+    # Domain isolation (preview only)
 
-    # Publish the joint state values for the non-fixed joints in the URDF file.
-    start_joint_state_publisher_cmd = Node(
-        condition=UnlessCondition(gui),
-        package="joint_state_publisher",
-        executable="joint_state_publisher",
-        name="joint_state_publisher",
+    # Sandbox the preview on its own DDS domain so its fake /joint_states and /tf can never
+    # interfere with the live rover. Set before any node is launched so the nodes below inherit it.
+    ld.add_action(
+        SetEnvironmentVariable(
+            name="ROS_DOMAIN_ID",
+            value=LaunchConfiguration("preview_domain_id"),
+            condition=IfCondition(is_preview),
+        )
     )
 
-    # A GUI to manipulate the joint state values
-    start_joint_state_publisher_gui_node = Node(
-        condition=IfCondition(gui),
-        package="joint_state_publisher_gui",
-        executable="joint_state_publisher_gui",
-        name="joint_state_publisher_gui",
+    ####################################################################################
+    # Launch Nodes
+
+    # Robot State Publisher (preview only). In 'live' mode we skip it: the rover already
+    # publishes /robot_description and /tf, and a second RSP would be a duplicate TF authority.
+    ld.add_action(
+        Node(
+            package="robot_state_publisher",
+            executable="robot_state_publisher",
+            parameters=[
+                {
+                    "use_sim_time": False,
+                    "robot_description": ParameterValue(
+                        Command(
+                            [
+                                "xacro ",
+                                PathJoinSubstitution(
+                                    [pkg_share, "urdf", "ASTRA_Arm.urdf"]
+                                ),
+                            ]
+                        ),
+                        value_type=str,
+                    ),
+                }
+            ],
+            condition=IfCondition(is_preview),
+        )
     )
 
-    # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
-    start_robot_state_publisher_cmd = Node(
-        condition=IfCondition(use_robot_state_pub),
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        parameters=[
-            {
-                "use_sim_time": use_sim_time,
-                "robot_description": ParameterValue(
-                    Command(["xacro ", urdf_model]), value_type=str
-                ),
-            }
-        ],
-        arguments=[default_urdf_model_path],
+    # Joint State Publisher GUI - publish and graphically modify joint states in preview
+    # mode. In 'live' mode the joint states come from the running rover/sim instead.
+    ld.add_action(
+        Node(
+            package="joint_state_publisher_gui",
+            executable="joint_state_publisher_gui",
+            name="joint_state_publisher_gui",
+            condition=IfCondition(is_preview),
+        )
     )
 
-    # Launch RViz
-    start_rviz_cmd = Node(
-        condition=IfCondition(use_rviz),
-        package="rviz2",
-        executable="rviz2",
-        name="rviz2",
-        output="screen",
-        arguments=["-d", rviz_config_file],
+    # RViz - always spawned (the whole point of this launch); consumes /robot_description
+    # from the local RSP in preview, or from the live rover/sim in 'live' mode.
+    ld.add_action(
+        Node(
+            package="rviz2",
+            executable="rviz2",
+            name="rviz2",
+            output="screen",
+            arguments=[
+                "-d",
+                PathJoinSubstitution([pkg_share, "config", "display.rviz"]),
+            ],
+        )
     )
 
-    # Create the launch description and populate
-    ld = LaunchDescription()
-
-    # Declare the launch options
-    ld.add_action(declare_urdf_model_path_cmd)
-    ld.add_action(declare_rviz_config_file_cmd)
-    ld.add_action(declare_use_joint_state_publisher_cmd)
-    ld.add_action(declare_use_robot_state_pub_cmd)
-    ld.add_action(declare_use_rviz_cmd)
-    ld.add_action(declare_use_sim_time_cmd)
-
-    # Add any actions
-    ld.add_action(start_joint_state_publisher_cmd)
-    ld.add_action(start_joint_state_publisher_gui_node)
-    ld.add_action(start_robot_state_publisher_cmd)
-    ld.add_action(start_rviz_cmd)
-
     return ld