integration test added for arm

Author: Roozki

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/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

@@ -5,28 +5,44 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../external_pkgs/Catch2 ${CMAKE_CURRENT_BINARY_DIR}/external_pkgs/Catch2) # Include Catch2 for unit tests
+# 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)
+# target_link_libraries(integration_tester 
+#   Catch2::Catch2)
 
 install(
   TARGETS 
   integration_tester
+  example_multiply_by_two_node
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/integration_test/include/integration_test/integration_helper_macros.h

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

src/integration_test/package.xml

@@ -12,6 +12,13 @@
   <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>

src/integration_test/src/assert_node.cpp

@@ -1,9 +1,12 @@
-// name?
-
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/joy.hpp> 
 #include <example_interfaces/msg/int32.hpp> 
-#include <catch2/catch_all.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 {
@@ -33,11 +36,14 @@ IntegrationTestNode::IntegrationTestNode() : Node("integration_test_node")
         "/joy", qos);
 
     checker_pub = this->create_publisher<example_interfaces::msg::Int32>(
-        "/integration/test_int", qos);
+        "/integration/test_int/output", qos);
 
     checker_sub = this->create_subscription<example_interfaces::msg::Int32>(
-        "/integration/test_int", qos, std::bind(&IntegrationTestNode::test_callback, this, std::placeholders::_1));
-
+        "/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)
@@ -59,5 +65,12 @@ int main(int argc, char *argv[])
 
 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);
+}

src/integration_test/src/integration_test_fixtures.cpp

@@ -0,0 +1,54 @@
+// This class is meant to be a helper class for integration tests.
+#include <string>
+#include <unordered_map>
+#include <cstdint>
+
+// Work in progress...
+
+// A single test
+class TestRunner {
+public:
+    TestRunner(std::string name) {};
+
+    void assert(bool assertion) 
+    {
+
+    }
+
+    void assert(float val1, float val2, float thresh = 0.0000001)
+    {
+
+    }
+
+
+private:
+    std::string name;
+};
+
+class IntegrationTestHelper {
+public:
+    IntegrationTestHelper() : next_test_index(0), current_test_index(0) {};
+
+    TestRunner& add_test(std::string name);
+
+private:
+    std::unordered_map<uint32_t, TestRunner> tests;
+    uint32_t next_test_index;
+    uint32_t current_test_index;
+};
+
+TestRunner& IntegrationTestHelper::add_test(std::string name)
+{
+    current_test_index = next_test_index;
+    tests.emplace(current_test_index, TestRunner(name));
+    next_test_index++;
+    return tests[current_test_index];
+}
+
+
+
+
+
+// Example usage:
+
+// auto new_test = test.add_test("My new test");