fix: I hate python (#21)

BioControl.drill expects an int, have to explicitly cast to an int or headless will crash
Makes main branch functional

.envrc

@@ -0,0 +1 @@
+use flake

.gitignore

@@ -10,4 +10,7 @@ log/
 .vscode/
 
 #Pycache folder
-__pycache__/
+__pycache__/
+
+#Direnv
+.direnv/

.gitmodules

@@ -1,3 +1,6 @@
 [submodule "src/ros2_interfaces_pkg"]
 	path = src/ros2_interfaces_pkg
-	url = git@github.com:SHC-ASTRA/ros2_interfaces_pkg.git
+	url = ../ros2_interfaces_pkg
+[submodule "src/astra_description"]
+	path = src/astra_descriptions
+	url = ../astra_descriptions

auto_start/auto_start_anchor.sh

@@ -21,4 +21,4 @@ source /home/clucky/rover-ros2/install/setup.bash
 cd /home/clucky/rover-ros2/
 
 # Launch the ROS 2 node with the desired mode
-ros2 launch rover_launch.py mode:=anchor
+ros2 launch anchor_pkg rover.launch.py mode:=anchor

flake.nix

@@ -1,30 +1,72 @@
 {
+  description = "Development environment for ASTRA Anchor";
+
   inputs = {
     nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
-    nixpkgs.follows = "nix-ros-overlay/nixpkgs";  # IMPORTANT!!!
+    nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
   };
-  outputs = { self, nix-ros-overlay, nixpkgs }:
-    nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (system:
+
+  outputs =
+    {
+      self,
+      nix-ros-overlay,
+      nixpkgs,
+    }:
+    nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
+      system:
       let
         pkgs = import nixpkgs {
           inherit system;
           overlays = [ nix-ros-overlay.overlays.default ];
         };
-      in {
+      in
+      {
         devShells.default = pkgs.mkShell {
           name = "ASTRA Anchor";
-          packages = [
-            pkgs.colcon
-            pkgs.python312Packages.pyserial
-            pkgs.python312Packages.pygame
-            (with pkgs.rosPackages.humble; buildEnv {
-              paths = [
-                ros-core ros2cli ros2run ros2bag ament-cmake-core python-cmake-module
-              ];
-            })
+          packages = with pkgs; [
+            colcon
+            (python312.withPackages (
+              p: with p; [
+                pyserial
+                pygame
+                scipy
+                crccheck
+                black
+              ]
+            ))
+            (
+              with rosPackages.humble;
+              buildEnv {
+                paths = [
+                  ros-core
+                  ros2cli
+                  ros2run
+                  ros2bag
+                  rviz2
+                  xacro
+                  ament-cmake-core
+                  python-cmake-module
+                  diff-drive-controller
+                  parameter-traits
+                  generate-parameter-library
+                  joint-state-publisher-gui
+                  robot-state-publisher
+                  ros2-control
+                  controller-manager
+                  # ros2-controllers nixpkg does not build :(
+                ];
+              }
+            )
           ];
+          shellHook = ''
+            # Display stuff
+            export DISPLAY=''${DISPLAY:-:0}
+            export QT_X11_NO_MITSHM=1
+          '';
         };
-      });
+      }
+    );
+
   nixConfig = {
     extra-substituters = [ "https://ros.cachix.org" ];
     extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -43,30 +43,10 @@ class SerialRelay(Node):
         # Initalize node with name
         super().__init__("anchor_node")#previously 'serial_publisher'
 
-        # New pub/sub with VicCAN
-        self.fromvic_debug_pub_ = self.create_publisher(String, '/anchor/from_vic/debug', 20)
-        self.fromvic_core_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/core', 20)
-        self.fromvic_arm_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/arm', 20)
-        self.fromvic_bio_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/bio', 20)
-
-        self.mock_mcu_sub_ = self.create_subscription(String, '/anchor/from_vic/mock_mcu', self.on_mock_fromvic, 20)
-        self.tovic_sub_ = self.create_subscription(VicCAN, '/anchor/to_vic/relay', self.on_relay_tovic_viccan, 20)
-        self.tovic_debug_sub_ = self.create_subscription(String, '/anchor/to_vic/relay_string', self.on_relay_tovic_string, 20)
-
-
-        # Create publishers 
-        self.arm_pub = self.create_publisher(String, '/anchor/arm/feedback', 10)
-        self.core_pub = self.create_publisher(String, '/anchor/core/feedback', 10)
-        self.bio_pub = self.create_publisher(String, '/anchor/bio/feedback', 10)
-
-        self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
-
-        # Create a subscriber 
-        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.on_relay_tovic_string, 10)
-
         # Loop through all serial devices on the computer to check for the MCU
         self.port = None
-        # self.port = "/tmp/ttyACM9"  # Fake port, for debugging
+        if port_override := os.getenv("PORT_OVERRIDE"):
+            self.port = port_override
         ports = SerialRelay.list_serial_ports()
         for i in range(4):
             if self.port is not None:
@@ -97,6 +77,27 @@ class SerialRelay(Node):
         self.ser.write(b"can_relay_mode,on\n")
         atexit.register(self.cleanup)
 
+        # New pub/sub with VicCAN
+        self.fromvic_debug_pub_ = self.create_publisher(String, '/anchor/from_vic/debug', 20)
+        self.fromvic_core_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/core', 20)
+        self.fromvic_arm_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/arm', 20)
+        self.fromvic_bio_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/bio', 20)
+
+        self.mock_mcu_sub_ = self.create_subscription(String, '/anchor/from_vic/mock_mcu', self.on_mock_fromvic, 20)
+        self.tovic_sub_ = self.create_subscription(VicCAN, '/anchor/to_vic/relay', self.on_relay_tovic_viccan, 20)
+        self.tovic_debug_sub_ = self.create_subscription(String, '/anchor/to_vic/relay_string', self.on_relay_tovic_string, 20)
+
+
+        # Create publishers 
+        self.arm_pub = self.create_publisher(String, '/anchor/arm/feedback', 10)
+        self.core_pub = self.create_publisher(String, '/anchor/core/feedback', 10)
+        self.bio_pub = self.create_publisher(String, '/anchor/bio/feedback', 10)
+
+        self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
+
+        # Create a subscriber 
+        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.on_relay_tovic_string, 10)
+
 
     def run(self):
         # This thread makes all the update processes run in the background
@@ -171,9 +172,37 @@ class SerialRelay(Node):
         """ Relay a string message from the MCU to the appropriate VicCAN topic """
         self.fromvic_debug_pub_.publish(String(data=msg))
         parts = msg.strip().split(",")
-        if len(parts) < 3 or parts[0] != "can_relay_fromvic":
+        if len(parts) > 0 and parts[0] != "can_relay_fromvic":
+            self.get_logger().debug(f"Ignoring non-VicCAN message: '{msg.strip()}'")
             return
 
+        # String validation
+        malformed: bool = False
+        malformed_reason: str = ""
+        if len(parts) < 3 or len(parts) > 7:
+            malformed = True
+            malformed_reason = f"invalid argument count (expected [3,7], got {len(parts)})"
+        elif parts[1] not in ["core", "arm", "digit", "citadel", "broadcast"]:
+            malformed = True
+            malformed_reason = f"invalid mcu_name '{parts[1]}'"
+        elif not(parts[2].isnumeric()) or int(parts[2]) < 0:
+            malformed = True
+            malformed_reason = f"command_id '{parts[2]}' is not a non-negative integer"
+        else:
+            for x in parts[3:]:
+                try:
+                    float(x)
+                except ValueError:
+                    malformed = True
+                    malformed_reason = f"data '{x}' is not a float"
+                    break
+
+        if malformed:
+            self.get_logger().warning(f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}")
+            return
+
+        # Have valid VicCAN message
+
         output = VicCAN()
         output.mcu_name = parts[1]
         output.command_id = int(parts[2])

src/anchor_pkg/package.xml

@@ -8,6 +8,10 @@
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-serial</depend>
+
+  <build_depend>black</build_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/anchor_pkg/setup.py

@@ -1,4 +1,6 @@
 from setuptools import find_packages, setup
+from os import path
+from glob import glob
 
 package_name = 'anchor_pkg'
 
@@ -9,7 +11,8 @@ setup(
     data_files=[
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        (path.join("share", package_name), ['package.xml']),
+        (path.join("share", package_name, "launch"), glob("launch/*"))
     ],
     install_requires=['setuptools'],
     zip_safe=True,

src/arm_pkg/package.xml

@@ -8,7 +8,11 @@
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-numpy</depend>
   <depend>ros2_interfaces_pkg</depend>
+  <!-- TODO: remove after refactored out -->
+  <exec_depend>python3-ikpy-pip</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/bio_pkg/package.xml

@@ -8,6 +8,7 @@
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
+  <depend>common_interfaces</depend>
   <depend>ros2_interfaces_pkg</depend>
 
   <test_depend>ament_copyright</test_depend>

src/core_gazebo/CMakeLists.txt

@@ -1,36 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(core_gazebo)
-
-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(controller_manager REQUIRED)
-find_package(gripper_controllers REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(ros2_control REQUIRED)
-find_package(ros2_controllers REQUIRED)
-find_package(trajectory_msgs REQUIRED)
-find_package(xacro REQUIRED)
-
-# Copy necessary files to designated locations in the project
-install (
-  DIRECTORY config launch models worlds
-  DESTINATION share/${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()

src/core_gazebo/config/ros_gz_bridge.yaml

@@ -1,25 +0,0 @@
-# gz topic published by Sensors plugin
-- ros_topic_name: "camera_head/depth/camera_info"
-  gz_topic_name: "camera_head/camera_info"
-  ros_type_name: "sensor_msgs/msg/CameraInfo"
-  gz_type_name: "gz.msgs.CameraInfo"
-  direction: GZ_TO_ROS
-  lazy: true # Determines whether connections are created immediately at startup (when false) or only when data is actually requested by a subscriber (when true), helping to conserve system resources at the cost of potential initial delays in data flow.
-
-# gz topic published by Sensors plugin
-- ros_topic_name: "camera_head/depth/color/points"
-  gz_topic_name: "camera_head/points"
-  ros_type_name: "sensor_msgs/msg/PointCloud2"
-  gz_type_name: "gz.msgs.PointCloudPacked"
-  direction: GZ_TO_ROS
-  lazy: true
-  
-# Clock configuration
-- ros_topic_name: "clock"
-  gz_topic_name: "clock"
-  ros_type_name: "rosgraph_msgs/msg/Clock"
-  gz_type_name: "gz.msgs.Clock"
-  direction: GZ_TO_ROS
-  lazy: false
-  
-

src/core_gazebo/launch/core.gazebo.launch.py

@@ -1,286 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    IncludeLaunchDescription
-)
-from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-
-def generate_launch_description():
-    """
-    Generate a launch description for the Gazebo simulation.
-
-    This function sets up all necessary parameters, paths, and nodes required to launch
-    the Gazebo simulation with a robot. It handles:
-    1. Setting up package paths and constants
-    2. Declaring launch arguments for robot configuration
-    3. Setting up the Gazebo environment
-    4. Spawning the robot in simulation
-
-    Returns:
-        LaunchDescription: A complete launch description for the simulation
-    """
-    # Constants for paths to different files and folders
-    package_name_gazebo = 'core_gazebo'
-    package_name_description = 'core_rover_description'
-    # package_name_moveit = 'mycobot_moveit_config'
-
-    default_robot_name = 'core_rover'
-    gazebo_models_path = 'models'
-    default_world_file = 'pick_and_place_demo.world'
-    gazebo_worlds_path = 'worlds'
-
-    ros_gz_bridge_config_file_path = 'config/ros_gz_bridge.yaml'
-
-    # Set the path to different files and folders
-    pkg_ros_gz_sim = FindPackageShare(package='ros_gz_sim').find('ros_gz_sim')
-    pkg_share_gazebo = FindPackageShare(package=package_name_gazebo).find(package_name_gazebo)
-    pkg_share_description = FindPackageShare(
-        package=package_name_description).find(package_name_description)
-    # pkg_share_moveit = FindPackageShare(package=package_name_moveit).find(package_name_moveit)
-
-    gazebo_models_path = os.path.join(pkg_share_gazebo, gazebo_models_path)
-    default_ros_gz_bridge_config_file_path = os.path.join(
-        pkg_share_gazebo, ros_gz_bridge_config_file_path)
-    
-    # Get the parent directory of the package share to access all ROS packages
-    ros_packages_path = os.path.dirname(pkg_share_description)
-
-    # Launch configuration variables
-    jsp_gui = LaunchConfiguration('jsp_gui')
-    load_controllers = LaunchConfiguration('load_controllers')
-    robot_name = LaunchConfiguration('robot_name')
-    use_rviz = LaunchConfiguration('use_rviz')
-    use_camera = LaunchConfiguration('use_camera')
-    use_gazebo = LaunchConfiguration('use_gazebo')
-    use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
-    use_sim_time = LaunchConfiguration('use_sim_time')
-    world_file = LaunchConfiguration('world_file')
-
-    world_path = PathJoinSubstitution([
-        pkg_share_gazebo,
-        gazebo_worlds_path,
-        world_file
-    ])
-
-    # Set the pose configuration variables
-    x = LaunchConfiguration('x')
-    y = LaunchConfiguration('y')
-    z = LaunchConfiguration('z')
-    roll = LaunchConfiguration('roll')
-    pitch = LaunchConfiguration('pitch')
-    yaw = LaunchConfiguration('yaw')
-
-
-    ################################################################################################
-    # Declare the launch arguments
-
-    declare_robot_name_cmd = DeclareLaunchArgument(
-        name='robot_name',
-        default_value=default_robot_name,
-        description='The name for the robot')
-
-    declare_load_controllers_cmd = DeclareLaunchArgument(
-        name='load_controllers',
-        default_value='true',
-        description='Flag to enable loading of ROS 2 controllers')
-
-    declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
-        name='use_robot_state_pub',
-        default_value='true',
-        description='Flag to enable robot state publisher')
-
-    # GUI and visualization arguments
-    declare_jsp_gui_cmd = DeclareLaunchArgument(
-        name='jsp_gui',
-        default_value='false',
-        description='Flag to enable joint_state_publisher_gui')
-
-    declare_use_camera_cmd = DeclareLaunchArgument(
-        name='use_camera',
-        default_value='false',
-        description='Flag to enable the RGBD camera for Gazebo point cloud simulation')
-
-    declare_use_gazebo_cmd = DeclareLaunchArgument(
-        name='use_gazebo',
-        default_value='true',
-        description='Flag to enable Gazebo')
-
-    declare_use_rviz_cmd = DeclareLaunchArgument(
-        name='use_rviz',
-        default_value='true',
-        description='Flag to enable RViz')
-
-    declare_use_sim_time_cmd = DeclareLaunchArgument(
-        name='use_sim_time',
-        default_value='true',
-        description='Use simulation (Gazebo) clock if true')
-
-    declare_world_cmd = DeclareLaunchArgument(
-        name='world_file',
-        default_value=default_world_file,
-        description='World file name (e.g., simple_demo.world, pick_and_place_demo.world)')
-
-    # Pose arguments
-    declare_x_cmd = DeclareLaunchArgument(
-        name='x',
-        default_value='0.0',
-        description='x component of initial position, meters')
-
-    declare_y_cmd = DeclareLaunchArgument(
-        name='y',
-        default_value='0.0',
-        description='y component of initial position, meters')
-
-    declare_z_cmd = DeclareLaunchArgument(
-        name='z',
-        default_value='0.75',
-        description='z component of initial position, meters')
-
-    declare_roll_cmd = DeclareLaunchArgument(
-        name='roll',
-        default_value='0.0',
-        description='roll angle of initial orientation, radians')
-
-    declare_pitch_cmd = DeclareLaunchArgument(
-        name='pitch',
-        default_value='0.0',
-        description='pitch angle of initial orientation, radians')
-
-    declare_yaw_cmd = DeclareLaunchArgument(
-        name='yaw',
-        default_value='0.0',
-        description='yaw angle of initial orientation, radians')
-
-
-    ################################################################################################
-    # Launch stuff
-
-    # Include Robot State Publisher launch file if enabled
-    robot_state_publisher_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([
-            os.path.join(pkg_share_description, 'launch', 'robot_state_publisher.launch.py')
-        ]),
-        launch_arguments={
-            'jsp_gui': jsp_gui,
-            'use_camera': use_camera,
-            'use_gazebo': use_gazebo,
-            'use_rviz': use_rviz,
-            'use_sim_time': use_sim_time
-        }.items(),
-        condition=IfCondition(use_robot_state_pub)
-    )
-
-    # # Include ROS 2 Controllers launch file if enabled
-    # load_controllers_cmd = IncludeLaunchDescription(
-    #     PythonLaunchDescriptionSource([
-    #         os.path.join(pkg_share_moveit, 'launch', 'load_ros2_controllers.launch.py')
-    #     ]),
-    #     launch_arguments={
-    #         'use_sim_time': use_sim_time
-    #     }.items(),
-    #     condition=IfCondition(load_controllers)
-    # )
-
-    # Set Gazebo model path - include both models directory and ROS packages
-    set_env_vars_resources = AppendEnvironmentVariable(
-        'GZ_SIM_RESOURCE_PATH',
-        gazebo_models_path)
-    
-    # Add ROS packages path so Gazebo can resolve package:// URIs
-    set_env_vars_packages = AppendEnvironmentVariable(
-        'GZ_SIM_RESOURCE_PATH',
-        os.path.dirname(pkg_share_description))
-
-    # Start Gazebo with optimized arguments
-    start_gazebo_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
-        launch_arguments=[('gz_args', [' -r -v 3 --render-engine ogre2 ', world_path])])
-
-    # Bridge ROS topics and Gazebo messages for establishing communication
-    start_gazebo_ros_bridge_cmd = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
-        parameters=[{
-            'config_file': default_ros_gz_bridge_config_file_path,
-        }],
-        output='screen'
-    )
-
-    # Includes optimizations to minimize latency and bandwidth when streaming image data
-    start_gazebo_ros_image_bridge_cmd = Node(
-        package='ros_gz_image',
-        executable='image_bridge',
-        arguments=[
-            '/camera_head/depth_image',
-            '/camera_head/image',
-        ],
-        remappings=[
-            ('/camera_head/depth_image', '/camera_head/depth/image_rect_raw'),
-            ('/camera_head/image', '/camera_head/color/image_raw'),
-        ],
-        condition=IfCondition(use_camera)
-    )
-
-    # Spawn the robot
-    start_gazebo_ros_spawner_cmd = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
-        arguments=[
-            '-topic', '/robot_description',
-            '-name', robot_name,
-            '-allow_renaming', 'true',
-            '-x', x,
-            '-y', y,
-            '-z', z,
-            '-R', roll,
-            '-P', pitch,
-            '-Y', yaw
-        ])
-
-
-    ################################################################################################
-    # Launch description
-
-    ld = LaunchDescription()
-
-    # Declare the launch options
-    ld.add_action(declare_robot_name_cmd)
-    ld.add_action(declare_jsp_gui_cmd)
-    ld.add_action(declare_load_controllers_cmd)
-    ld.add_action(declare_use_camera_cmd)
-    ld.add_action(declare_use_gazebo_cmd)
-    ld.add_action(declare_use_rviz_cmd)
-    ld.add_action(declare_use_robot_state_pub_cmd)
-    ld.add_action(declare_use_sim_time_cmd)
-    ld.add_action(declare_world_cmd)
-
-    # Add pose arguments
-    ld.add_action(declare_x_cmd)
-    ld.add_action(declare_y_cmd)
-    ld.add_action(declare_z_cmd)
-    ld.add_action(declare_roll_cmd)
-    ld.add_action(declare_pitch_cmd)
-    ld.add_action(declare_yaw_cmd)
-
-    # Add the actions to the launch description
-    ld.add_action(set_env_vars_resources)
-    ld.add_action(set_env_vars_packages)
-    ld.add_action(robot_state_publisher_cmd)
-    # ld.add_action(load_controllers_cmd)
-    ld.add_action(start_gazebo_cmd)
-    ld.add_action(start_gazebo_ros_bridge_cmd)
-    ld.add_action(start_gazebo_ros_image_bridge_cmd)
-    ld.add_action(start_gazebo_ros_spawner_cmd)
-
-    return ld

