README.md

@@ -1,11 +1,7 @@
 # compLIB
 
-Rewrite for ROS is the live packaged version since 18.07.2023.
-
 # Dependencies
 
-TODO: document
-
 ## Building documentation
 ```
 pip install sphinx-rtd-theme

compLib/CMakeLists.txt

@@ -25,7 +25,7 @@ find_package(geometry_msgs REQUIRED)
 # 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 src/controls.cpp)
+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)

compLib/include/controls.h

@@ -1,24 +0,0 @@
-#ifndef CONTROLS_H
-#define CONTROLS_H
-
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
-
-#include "irobot_create_msgs/msg/interface_buttons.hpp"
-#include "irobot_create_msgs/msg/lightring_leds.hpp"
-
-class ButtonPressNode : public rclcpp::Node
-{
-public:
-    ButtonPressNode();
-    void bt1_wait();
-    void bt2_wait();
-    void kill();
-private:
-    void result_callback(const irobot_create_msgs::msg::InterfaceButtons::SharedPtr result);
-    rclcpp::Subscription<irobot_create_msgs::msg::InterfaceButtons>::SharedPtr interface_buttons_subscriber_;
-    bool button1{false};
-    bool button2{false};
-};
-
-#endif

compLib/include/motor.h

@@ -10,7 +10,6 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 
 #include "irobot_create_msgs/action/drive_distance.hpp"
-#include "irobot_create_msgs/action/drive_arc.hpp"
 #include "irobot_create_msgs/action/rotate_angle.hpp"
 
 class DriveDistNode : public rclcpp::Node
@@ -52,16 +51,4 @@ private:
     bool processing;
 };
 
-class DriveArcNode : public rclcpp::Node
-{
-public:
-    DriveArcNode();
-    void drive_arc(float angle, float radius, float velocity, int direction=1);
-    void kill();
-private:
-    void result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveArc>::WrappedResult & result);
-    rclcpp_action::Client<irobot_create_msgs::action::DriveArc>::SharedPtr drive_arc_action_;
-    bool processing;
-};
-
 #endif

compLib/src/controls.cpp

@@ -1,53 +0,0 @@
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
-
-#include "irobot_create_msgs/msg/interface_buttons.hpp"
-
-#include "controls.h"
-
-ButtonPressNode::ButtonPressNode()
-: Node("button_press_node")
-{
-    interface_buttons_subscriber_ = this->create_subscription<irobot_create_msgs::msg::InterfaceButtons>(
-        "/interface_buttons",
-        rclcpp::SensorDataQoS(),
-        std::bind(&ButtonPressNode::result_callback, this, std::placeholders::_1)
-    );
-}
-
-void ButtonPressNode::bt1_wait()
-{
-    RCLCPP_INFO(this->get_logger(), "Wait for button 1...");
-    button1 = false;
-    while (!button1) {}
-    button1 = false;
-}
-
-void ButtonPressNode::bt2_wait()
-{
-    RCLCPP_INFO(this->get_logger(), "Wait for button 2...");
-    button2 = false;
-    while (!button2) {}
-    button2 = false;
-}
-
-void ButtonPressNode::result_callback(const irobot_create_msgs::msg::InterfaceButtons::SharedPtr result)
-{
-    if (result->button_1.is_pressed) {
-        button1 = true;
-    }
-
-    if (result->button_2.is_pressed) {
-        button2 = true;
-    }
-
-    if (result->button_power.is_pressed) {
-
-    }
-}
-
-void ButtonPressNode::kill()
-{
-    RCLCPP_INFO(this->get_logger(), "ButtonPressNode killed");
-    rclcpp::shutdown();
-}

compLib/src/main.cpp

@@ -4,7 +4,6 @@
 #include <mutex>
 
 #include "motor.h"
-#include "controls.h"
 
 std::mutex action_mutex;
 
@@ -24,16 +23,6 @@ void run_node2(std::shared_ptr<RotateAngleNode> node)
     rclcpp::spin(node);
 }
 
-void run_node3(std::shared_ptr<DriveArcNode> node)
-{
-    rclcpp::spin(node);
-}
-
-void run_node4(std::shared_ptr<ButtonPressNode> node)
-{
-    rclcpp::spin(node);
-}
-
 int main(int argc, char * argv[])
 {
     rclcpp::init(argc, argv);
@@ -41,26 +30,16 @@ 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>();
-    auto bpn = std::make_shared<ButtonPressNode>();
     
     std::thread t;
     std::thread t1;
     std::thread t2;
-    std::thread t3;
-    std::thread t4;
 
     t = std::thread(run_node, ddn);
     t1 = std::thread(run_node1, ssn);
     t2 = std::thread(run_node2, ran);
-    t3 = std::thread(run_node3, dan);
-    t4 = std::thread(run_node4, bpn);
 
-    bpn->bt1_wait();
+    ssn->drive(0.2);
-    bpn->bt2_wait();
-    bpn->bt1_wait();
-
-    ssn->drive(0.3);
 
     std::this_thread::sleep_for (std::chrono::milliseconds(2000));
 
@@ -75,19 +54,13 @@ 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();
-    bpn->kill();
 
     t.join();
     t1.join();
     t2.join();
-    t3.join();
-    t4.join();
 
     return 0;
 }

compLib/src/motor.cpp

@@ -10,7 +10,6 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 
 #include "irobot_create_msgs/action/drive_distance.hpp"
-#include "irobot_create_msgs/action/drive_arc.hpp"
 #include "irobot_create_msgs/action/rotate_angle.hpp"
 
 #include "motor.h"
@@ -29,7 +28,7 @@ DriveDistNode::DriveDistNode()
 
 void DriveDistNode::drive_dist(float meters, float velocity)
 {
-    RCLCPP_INFO(this->get_logger(), "drive dist");
+    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();
@@ -46,9 +45,9 @@ void DriveDistNode::drive_dist(float meters, float velocity)
 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:
-            RCLCPP_INFO(this->get_logger(), "finished dist");
             return;
         case rclcpp_action::ResultCode::ABORTED:
             RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
@@ -64,7 +63,7 @@ void DriveDistNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot
 
 void DriveDistNode::kill()
 {
-    RCLCPP_INFO(this->get_logger(), "DriveDistNode killed");
+    std::cout << "DriveDistNode killed" << std::endl;
     rclcpp::shutdown();
 }
 
@@ -79,6 +78,7 @@ SetSpeedNode::SetSpeedNode()
 
 void SetSpeedNode::drive_loop(float speed)
 {
+    std::cout << "Loop" << std::endl;
     while (run)
     {
         set_speed(speed);
@@ -102,20 +102,20 @@ void SetSpeedNode::set_speed(float speed)
 
 void SetSpeedNode::drive(float speed)
 {
-    RCLCPP_INFO(this->get_logger(), "Start drive");
+    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;
-    RCLCPP_INFO(this->get_logger(), "Stop drive");
 }
 
 void SetSpeedNode::kill()
 {
-    RCLCPP_INFO(this->get_logger(), "SetSpeedNode killed");
+    std::cout << "SetSpeedNode killed" << std::endl;
     rclcpp::shutdown();
 }
 
@@ -148,9 +148,9 @@ void RotateAngleNode::rotate_angle(float angle, float velocity)
 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:
-            RCLCPP_INFO(this->get_logger(), "finished rotation");
             return;
         case rclcpp_action::ResultCode::ABORTED:
             RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
@@ -166,60 +166,6 @@ void RotateAngleNode::result_callback(const rclcpp_action::ClientGoalHandle<irob
 
 void RotateAngleNode::kill()
 {
-    RCLCPP_INFO(this->get_logger(), "RotateAngleNode killed");
+    std::cout << "RotateAngleNode killed" << std::endl;
-    rclcpp::shutdown();
-}
-
-DriveArcNode::DriveArcNode()
-: Node("drive_arc_node")
-{
-    drive_arc_action_ = rclcpp_action::create_client<irobot_create_msgs::action::DriveArc>(
-        this,
-        "/drive_arc"
-    );
-}
-
-
-void DriveArcNode::drive_arc(float angle, float radius, float velocity, int direction)
-{
-    processing = true;
-    angle *= pi / 180;
-    auto data = irobot_create_msgs::action::DriveArc::Goal();
-    auto send_goal_options = rclcpp_action::Client<irobot_create_msgs::action::DriveArc>::SendGoalOptions();
-    send_goal_options.result_callback =
-        std::bind(&DriveArcNode::result_callback, this, std::placeholders::_1);
-    
-
-    data.angle = angle;
-    data.radius = radius;
-    data.translate_direction = direction;
-    data.max_translation_speed = velocity;
-    drive_arc_action_->async_send_goal(data, send_goal_options);
-
-    while (processing) {}
-}
-
-void DriveArcNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveArc>::WrappedResult & result)
-{
-    processing = false;
-    switch (result.code) {
-        case rclcpp_action::ResultCode::SUCCEEDED:
-            RCLCPP_INFO(this->get_logger(), "finished arc");
-            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 DriveArcNode::kill()
-{
-    RCLCPP_INFO(this->get_logger(), "DriveArcNode killed");
     rclcpp::shutdown();
 }