Merge branch 'main' of https://github.com/WATonomous/humanoid into gtranqui-interfacing

gavintranquilino · gavintranquilino · commit 0b642d6e20d4 · 2025-06-11T17:39:32.000-04:00
diff --git a/autonomy/wato_msgs/common_msgs/CMakeLists.txt b/autonomy/wato_msgs/common_msgs/CMakeLists.txt
@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.10)
+project(common_msgs)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+set(msg_files
+    msg/ArmPose.msg
+    msg/HandPose.msg
+    msg/JointState.msg
+    )
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+    ${msg_files}
+    DEPENDENCIES std_msgs geometry_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()
diff --git a/autonomy/wato_msgs/common_msgs/msg/ArmPose.msg b/autonomy/wato_msgs/common_msgs/msg/ArmPose.msg
@@ -0,0 +1,11 @@
+std_msgs/Header header
+
+string name
+
+common_msgs/JointState shoulder
+common_msgs/JointState elbow
+common_msgs/JointState wrist
+
+# Optional Overall Hand Pose
+bool include_hand_pose
+common_msgs/HandPose hand_pose
diff --git a/autonomy/wato_msgs/common_msgs/msg/HandPose.msg b/autonomy/wato_msgs/common_msgs/msg/HandPose.msg
@@ -0,0 +1,28 @@
+std_msgs/Header header
+
+string name
+
+# Thumb
+common_msgs/JointState thumb_cmc
+common_msgs/JointState thumb_mcp
+common_msgs/JointState thumb_ip
+
+# Index
+common_msgs/JointState index_mcp
+common_msgs/JointState index_pip
+common_msgs/JointState index_dip
+
+# Middle
+common_msgs/JointState middle_mcp
+common_msgs/JointState middle_pip
+common_msgs/JointState middle_dip
+
+# Ring
+common_msgs/JointState ring_mcp
+common_msgs/JointState ring_pip
+common_msgs/JointState ring_dip
+
+# Pinky
+common_msgs/JointState pinky_mcp
+common_msgs/JointState pinky_pip
+common_msgs/JointState pinky_dip
diff --git a/autonomy/wato_msgs/common_msgs/msg/JointState.msg b/autonomy/wato_msgs/common_msgs/msg/JointState.msg
@@ -0,0 +1,9 @@
+std_msgs/Header header
+
+string name
+
+float64[] position  
+float64[] velocity
+float64[] effort
+
+geometry_msgs/Quaternion orientation
diff --git a/autonomy/wato_msgs/common_msgs/package.xml b/autonomy/wato_msgs/common_msgs/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>common_msgs</name>
+  <version>0.0.0</version>
+  <description>Common ROS2 message definitions for the WATO Humanoid project.</description>
+
+  <maintainer email="gavintranquilino@gmail.com">Gavin Tranquilino</maintainer>
+  <license>Apache2.0</license> 
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/docs/Architecture_Map.odg b/docs/Architecture_Map.odg
diff --git a/utils/README.md b/utils/README.md
@@ -0,0 +1,32 @@
+# Utils
+
+## `create-package.bash`
+
+### Usage
+`create-package.bash $package_name $module_name [-p] [-h]`
+- $package_name -> required, str: name of package
+- $module_name -> required, str: name of parent module
+- -p -> optional, flag: build python package. Defaults to a cpp package.
+- -h -> optional, flag: help description.
+
+- ⚠️ Double check for these before running the script: 
+    - The `module_name` directory must exist beforehand for the script to work properly.
+    - The `modules/docker-compose.module_name.yaml` must exist for the script to run.
+
+### Description
+- Creates a ros2 python or cpp `package_name` in the `module_name`
+- Creates missing boilerplate files
+    - If CPP build, these are:
+        - launch/`package_name`.launch.py
+        - config/params.yaml
+        - src/`package_name`_node.cpp
+        - include/`package_name`_node.hpp
+        - src/`package_name`_core.cpp
+        - include/`package_name`_core.hpp
+        - test/`package_name`_test.cpp
+    - If Python build, these are:
+        - launch/`package_name`.launch.py
+        - config/params.yaml
+        - `package_name`/`package_name`_node.py
+        - `package_name`/`package_name`_core.py
+- Modifies module_name.interfacing.Dockerfile to copy in package code into container.
diff --git a/utils/boilerplate-files/foo.launch.py b/utils/boilerplate-files/foo.launch.py
@@ -0,0 +1,87 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def launch_setup(context, *args, **kwargs):
+    """
+    Dynamically determine package name and configuration path.
+
+    This function is called by OpaqueFunction to resolve launch arguments
+    and create the Node action.
+    """
+    # These values are resolved from the DeclareLaunchArgument definitions below
+    package_name = LaunchConfiguration('package_name').perform(context)
+    executable_name = LaunchConfiguration('executable_name').perform(context)
+    node_name = LaunchConfiguration('node_name').perform(context)
+
+    # Construct the path to the parameter file within the current package
+    # This assumes your params.yaml is in a 'config' subdirectory within your package's share directory.
+    param_file_path = os.path.join(
+        get_package_share_directory(package_name),
+        'config',
+        'params.yaml'  # <-- REPLACE if your parameter file has a different name
+    )
+
+    # Check if the params.yaml file actually exists
+    node_parameters = []
+    if os.path.exists(param_file_path):
+        node_parameters.append(param_file_path)
+        print(f"INFO: Using parameter file for {node_name}: {param_file_path}")
+    else:
+        print(f"WARNING: No params.yaml found for {node_name} at {param_file_path}. Launching without parameters.")
+
+
+    # Define the Node action
+    node = Node(
+        package=package_name,
+        executable=executable_name,
+        name=node_name,
+        parameters=node_parameters,
+        output='screen',  # Optional: 'screen' or 'log'. 'screen' prints output to the console.
+        emulate_tty=True, # Optional: Set to True for colored output in the console.
+    )
+
+    return [node]
+
+
+def generate_launch_description():
+    """
+    Generate the launch description for a generic ROS 2 package.
+
+    This template is designed to be placed in any ROS 2 package's
+    launch directory. It expects the package to have a main executable
+    and optionally a 'config/params.yaml' file.
+    """
+    return LaunchDescription([
+        # Declare the package name argument.
+        # This argument specifies WHICH ROS 2 package this launch file should target.
+        # When running, you will set this:
+        # e.g., ros2 launch <path_to_this_launch_file> generic_package.launch.py package_name:=your_actual_package_name
+        DeclareLaunchArgument(
+            'package_name',
+            description='Name of the ROS 2 package to launch.'
+        ),
+        # Declare the executable name argument.
+        # This argument specifies WHICH executable within the 'package_name' should be run.
+        # When running, you will set this:
+        # e.g., ros2 launch ... executable_name:=your_node_executable_name
+        DeclareLaunchArgument(
+            'executable_name',
+            description='Name of the executable to run from the package.'
+        ),
+        # Declare the node name argument (optional, defaults to executable_name)
+        # This argument sets the ROS 2 node name. If not provided, it defaults to the executable name.
+        # e.g., ros2 launch ... node_name:=my_custom_node_name
+        DeclareLaunchArgument(
+            'node_name',
+            default_value=LaunchConfiguration('executable_name'), # Defaults to the executable name
+            description='Name to assign to the ROS 2 node.'
+        ),
+        # OpaqueFunction defers the creation of the Node action until launch arguments are resolved.
+        # This is necessary because we need the actual string values of package_name, executable_name, etc.
+        OpaqueFunction(function=launch_setup)
+    ])
diff --git a/utils/boilerplate-files/foo_core.cpp b/utils/boilerplate-files/foo_core.cpp
@@ -0,0 +1,5 @@
+#include "foo_core.hpp"
+
+FooCore::FooCore() {
+    
+}
diff --git a/utils/boilerplate-files/foo_core.hpp b/utils/boilerplate-files/foo_core.hpp
@@ -0,0 +1,17 @@
+#ifndef FOO_CORE_HPP_
+#define FOO_CORE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+
+class FooCore {
+public:
+  /*
+   * Foo core constructor.
+   */
+  FooCore();
+
+private:
+  
+};
+
+#endif
diff --git a/utils/boilerplate-files/foo_core.py b/utils/boilerplate-files/foo_core.py
@@ -0,0 +1,4 @@
+
+class FooCore():
+    def __init__(self):
+        pass
diff --git a/utils/boilerplate-files/foo_node.cpp b/utils/boilerplate-files/foo_node.cpp
@@ -0,0 +1,5 @@
+#include "foo_node.hpp"
+
+FooNode::FooNode() {
+    
+}
diff --git a/utils/boilerplate-files/foo_node.hpp b/utils/boilerplate-files/foo_node.hpp
@@ -0,0 +1,19 @@
+#ifndef FOO_NODE_HPP_
+#define FOO_NODE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "foo_core.hpp"
+
+class FooNode : public rclcpp::Node {
+public:
+  /*
+   * Foo node constructor.
+   */
+  FooNode();
+
+private:
+  
+};
+
+#endif
diff --git a/utils/boilerplate-files/foo_node.py b/utils/boilerplate-files/foo_node.py
@@ -0,0 +1,29 @@
+import rclpy
+from rclpy.node import Node
+
+from foo.foo_core import FooCore
+
+class Foo(Node):
+    def __init__(self):
+        super().__init__('python_foo')
+
+        # Declare and get the parameters
+        self.declare_parameter('version', 1)
+
+        self.__foo = FooCore()
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    python_foo = Foo()
+
+    rclpy.spin(python_foo)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    python_foo.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
diff --git a/utils/boilerplate-files/foo_test.cpp b/utils/boilerplate-files/foo_test.cpp
@@ -0,0 +1,54 @@
+#include "rclcpp/rclcpp.hpp"
+#include "gtest/gtest.h"
+// Google testing documentation at https://google.github.io/googletest/primer.html
+
+#include "foo_core.hpp"
+
+// The fixture for testing class Foo.
+class FooTest : public testing::Test {
+ protected:
+  // You can remove any or all of the following functions if their bodies would
+  // be empty.
+
+  FooTest() {
+     // You can do set-up work for each test here.
+  }
+
+  ~FooTest() override {
+     // You can do clean-up work that doesn't throw exceptions here.
+  }
+
+  // If the constructor and destructor are not enough for setting up
+  // and cleaning up each test, you can define the following methods:
+
+  void SetUp() override {
+     // Code here will be called immediately after the constructor (right
+     // before each test).
+  }
+
+  void TearDown() override {
+     // Code here will be called immediately after each test (right
+     // before the destructor).
+  }
+
+  // Class members declared here can be used by all tests in the test suite
+  // for Foo.
+};
+
+// Tests that the Foo::Bar() method does Abc.
+TEST_F(FooTest, MethodBarDoesAbc) {
+  const std::string input_filepath = "this/package/testdata/myinputfile.dat";
+  const std::string output_filepath = "this/package/testdata/myoutputfile.dat";
+  Foo f;
+  EXPECT_EQ(f.Bar(input_filepath, output_filepath), 0);
+}
+
+// Tests that Foo does Xyz.
+TEST_F(FooTest, DoesXyz) {
+  // Exercises the Xyz feature of Foo.
+}
+
+int main(int argc, char **argv) {
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/utils/create_package.bash b/utils/create_package.bash

-Original file line number
+Diff line change
@@ @@ -0,0 +1,5 @@ @@
 +#include "foo_core.hpp"
++
 +FooCore::FooCore() {
++
 +}