src/core_gazebo/package.xml

@@ -1,18 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>core_gazebo</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="ds0196@uah.edu">David</maintainer>
-  <license>AGPL-3.0-only</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/core_gazebo/worlds/empty.world

@@ -1,89 +0,0 @@
-<?xml version="1.0" ?>
-
-<sdf version="1.6">
-  <world name="default">
-
-    <!-- Plugin for simulating physics -->
-    <plugin
-      filename="gz-sim-physics-system"
-      name="gz::sim::systems::Physics">
-      <engine>
-        <filename>libgz-physics-bullet-featherstone-plugin.so</filename>
-      </engine>
-    </plugin>
-
-    <!-- Plugin for handling user commands -->
-    <plugin
-      filename="gz-sim-user-commands-system"
-      name="gz::sim::systems::UserCommands">
-    </plugin>
-
-    <!-- Plugin for broadcasting scene updates -->
-    <plugin
-      filename="gz-sim-scene-broadcaster-system"
-      name="gz::sim::systems::SceneBroadcaster">
-    </plugin>
-
-    <!-- Plugin for handling sensors like the LIDAR -->
-    <plugin
-      filename="gz-sim-sensors-system"
-      name="gz::sim::systems::Sensors">
-      <render_engine>ogre2</render_engine>
-    </plugin>
-
-    <!-- Plugin for IMU -->
-    <plugin filename="gz-sim-imu-system"
-      name="gz::sim::systems::Imu">
-    </plugin>
-
-    <!-- To add realistic gravity, do: 0.0 0.0 -9.8, otherwise do 0.0 0.0 0.0 -->
-    <gravity>0.0 0.0 -9.8</gravity>
-
-    <!-- Include a model of the Sun from an external URI -->
-    <include>
-      <uri>
-        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun
-      </uri>
-    </include>
-
-    <!-- Local Ground Plane with modified friction -->
-    <model name="ground_plane">
-      <static>true</static>
-      <link name="link">
-        <collision name="collision">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-            </plane>
-          </geometry>
-          <surface>
-            <friction>
-              <torsional>
-                <coefficient>0.0</coefficient>
-              </torsional>
-            </friction>
-          </surface>
-        </collision>
-        <visual name="visual">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-          <material>
-            <ambient>0.8 0.8 0.8 1</ambient>
-            <diffuse>0.8 0.8 0.8 1</diffuse>
-            <specular>0.8 0.8 0.8 1</specular>
-          </material>
-        </visual>
-      </link>
-    </model>
-
-    <!-- Define scene properties -->
-    <scene>
-      <shadows>false</shadows>
-    </scene>
-
-  </world>
-</sdf>

