Skip to content

Commit 91d962b

Browse files
committed
integration test added for arm
1 parent 50bdc8b commit 91d962b

File tree

12 files changed

+387
-21
lines changed

12 files changed

+387
-21
lines changed

src/arm_control/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
2929
trajectory_msgs
3030
rover_msgs
3131
moteus_msgs
32+
arm_hardware_interface
3233
)
3334

3435
find_package(ament_cmake REQUIRED)

src/arm_control/include/armControlParams.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#define EE_SPEED_SCALE 1
1717
#define PI 3.14
1818

19+
//TODO get away from this style of conditional compiling.
1920
// Uncomment the one you want, comment the one you dont
2021
#define SELECT_MOTEUS_ARM
2122
// #define SELECT_OLD_ARM

src/arm_control/include/cbs_interface.h

Lines changed: 5 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,7 @@
44
#include "armControlParams.h"
55
#include "rover_msgs/msg/generic_panel.hpp"
66
#include "geometry_msgs/msg/twist_stamped.hpp"
7-
8-
7+
#include <arm_hardware_interface/ArmSerialProtocol.h>
98

109
class CBSArmInterface : public rclcpp::Node
1110
{
@@ -14,19 +13,17 @@ class CBSArmInterface : public rclcpp::Node
1413
auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local();
1514
arm_cmd_publisher = this->create_publisher<rover_msgs::msg::ArmCommand>("/arm/command", qos);
1615

17-
arm_panel_subscriber = this->create_subscription<rover_msgs::msg::ArmPanel>(
16+
arm_panel_subscriber = this->create_subscription<rover_msgs::msg::ArmPanel>(
1817
"/cbs/arm_panel", 10, std::bind(&CBSArmInterface::arm_panel_callback, this, std::placeholders::_1));
1918
left_panel_subscriber = this->create_subscription<rover_msgs::msg::GenericPanel>(
2019
"/cbs/left_panel_a", 10, std::bind(&CBSArmInterface::left_panel_callback, this, std::placeholders::_1));
21-
22-
arm_ik_pub = this->create_publisher<geometry_msgs::msg::TwistStamped>(
23-
"/arm_moveit_control/delta_twist_cmds", qos);
20+
21+
arm_ik_pub = this->create_publisher<geometry_msgs::msg::TwistStamped>(
22+
"/arm_moveit_control/delta_twist_cmds", qos);
2423
// arm_panel_timer = this->create_wall_timer( //Timer setup if we need it
2524
// std::chrono::milliseconds(10), // Timer interval
2625
// std::bind(&CBSManagerNode::armPanelPoll, this) // Callback function
2726
// );
28-
29-
3027
}
3128

3229
~CBSArmInterface(){

src/arm_control/src/cbs_interface.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ void CBSArmInterface::arm_panel_callback(const rover_msgs::msg::ArmPanel::Shared
4545

4646

4747
rover_msgs::msg::ArmCommand cmd_msg;
48-
cmd_msg.cmd_type = 'V'; //!SHOULD BE FROM ArmSerialProtocol.h
48+
cmd_msg.cmd_type = ABS_VEL_CMD; //!SHOULD BE FROM ArmSerialProtocol.h
4949
cmd_msg.velocities.resize(NUM_JOINTS);
5050
cmd_msg.velocities[0] = (static_cast<float>(msg->left.x) - 50)/100 * max_joysticks_output_speed_deg[0]*2;
5151
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

Lines changed: 21 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -5,28 +5,44 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
55
add_compile_options(-Wall -Wextra -Wpedantic)
66
endif()
77

8-
add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../external_pkgs/Catch2 ${CMAKE_CURRENT_BINARY_DIR}/external_pkgs/Catch2) # Include Catch2 for unit tests
8+
# Catch2 did not work very well. We tried, its just difficult to integrate in a scalable way.
9+
# leaving this stale code just commented to keep record that we tried it.
10+
# add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../external_pkgs/Catch2 ${CMAKE_CURRENT_BINARY_DIR}/external_pkgs/Catch2) # Include Catch2 for unit tests
911

1012
include_directories(include/)
1113

12-
1314
# find dependencies
1415
find_package(ament_cmake REQUIRED)
1516
find_package(rclcpp REQUIRED)
1617
find_package(rover_utils REQUIRED)
1718
find_package(sensor_msgs REQUIRED)
1819
find_package(example_interfaces REQUIRED)
1920

20-
2121
add_executable(integration_tester src/assert_node.cpp)
22+
add_executable(example_multiply_by_two_node src/example_multiply_by_two_node.cpp)
2223
ament_target_dependencies(integration_tester rclcpp sensor_msgs rover_utils example_interfaces)
24+
ament_target_dependencies(example_multiply_by_two_node rclcpp sensor_msgs rover_utils example_interfaces)
25+
26+
if(BUILD_TESTING)
27+
# Integration tests
28+
find_package(ament_cmake_ros REQUIRED)
29+
find_package(launch_testing_ament_cmake REQUIRED)
30+
function(add_ros_isolated_launch_test path)
31+
set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py")
32+
add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN})
33+
endfunction()
34+
add_ros_isolated_launch_test(test/test_launch_test.py)
35+
add_ros_isolated_launch_test(test/test_arm_pipeline.py)
36+
# Add more tests here
37+
endif()
2338

