can over tcp version 1;

Author: lyhyl

include/barrett/bus/can_over_tcp_socket.h

@@ -33,6 +33,7 @@ class CANOverTCPSocket : public CommunicationsBus
 	 */
     CANOverTCPSocket();
     CANOverTCPSocket(int port) throw(std::runtime_error);
+    CANOverTCPSocket(const char *ip, const char *port) throw(std::runtime_error);
     ~CANOverTCPSocket();
     /** getMutex() method gets and locks interthread data exchange assuring nothing critical is happening in either thread.
 	 */
@@ -61,6 +62,9 @@ class CANOverTCPSocket : public CommunicationsBus
 
     mutable unsigned char packetData[64];
 
+    const char *ip_;
+    const char *port_;
+
 private:
     DISALLOW_COPY_AND_ASSIGN(CANOverTCPSocket);
 };

include/barrett/can_over_tcp_main_function.h

@@ -0,0 +1,97 @@
+/**
+ *	Copyright 2019-2119 Wingkou <[email protected]>
+ */
+
+/**
+ * @file can_over_tcp_main_function.h
+ * @date 10/22/2019
+ * @author Wingkou
+ */
+
+#ifndef CAN_OVER_TCP_MAIN_FUNCTION_H_
+#define CAN_OVER_TCP_MAIN_FUNCTION_H_
+
+#include <barrett/exception.h>
+#include <barrett/products/product_manager.h>
+#include <barrett/bus/bus_manager.h>
+#include <barrett/systems/wam.h>
+
+#include "can_over_tcp_socket.h"
+
+#ifdef BARRETT_SMF_VALIDATE_ARGS
+bool validate_args(int argc, char **argv);
+#endif
+
+#ifdef BARRETT_SMF_CONFIGURE_PM
+bool configure_pm(int argc, char **argv, ::barrett::ProductManager &pm);
+#endif
+
+#ifndef BARRETT_SMF_NO_DECLARE
+template <size_t DOF>
+int wam_main(int argc, char **argv, ::barrett::ProductManager &pm, ::barrett::systems::Wam<DOF> &wam);
+#endif
+
+#ifndef BARRETT_SMF_DONT_WAIT_FOR_SHIFT_ACTIVATE
+#define BARRETT_SMF_WAIT_FOR_SHIFT_ACTIVATE true
+#else
+#define BARRETT_SMF_WAIT_FOR_SHIFT_ACTIVATE false
+#endif
+
+#ifndef BARRETT_SMF_DONT_PROMPT_ON_ZEROING
+#define BARRETT_SMF_PROMPT_ON_ZEROING true
+#else
+#define BARRETT_SMF_PROMPT_ON_ZEROING false
+#endif
+
+#ifndef BARRETT_SMF_WAM_CONFIG_PATH
+#define BARRETT_SMF_WAM_CONFIG_PATH NULL
+#endif
+
+int main(int argc, char **argv)
+{
+    if (argc < 3)
+    {
+        std::cerr << "Usage: your_program_name <host> <port> [other options]\n";
+        return 1;
+    }
+
+    // Give us pretty stack-traces when things die
+    ::barrett::installExceptionHandler();
+
+    // #ifdef BARRETT_SMF_VALIDATE_ARGS
+    //     if (!validate_args(argc, argv))
+    //     {
+    //         return 1;
+    //     }
+    // #endif
+
+    ::barrett::ProductManager pm(NULL, new ::barrett::bus::BusManager(new ::barrett::bus::CANOverTCPSocket(argv[1], argv[2])));
+    // #ifdef BARRETT_SMF_CONFIGURE_PM
+    // 	if ( !configure_pm(argc, argv, pm) ) {
+    // 		return 1;
+    // 	}
+    // #endif
+
+    pm.waitForWam(BARRETT_SMF_PROMPT_ON_ZEROING);
+    pm.wakeAllPucks();
+    // TODO(JH): Rehab Update implement and test
+    if (pm.foundWam3())
+    {
+        return wam_main(argc, argv, pm, *pm.getWam3(BARRETT_SMF_WAIT_FOR_SHIFT_ACTIVATE, BARRETT_SMF_WAM_CONFIG_PATH));
+    }
+    else if (pm.foundWam4())
+    {
+        return wam_main(argc, argv, pm, *pm.getWam4(BARRETT_SMF_WAIT_FOR_SHIFT_ACTIVATE, BARRETT_SMF_WAM_CONFIG_PATH));
+    }
+    else if (pm.foundWam7())
+    {
+        return wam_main(argc, argv, pm, *pm.getWam7(BARRETT_SMF_WAIT_FOR_SHIFT_ACTIVATE, BARRETT_SMF_WAM_CONFIG_PATH));
+    }
+    else
+    {
+        printf(">>> ERROR: No WAM was found. Perhaps you have found a bug in ProductManager::waitForWam().\n");
+        return 1;
+    }
+}
+
+#endif /* BARRETT_STANDARD_MAIN_FUNCTION_H_ */

src/bus/can_over_tcp_server.cpp

@@ -9,7 +9,7 @@
  */
 
 #include <barrett/os.h>
-#include <barrett/bus/can_over_tcp_server.h>
+#include "can_over_tcp_server.h"
 
 namespace barrett
 {
@@ -78,6 +78,7 @@ class session
         }
         else
         {
+            std::cout << "handle_read_header died" << std::endl;
             delete this;
         }
     }
@@ -103,9 +104,18 @@ class session
             // TODO
         }
 