src/core_gazebo/worlds/pick_and_place_demo.world

@@ -1,83 +0,0 @@
-<?xml version="1.0" ?>
-
-<sdf version="1.7">
-  <world name="default">
-
-    <!-- Plugin for simulating physics -->
-    <plugin
-      filename="gz-sim-physics-system"
-      name="gz::sim::systems::Physics">
-    </plugin>
-
-    <!-- Plugin for handling user commands -->
-    <plugin
-      filename="gz-sim-user-commands-system"
-      name="gz::sim::systems::UserCommands">
-    </plugin>
-
-    <!-- Plugin for broadcasting scene updates -->
-    <plugin
-      filename="gz-sim-scene-broadcaster-system"
-      name="gz::sim::systems::SceneBroadcaster">
-    </plugin>
-
-    <!-- Plugin for sensor handling -->
-    <plugin
-      filename="gz-sim-sensors-system"
-      name="gz::sim::systems::Sensors">
-      <render_engine>ogre2</render_engine>
-    </plugin>
-
-    <!-- To add realistic gravity, do: 0.0 0.0 -9.8, otherwise do 0.0 0.0 0.0 -->
-    <gravity>0.0 0.0 -9.8</gravity>
-
-    <!-- Include a model of the Sun from an external URI -->
-    <include>
-      <uri>
-        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun
-      </uri>
-    </include>
-
-    <!-- Include a model of the Ground Plane from an external URI -->
-    <include>
-      <uri>
-        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane
-      </uri>
-    </include>
-
-    <!-- Include the cylinder model -->
-    <!-- <include>
-      <uri>model://red_cylinder</uri>
-      <name>red_cylinder</name>
-      <pose>0.22 0.12 0.175 0 0 0</pose>
-    </include> -->
-
-    <!-- Include the other objects -->
-
-    <!-- <include>
-      <uri>model://mustard</uri>
-      <pose>0.7 0.15 0.08 0 0 0</pose>
-    </include>
-
-    <include>
-      <uri>model://cheezit_big_original</uri>
-      <pose>0.64 0.23 0.17 1.571 0 0</pose>
-    </include>
-
-    <include>
-      <uri>model://cardboard_box</uri>
-      <pose>0.65 0.60 0.3 0 0 0.5</pose>
-    </include>
-
-    <include>
-      <uri>model://coke_can</uri>
-      <pose>0.5 0.15 0.15 0 0 2.3</pose>
-    </include> -->
-
-    <!-- Define scene properties -->
-    <scene>
-      <shadows>false</shadows>
-    </scene>
-
-  </world>
-</sdf>

src/core_pkg/core_pkg/core_node.py

@@ -14,12 +14,12 @@ import sys
 import threading
 import glob
 from scipy.spatial.transform import Rotation
-from math import copysign
+from math import copysign, pi
 
 from std_msgs.msg import String, Header
-from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus
+from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, JointState
 from geometry_msgs.msg import TwistStamped, Twist
-from ros2_interfaces_pkg.msg import CoreControl, CoreFeedback
+from ros2_interfaces_pkg.msg import CoreControl, CoreFeedback, RevMotorState
 from ros2_interfaces_pkg.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
 
 
@@ -28,7 +28,7 @@ thread = None
 
 CORE_WHEELBASE = 0.836  # meters
 CORE_WHEEL_RADIUS = 0.171  # meters
-CORE_GEAR_RATIO = 100  # Clucky: 100:1, Testbed: 64:1
+CORE_GEAR_RATIO = 100.0  # Clucky: 100:1, Testbed: 64:1
 
 control_qos = qos.QoSProfile(
     history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -41,6 +41,20 @@ control_qos = qos.QoSProfile(
     liveliness_lease_duration=Duration(seconds=5)
 )
 
+# Used to verify the length of an incoming VicCAN feedback message
+# Key is VicCAN command_id, value is expected length of data list
+viccan_msg_len_dict = {
+    48: 1,
+    49: 1,
+    50: 2,
+    51: 4,
+    52: 4,
+    53: 4,
+    54: 4,
+    56: 4,  # really 3, but viccan
+    58: 4   # ditto
+}
+
 
 class SerialRelay(Node):
     def __init__(self):
@@ -67,7 +81,7 @@ class SerialRelay(Node):
         # Control
 
         # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
-        self.cmd_vel_sub_ = self.create_subscription(TwistStamped, '/cmd_vel', self.cmd_vel_callback, qos_profile=control_qos)
+        self.cmd_vel_sub_ = self.create_subscription(TwistStamped, '/cmd_vel', self.cmd_vel_callback, 1)
         # manual twist -- [-1, 1] rather than real units
         self.twist_man_sub_ = self.create_subscription(Twist, '/core/twist', self.twist_man_callback, qos_profile=control_qos)
         # manual flags -- brake mode and max duty cycle
@@ -77,26 +91,28 @@ class SerialRelay(Node):
         # Feedback
 
         # Consolidated and organized core feedback
-        self.feedback_new_pub_ = self.create_publisher(NewCoreFeedback, '/core/feedback_new', 10)
+        self.feedback_new_pub_ = self.create_publisher(NewCoreFeedback, '/core/feedback_new', qos_profile=qos.qos_profile_sensor_data)
         self.feedback_new_state = NewCoreFeedback()
         self.feedback_new_state.fl_motor.id = 1
         self.feedback_new_state.bl_motor.id = 2
         self.feedback_new_state.fr_motor.id = 3
         self.feedback_new_state.br_motor.id = 4
         self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)  # TODO: not sure about this
+        # Joint states for topic-based controller
+        self.joint_state_pub_ = self.create_publisher(JointState, '/core/joint_states', qos_profile=qos.qos_profile_sensor_data)
         # IMU (embedded BNO-055)
-        self.imu_pub_ = self.create_publisher(Imu, '/core/imu', 10)
+        self.imu_pub_ = self.create_publisher(Imu, '/core/imu', qos_profile=qos.qos_profile_sensor_data)
         self.imu_state = Imu()
         self.imu_state.header.frame_id = "core_bno055"
         # GPS (embedded u-blox M9N)
-        self.gps_pub_ = self.create_publisher(NavSatFix, '/gps/fix', 10)
+        self.gps_pub_ = self.create_publisher(NavSatFix, '/gps/fix', qos_profile=qos.qos_profile_sensor_data)
         self.gps_state = NavSatFix()
         self.gps_state.header.frame_id = "core_gps_antenna"
         self.gps_state.status.service = NavSatStatus.SERVICE_GPS
         self.gps_state.status.status = NavSatStatus.STATUS_NO_FIX
         self.gps_state.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
         # Barometer (embedded BMP-388)
