Feat/rowzaw/integration test fixtures

.github/workflows/install_and_build.yaml

@@ -1,37 +1,76 @@
 name: Run setup and colcon build for Dev
 on: push
+
 jobs:
+
   setup_and_build_check:
     runs-on: ubuntu-22.04
     steps:
-     - uses: actions/checkout@v4
-     - name: enviroment setupg
-       run: |
-         export ROVERFLAKE_ROOT="${GITHUB_WORKSPACE}"
-         yes | ./setup_scripts/setup_everything_common.sh
-     - name: build
-       run: |
-         cd $ROVERFLAKE_ROOT
-         echo $ROS_DISTRO
-         source /opt/ros/humble/setup.bash
-         echo $ROS_DISTRO
-         colcon build --symlink-install --packages-skip sweeppy  
-     - name: Notify Discord (success)
-       if: ${{ success() }}
-       uses: appleboy/discord-action@v1.0.0
-       with:
-        webhook_id: ${{ secrets.DISCORD_BOT_WEBHOOK_ID }} # it also accepts full URL
-        webhook_token: ${{ secrets.DISCORD_BOT_WEBHOOK_TOKEN }} # it also accepts full URL
-        message: |
-              *I'm happy now and I'll let you merge ${{ github.actor }}. _good job!_* || ${{ github.repository }} on `${{ github.ref_name }}`
-              ${{ github.server_url }}/${{ github.repository }}/actions/runs/${{ github.run_id }}
-     - name: Notify Discord (failure)
-       if: ${{ failure() }}
-       uses: appleboy/discord-action@v1.0.0
-       with:
-        webhook_id: ${{ secrets.DISCORD_BOT_WEBHOOK_ID }} # it also accepts full URL
-        webhook_token: ${{ secrets.DISCORD_BOT_WEBHOOK_TOKEN }} # it also accepts full URL
-        message: |
-              _you have saddened me ${{ github.actor }}_
-              ${{ github.server_url }}/${{ github.repository }}/actions/runs/${{ github.run_id }}
-
+      - uses: actions/checkout@v4
+
+      - name: enviroment setupg
+        run: |
+          export ROVERFLAKE_ROOT="${GITHUB_WORKSPACE}"
+          yes | ./setup_scripts/setup_everything_common.sh
+
+      - name: build
+        run: |
+          cd $ROVERFLAKE_ROOT
+          echo $ROS_DISTRO
+          source /opt/ros/humble/setup.bash
+          echo $ROS_DISTRO
+          colcon build --symlink-install --packages-skip sweeppy
+
+      - uses: actions/upload-artifact@v4
+        with:
+          name: rover_ws
+          path: ${GITHUB_WORKSPACE}/install/
+
+      - name: Notify Discord (success)
+        if: ${{ success() }}
+        uses: appleboy/discord-action@v1.0.0
+        with:
+          webhook_id: ${{ secrets.DISCORD_BOT_WEBHOOK_ID }}
+          webhook_token: ${{ secrets.DISCORD_BOT_WEBHOOK_TOKEN }}
+          message: |
+            *I'm happy now and I'll let you merge ${{ github.actor }}. _good job!_* || ${{ github.repository }} on `${{ github.ref_name }}`
+            ${{ github.server_url }}/${{ github.repository }}/actions/runs/${{ github.run_id }}
+
+      - name: Notify Discord (failure)
+        if: ${{ failure() }}
+        uses: appleboy/discord-action@v1.0.0
+        with:
+          webhook_id: ${{ secrets.DISCORD_BOT_WEBHOOK_ID }}
+          webhook_token: ${{ secrets.DISCORD_BOT_WEBHOOK_TOKEN }}
+          message: |
+            _you have saddened me ${{ github.actor }}_
+            ${{ github.server_url }}/${{ github.repository }}/actions/runs/${{ github.run_id }}
+
+  integration_tests:
+    runs-on: ubuntu-22.04
+    needs: setup_and_build_check
+    steps:
+      - uses: actions/checkout@v4
+
+      - uses: actions/download-artifact@v4
+        with:
+          name: rover_ws
+          path: ${GITHUB_WORKSPACE}/install/
+
+      - name: run integration tests
+        run: |
+          cd ${GITHUB_WORKSPACE}
+          source install/setup.bash
+          colcon test --packages-select integration_tests
+          colcon test-result --verbose
+
+      - name: Notify Discord (failure)
+        if: ${{ failure() }}
+        uses: appleboy/discord-action@v1.0.0
+        with:
+          webhook_id: ${{ secrets.DISCORD_BOT_WEBHOOK_ID }}
+          webhook_token: ${{ secrets.DISCORD_BOT_WEBHOOK_TOKEN }}
+          message: |
+            _Unit tests have failed ${{ github.actor }}_
+            ${{ github.server_url }}/${{ github.repository }}/actions/runs/${{ github.run_id }}
+            @rowan Check for integration misshaps

src/arm_control/CMakeLists.txt

@@ -29,6 +29,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   trajectory_msgs
   rover_msgs
   moteus_msgs
+  arm_hardware_interface
 )
 
 find_package(ament_cmake REQUIRED)