24-
target_link_libraries(integration_tester
25-
Catch2::Catch2)
39+
# target_link_libraries(integration_tester
40+
# Catch2::Catch2)
2641

2742
install(
2843
TARGETS
2944
integration_tester
45+
example_multiply_by_two_node
3046
DESTINATION lib/${PROJECT_NAME}
3147
)
3248

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
#pragma once
2+
3+
4+
// #define APPROX()

src/integration_test/package.xml

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,13 @@
1212
<depend>rclcpp</depend>
1313
<depend>rover_utils</depend>
1414
<depend>sensor_msgs</depend>
15+
16+
<test_depend>ament_cmake_ros</test_depend>
17+
<test_depend>launch</test_depend>
18+
<test_depend>launch_ros</test_depend>
19+
<test_depend>launch_testing</test_depend>
20+
<test_depend>launch_testing_ament_cmake</test_depend>
21+
<test_depend>rclpy</test_depend>
1522

1623
<test_depend>ament_lint_auto</test_depend>
1724
<test_depend>ament_lint_common</test_depend>

src/integration_test/src/assert_node.cpp

Lines changed: 20 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,12 @@
1-
// name?
2-
31
#include <rclcpp/rclcpp.hpp>
42
#include <sensor_msgs/msg/joy.hpp>
53
#include <example_interfaces/msg/int32.hpp>
6-
#include <catch2/catch_all.hpp>
4+
// #include <catch2/catch_all.hpp>
5+
6+
// This node is a work in progress, playing around with different ways to setup integration tests.
7+
// We are aiming for a standard, easy way to add tests, and an easy way to get test results.
8+
9+
#define TEST_INT_VALUE 50
710

811
// Class definition
912
class IntegrationTestNode : public rclcpp::Node {
@@ -33,11 +36,14 @@ IntegrationTestNode::IntegrationTestNode() : Node("integration_test_node")
3336
"/joy", qos);
3437

3538
checker_pub = this->create_publisher<example_interfaces::msg::Int32>(
36-
"/integration/test_int", qos);
39+
"/integration/test_int/output", qos);
3740

3841
checker_sub = this->create_subscription<example_interfaces::msg::Int32>(
39-
"/integration/test_int", qos, std::bind(&IntegrationTestNode::test_callback, this, std::placeholders::_1));
40-
42+
"/integration/test_int/input", qos, std::bind(&IntegrationTestNode::test_callback, this, std::placeholders::_1));
43+
44+
// example_interfaces::msg::Int32 test_msg;
45+
// test_msg.data = TEST_INT_VALUE;
46+
// checker_pub->publish(test_msg);
4147
}
4248