-        self.baro_pub_ = self.create_publisher(Barometer, '/core/baro', 10)
+        self.baro_pub_ = self.create_publisher(Barometer, '/core/baro', qos_profile=qos.qos_profile_sensor_data)
         self.baro_state = Barometer()
         self.baro_state.header.frame_id = "core_bmp388"
 
@@ -355,102 +371,117 @@ class SerialRelay(Node):
     def relay_fromvic(self, msg: VicCAN):
         # Assume that the message is coming from Core
         # skill diff if not
-        
-        # TODO: add len(msg.data) checks to each feedback message
 
-        # GNSS
-        if msg.command_id == 48:  # GNSS Latitude
-            self.gps_state.latitude = float(msg.data[0])
-        elif msg.command_id == 49:  # GNSS Longitude
-            self.gps_state.longitude = float(msg.data[0])
-        elif msg.command_id == 50:  # GNSS Satellite count and altitude
-            self.gps_state.status.status = NavSatStatus.STATUS_FIX if int(msg.data[0]) >= 3 else NavSatStatus.STATUS_NO_FIX
-            self.gps_state.altitude = float(msg.data[1])
-            self.gps_state.header.stamp = msg.header.stamp
-            self.gps_pub_.publish(self.gps_state)
-        # IMU
-        elif msg.command_id == 51:  # Gyro x, y, z, and imu calibration
-            self.feedback_new_state.imu_calib = round(float(msg.data[3]))
-            self.imu_state.angular_velocity.x = float(msg.data[0])
-            self.imu_state.angular_velocity.y = float(msg.data[1])
-            self.imu_state.angular_velocity.z = float(msg.data[2])
-            self.imu_state.header.stamp = msg.header.stamp
-        elif msg.command_id == 52:  # Accel x, y, z, heading
-            self.imu_state.linear_acceleration.x = float(msg.data[0])
-            self.imu_state.linear_acceleration.y = float(msg.data[1])
-            self.imu_state.linear_acceleration.z = float(msg.data[2])
-            # Deal with quaternion
-            r = Rotation.from_euler('z', float(msg.data[3]), degrees=True)
-            q = r.as_quat()
-            self.imu_state.orientation.x = q[0]
-            self.imu_state.orientation.y = q[1]
-            self.imu_state.orientation.z = q[2]
-            self.imu_state.orientation.w = q[3]
-            self.imu_state.header.stamp = msg.header.stamp
-            self.imu_pub_.publish(self.imu_state)
-        # REV Motors
-        elif msg.command_id == 53:  # REV SPARK MAX feedback
-            motorId = round(float(msg.data[0]))
-            temp = float(msg.data[1]) / 10.0
-            voltage = float(msg.data[2]) / 10.0
-            current = float(msg.data[3]) / 10.0
-            if motorId == 1:
-                self.feedback_new_state.fl_motor.temperature = temp
-                self.feedback_new_state.fl_motor.voltage = voltage
-                self.feedback_new_state.fl_motor.current = current
-                self.feedback_new_state.fl_motor.header.stamp = msg.header.stamp
-            elif motorId == 2:
-                self.feedback_new_state.bl_motor.temperature = temp
-                self.feedback_new_state.bl_motor.voltage = voltage
-                self.feedback_new_state.bl_motor.current = current
-                self.feedback_new_state.bl_motor.header.stamp = msg.header.stamp
-            elif motorId == 3:
-                self.feedback_new_state.fr_motor.temperature = temp
-                self.feedback_new_state.fr_motor.voltage = voltage
-                self.feedback_new_state.fr_motor.current = current
-                self.feedback_new_state.fr_motor.header.stamp = msg.header.stamp
-            elif motorId == 4:
-                self.feedback_new_state.br_motor.temperature = temp
-                self.feedback_new_state.br_motor.voltage = voltage
-                self.feedback_new_state.br_motor.current = current
-                self.feedback_new_state.br_motor.header.stamp = msg.header.stamp
-            self.feedback_new_pub_.publish(self.feedback_new_state)
-        # Board voltage
-        elif msg.command_id == 54:  # Voltages batt, 12, 5, 3, all * 100
-            self.feedback_new_state.board_voltage.vbatt = float(msg.data[0]) / 100.0
-            self.feedback_new_state.board_voltage.v12 = float(msg.data[1]) / 100.0
-            self.feedback_new_state.board_voltage.v5 = float(msg.data[2]) / 100.0
-            self.feedback_new_state.board_voltage.v3 = float(msg.data[3]) / 100.0
-        # Baro
-        elif msg.command_id == 56:  # BMP temperature, altitude, pressure
-            self.baro_state.temperature = float(msg.data[0])
-            self.baro_state.altitude = float(msg.data[1])
-            self.baro_state.pressure = float(msg.data[2])
-            self.baro_state.header.stamp = msg.header.stamp
-            self.baro_pub_.publish(self.baro_state)
-        # REV Motors (pos and vel)
-        elif msg.command_id == 58:  # REV position and velocity
-            motorId = round(float(msg.data[0]))
-            position = float(msg.data[1])
-            velocity = float(msg.data[2])
-            if motorId == 1:
-                self.feedback_new_state.fl_motor.position = position
-                self.feedback_new_state.fl_motor.velocity = velocity
-                self.feedback_new_state.fl_motor.header.stamp = msg.header.stamp
-            elif motorId == 2:
-                self.feedback_new_state.bl_motor.position = position
-                self.feedback_new_state.bl_motor.velocity = velocity
-                self.feedback_new_state.bl_motor.header.stamp = msg.header.stamp
-            elif motorId == 3:
-                self.feedback_new_state.fr_motor.position = position
-                self.feedback_new_state.fr_motor.velocity = velocity
-                self.feedback_new_state.fr_motor.header.stamp = msg.header.stamp
-            elif motorId == 4:
-                self.feedback_new_state.br_motor.position = position
-                self.feedback_new_state.br_motor.velocity = velocity
-                self.feedback_new_state.br_motor.header.stamp = msg.header.stamp
-        else:
-            return
+        # Check message len to prevent crashing on bad data
+        if msg.command_id in viccan_msg_len_dict:
+            expected_len = viccan_msg_len_dict[msg.command_id]
+            if len(msg.data) != expected_len:
+                self.get_logger().warning(f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})")
+                return
+
+        match msg.command_id:
+            # GNSS
+            case 48:  # GNSS Latitude
+                self.gps_state.latitude = float(msg.data[0])
+            case 49:  # GNSS Longitude
+                self.gps_state.longitude = float(msg.data[0])
+            case 50:  # GNSS Satellite count and altitude
+                self.gps_state.status.status = NavSatStatus.STATUS_FIX if int(msg.data[0]) >= 3 else NavSatStatus.STATUS_NO_FIX
+                self.gps_state.altitude = float(msg.data[1])
+                self.gps_state.header.stamp = msg.header.stamp
+                self.gps_pub_.publish(self.gps_state)
+            # IMU
+            case 51:  # Gyro x, y, z, and imu calibration
+                self.feedback_new_state.imu_calib = round(float(msg.data[3]))
+                self.imu_state.angular_velocity.x = float(msg.data[0])
+                self.imu_state.angular_velocity.y = float(msg.data[1])
+                self.imu_state.angular_velocity.z = float(msg.data[2])
+                self.imu_state.header.stamp = msg.header.stamp
+            case 52:  # Accel x, y, z, heading
+                self.imu_state.linear_acceleration.x = float(msg.data[0])
+                self.imu_state.linear_acceleration.y = float(msg.data[1])
+                self.imu_state.linear_acceleration.z = float(msg.data[2])
+                # Deal with quaternion
+                r = Rotation.from_euler('z', float(msg.data[3]), degrees=True)
+                q = r.as_quat()
+                self.imu_state.orientation.x = q[0]
+                self.imu_state.orientation.y = q[1]
+                self.imu_state.orientation.z = q[2]
+                self.imu_state.orientation.w = q[3]
+                self.imu_state.header.stamp = msg.header.stamp
+                self.imu_pub_.publish(self.imu_state)
+            # REV Motors
+            case 53:  # REV SPARK MAX feedback
+                motorId = round(float(msg.data[0]))
+                temp = float(msg.data[1]) / 10.0
+                voltage = float(msg.data[2]) / 10.0
+                current = float(msg.data[3]) / 10.0
+                motor: RevMotorState | None = None
+                match motorId:
+                    case 1:
+                        motor = self.feedback_new_state.fl_motor
+                    case 2:
+                        motor = self.feedback_new_state.bl_motor
+                    case 3:
+                        motor = self.feedback_new_state.fr_motor
+                    case 4:
+                        motor = self.feedback_new_state.br_motor
+                    case _:
+                        self.get_logger().warning(f"Ignoring REV motor feedback 53 with invalid motorId {motorId}")
+                        return
+
+                if motor:
+                    motor.temperature = temp
+                    motor.voltage = voltage
+                    motor.current = current
+                    motor.header.stamp = msg.header.stamp
+
+                self.feedback_new_pub_.publish(self.feedback_new_state)
+            # Board voltage
+            case 54:  # Voltages batt, 12, 5, 3, all * 100
+                self.feedback_new_state.board_voltage.vbatt = float(msg.data[0]) / 100.0
+                self.feedback_new_state.board_voltage.v12 = float(msg.data[1]) / 100.0
+                self.feedback_new_state.board_voltage.v5 = float(msg.data[2]) / 100.0
+                self.feedback_new_state.board_voltage.v3 = float(msg.data[3]) / 100.0
+            # Baro
+            case 56:  # BMP temperature, altitude, pressure
+                self.baro_state.temperature = float(msg.data[0])
+                self.baro_state.altitude = float(msg.data[1])
+                self.baro_state.pressure = float(msg.data[2])
+                self.baro_state.header.stamp = msg.header.stamp
+                self.baro_pub_.publish(self.baro_state)
+            # REV Motors (pos and vel)
+            case 58:  # REV position and velocity
+                motorId = round(float(msg.data[0]))
+                position = float(msg.data[1])
+                velocity = float(msg.data[2])
+                joint_state_msg = JointState()  # TODO: not sure if all motors should be in each message or not
+                joint_state_msg.position = [position * (2 * pi) / CORE_GEAR_RATIO]  # revolutions to radians
+                joint_state_msg.velocity = [velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO]  # RPM to rad/s
+
+                motor: RevMotorState | None = None
+
+                match motorId:
+                    case 1:
+                        motor = self.feedback_new_state.fl_motor
+                        joint_state_msg.name = ["fl_motor_joint"]
+                    case 2:
+                        motor = self.feedback_new_state.bl_motor
+                        joint_state_msg.name = ["bl_motor_joint"]
+                    case 3:
+                        motor = self.feedback_new_state.fr_motor
+                        joint_state_msg.name = ["fr_motor_joint"]
+                    case 4:
+                        motor = self.feedback_new_state.br_motor
+                        joint_state_msg.name = ["br_motor_joint"]
+                    case _:
+                        self.get_logger().warning(f"Ignoring REV motor feedback 58 with invalid motorId {motorId}")
+                        return
+
+                joint_state_msg.header.stamp = msg.header.stamp
+                self.joint_state_pub_.publish(joint_state_msg)
+            case _:
+                return
 
 
     def publish_feedback(self):