+        // printf("<<bus id:%d len:%d data:", busId, len);
+        // for (size_t i = 0; i < len; i++)
+        // {
+        //     printf("%X", (unsigned char)data_[i]);
+        // }
+        // putchar('\n');
+
         // msg : ret(4)
         int32_t ret = cansocket_->send(busId, reinterpret_cast<const unsigned char *>(data_), len);
 
+        // printf(">>ret:%d\n", ret);
+
         memcpy(data_, &ret, 4);
         boost::asio::write(socket_, boost::asio::buffer(data_, 4));
     }
@@ -123,11 +133,25 @@ class session
 
         int blocking = readInt32(reinterpret_cast<const unsigned char *>(data_));
 
+        // printf("<<blocking:%d\n", blocking);
+
         // msg : ret(4) + busId(4) + len(4) + data
         int busId;
         size_t len;
         int32_t ret = cansocket_->receiveRaw(busId, reinterpret_cast<unsigned char *>(data_ + 12), len, blocking == 1);
 
+        if (ret != 0)
+        {
+            len = 0;
+        }
+
+        // printf(">>ret:%d bus id:%d len:%d data:", ret, busId, (int)len);
+        // for (size_t i = 0; i < len; i++)
+        // {
+        //     printf("%X", (unsigned char)data_[i + 12]);
+        // }
+        // putchar('\n');
+
         int32_t dbusId = busId;
         int32_t dlen = len;
         memcpy(data_, &ret, 4);
@@ -152,6 +176,8 @@ CANOverTCPServer::CANOverTCPServer(boost::asio::io_service &io_service, short po
 {
     cansocket.open(canPort);
 
+    std::cout << "CAN Socket is " << (cansocket.isOpen() ? "opened" : "closed") << std::endl;
+
     session *new_session = new session(io_service_, &cansocket);
     acceptor_.async_accept(new_session->socket(),
                            boost::bind(&CANOverTCPServer::handle_accept, this, new_session,
@@ -164,13 +190,18 @@ void CANOverTCPServer::handle_accept(session *new_session,
     if (!error)
     {
         new_session->start();
+
+        std::cout << "session started" << std::endl;
+
         new_session = new session(io_service_, &cansocket);
         acceptor_.async_accept(new_session->socket(),
                                boost::bind(&CANOverTCPServer::handle_accept, this, new_session,
                                            boost::asio::placeholders::error));
     }
     else
     {
+        std::cout << "session died" << std::endl;
+
         delete new_session;
     }
 }

src/bus/can_over_tcp_socket.cpp

@@ -17,7 +17,7 @@
 #include <barrett/os.h>
 #include <barrett/thread/real_time_mutex.h>
 #include <barrett/products/puck.h>
-#include <barrett/bus/can_over_tcp_socket.h>
+#include "can_over_tcp_socket.h"
 
 namespace barrett
 {
@@ -28,6 +28,11 @@ CANOverTCPSocket::CANOverTCPSocket() : mutex()
 {
 }
 
+CANOverTCPSocket::CANOverTCPSocket(const char *ip, const char *port) throw(std::runtime_error) : ip_(ip), port_(port)
+{
+    open(0);
+}
+
 CANOverTCPSocket::CANOverTCPSocket(int port) throw(std::runtime_error) : mutex()
 {
     open(port);
@@ -47,8 +52,10 @@ void CANOverTCPSocket::open(int port) throw(std::logic_error, std::runtime_error
 
     logMessage("CANOverTCPSocket::open(%d) using CAN Over TCP driver") % port;
 
+    std::cout << "using CAN Over TCP driver" << std::endl;
+
     tcp::resolver resolver(io_service);
-    tcp::resolver::query query(tcp::v4(), "localhost", "12345");
+    tcp::resolver::query query(tcp::v4(), ip_, port_);
     tcp::resolver::iterator iterator = resolver.resolve(query);
 
     socket = new tcp::socket(io_service);
@@ -84,20 +91,32 @@ int CANOverTCPSocket::send(int busId, const unsigned char *data, size_t len) con
     memcpy(packetData + 12, data, len);
     boost::asio::write(*socket, boost::asio::buffer(packetData, len + 12));
 
+    // printf("<<bus id:%d len:%d data:", busId, (int)len);
+    // for (size_t i = 0; i < len; i++)
+    // {
+    //     printf("%X", (unsigned char)data[i]);
+    // }
+    // putchar('\n');
+
     // msg : ret(4)
     const size_t reply_length = 4;
     char reply[reply_length];
     size_t actual_reply_length = boost::asio::read(*socket, boost::asio::buffer(reply, reply_length));
 
     assert(actual_reply_length == reply_length);
 
-    return *reinterpret_cast<int32_t *>(reply);
+    int ret = *reinterpret_cast<int32_t *>(reply);
+    // printf(">>ret:%d\n", ret);
+
+    return ret;
 }
 
 int CANOverTCPSocket::receiveRaw(int &busId, unsigned char *data, size_t &len, bool blocking) const
 {
     BARRETT_SCOPED_LOCK(mutex);
 
+    // printf("<<blocking:%d\n", blocking);
+
     // msg : type(4) + blocking(4)
     int32_t type = 1;
     int32_t dblocking = blocking ? 1 : 0;
@@ -120,6 +139,13 @@ int CANOverTCPSocket::receiveRaw(int &busId, unsigned char *data, size_t &len, b
 
     assert(actual_data_length == len);
 
+    // printf(">>ret:%d bus id:%d len:%d data:", ret, busId, (int)len);
+    // for (size_t i = 0; i < len; i++)
+    // {
+    //     printf("%X", (unsigned char)data[i]);
+    // }
+    // putchar('\n');
+
     return ret;
 }