Add drive arc functionality

compLib/src/main.cpp

@@ -23,6 +23,11 @@ void run_node2(std::shared_ptr<RotateAngleNode> node)
     rclcpp::spin(node);
 }
 
+void run_node3(std::shared_ptr<DriveArcNode> node)
+{
+    rclcpp::spin(node);
+}
+
 int main(int argc, char * argv[])
 {
     rclcpp::init(argc, argv);
@@ -30,16 +35,19 @@ int main(int argc, char * 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>();
     
     std::thread t;
     std::thread t1;
     std::thread t2;
+    std::thread t3;
 
     t = std::thread(run_node, ddn);
     t1 = std::thread(run_node1, ssn);
     t2 = std::thread(run_node2, ran);
+    t3 = std::thread(run_node3, dan);
 
-    ssn->drive(0.2);
+    ssn->drive(0.3);
 
     std::this_thread::sleep_for (std::chrono::milliseconds(2000));
 
@@ -54,13 +62,17 @@ int main(int argc, char * argv[])
 
     // 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();
 
     t.join();
     t1.join();
     t2.join();
+    t3.join();
 
     return 0;
 }