src/core_pkg/package.xml

@@ -8,6 +8,9 @@
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-scipy</depend>
+  <depend>python-crccheck-pip</depend>
   <depend>ros2_interfaces_pkg</depend>
 
   <test_depend>ament_copyright</test_depend>

src/core_rover_description/config/joint_names_core_rover_description.yaml

@@ -1 +0,0 @@
-controller_joint_names: ['', 'right_suspension_joint', 'br_wheel_axis', 'fr_wheel_joint', 'averaging_bar_axis', 'left_suspension_joint', 'bl_wheel_joint', 'fl_wheel_joint', ]

src/core_rover_description/config/rviz_basic_settings.rviz

@@ -1,229 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-        - /RobotModel1
-        - /TF1
-        - /TF1/Frames1
-      Splitter Ratio: 0.5
-    Tree Height: 617
-  - Class: rviz_common/Selection
-    Name: Selection
-  - Class: rviz_common/Tool Properties
-    Expanded:
-      - /2D Goal Pose1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz_common/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz_default_plugins/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 1
-      Class: rviz_default_plugins/RobotModel
-      Collision Enabled: false
-      Description File: ""
-      Description Source: Topic
-      Description Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_description
-      Enabled: true
-      Links:
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-        base_footprint:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        drivewhl_l_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        drivewhl_r_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        front_caster:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        gps_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        imu_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        lidar_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Name: RobotModel
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Class: rviz_default_plugins/TF
-      Enabled: true
-      Frame Timeout: 15
-      Frames:
-        All Enabled: false
-        base_footprint:
-          Value: false
-        base_link:
-          Value: false
-        drivewhl_l_link:
-          Value: true
-        drivewhl_r_link:
-          Value: true
-        front_caster:
-          Value: false
-        gps_link:
-          Value: false
-        imu_link:
-          Value: false
-        lidar_link:
-          Value: false
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: false
-      Show Axes: true
-      Show Names: true
-      Tree:
-        base_footprint:
-          base_link:
-            drivewhl_l_link:
-              {}
-            drivewhl_r_link:
-              {}
-            front_caster:
-              {}
-            gps_link:
-              {}
-            imu_link:
-              {}
-            lidar_link:
-              {}
-      Update Interval: 0
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: base_link
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz_default_plugins/Interact
-      Hide Inactive Objects: true
-    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz_default_plugins/Select
-    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz_default_plugins/Measure
-      Line color: 128; 128; 0
-    - Class: rviz_default_plugins/SetInitialPose
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /initialpose
-    - Class: rviz_default_plugins/SetGoal
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /goal_pose
-    - Class: rviz_default_plugins/PublishPoint
-      Single click: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /clicked_point
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 4.434264183044434
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 0.31193429231643677
-        Y: 0.11948385089635849
-        Z: -0.4807402193546295
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.490397572517395
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 1.0503965616226196
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 846
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1200
-  X: 246
-  Y: 77

src/core_rover_description/launch/display.launch.py

@@ -1,117 +0,0 @@
-# Author: Addison Sears-Collins
-# Date: September 14, 2021
-# Description: Launch a two-wheeled robot URDF file using Rviz.
-# https://automaticaddison.com
- 
-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_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-from launch_ros.parameter_descriptions import ParameterValue
- 
-def generate_launch_description():
- 
-  # Set the path to this package.
-  pkg_share = FindPackageShare(package='core_rover_description').find('core_rover_description')
- 
-  # Set the path to the RViz configuration settings
-  default_rviz_config_path = os.path.join(pkg_share, 'config/rviz_basic_settings.rviz')
- 
-  # Set the path to the URDF file
-  default_urdf_model_path = os.path.join(pkg_share, 'urdf/core_rover_description.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')
-   
-  declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
-    name='use_robot_state_pub',
-    default_value='True',
-    description='Whether to start the robot state publisher')
- 
-  declare_use_rviz_cmd = DeclareLaunchArgument(
-    name='use_rviz',
-    default_value='True',
-    description='Whether to start RVIZ')
-     
-  declare_use_sim_time_cmd = DeclareLaunchArgument(
-    name='use_sim_time',
-    default_value='True',
-    description='Use simulation (Gazebo) clock if true')
-    
-  # Specify the actions
- 
-  # 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')
- 
-  # 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')
- 
-  # 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])
- 
-  # Launch RViz
-  start_rviz_cmd = Node(
-    condition=IfCondition(use_rviz),
-    package='rviz2',
-    executable='rviz2',
-    name='rviz2',
-    output='screen',
-    arguments=['-d', rviz_config_file])
-   
-  # 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

src/core_rover_description/launch/robot_state_publisher.launch.py

