Implemented bluetooth connection

Author: RiscadoA

CMakeLists.txt

@@ -29,6 +29,8 @@ if(NOT BUILD_ONLY_DOCS)
         "src/app/app.cpp"
         "src/app/webots_app.hpp"
         "src/app/webots_app.cpp"
+        "src/app/hs_app.hpp"
+        "src/app/hs_app.cpp"
     )
 
     add_executable(controller ${SOURCES})

src/app/hs_app.cpp

@@ -0,0 +1,108 @@
+#include "hs_app.hpp"
+
+#include "../behaviour/manual.hpp"
+#include "../behaviour/line_follower.hpp"
+
+#include <iostream>
+
+using namespace hs::app;
+
+#ifdef USE_BLUETOOTH
+
+#include <unistd.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/rfcomm.h>
+
+HSApp::HSApp(const robot::Robot& robot, behaviour::Manager& manager) : App(robot, manager)
+{
+    this->joystickX = 0.0;
+    this->joystickY = 0.0;
+    this->cameraAngle = 0.0;
+
+    // Allocate socket
+    this->socket = ::socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+
+    // Bind socket to port 1 of the first available local bluetooth adapter
+    sockaddr_rc locAddr = {0};
+    locAddr.rc_family = AF_BLUETOOTH;
+    locAddr.rc_bdaddr = {{0, 0, 0, 0, 0, 0}};
+    locAddr.rc_channel = HSApp::RFCOMM_CHANNEL;
+    ::bind(this->socket, (sockaddr*)&locAddr, sizeof(locAddr));
+
+    // Put socket into listening mode
+    ::listen(this->socket, 1);
+
+    // Accept one connection
+    std::cout << "Waiting for HS Robot App to connect..." << std::endl;
+    sockaddr_rc remAddr = {0};
+    socklen_t remAddrLen = sizeof(remAddr);
+    this->client = ::accept(this->socket, reinterpret_cast<sockaddr*>(&remAddr), &remAddrLen);
+    if (this->client < 0)
+    {
+        std::cerr << "Connection to HS Robot App failed, couldn't accept socket connection (accept() returned < 0)"
+                  << std::endl;
+        abort();
+    }
+
+    // Get address string from client and print it
+    char addrStr[100];
+    ba2str(reinterpret_cast<const bdaddr_t*>(&remAddr), addrStr);
+    std::cout << "Connected to HS Robot App (" << addrStr << ")" << std::endl;
+}
+
+HSApp::~HSApp()
+{
+    std::cout << "Closed HS Robot App connection" << std::endl;
+    ::close(this->client);
+    ::close(this->socket);
+}
+
+double HSApp::getJoystickX() const
+{
+    return this->joystickX;
+}
+
+double HSApp::getJoystickY() const
+{
+    return this->joystickY;
+}
+
+double HSApp::getCameraAngle() const
+{
+    return this->cameraAngle;
+}
+
+void HSApp::update()
+{
+    // TODO:
+    // Implement bluetooth protocol
+}
+
+#else
+
+HSApp::HSApp(const robot::Robot& robot, behaviour::Manager& manager) : App(robot, manager)
+{
+    abort(); // Unsupported operation
+}
+
+double HSApp::getJoystickX() const
+{
+    abort(); // Unsupported operation
+}
+
+double HSApp::getJoystickY() const
+{
+    abort(); // Unsupported operation
+}
+
+double HSApp::getCameraAngle() const
+{
+    abort(); // Unsupported operation
+}
+
+void HSApp::update()
+{
+    abort(); // Unsupported operation
+}
+
+#endif // USE_BLUETOOTH

src/app/hs_app.hpp

@@ -0,0 +1,42 @@
+#ifndef HS_APP_HS_APP_HPP
+#define HS_APP_HS_APP_HPP
+
+#include "app.hpp"
+
+namespace hs::app
+{
+    /// Implements the abstract App class using the real HS Robot application.
+    class HSApp : public App
+    {
+    public:
+        /// @param robot   The robot which this app controls.
+        /// @param manager The behaviour manager of the robot.
+        HSApp(const robot::Robot& robot, behaviour::Manager& manager);
+        virtual ~HSApp() override;
+
+        // Abstract methods implementation
+        virtual double getJoystickX() const override;
+        virtual double getJoystickY() const override;
+        virtual double getCameraAngle() const override;
+        virtual void update() override;
+
+    private:
+#ifdef USE_BLUETOOTH
+
+        static inline const uint8_t RFCOMM_CHANNEL = 1; ///< RFCOMM channel.
+
+        /// Registers the SDP service so that it can be discovered by the client.
+        void registerSDPService();
+
+        int socket; ///< Listener socket.
+        int client; ///< Client socket.
+
+        double joystickX;   ///< Last joystick X axis value.
+        double joystickY;   ///< Last joystick Y axis value.
+        double cameraAngle; ///< Target camera angle.
+
+#endif // USE_BLUETOOTH
+    };
+} // namespace hs::app
+
+#endif // HS_APP_HS_APP_HPP

src/app/webots_app.hpp

@@ -37,4 +37,4 @@ namespace hs::app
     };
 } // namespace hs::app
 
-#endif // HS_APP_WEBOTS_APP_HPP
+#endif // HS_APP_WEBOTS_APP_HPP

src/main.cpp

@@ -8,6 +8,7 @@
 #include "behaviour/line_follower.hpp"
 
 #include "app/webots_app.hpp"
+#include "app/hs_app.hpp"
 
 using namespace hs;
 using namespace hs::robot;
@@ -23,7 +24,7 @@ int main()
     Manager* manager = new Manager(*robot);
 
     // Setup app
-    App* app = new WebotsApp(*robot, *manager);
+    App* app = new HSApp(*robot, *manager);
 
     // Register behaviour types
     manager->registerType<Manual>(std::ref(*app));