Added basic drive functions

compLib/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.26)
+project(comp_lib)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(irobot_create_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+# add_executable(talker src/publisher_member_function.cpp)
+# ament_target_dependencies(talker rclcpp std_msgs irobot_create_msgs)
+
+# add_executable(listener src/subscriber_member_function.cpp)
+# ament_target_dependencies(listener rclcpp std_msgs irobot_create_msgs)
+
+# add_executable(drive src/drive_action.cpp)
+# ament_target_dependencies(drive rclcpp rclcpp_action std_msgs irobot_create_msgs)
+
+# add_executable(set_vel src/set_speed.cpp)
+# ament_target_dependencies(set_vel rclcpp rclcpp_action std_msgs irobot_create_msgs geometry_msgs)
+
+add_executable(compLib_node src/main.cpp src/motor.cpp)
+ament_target_dependencies(compLib_node rclcpp rclcpp_action std_msgs irobot_create_msgs geometry_msgs)
+
+target_include_directories(compLib_node PRIVATE include)
+
+install(TARGETS
+  #talker
+  #listener
+  #drive
+  #set_vel
+  compLib_node
+  DESTINATION lib/${PROJECT_NAME})
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

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

compLib/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>comp_lib</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="matthias@todo.todo">matthias</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>std_msgs</depend>
+  <depend>irobot_create_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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();
+}