@@ -1,238 +0,0 @@
-#!/usr/bin/env python3
-"""
-Launch RViz visualization for the mycobot robot.
-
-This launch file sets up the complete visualization environment for the mycobot robot,
-including robot state publisher, joint state publisher, and RViz2. It handles loading
-and processing of URDF/XACRO files and controller configurations.
-
-:author: Addison Sears-Collins
-:date: November 15, 2024
-"""
-import os
-from pathlib import Path
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.conditions import IfCondition, UnlessCondition
-from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.parameter_descriptions import ParameterValue
-from launch_ros.substitutions import FindPackageShare
-
-
-def process_ros2_controllers_config(context):
-    """Process the ROS 2 controller configuration yaml file before loading the URDF.
-
-    This function reads a template configuration file, replaces placeholder values
-    with actual configuration, and writes the processed file to both source and
-    install directories.
-
-    Args:
-        context: Launch context containing configuration values
-
-    Returns:
-        list: Empty list as required by OpaqueFunction
-    """
-
-    # Get the configuration values
-    prefix = LaunchConfiguration('prefix').perform(context)
-    flange_link = LaunchConfiguration('flange_link').perform(context)
-    robot_name = LaunchConfiguration('robot_name').perform(context)
-
-    home = str(Path.home())
-
-    # Define both source and install paths
-    src_config_path = os.path.join(
-        home,
-        'ros2_ws/src/mycobot_ros2/mycobot_moveit_config/config',
-        robot_name
-    )
-    install_config_path = os.path.join(
-        home,
-        'ros2_ws/install/mycobot_moveit_config/share/mycobot_moveit_config/config',
-        robot_name
-    )
-
-    # Read from source template
-    template_path = os.path.join(src_config_path, 'ros2_controllers_template.yaml')
-    with open(template_path, 'r', encoding='utf-8') as file:
-        template_content = file.read()
-
-    # Create processed content (leaving template untouched)
-    processed_content = template_content.replace('${prefix}', prefix)
-    processed_content = processed_content.replace('${flange_link}', flange_link)
-
-    # Write processed content to both source and install directories
-    for config_path in [src_config_path, install_config_path]:
-        os.makedirs(config_path, exist_ok=True)
-        output_path = os.path.join(config_path, 'ros2_controllers.yaml')
-        with open(output_path, 'w', encoding='utf-8') as file:
-            file.write(processed_content)
-
-    return []
-
-
-# Define the arguments for the XACRO file
-ARGUMENTS = [
-    DeclareLaunchArgument('robot_name', default_value='mycobot_280',
-                          description='Name of the robot'),
-    DeclareLaunchArgument('prefix', default_value='',
-                          description='Prefix for robot joints and links'),
-    DeclareLaunchArgument('add_world', default_value='true',
-                          choices=['true', 'false'],
-                          description='Whether to add world link'),
-    DeclareLaunchArgument('base_link', default_value='base_link',
-                          description='Name of the base link'),
-    DeclareLaunchArgument('base_type', default_value='g_shape',
-                          description='Type of the base'),
-    DeclareLaunchArgument('flange_link', default_value='link6_flange',
-                          description='Name of the flange link'),
-    DeclareLaunchArgument('gripper_type', default_value='adaptive_gripper',
-                          description='Type of the gripper'),
-    DeclareLaunchArgument('use_camera', default_value='false',
-                          choices=['true', 'false'],
-                          description='Whether to use the RGBD Gazebo plugin for point cloud'),
-    DeclareLaunchArgument('use_gazebo', default_value='false',
-                          choices=['true', 'false'],
-                          description='Whether to use Gazebo simulation'),
-    DeclareLaunchArgument('use_gripper', default_value='true',
-                          choices=['true', 'false'],
-                          description='Whether to attach a gripper')
-]
-
-
-def generate_launch_description():
-    """Generate the launch description for the mycobot robot visualization.
-
-    This function sets up all necessary nodes and parameters for visualizing
-    the mycobot robot in RViz, including:
-    - Robot state publisher for broadcasting transforms
-    - Joint state publisher for simulating joint movements
-    - RViz for visualization
-
-    Returns:
-        LaunchDescription: Complete launch description for the visualization setup
-    """
-    # Define filenames
-    urdf_package = 'core_rover_description'
-    urdf_filename = 'core_rover_description.urdf'
-    rviz_config_filename = 'rviz_basic_settings.rviz'
-
-    # Set paths to important files
-    pkg_share_description = FindPackageShare(urdf_package)
-    default_urdf_model_path = PathJoinSubstitution(
-        [pkg_share_description, 'urdf', urdf_filename])
-    default_rviz_config_path = PathJoinSubstitution(
-        [pkg_share_description, 'rviz', rviz_config_filename])
-
-    # Launch configuration variables
-    jsp_gui = LaunchConfiguration('jsp_gui')
-    rviz_config_file = LaunchConfiguration('rviz_config_file')
-    urdf_model = LaunchConfiguration('urdf_model')
-    use_jsp = LaunchConfiguration('use_jsp')
-    use_rviz = LaunchConfiguration('use_rviz')
-    use_sim_time = LaunchConfiguration('use_sim_time')
-
-    # Declare the launch arguments
-    declare_jsp_gui_cmd = DeclareLaunchArgument(
-        name='jsp_gui',
-        default_value='true',
-        choices=['true', 'false'],
-        description='Flag to enable joint_state_publisher_gui')
-
-    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_urdf_model_path_cmd = DeclareLaunchArgument(
-        name='urdf_model',
-        default_value=default_urdf_model_path,
-        description='Absolute path to robot urdf file')
-
-    declare_use_jsp_cmd = DeclareLaunchArgument(
-        name='use_jsp',
-        default_value='false',
-        choices=['true', 'false'],
-        description='Enable the joint state publisher')
-
-    declare_use_rviz_cmd = DeclareLaunchArgument(
-        name='use_rviz',
-        default_value='true',
-        description='Whether to start RVIZ')
-
-    declare_use_sim_time_cmd = DeclareLaunchArgument(
-        name='use_sim_time',
-        default_value='false',
-        description='Use simulation (Gazebo) clock if true')
-
-    robot_description_content = ParameterValue(Command([
-        'xacro', ' ', urdf_model, ' ',
-        'robot_name:=', LaunchConfiguration('robot_name'), ' ',
-        'prefix:=', LaunchConfiguration('prefix'), ' ',
-        'add_world:=', LaunchConfiguration('add_world'), ' ',
-        'base_link:=', LaunchConfiguration('base_link'), ' ',
-        'base_type:=', LaunchConfiguration('base_type'), ' ',
-        'flange_link:=', LaunchConfiguration('flange_link'), ' ',
-        'gripper_type:=', LaunchConfiguration('gripper_type'), ' ',
-        'use_camera:=', LaunchConfiguration('use_camera'), ' ',
-        'use_gazebo:=', LaunchConfiguration('use_gazebo'), ' ',
-        'use_gripper:=', LaunchConfiguration('use_gripper')
-    ]), value_type=str)
-
-    # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
-    start_robot_state_publisher_cmd = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        output='screen',
-        parameters=[{
-            'use_sim_time': use_sim_time,
-            'robot_description': robot_description_content}])
-
-    # Publish the joint state values for the non-fixed joints in the URDF file.
-    start_joint_state_publisher_cmd = Node(
-        package='joint_state_publisher',
-        executable='joint_state_publisher',
-        name='joint_state_publisher',
-        parameters=[{'use_sim_time': use_sim_time}],
-        condition=IfCondition(use_jsp))
-
-    # Depending on gui parameter, either launch joint_state_publisher or joint_state_publisher_gui
-    start_joint_state_publisher_gui_cmd = Node(
-        package='joint_state_publisher_gui',
-        executable='joint_state_publisher_gui',
-        name='joint_state_publisher_gui',
-        parameters=[{'use_sim_time': use_sim_time}],
-        condition=IfCondition(jsp_gui))
-
-    # Launch RViz
-    start_rviz_cmd = Node(
-        condition=IfCondition(use_rviz),
-        package='rviz2',
-        executable='rviz2',
-        output='screen',
-        arguments=['-d', rviz_config_file],
-        parameters=[{'use_sim_time': use_sim_time}])
-
-    # Create the launch description and populate
-    ld = LaunchDescription(ARGUMENTS)
-
-    # Process the controller configuration before starting nodes
-    # ld.add_action(OpaqueFunction(function=process_ros2_controllers_config))
-
-    # Declare the launch options
-    ld.add_action(declare_jsp_gui_cmd)
-    ld.add_action(declare_rviz_config_file_cmd)
-    ld.add_action(declare_urdf_model_path_cmd)
-    ld.add_action(declare_use_jsp_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_cmd)
-    ld.add_action(start_robot_state_publisher_cmd)
-    ld.add_action(start_rviz_cmd)
-
-    return ld

src/core_rover_description/package.xml

@@ -1,17 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>core_rover_description</name>
-  <version>1.0.0</version>
-  <description>This package contains configuration data, 3D models and launch files for Clucky Core</description>
-  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
-  <license>AGPL-3.0-only</license>
-
-  <depend>rclpy</depend>
-  <depend>robot_state_publisher</depend>
-  <depend>rviz2</depend>
-
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>

src/core_rover_description/setup.cfg

@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/core_ik_pkg
-[install]
-install_scripts=$base/lib/core_ik_pkg

src/core_rover_description/setup.py

@@ -1,28 +0,0 @@
-from setuptools import find_packages, setup
-import os
-from glob import glob
-
-package_name = 'core_rover_description'
-
-list_of_folders = ["config", "launch", "meshes", "textures", "urdf"]
-
-setup_data_files = [
-    ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-    ('share/' + package_name, ['package.xml'])
-]
-
-for folder in list_of_folders:
-    setup_data_files.append((os.path.join('share', package_name, folder), glob(folder + '/*')))
-
-setup(
-    name=package_name,
-    version='1.0.0',
-    packages=find_packages(exclude=['test']),
-    data_files=setup_data_files,  # created above with for loop
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='David Sharpe',
-    maintainer_email='ds0196@uah.edu',
-    description='This package contains configuration data, 3D models and launch files for Clucky Core',
-    license='BSD'
-)

src/core_rover_description/urdf/core_rover_description.csv