4349
// Main function (entry point of node)
@@ -59,5 +65,12 @@ int main(int argc, char *argv[])
5965

6066
void IntegrationTestNode::test_callback(example_interfaces::msg::Int32 msg)
6167
{
62-
68+
example_interfaces::msg::Int32 output;
69+
output.data = msg.data *2;
70+
checker_pub->publish(output);
71+
// TEST_CASE("integration test check")
72+
// {
73+
// REQUIRE(true);
74+
// REQUIRE(msg.data == TEST_INT_VALUE);
75+
// }
6376
}
Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
#include <rclcpp/rclcpp.hpp>
2+
#include <example_interfaces/msg/int32.hpp>
3+
4+
// This node is just an example node to show integration tests
5+
// Its just one publisher and one subscriber, give it an integer and it just doubles it
6+
7+
// Class definition
8+
class MultiplyBy2Node : public rclcpp::Node {
9+
public:
10+
MultiplyBy2Node(); // Constructor is defined in .cpp.
11+
12+
private:
13+
// SUBS AND PUBS
14+
rclcpp::Publisher<example_interfaces::msg::Int32>::SharedPtr output_pub;
15+
rclcpp::Subscription<example_interfaces::msg::Int32>::SharedPtr input_sub;
16+
17+
// Callbacks
18+
void test_callback(example_interfaces::msg::Int32 msg);
19+
20+
};
21+
22+
// Constructor
23+
MultiplyBy2Node::MultiplyBy2Node() : Node("multiply_by_two_integration_test_example_node")
24+
{ // 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..
25+
26+
// Quality of service example
27+
auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local();
28+
29+
output_pub = this->create_publisher<example_interfaces::msg::Int32>(
30+
"/integration/test_int/output", qos);
31+
32+
input_sub = this->create_subscription<example_interfaces::msg::Int32>(
33+
"/integration/test_int/input", qos, std::bind(&MultiplyBy2Node::test_callback, this, std::placeholders::_1));
34+
35+
RCLCPP_INFO(this->get_logger(), "Example Log message: Node has started");
36+
}
37+
38+
int main(int argc, char *argv[])
39+
{
40+
rclcpp::init(argc, argv);
41+
auto node = std::make_shared<MultiplyBy2Node>();
42+
rclcpp::spin(node);
43+
rclcpp::shutdown();
44+
return 0;
45+
}
46+
47+
void MultiplyBy2Node::test_callback(example_interfaces::msg::Int32 msg)
48+
{
49+
example_interfaces::msg::Int32 output;
50+
output.data = msg.data *2;
51+
output_pub->publish(output);
52+
}
Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
// This class is meant to be a helper class for integration tests.
2+
#include <string>
3+
#include <unordered_map>
4+
#include <cstdint>
5+
6+
// Work in progress...
7+
8+
// A single test
9+
class TestRunner {
10+
public:
11+
TestRunner(std::string name) {};
12+
13+
void assert(bool assertion)
14+
{
15+
16+
}
17+
18+
void assert(float val1, float val2, float thresh = 0.0000001)
19+
{
20+
21+
}
22+
23+
24+
private:
25+
std::string name;
26+
};
27+
28+
class IntegrationTestHelper {
29+
public:
30+
IntegrationTestHelper() : next_test_index(0), current_test_index(0) {};
31+
32+
TestRunner& add_test(std::string name);
33+
34+
private:
35+
std::unordered_map<uint32_t, TestRunner> tests;
36+
uint32_t next_test_index;
37+
uint32_t current_test_index;
38+
};
39+
40+
TestRunner& IntegrationTestHelper::add_test(std::string name)
41+
{
42+
current_test_index = next_test_index;
43+
tests.emplace(current_test_index, TestRunner(name));
44+
next_test_index++;
45+
return tests[current_test_index];
46+
}
47+
48+
49+
50+
51+
52+
// Example usage:
53+
54+
// auto new_test = test.add_test("My new test");

0 commit comments

Comments
 (0)