Added basic drive functions

compLib/include/comp_lib_node.h

@@ -0,0 +1,14 @@
+#ifndef ROS_NODE_H
+#define ROS_NODE_H
+
+#include "rclcpp/rclcpp.hpp"
+
+class CompLibNode : public rclcpp::Node
+{
+public:
+    CompLibNode();
+
+    
+};
+
+#endif

compLib/include/motor.h

@@ -0,0 +1,54 @@
+#ifndef MOTOR_H
+#define MOTOR_H
+
+#include <thread>
+#include <memory>
+
+#include <geometry_msgs/msg/twist.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "irobot_create_msgs/action/drive_distance.hpp"
+#include "irobot_create_msgs/action/rotate_angle.hpp"
+
+class DriveDistNode : public rclcpp::Node
+{
+public:
+    DriveDistNode();
+    void drive_dist(float meters, float velocity);
+    void kill();
+private:
+    void result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveDistance>::WrappedResult & result);
+    rclcpp_action::Client<irobot_create_msgs::action::DriveDistance>::SharedPtr drive_dist_action_;
+    bool processing;
+};
+
+class SetSpeedNode : public rclcpp::Node
+{
+public:
+    SetSpeedNode();
+    void drive(float speed);
+    void stop();
+    void kill();
+private:
+    void set_speed(float speed);
+    void drive_loop(float speed);
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr speed_publisher_;
+    bool run = true;
+    std::thread t;
+};
+
+class RotateAngleNode : public rclcpp::Node
+{
+public:
+    RotateAngleNode();
+    void rotate_angle(float angle, float velocity);
+    void kill();
+private:
+    void result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::RotateAngle>::WrappedResult & result);
+    rclcpp_action::Client<irobot_create_msgs::action::RotateAngle>::SharedPtr rotate_angle_action_;
+    bool processing;
+};
+
+#endif

compLib/include/sequence_lock.h

@@ -0,0 +1,11 @@
+#ifndef SEQUENCE_LOCK_H
+#define SEQUENCE_LOCK_H
+
+#include <mutex>
+
+namespace SequenceLock
+{
+    std::mutex m;
+}
+
+#endif