@@ -1,9 +0,0 @@
-Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
-base_link,-6.65913085716885E-06,0.109571969438869,-0.0865730790907174,0,0,0,75.3855391061812,2.40019927956544,0.000116237424420155,-0.00156114013755156,2.05138658583389,0.00559155518698155,4.26537268155378,0,0,0,0,0,0,package://core_rover_description/meshes/base_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://core_rover_description/meshes/base_link.STL,,Shell & Averaging System-1/CSC Shell Assembly-1/CSC V4 Shell-1;Motor and 6 pin connector mount-2;Motor and 6 pin connector mount-1;Inside Motor and 6 pin connector mount plate-1;Shell & Averaging System-1/Diff Box Backing Plate V2-1;Shell & Averaging System-1/Averaging System Outer Plate Backing V1-1;Shell & Averaging System-1/Arm Bracket Assembly Right V3-1/L bracket-1;Shell & Averaging System-1/MirrorArm Bracket Assembly-2/MirrorArm Bolts-1;Shell & Averaging System-1/Arm Bracket Assembly Right V3-1/Arm Bolts-1;Elec_Box_Assem-1/Box_Base-1;Elec_Box_Assem-1/Box_Left_Wall-1;Elec_Box_Assem-1/Box_Front_Wall-1;Elec_Box_Assem-1/Box_Right_Wall-1;Elec_Box_Assem-1/POE Switch (Measured with hole pattern)-1;Elec_Box_Assem-1/Ethernet Switch (Measured with hole pattern)-1;Elec_Box_Assem-1/NUC13ANH_NUC13LCH-1/NUC13ANH_NUC13LCH.STP-1/00_WS_ALL_ASM_CHECK_ASM_1_ASM_1.STP-1/WS_ME_PLACEMENT_ASM_ASM_1_ASM_2.STP-1/WS_ME_TALL_ASSY_ASM_1_ASM_1.STP-1/WS_TOP_COVER_ASM_ASM_1_ASM_1.STP-1/AN_TOP_COVER_1_2.STP-1;Elec_Box_Assem-1/NUC13ANH_NUC13LCH-1/NUC13ANH_NUC13LCH.STP-1/00_WS_ALL_ASM_CHECK_ASM_1_ASM_1.STP-1/WS_ME_PLACEMENT_ASM_ASM_1_ASM_2.STP-1/WS_ME_TALL_ASSY_ASM_1_ASM_1.STP-1/WS_TALL_MID_FRAME_ASM_ASM_1_ASM_1.STP-1/WS_TALL_MID_FRAME_NEW_1_1.STP-1;Elec_Box_Assem-1/SolidStateRelay-1;Elec_Box_Assem-1/100AFuse-1;Elec_Box_Assem-1/Box_Back_Wall-1;Rear_Wall_Sheath_Outer-2;Rear_Wall_Sheath_Outer-1;Shell & Averaging System-1/MirrorArm Bracket Assembly-2/MirrorL bracket-1;Shell & Averaging System-1/Chasis C-Channel Lip-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Core Mount Front Plate-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Core Mount Front Plate-2;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Easy To weld Tube Spacer-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Easy To weld Tube Spacer-9;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Easy To weld Tube Spacer-10;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Easy To weld Tube Spacer-8;Shell & Averaging System-1/Averaging System Outer Plate V3-1;Shell & Averaging System-1/Diff V5 Assem-2/Diff Box V5-1;Shell & Averaging System-1/Averaging System Outer Plate V3-2;Shell & Averaging System-1/picatinny95mm-1;Shell & Averaging System-1/picatinny95mm-2;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/90044A425_Black-Oxide Alloy Steel Socket Head Screw-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/90044A425_Black-Oxide Alloy Steel Socket Head Screw-3;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/90044A425_Black-Oxide Alloy Steel Socket Head Screw-4;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/90044A425_Black-Oxide Alloy Steel Socket Head Screw-2;Shell & Averaging System-1/6436K14 .5 Shaft Collar-2;Shell & Averaging System-1/6436K14 .5 Shaft Collar-4;Shell & Averaging System-1/6436K14 .5 Shaft Collar-3;Shell & Averaging System-1/6436K14 .5 Shaft Collar-1;Rear_Wall_Sheath_Inner-1;Shell & Averaging System-1/8896T69 SS U Bolt 1.375in-1;Shell & Averaging System-1/8896T69 SS U Bolt 1.375in-2;Shell & Averaging System-1/Averaging Shaft V3-2;Shell & Averaging System-1/Averaging Shaft V3-3;Shell & Averaging System-1/Diff V5 Assem-2/2342K186_Bearing .5 shaft sealed-3;Shell & Averaging System-1/2342K186_Bearing .5 shaft sealed-1;Shell & Averaging System-1/2342K186_Bearing .5 shaft sealed-2,Origin_global,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
-right_suspension_member,0.0011302,-0.12483,0.029359,0,0,0,1.7681,0.0021448,-1.4957E-06,-2.5225E-08,0.0068473,0.00021191,0.0058417,0,0,0,0,0,0,package://core_rover_description/meshes/right_suspension_member.STL,1,1,1,1,0,0,0,0,0,0,package://core_rover_description/meshes/right_suspension_member.STL,,A- POST BOND SUSPENSION (1)-4/P- CENTER LEG [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-4/P- LEG ATTACHMENT CF COVER [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-4/P- CORE ATTACHMENT [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-4/P- AXIS PLATE [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-4/P- LEG ATTACHMENT CF COVER [POST BOND SUSPENSION] (1)-2;A- POST BOND SUSPENSION (1)-4/P- SIDE LEGS [POST BOND SUSPENSION] (1)-2;Gearbox-Wheel Assembly - MIRROR-6/Gearbox Casing-1;Gearbox-Wheel Assembly - MIRROR-6/REV-21-1651.step-1;Gearbox-Wheel Assembly - MIRROR-6/Bonding Tube-1;Gearbox-Wheel Assembly - MIRROR-6/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-XXXX - 57 Sport 3 Stage Ring Gear Housing.STEP-1;Gearbox-Wheel Assembly - MIRROR-6/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-3765 - 57 Sport Motor Block.STEP-1;Gearbox-Wheel Assembly - MIRROR-7/Gearbox Casing-1;A- POST BOND SUSPENSION (1)-4/P- SIDE LEGS [POST BOND SUSPENSION] (1)-1;Gearbox-Wheel Assembly - MIRROR-7/REV-21-1651.step-1;Gearbox-Wheel Assembly - MIRROR-7/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-3765 - 57 Sport Motor Block.STEP-1;Gearbox-Wheel Assembly - MIRROR-7/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-XXXX - 57 Sport 3 Stage Ring Gear Housing.STEP-1;Gearbox-Wheel Assembly - MIRROR-7/Bonding Tube-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/6045N122_Low-Carbon Steel Round Tube 2-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/94645A111_High-Strength Steel Nylon-Insert Locknut-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/96144A239_Fine-Thread Alloy Steel Socket Head Screw-1,Origin_right_suspension_manual,right_suspension_axis,right_suspension_joint,revolute,-0.33067,0.028531,-0.15443,1.5708,0,-1.5708,base_link,0,0,-1,0,0,-0.38,0.38,,,,,,,,
-br_wheel,-0.0476013497459569,-1.09801088221673E-08,6.12845485470359E-09,0,0,0,5.74175792756056,0.115468209215025,7.19767266741869E-10,-2.90603405999934E-10,0.0647068246341468,7.16669744358495E-07,0.0632464587169257,0,0,0,0,0,0,package://core_rover_description/meshes/br_wheel.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://core_rover_description/meshes/br_wheel.STL,,Gearbox-Wheel Assembly - MIRROR-7/Updated Wheel Starboard-1,Origin_br_wheel_axis,br_wheel_axis,br_wheel_axis,continuous,-0.41838,-0.17155,0.12984,0.52965,-1.5708,0,right_suspension_member,1,0,0,,,,,,,,,,,,
-fr_wheel,-0.0476013497450763,-1.09822306249008E-08,6.12913009234717E-09,0,0,0,5.74175792767292,0.115468209216444,7.20424942594475E-10,-2.90800382125705E-10,0.0647068246345494,7.16670201096685E-07,0.063246458718483,0,0,0,0,0,0,package://core_rover_description/meshes/fr_wheel.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://core_rover_description/meshes/fr_wheel.STL,,Gearbox-Wheel Assembly - MIRROR-6/Updated Wheel Starboard-1,Origin_fr_wheel_joint,fr_wheel_axis,fr_wheel_joint,continuous,0.41838,-0.17155,0.12984,1.5769,-1.5708,0,right_suspension_member,1,0,0,,,,,,,,,,,,
-averaging_bar,5.12266309124314E-10,0.0103345512865068,6.28647148226413E-10,0,0,0,0.872230516841863,0.000102234478668288,1.26173058927933E-11,-1.00509715989169E-11,0.0236921798518964,1.54032135551904E-11,0.0236577000709469,0,0,0,0,0,0,package://core_rover_description/meshes/averaging_bar.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://core_rover_description/meshes/averaging_bar.STL,,A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Old Averaging Bar (Modified)-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/92981A220_Alloy Steel Shoulder Screws-2;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/92981A220_Alloy Steel Shoulder Screws-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/6045N122_Low-Carbon Steel Round Tube-2;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/6045N122_Low-Carbon Steel Round Tube-1,Origin_averaging_bar_axis,averaging_bar_axis,averaging_bar_axis,revolute,0,-0.23362,-0.0508,0,-0.0060774,-3.1416,base_link,0,-1,0,0,0,-0.38,0.38,,,,,,,,
-left_suspension_member,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,package://core_rover_description/meshes/left_suspension_member.STL,1,1,1,1,0,0,0,0,0,0,package://core_rover_description/meshes/left_suspension_member.STL,,A- POST BOND SUSPENSION (1)-3/P- LEG ATTACHMENT CF COVER [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-3/P- CENTER LEG [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-3/P- LEG ATTACHMENT CF COVER [POST BOND SUSPENSION] (1)-2;A- POST BOND SUSPENSION (1)-3/P- SIDE LEGS [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-3/P- SIDE LEGS [POST BOND SUSPENSION] (1)-2;Gearbox-Wheel Assembly - MIRROR-2/Gearbox Casing-1;Gearbox-Wheel Assembly - MIRROR-2/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-XXXX - 57 Sport 3 Stage Ring Gear Housing.STEP-1;Gearbox-Wheel Assembly - MIRROR-2/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-3765 - 57 Sport Motor Block.STEP-1;Gearbox-Wheel Assembly - MIRROR-1/Gearbox Casing-1;Gearbox-Wheel Assembly - MIRROR-1/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-XXXX - 57 Sport 3 Stage Ring Gear Housing.STEP-1;Gearbox-Wheel Assembly - MIRROR-1/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-3765 - 57 Sport Motor Block.STEP-1;Gearbox-Wheel Assembly - MIRROR-1/REV-21-1651.step-1;Gearbox-Wheel Assembly - MIRROR-1/Bonding Tube-1;Gearbox-Wheel Assembly - MIRROR-2/REV-21-1651.step-1;Gearbox-Wheel Assembly - MIRROR-2/Bonding Tube-1;A- POST BOND SUSPENSION (1)-3/P- CORE ATTACHMENT [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-3/P- AXIS PLATE [POST BOND SUSPENSION] (1)-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/6045N122_Low-Carbon Steel Round Tube 2-2;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/96144A239_Fine-Thread Alloy Steel Socket Head Screw-2;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/94645A111_High-Strength Steel Nylon-Insert Locknut-2,Origin_left_suspension_manual,left_suspension_axis,left_suspension_joint,revolute,0.33067,0.028531,-0.15443,1.5708,0,1.5708,base_link,0,0,-1,0,0,-0.38,0.38,,,,,,,,
-bl_wheel,-0.0476013497470983,-1.09827822947217E-08,6.12940226352165E-09,0,0,0,5.7417579274106,0.115468209213072,7.20585760708907E-10,-2.90859312873504E-10,0.0647068246335877,7.16669141063303E-07,0.0632464587148044,0,0,0,0,0,0,package://core_rover_description/meshes/bl_wheel.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://core_rover_description/meshes/bl_wheel.STL,,Gearbox-Wheel Assembly - MIRROR-1/Updated Wheel Starboard-1,Origin_bl_wheel_joint,bl_wheel_axis,bl_wheel_joint,continuous,0.41838,-0.17155,0.12984,1.5769,-1.5708,0,left_suspension_member,1,0,0,,,,,,,,,,,,
-fl_wheel,-0.0476013497448114,-1.09827921201955E-08,6.12933048760311E-09,0,0,0,5.74175792770551,0.115468209216877,7.20541990109492E-10,-2.90874836866816E-10,0.0647068246346634,7.16670369639572E-07,0.06324645871897,0,0,0,0,0,0,package://core_rover_description/meshes/fl_wheel.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://core_rover_description/meshes/fl_wheel.STL,,Gearbox-Wheel Assembly - MIRROR-2/Updated Wheel Starboard-1,Origin_fl_wheel_joint,fl_wheel_axis,fl_wheel_joint,continuous,-0.418377874454344,-0.171553199039643,0.129841324042317,0.529654981264331,-1.5707963267949,0,left_suspension_member,1,0,0,,,,,,,,,,,,

src/core_rover_description/urdf/core_rover_description.urdf

