From 15e172d066c3ef83204bf486dc935985ac5c9a29 Mon Sep 17 00:00:00 2001
From: Wojciech Kwolek <me@irth.pl>
Date: Thu, 20 May 2021 12:17:40 +0000
Subject: [PATCH] implement clutch

closes #10
---
 src/ros2aria/CMakeLists.txt   |  3 ++-
 src/ros2aria/src/clutch.cpp   | 12 ++++++++++++
 src/ros2aria/src/ros2aria.cpp |  2 ++
 src/ros2aria/src/ros2aria.hpp |  4 ++++
 4 files changed, 20 insertions(+), 1 deletion(-)
 create mode 100644 src/ros2aria/src/clutch.cpp

diff --git a/src/ros2aria/CMakeLists.txt b/src/ros2aria/CMakeLists.txt
index 9aeebaa..36928ce 100644
--- a/src/ros2aria/CMakeLists.txt
+++ b/src/ros2aria/CMakeLists.txt
@@ -37,7 +37,8 @@ add_executable(ros2aria
   src/sonar.cpp
   src/pose.cpp
   src/gripper.cpp
-  src/wheels.cpp)
+  src/wheels.cpp
+  src/clutch.cpp)
   
 ament_target_dependencies(ros2aria rclcpp)
 ament_target_dependencies(ros2aria std_srvs)
diff --git a/src/ros2aria/src/clutch.cpp b/src/ros2aria/src/clutch.cpp
new file mode 100644
index 0000000..5736226
--- /dev/null
+++ b/src/ros2aria/src/clutch.cpp
@@ -0,0 +1,12 @@
+#include "ros2aria.hpp"
+
+void Ros2Aria::clutch_callback(const std_msgs::msg::Bool::SharedPtr msg)
+{
+    std::lock_guard<RAIIBot> lock(*robot.get());
+
+    auto r = robot->getRobot();
+    if (msg->data)
+        r->enableMotors();
+    else
+        r->disableMotors();
+}
\ No newline at end of file
diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index d692237..bfccbc9 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -12,6 +12,8 @@ Ros2Aria::Ros2Aria()
     RCLCPP_INFO(this->get_logger(), "starting subscribers and services");
     cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
         "cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1));
+    clutch_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+        "clutch", 10, std::bind(&Ros2Aria::clutch_callback, this, _1));
 
     sonar_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud>("sonar", 10);
     sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10);
diff --git a/src/ros2aria/src/ros2aria.hpp b/src/ros2aria/src/ros2aria.hpp
index 3d9783a..9e9aa1f 100644
--- a/src/ros2aria/src/ros2aria.hpp
+++ b/src/ros2aria/src/ros2aria.hpp
@@ -7,6 +7,7 @@
 #include "geometry_msgs/msg/twist.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "std_srvs/srv/empty.hpp"
+#include "std_msgs/msg/bool.hpp"
 
 #include "./raiibot.cpp"
 
@@ -47,6 +48,9 @@ private:
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
     void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
 
+    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr clutch_sub_;
+    void clutch_callback(const std_msgs::msg::Bool::SharedPtr msg);
+
     // services
     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;
     void stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const;