93 lines
No EOL
1.8 KiB
C++
93 lines
No EOL
1.8 KiB
C++
#include <thread>
|
|
#include <memory>
|
|
#include <chrono>
|
|
#include <mutex>
|
|
|
|
#include "motor.h"
|
|
#include "controls.h"
|
|
|
|
std::mutex action_mutex;
|
|
|
|
// #lazyness
|
|
void run_node(std::shared_ptr<DriveDistNode> node)
|
|
{
|
|
rclcpp::spin(node);
|
|
}
|
|
|
|
void run_node1(std::shared_ptr<SetSpeedNode> node)
|
|
{
|
|
rclcpp::spin(node);
|
|
}
|
|
|
|
void run_node2(std::shared_ptr<RotateAngleNode> node)
|
|
{
|
|
rclcpp::spin(node);
|
|
}
|
|
|
|
void run_node3(std::shared_ptr<DriveArcNode> node)
|
|
{
|
|
rclcpp::spin(node);
|
|
}
|
|
|
|
void run_node4(std::shared_ptr<ButtonPressNode> node)
|
|
{
|
|
rclcpp::spin(node);
|
|
}
|
|
|
|
int main(int argc, char * argv[])
|
|
{
|
|
rclcpp::init(argc, argv);
|
|
|
|
auto ddn = std::make_shared<DriveDistNode>();
|
|
auto ssn = std::make_shared<SetSpeedNode>();
|
|
auto ran = std::make_shared<RotateAngleNode>();
|
|
auto dan = std::make_shared<DriveArcNode>();
|
|
auto bpn = std::make_shared<ButtonPressNode>();
|
|
|
|
std::thread t;
|
|
std::thread t1;
|
|
std::thread t2;
|
|
std::thread t3;
|
|
std::thread t4;
|
|
|
|
t = std::thread(run_node, ddn);
|
|
t1 = std::thread(run_node1, ssn);
|
|
t2 = std::thread(run_node2, ran);
|
|
t3 = std::thread(run_node3, dan);
|
|
t4 = std::thread(run_node4, bpn);
|
|
|
|
bpn->bt1_wait();
|
|
bpn->bt2_wait();
|
|
bpn->bt1_wait();
|
|
|
|
ssn->drive(0.3);
|
|
|
|
std::this_thread::sleep_for (std::chrono::milliseconds(2000));
|
|
|
|
ssn->stop();
|
|
|
|
// std::this_thread::sleep_for (std::chrono::milliseconds(1000));
|
|
|
|
ran->rotate_angle(-45, 0.5);
|
|
// std::this_thread::sleep_for (std::chrono::milliseconds(5000));
|
|
|
|
ddn->drive_dist(0.2, 0.3);
|
|
|
|
// std::this_thread::sleep_for (std::chrono::milliseconds(5000));
|
|
|
|
dan->drive_arc(90, 0.5, 0.5);
|
|
|
|
ddn->kill();
|
|
ssn->kill();
|
|
ddn->kill();
|
|
dan->kill();
|
|
bpn->kill();
|
|
|
|
t.join();
|
|
t1.join();
|
|
t2.join();
|
|
t3.join();
|
|
t4.join();
|
|
|
|
return 0;
|
|
} |