@@ -1,457 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
-     Commit Version:   Build Version: 1.6.9403.28032
-     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
-<robot
-  name="core_rover_description">
-
-  <link
-    name="base_link">
-    <inertial>
-      <origin
-        xyz="-6.65913085716885E-06 0.109571969438869 -0.0865730790907174"
-        rpy="0 0 0" />
-      <mass
-        value="75.3855391061812" />
-      <inertia
-        ixx="2.40019927956544"
-        ixy="0.000116237424420155"
-        ixz="-0.00156114013755156"
-        iyy="2.05138658583389"
-        iyz="0.00559155518698155"
-        izz="4.26537268155378" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/base_link.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.01764195448412081 0.013702083043526807 0.030713443727452196 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/base_link.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <link
-    name="right_suspension_member">
-    <inertial>
-      <origin
-        xyz="0.0011302 -0.12483 0.029359"
-        rpy="0 0 0" />
-      <mass
-        value="1.7681" />
-      <inertia
-        ixx="0.0021448"
-        ixy="-1.4957E-06"
-        ixz="-2.5225E-08"
-        iyy="0.0068473"
-        iyz="0.00021191"
-        izz="0.0058417" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/right_suspension_member.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.35 0.35 0.35 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/right_suspension_member.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="right_suspension_joint"
-    type="revolute">
-    <origin
-      xyz="-0.33067 0.028531 -0.15443"
-      rpy="1.5708 0 -1.5708" />
-    <parent
-      link="base_link" />
-    <child
-      link="right_suspension_member" />
-    <axis
-      xyz="0 0 -1" />
-    <limit
-      lower="-0.38"
-      upper="0.38"
-      effort="0"
-      velocity="0" />
-    <mimic
-      joint="left_suspension_joint"
-      multiplier="1"
-      offset="0" />
-  </joint>
-
-  <link
-    name="br_wheel">
-    <inertial>
-      <origin
-        xyz="-0.0476013497459569 -1.09801088221673E-08 6.12845485470359E-09"
-        rpy="0 0 0" />
-      <mass
-        value="5.74175792756056" />
-      <inertia
-        ixx="0.115468209215025"
-        ixy="7.19767266741869E-10"
-        ixz="-2.90603405999934E-10"
-        iyy="0.0647068246341468"
-        iyz="7.16669744358495E-07"
-        izz="0.0632464587169257" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/br_wheel.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.01764195448412081 0.013702083043526807 0.030713443727452196 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/br_wheel.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="br_wheel_axis"
-    type="continuous">
-    <origin
-      xyz="-0.41838 -0.17155 0.12984"
-      rpy="0.52965 -1.5708 0" />
-    <parent
-      link="right_suspension_member" />
-    <child
-      link="br_wheel" />
-    <axis
-      xyz="1 0 0" />
-  </joint>
-
-  <link
-    name="fr_wheel">
-    <inertial>
-      <origin
-        xyz="-0.0476013497450763 -1.09822306249008E-08 6.12913009234717E-09"
-        rpy="0 0 0" />
-      <mass
-        value="5.74175792767292" />
-      <inertia
-        ixx="0.115468209216444"
-        ixy="7.20424942594475E-10"
-        ixz="-2.90800382125705E-10"
-        iyy="0.0647068246345494"
-        iyz="7.16670201096685E-07"
-        izz="0.063246458718483" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/fr_wheel.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.01764195448412081 0.013702083043526807 0.030713443727452196 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/fr_wheel.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="fr_wheel_joint"
-    type="continuous">
-    <origin
-      xyz="0.41838 -0.17155 0.12984"
-      rpy="1.5769 -1.5708 0" />
-    <parent
-      link="right_suspension_member" />
-    <child
-      link="fr_wheel" />
-    <axis
-      xyz="1 0 0" />
-  </joint>
-
-  <link
-    name="averaging_bar">
-    <inertial>
-      <origin
-        xyz="5.12266309124314E-10 0.0103345512865068 6.28647148226413E-10"
-        rpy="0 0 0" />
-      <mass
-        value="0.872230516841863" />
-      <inertia
-        ixx="0.000102234478668288"
-        ixy="1.26173058927933E-11"
-        ixz="-1.00509715989169E-11"
-        iyy="0.0236921798518964"
-        iyz="1.54032135551904E-11"
-        izz="0.0236577000709469" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/averaging_bar.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/averaging_bar.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="averaging_bar_axis"
-    type="revolute">
-    <origin
-      xyz="0 -0.23362 -0.0508"
-      rpy="0 -0.0060774 -3.1416" />
-    <parent
-      link="base_link" />
-    <child
-      link="averaging_bar" />
-    <axis
-      xyz="0 -1 0" />
-    <limit
-      lower="-0.38"
-      upper="0.38"
-      effort="0"
-      velocity="0" />
-    <mimic
-      joint="left_suspension_joint"
-      multiplier="-1"
-      offset="0" />
-  </joint>
-
-  <link
-    name="left_suspension_member">
-    <inertial>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <mass
-        value="1.7681" />
-      <inertia
-        ixx="0"
-        ixy="0"
-        ixz="0"
-        iyy="0"
-        iyz="0"
-        izz="0" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/left_suspension_member.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.35 0.35 0.35 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/left_suspension_member.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="left_suspension_joint"
-    type="revolute">
-    <origin
-      xyz="0.33067 0.028531 -0.15443"
-      rpy="1.5708 0 1.5708" />
-    <parent
-      link="base_link" />
-    <child
-      link="left_suspension_member" />
-    <axis
-      xyz="0 0 -1" />
-    <limit
-      lower="-0.38"
-      upper="0.38"
-      effort="0"
-      velocity="0" />
-  </joint>
-
-  <link
-    name="bl_wheel">
-    <inertial>
-      <origin
-        xyz="-0.0476013497470983 -1.09827822947217E-08 6.12940226352165E-09"
-        rpy="0 0 0" />
-      <mass
-        value="5.7417579274106" />
-      <inertia
-        ixx="0.115468209213072"
-        ixy="7.20585760708907E-10"
-        ixz="-2.90859312873504E-10"
-        iyy="0.0647068246335877"
-        iyz="7.16669141063303E-07"
-        izz="0.0632464587148044" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/bl_wheel.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.01764195448412081 0.013702083043526807 0.030713443727452196 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/bl_wheel.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="bl_wheel_joint"
-    type="continuous">
-    <origin
-      xyz="0.41838 -0.17155 0.12984"
-      rpy="1.5769 -1.5708 0" />
-    <parent
-      link="left_suspension_member" />
-    <child
-      link="bl_wheel" />
-    <axis
-      xyz="1 0 0" />
-  </joint>
-
-  <link
-    name="fl_wheel">
-    <inertial>
-      <origin
-        xyz="-0.0476013497448114 -1.09827921201955E-08 6.12933048760311E-09"
-        rpy="0 0 0" />
-      <mass
-        value="5.74175792770551" />
-      <inertia
-        ixx="0.115468209216877"
-        ixy="7.20541990109492E-10"
-        ixz="-2.90874836866816E-10"
-        iyy="0.0647068246346634"
-        iyz="7.16670369639572E-07"
-        izz="0.06324645871897" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/fl_wheel.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.01764195448412081 0.013702083043526807 0.030713443727452196 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/fl_wheel.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="fl_wheel_joint"
-    type="continuous">
-    <origin
-      xyz="-0.418377874454344 -0.171553199039643 0.129841324042317"
-      rpy="0.529654981264331 -1.5707963267949 0" />
-    <parent
-      link="left_suspension_member" />
-    <child
-      link="fl_wheel" />
-    <axis
-      xyz="1 0 0" />
-  </joint>
-
-</robot>

src/headless_pkg/package.xml

@@ -8,6 +8,8 @@
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-pygame</depend>
   <depend>ros2_interfaces_pkg</depend>
 
   <test_depend>ament_copyright</test_depend>

src/headless_pkg/src/headless_node.py

@@ -7,11 +7,11 @@ import signal
 import time
 import atexit
 
-import serial
 import os
 import sys
 import threading
 import glob
+from math import copysign
 
 from std_msgs.msg import String
 from geometry_msgs.msg import Twist
@@ -48,6 +48,14 @@ class Headless(Node):
         # Initialize pygame first
         pygame.init()
         pygame.joystick.init()
+        super().__init__("headless")
+
+        # Wait for anchor to start
+        pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
+        while len(pub_info) == 0:
+            self.get_logger().info("Waiting for anchor to start...")
+            time.sleep(1.0)
+            pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
 
         # Wait for a gamepad to be connected
         print("Waiting for gamepad connection...")
@@ -65,8 +73,6 @@ class Headless(Node):
         self.gamepad.init()
         print(f'Gamepad Found: {self.gamepad.get_name()}')
 
-        # Now initialize the ROS2 node
-        super().__init__("headless")
         self.create_timer(0.15, self.send_controls)
 
         self.core_publisher = self.create_publisher(CoreControl, '/core/control', 2)
@@ -173,7 +179,7 @@ class Headless(Node):
 
             # Forward/back and Turn
             input.linear.x = -1.0 * left_stick_y
-            input.angular.z = -1.0 * right_stick_x ** 2  # Exponent for finer control (curve)
+            input.angular.z = -1.0 * copysign(right_stick_x ** 2, right_stick_x)  # Exponent for finer control (curve)
 
             # Publish
             self.core_twist_pub_.publish(input)
@@ -262,7 +268,14 @@ class Headless(Node):
 
 
             # BIO
-            bio_input = BioControl(bio_arm=int(left_stick_y * -100), drill_arm=int(round(right_stick_y) * -100))
+            bio_input = BioControl(
+                bio_arm=int(left_stick_y * -100),
+                drill_arm=int(round(right_stick_y) * -100)
+            )
+
+            # Drill motor (FAERIE)
+            if deadzone(left_trigger) > 0 or deadzone(right_trigger) > 0:
+                bio_input.drill = int(30 * (right_trigger - left_trigger))  # Max duty cycle 30%
 
             self.core_publisher.publish(CORE_STOP_MSG)
             self.arm_publisher.publish(arm_input)

src/latency_tester/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.22)
 project(latency_tester)
 
 # Default to C++14

src/latency_tester/package.xml

@@ -9,7 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
-  <depend>std_msgs</depend>
+  <depend>common_interfaces</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>