src/arm_control/include/armControlParams.h

@@ -16,6 +16,7 @@
 #define EE_SPEED_SCALE 1
 #define PI 3.14
 
+//TODO get away from this style of conditional compiling. 
 // Uncomment the one you want, comment the one you dont
     #define SELECT_MOTEUS_ARM
     // #define SELECT_OLD_ARM

src/arm_control/include/cbs_interface.h

@@ -4,8 +4,7 @@
 #include "armControlParams.h"
 #include "rover_msgs/msg/generic_panel.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
-
-
+#include <arm_hardware_interface/ArmSerialProtocol.h>
 
 class CBSArmInterface : public rclcpp::Node
 {
@@ -14,19 +13,17 @@ class CBSArmInterface : public rclcpp::Node
     auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local();
     arm_cmd_publisher = this->create_publisher<rover_msgs::msg::ArmCommand>("/arm/command", qos);
 
-     arm_panel_subscriber = this->create_subscription<rover_msgs::msg::ArmPanel>(
+    arm_panel_subscriber = this->create_subscription<rover_msgs::msg::ArmPanel>(
             "/cbs/arm_panel", 10, std::bind(&CBSArmInterface::arm_panel_callback, this, std::placeholders::_1));
     left_panel_subscriber = this->create_subscription<rover_msgs::msg::GenericPanel>(
             "/cbs/left_panel_a", 10, std::bind(&CBSArmInterface::left_panel_callback, this, std::placeholders::_1));
-     
-            arm_ik_pub = this->create_publisher<geometry_msgs::msg::TwistStamped>(
-                "/arm_moveit_control/delta_twist_cmds", qos);  
+
+    arm_ik_pub = this->create_publisher<geometry_msgs::msg::TwistStamped>(
+        "/arm_moveit_control/delta_twist_cmds", qos);  
 //    arm_panel_timer = this->create_wall_timer( //Timer setup if we need it
 //         std::chrono::milliseconds(10),  // Timer interval
 //         std::bind(&CBSManagerNode::armPanelPoll, this) // Callback function
 //     );
-
-
     }
 
     ~CBSArmInterface(){

src/arm_control/package.xml

@@ -25,6 +25,7 @@
   <depend>rover_msgs</depend>
   <depend>trajectory_msgs</depend>
   <depend>moveit_servo</depend>
+  <depend>arm_hardware_interface</depend>
 
 
 

src/arm_control/src/cbs_interface.cpp

@@ -45,7 +45,7 @@ void CBSArmInterface::arm_panel_callback(const rover_msgs::msg::ArmPanel::Shared
 
 
     rover_msgs::msg::ArmCommand cmd_msg;
-    cmd_msg.cmd_type = 'V'; //!SHOULD BE FROM ArmSerialProtocol.h
+    cmd_msg.cmd_type = ABS_VEL_CMD; //!SHOULD BE FROM ArmSerialProtocol.h
     cmd_msg.velocities.resize(NUM_JOINTS);
     cmd_msg.velocities[0] = (static_cast<float>(msg->left.x) - 50)/100 * max_joysticks_output_speed_deg[0]*2;
      cmd_msg.velocities[1] = (static_cast<float>(msg->left.y) - 50)/100 * max_joysticks_output_speed_deg[1]*2 *-1;

src/integration_test/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.10)
+project(integration_test)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Catch2 did not work very well. We tried, its just difficult to integrate in a scalable way.
+# leaving this stale code just commented to keep record that we tried it. 
+# add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../external_pkgs/Catch2 ${CMAKE_CURRENT_BINARY_DIR}/external_pkgs/Catch2) # Include Catch2 for unit tests
+
+include_directories(include/)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rover_utils REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(example_interfaces REQUIRED)
+
+add_executable(integration_tester src/assert_node.cpp)
+add_executable(example_multiply_by_two_node src/example_multiply_by_two_node.cpp)
+ament_target_dependencies(integration_tester rclcpp sensor_msgs rover_utils example_interfaces)
+ament_target_dependencies(example_multiply_by_two_node rclcpp sensor_msgs rover_utils example_interfaces)
+
+if(BUILD_TESTING)
+  # Integration tests
+  find_package(ament_cmake_ros REQUIRED)
+  find_package(launch_testing_ament_cmake REQUIRED)
+  function(add_ros_isolated_launch_test path)
+    set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py")
+    add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN})
+  endfunction()
+  add_ros_isolated_launch_test(test/test_launch_test.py)
+  add_ros_isolated_launch_test(test/test_arm_pipeline.py)
+  # Add more tests here
+endif()
+
+# target_link_libraries(integration_tester 
+#   Catch2::Catch2)
+
+install(
+  TARGETS 
+  integration_tester
+  example_multiply_by_two_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

src/integration_test/brainstorm.txt

@@ -0,0 +1,4 @@
+How to integration tests?
+
+scopeout:
+1. can catch2 be used? or should not bother?

src/integration_test/include/integration_test/integration_helper_macros.h

@@ -0,0 +1,4 @@
+#pragma once
+
+
+// #define APPROX()

src/integration_test/package.xml

@@ -0,0 +1,29 @@
+<?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>integration_test</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="zawadzkirowan@gmail.com">rowan</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rover_utils</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>rclpy</test_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/integration_test/src/assert_node.cpp

@@ -0,0 +1,76 @@
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/joy.hpp> 
+#include <example_interfaces/msg/int32.hpp> 
+// #include <catch2/catch_all.hpp>
+
+// This node is a work in progress, playing around with different ways to setup integration tests.
+// We are aiming for a standard, easy way to add tests, and an easy way to get test results.
+
+#define TEST_INT_VALUE 50
+
+// Class definition
+class IntegrationTestNode : public rclcpp::Node {
+public:
+    IntegrationTestNode(); // Constructor is defined in .cpp.
+
+private:
+    // SUBS AND PUBS
+    rclcpp::Publisher<sensor_msgs::msg::Joy>::SharedPtr joy_publisher;
+
+    rclcpp::Publisher<example_interfaces::msg::Int32>::SharedPtr checker_pub;
+    rclcpp::Subscription<example_interfaces::msg::Int32>::SharedPtr checker_sub;
+
+    // Callbacks
+    void test_callback(example_interfaces::msg::Int32 msg);
+
+};    
+
+// Constructor
+IntegrationTestNode::IntegrationTestNode() : Node("integration_test_node") 
+{ // We create a class using rclcpp::Node as a base class. You can still use another base class if you need, albeit sometimes with difficulties in passing args..
+
+    // Quality of service example
+    auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local();
+
+    joy_publisher = this->create_publisher<sensor_msgs::msg::Joy>(
+        "/joy", qos);
+
+    checker_pub = this->create_publisher<example_interfaces::msg::Int32>(
+        "/integration/test_int/output", qos);
+
+    checker_sub = this->create_subscription<example_interfaces::msg::Int32>(
+        "/integration/test_int/input", qos, std::bind(&IntegrationTestNode::test_callback, this, std::placeholders::_1));
+
+        // example_interfaces::msg::Int32 test_msg;
+        // test_msg.data = TEST_INT_VALUE;
+        // checker_pub->publish(test_msg);
+}
+
+// Main function (entry point of node)
+int main(int argc, char *argv[]) 
+{
+    // Init ros2 with args. 
+   rclcpp::init(argc, argv);
+
+    // Instatiate your node
+    auto node = std::make_shared<IntegrationTestNode>();
+    // Spin it! (this is what runs pubs and subs. This is a blocking function)
+    rclcpp::spin(node);
+    // There is also rclcpp::spinSome(node) if you need more control over when the node spins / need to not block while spinning.
+
+    // Code only reaches here if we crash or shutdown the node
+    rclcpp::shutdown();
+    return 0;
+}
+
+void IntegrationTestNode::test_callback(example_interfaces::msg::Int32 msg)
+{
+    example_interfaces::msg::Int32 output;
+    output.data = msg.data *2;
+    checker_pub->publish(output);
+    // TEST_CASE("integration test check")
+    // {
+    //     REQUIRE(true);
+    //     REQUIRE(msg.data == TEST_INT_VALUE);
+    // }
+}

src/integration_test/src/example_multiply_by_two_node.cpp

@@ -0,0 +1,52 @@
+#include <rclcpp/rclcpp.hpp>
+#include <example_interfaces/msg/int32.hpp> 
+
+// This node is just an example node to show integration tests
+// Its just one publisher and one subscriber, give it an integer and it just doubles it
+
+// Class definition
+class MultiplyBy2Node : public rclcpp::Node {
+public:
+    MultiplyBy2Node(); // Constructor is defined in .cpp.
+
+private:
+    // SUBS AND PUBS
+    rclcpp::Publisher<example_interfaces::msg::Int32>::SharedPtr output_pub;
+    rclcpp::Subscription<example_interfaces::msg::Int32>::SharedPtr input_sub;
+
+    // Callbacks
+    void test_callback(example_interfaces::msg::Int32 msg);
+
+};    
+
+// Constructor
+MultiplyBy2Node::MultiplyBy2Node() : Node("multiply_by_two_integration_test_example_node") 
+{ // We create a class using rclcpp::Node as a base class. You can still use another base class if you need, albeit sometimes with difficulties in passing args..
+
+    // Quality of service example
+    auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local();
+
+    output_pub = this->create_publisher<example_interfaces::msg::Int32>(
+        "/integration/test_int/output", qos);
+
+    input_sub = this->create_subscription<example_interfaces::msg::Int32>(
+        "/integration/test_int/input", qos, std::bind(&MultiplyBy2Node::test_callback, this, std::placeholders::_1));
+
+    RCLCPP_INFO(this->get_logger(), "Example Log message: Node has started");
+}
+
+int main(int argc, char *argv[]) 
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<MultiplyBy2Node>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}
+
+void MultiplyBy2Node::test_callback(example_interfaces::msg::Int32 msg)
+{
+    example_interfaces::msg::Int32 output;
+    output.data = msg.data *2;
+    output_pub->publish(output);
+}