Added basic drive functions

compLib/src/comp_lib_node.cpp

@@ -0,0 +1,10 @@
+#include "ros_node.h"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp" 
+
+CompLibNode::CompLibNode()
+: Node("CompLibNode")
+{
+
+}

compLib/src/main.cpp

@@ -0,0 +1,66 @@
+#include <thread>
+#include <memory>
+#include <chrono>
+#include <mutex>
+
+#include "motor.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);
+}
+
+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>();
+    
+    std::thread t;
+    std::thread t1;
+    std::thread t2;
+
+    t = std::thread(run_node, ddn);
+    t1 = std::thread(run_node1, ssn);
+    t2 = std::thread(run_node2, ran);
+
+    ssn->drive(0.2);
+
+    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));
+
+    ddn->kill();
+    ssn->kill();
+    ddn->kill();
+
+    t.join();
+    t1.join();
+    t2.join();
+
+    return 0;
+}

compLib/src/motor.cpp

@@ -0,0 +1,171 @@
+#include <chrono>
+#include <thread>
+#include <mutex>
+#include <future>
+#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"
+
+#include "motor.h"
+#include "sequence_lock.h"
+
+double pi = 2 * acos(0.0);
+
+DriveDistNode::DriveDistNode()
+: Node("drive_dist_node")
+{
+    drive_dist_action_ = rclcpp_action::create_client<irobot_create_msgs::action::DriveDistance>(
+        this,
+        "/drive_distance"
+    );
+}
+
+void DriveDistNode::drive_dist(float meters, float velocity)
+{
+    std::cout << "drive dist" << std::endl;
+    processing = true;
+    auto data = irobot_create_msgs::action::DriveDistance::Goal();
+    auto send_goal_options = rclcpp_action::Client<irobot_create_msgs::action::DriveDistance>::SendGoalOptions();
+    send_goal_options.result_callback =
+        std::bind(&DriveDistNode::result_callback, this, std::placeholders::_1);
+
+    data.distance = meters;
+    data.max_translation_speed = velocity;
+    drive_dist_action_->async_send_goal(data, send_goal_options);
+
+    while (processing) {}
+}
+
+void DriveDistNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveDistance>::WrappedResult & result)
+{
+    processing = false;
+    std::cout << "finished dist" << std::endl;
+    switch (result.code) {
+        case rclcpp_action::ResultCode::SUCCEEDED:
+            return;
+        case rclcpp_action::ResultCode::ABORTED:
+            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
+            return;
+        case rclcpp_action::ResultCode::CANCELED:
+            RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
+            return;
+        default:
+            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
+            return;
+    }
+}
+
+void DriveDistNode::kill()
+{
+    std::cout << "DriveDistNode killed" << std::endl;
+    rclcpp::shutdown();
+}
+
+SetSpeedNode::SetSpeedNode()
+: Node("set_speed_node")
+{
+    speed_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
+        "/cmd_vel",
+        rclcpp::SensorDataQoS()
+    );
+}
+
+void SetSpeedNode::drive_loop(float speed)
+{
+    std::cout << "Loop" << std::endl;
+    while (run)
+    {
+        set_speed(speed);
+        // sleep set as described at http://wiki.ros.org/Robots/TIAGo/Tutorials/motions/cmd_vel
+        std::this_thread::sleep_for (std::chrono::milliseconds(333));
+    }
+}
+
+void SetSpeedNode::set_speed(float speed)
+{
+    auto data = geometry_msgs::msg::Twist();
+
+    data.linear.x = speed;
+    data.linear.y = 0;
+    data.linear.z = 0;
+    data.angular.x = 0;
+    data.angular.x = 0;
+    data.angular.x = 0;
+    speed_publisher_->publish(data);
+}
+
+void SetSpeedNode::drive(float speed)
+{
+    std::cout << "Drive" << std::endl;
+    run = true;
+    t = std::thread(&SetSpeedNode::drive_loop, this, speed);
+}
+
+void SetSpeedNode::stop()
+{
+    std::cout << "stop" << std::endl;
+    run = false;
+}
+
+void SetSpeedNode::kill()
+{
+    std::cout << "SetSpeedNode killed" << std::endl;
+    rclcpp::shutdown();
+}
+
+RotateAngleNode::RotateAngleNode()
+: Node("rotate_angle_node")
+{
+    rotate_angle_action_ = rclcpp_action::create_client<irobot_create_msgs::action::RotateAngle>(
+        this,
+        "rotate_angle"
+    );
+}
+
+void RotateAngleNode::rotate_angle(float angle, float velocity)
+{
+    processing = true;
+    angle *= pi / 180;
+    auto data = irobot_create_msgs::action::RotateAngle::Goal();
+    auto send_goal_options = rclcpp_action::Client<irobot_create_msgs::action::RotateAngle>::SendGoalOptions();
+    send_goal_options.result_callback =
+        std::bind(&RotateAngleNode::result_callback, this, std::placeholders::_1);
+    
+
+    data.angle = angle;
+    data.max_rotation_speed = velocity;
+    rotate_angle_action_->async_send_goal(data, send_goal_options);
+
+    while (processing) {}
+}
+
+void RotateAngleNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::RotateAngle>::WrappedResult & result)
+{
+    processing = false;
+    std::cout << "finished rotation" << std::endl;
+    switch (result.code) {
+        case rclcpp_action::ResultCode::SUCCEEDED:
+            return;
+        case rclcpp_action::ResultCode::ABORTED:
+            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
+            return;
+        case rclcpp_action::ResultCode::CANCELED:
+            RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
+            return;
+        default:
+            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
+            return;
+    }
+}
+
+void RotateAngleNode::kill()
+{
+    std::cout << "RotateAngleNode killed" << std::endl;
+    rclcpp::shutdown();
+}