diff --git a/src/app/RosControlTest/groupMotors.cpp b/src/app/RosControlTest/groupMotors.cpp new file mode 100644 index 0000000..434423f --- /dev/null +++ b/src/app/RosControlTest/groupMotors.cpp @@ -0,0 +1,58 @@ +#include "ControlGroup.h" +#include "ControlGroupRosHandler.h" +#include "Group.h" +#include "MockMotor.h" +#include "ros/node_handle.h" +#include +#include +#include +#include + +auto main(int32_t argc, char **argv) -> int32_t { + std::cout << "wrevolution ROS test start..." << std::endl; + + // motors 1 - 3 + auto motor1{std::make_shared(1, "LeftBackMotor_1")}; + auto motor2{std::make_shared(2, "LeftMiddleMotor_2")}; + auto motor3{std::make_shared(3, "LeftFrontMotor_3")}; + + // motors 4 - 6 + auto motor4{std::make_shared(4, "RightBackMotor_4")}; + auto motor5{std::make_shared(5, "RightMiddleMotor_5")}; + auto motor6{std::make_shared(6, "RightFrontMotor_6")}; + + // motor group 1 + auto motorGroup1{std::make_shared("LeftGroupMotors")}; + motorGroup1->addControlGroup(motor1); + motorGroup1->addControlGroup(motor2); + motorGroup1->addControlGroup(motor3); + + // motor group 2 + auto motorGroup1{std::make_shared("RightGroupMotors")}; + motorGroup2->addControlGroup(motor4); + motorGroup2->addControlGroup(motor5); + motorGroup2->addControlGroup(motor6); + + std::cout << "Motor setup complete!" << std::endl; + + // Setup ROS Interface + ros::init(argc, argv, "RosControlTest"); + ros::NodeHandle node{}; + + std::vector> controlGroupHandlers{}; + + controlGroupHandlers.reserve(testingGroups.size()); + + for (const auto &controlGroup : testingGroups) { + controlGroupHandlers.push_back(std::make_unique(node, controlGroup)); + } + + std::cout << "ROS Setup complete!" << std::endl; + + // Spin ROS comms + ros::spin(); + + std::cout << "Shutting down..." << std::endl; + + return 0; +}