Added MBSysC websocket module

ExampleProjects/CartPendulum/userfctR/realtime/user_realtime_events.c

@@ -6,12 +6,14 @@
  * In order to use C++ features, you just need to change the extension of this file (.c) to .cc (or .cpp).
  */
  
-#ifdef SDL
+
 
 #include "realtime.h"
 #include "events_sdl.h"
 #include "user_realtime.h"
 
+#ifdef SDL
+
 /*! \brief handle inputs comming from the keyboard
  * 
  * \param[in,out] mbs_data Robotran main structure
@@ -92,3 +94,9 @@ void user_joystick_buttons(MbsData* mbs_data, int buttonID)
 }
 
 #endif
+
+#ifdef WEBSOCKET
+void user_keyboard(MbsData* mbs_data, Simu_realtime *realtime, int keyevent, int state){
+
+}
+#endif
\ No newline at end of file

ExampleProjects/CartPendulum/workR/src/main.c

@@ -193,7 +193,7 @@ int main(int argc, char const *argv[])
     mbs_dirdyn->options->dt0 = 1e-3;
     mbs_dirdyn->options->tf  = 10.0;
     mbs_dirdyn->options->save2file = 1;
-    //mbs_dirdyn->options->realtime = 1;
+    mbs_dirdyn->options->realtime = 1;
     
     mbs_run_dirdyn(mbs_dirdyn, mbs_data);
     

MBsysC/cmake_aux/flags/CMakeLists.txt

@@ -75,6 +75,9 @@ function(mbsysc_specific_flags)  # flag for lib_mbsysC only (not set by project)
     # enable frame capture to record screenshot of the simulation
     cmake_dependent_option(FLAG_FRAME_CAPTURE "Enable frame capture (need OpenGL)" OFF "FLAG_OPEN_GL" OFF)
 
+    # use the websocket to display in real-time the animation
+    cmake_dependent_option(FLAG_WEBSOCKET "Enable web 3D visualization" ON "FLAG_VISU" OFF)
+
 endfunction()
 
 ## -- Static, dynamic libraries -- ##
@@ -106,12 +109,14 @@ function(flags_check)
         set( FLAG_VISU OFF )
         set( FLAG_JAVA OFF )
         set( FLAG_OPEN_GL OFF )
+        set( FLAG_WEBSOCKET OFF )
     endif( )
 
     # 3D visu
     if (NOT FLAG_VISU)
         set( FLAG_JAVA OFF )
         set( FLAG_OPEN_GL OFF )
+        set( FLAG_WEBSOCKET OFF )
     endif( )
 
     # OpenGL only if no Java
@@ -165,6 +170,7 @@ function(flags_check)
     set( FLAG_VISU              ${FLAG_VISU}              PARENT_SCOPE )
     set( FLAG_JAVA              ${FLAG_JAVA}              PARENT_SCOPE )
     set( FLAG_OPEN_GL           ${FLAG_OPEN_GL}           PARENT_SCOPE )
+    set( FLAG_WEBSOCKET         ${FLAG_WEBSOCKET}           PARENT_SCOPE )
     set( FLAG_SHARED_LIB        ${FLAG_SHARED_LIB}        PARENT_SCOPE )
     set( FLAG_SEPARATE_BUILD    ${FLAG_SEPARATE_BUILD}    PARENT_SCOPE )
     set( FLAG_SEPARATE_SYMBOLIC ${FLAG_SEPARATE_SYMBOLIC} PARENT_SCOPE )
@@ -195,6 +201,7 @@ function(flags_clean)
         unset(FLAG_VISU CACHE)
         unset(FLAG_JAVA CACHE)
         unset(FLAG_OPEN_GL CACHE)
+        unset(FLAG_WEBSOCKET CACHE)
         unset(JNI_INCLUDE_JNI CACHE)
         unset(JNI_INCLUDE_JNI_MD CACHE)
         unset(SDL2_LIBRARIES_SDL2 CACHE)
@@ -267,5 +274,9 @@ function(definitions)
         set(LIB_MBSYSC_DEFINITIONS ${LIB_MBSYSC_DEFINITIONS} -DFRAME_CAPTURE)
     endif( )
 
+    if(FLAG_WEBSOCKET)
+        add_definitions( -DWEBSOCKET )
+    endif( )
+
     set(LIB_MBSYSC_DEFINITIONS ${LIB_MBSYSC_DEFINITIONS} PARENT_SCOPE)
 endfunction()

MBsysC/mbs_common/mbs_realtime/CMakeLists.txt

@@ -133,6 +133,14 @@ if (FLAG_REAL_TIME AND FLAG_OPEN_GL)
     include_directories ("${PROJECT_BINARY_DIR}/conf")
 endif ()
 
+# Websocket files
+if (FLAG_WEBSOCKET)
+    include_directories("./websocket")
+    increment_src( ${PROJECT_SOURCE_DIR}/websocket )
+    add_definitions( -DWEBSOCKET )
+    include_directories ("${PROJECT_BINARY_DIR}/conf")
+endif ()
+
 # list include directories (to find headers)
 init_include()
 set(INCLUDE_DIR ${INCLUDE_DIR} ${PROJECT_SOURCE_DIR}/realtime PARENT_SCOPE)
@@ -178,8 +186,8 @@ set(LIB_MBSYSC_REALTIME_PATH ${CMAKE_CURRENT_BINARY_DIR} PARENT_SCOPE)
 
 # include directories
 include_directories("./")
-include_directories("./sdl" "./sdl/auto_plot")
-include_directories("./realtime")
+include_directories("./sdl")
+include_directories("./realtime" "./realtime/auto_plot")
 include_directories("../mbs_struct")
 include_directories("../mbs_utilities")
 include_directories("../mbs_numerics")

MBsysC/mbs_common/mbs_realtime/sdl/auto_plot/auto_plot.c → MBsysC/mbs_common/mbs_realtime/realtime/auto_plot/auto_plot.c

 
-#ifdef SDL
 
 #include "auto_plot.h"
 #include "useful_functions.h"
@@ -106,4 +105,4 @@ int index_plot_string(AutoPlot *auto_plot, const char* label)
     return -1;
 }
 
-#endif
+

MBsysC/mbs_common/mbs_realtime/sdl/auto_plot/auto_plot.h → MBsysC/mbs_common/mbs_realtime/realtime/auto_plot/auto_plot.h

@@ -4,7 +4,6 @@
  * \brief functions used to automatically update the user curves plotted
  */
 
-#ifdef SDL
 
 #ifndef _AUTO_PLOT_H_
 #define _AUTO_PLOT_H_
@@ -30,5 +29,5 @@ void free_auto_plot(AutoPlot *auto_plot);
 void update_auto_plot(AutoPlot *auto_plot, double value, const char* label);
 int index_plot_string(AutoPlot *auto_plot, const char* label);
 
-#endif
+
 #endif

MBsysC/mbs_common/mbs_realtime/sdl/auto_plot/set_plot.c → MBsysC/mbs_common/mbs_realtime/realtime/auto_plot/set_plot.c

@@ -3,10 +3,6 @@
 #endif // !HAVE_M_PI
 
 #include "set_plot.h"
-
-#ifdef SDL
-
-
 #include "auto_plot.h"
 
 static AutoPlot *auto_plot; // static AutoPlot structure with all the current user plots information
@@ -21,10 +17,13 @@ void reset_flag_plot()
     flag_plot = 0;
 }
 
+
 /*! \brief initialize the static auto_plot structure
  *
  * \param[in,out] screen_sdl SDL gestion main structure
  */
+
+#ifdef SDL
 void init_set_plot(Screen_sdl *screen_sdl)
 {
     flag_plot = 1;
@@ -33,6 +32,17 @@ void init_set_plot(Screen_sdl *screen_sdl)
 
     screen_sdl->auto_plot = auto_plot;
 }
+#else
+void init_set_plot(Realtime_extern *realtime_ext, int max_nb_curves)
+{
+    flag_plot = 1;
+
+    auto_plot = init_auto_plot(max_nb_curves);
+
+    realtime_ext->auto_plot = auto_plot;
+
+}
+#endif
 
 /*! \brief release memory of auto_plot
  */
@@ -56,5 +66,3 @@ void set_plot(double value, char* label)
         update_auto_plot(auto_plot, value, label);
     }
 }
-
-#endif

MBsysC/mbs_common/mbs_realtime/sdl/auto_plot/set_plot.h → MBsysC/mbs_common/mbs_realtime/realtime/auto_plot/set_plot.h

@@ -11,9 +11,12 @@
 #include "plot_sdl.h"
 
 #ifdef SDL   
-    void reset_flag_plot();
     void init_set_plot(Screen_sdl *screen_sdl);
-    void free_set_plot();
+#else
+    void init_set_plot(Realtime_extern *realtime_ext, int max_nb_curves);
 #endif
 
+    void reset_flag_plot();
+    void free_set_plot();
+
 #endif

MBsysC/mbs_common/mbs_realtime/realtime/realtime.c

@@ -29,6 +29,11 @@
 #include "open_gl_c_int.h"
 #endif
 
+#ifdef WEBSOCKET
+#include "visu_websocket.h"
+#include "auto_plot/set_plot.h"
+#endif
+
 // -- Macros -- //
 
 #define USEC_TO_SEC 1.0e-6         //!< factor from us to s
@@ -127,6 +132,13 @@ void mbs_realtime_update(Simu_realtime *realtime, double tsim)
         update_past_visu(realtime, tsim);
     }
 #endif
+
+#ifdef WEBSOCKET
+    if (realtime->flag_plot)
+    {
+        update_plot_vectors(realtime, tsim);
+    }
+#endif
 }
 
 /*! \brief one loop iteration of the real-time process
@@ -223,6 +235,8 @@ int mbs_realtime_loop(Simu_realtime *realtime, double tsim)
                         mbs_msg("\t >Real-time> Error: update open GL ! \n");
                         return err;
                     }
+#elif (defined WEBSOCKET)
+                    update_websocket(realtime);
 #endif
                 }
             }
@@ -657,6 +671,11 @@ Simu_realtime* init_simu_realtime(MbsData* mbs_data, Realtime_option *options, i
             return NULL;
         }
     }
+#elif WEBSOCKET
+    {
+        init_set_plot(realtime->ext, 10);
+        // printf("\n Plot activated to websocket !\n");
+    }
 #else
     if (options->flag_plot)
     {

MBsysC/mbs_common/mbs_realtime/realtime/realtime.h

@@ -12,6 +12,7 @@
 #include "realtime_ext.h"
 #include "color_sdl.h"
 #include "mbs_data.h"
+#include "auto_plot/auto_plot.h"
 
 #include "mbs_project_fct_ptr.h"
 
@@ -33,6 +34,8 @@ typedef struct Realtime_extern
     Realtime_visu *visu; //!< Java structure
     #endif
 
+    AutoPlot *auto_plot; ///< automatic user plot update structure
+
     MbsData* mbs_data; //!< Robotran main structure
 
 } Realtime_extern;
@@ -77,6 +80,7 @@ struct Realtime_option
     int nb_models;  //!< number of models to load
     int *nb_q;       //!< number of joints in the .mbs used for visualization (for each model)
     char **mbs_file; //!< path and file name fot the .mbs file used for Java visualization (for each model)
+    char *project_path; //!< path for the project
 
     int start_viewpoint; //!< initial visu viewpoint ID
 

MBsysC/mbs_common/mbs_realtime/realtime/realtime_ext.c

@@ -27,6 +27,10 @@
 #include "open_gl_c_int.h"
 #endif
 
+#ifdef WEBSOCKET
+#include "visu_websocket.h"
+#endif
+
 #ifdef SDL
 
 /*! \brief initialize SDL real-time variables
@@ -211,6 +215,8 @@ Realtime_visu* init_realtime_visu(void *realtime_options, MbsData* mbs_data, int
         mbs_msg("\t >Real-time> Error [%d] :in init_open gl visu  ! \n", *err);
         return NULL;
     }
+#elif (defined WEBSOCKET)
+    init_websocket(visu, mbs_data, nb_models, nb_q, visu->cur_q, options->mbs_file, options->project_path, options->start_viewpoint);
 #endif
     *err = 0;
     return visu;
@@ -259,8 +265,9 @@ int free_realtime_visu(Realtime_visu *visu)
         mbs_msg("\t >Real-time> Error:  free open GL ! \n");
         return err;
     }
+#elif (defined WEBSOCKET)
+    free_websocket(visu->visu_class);
 #endif
-
     free(visu);
     return 0;
 }

MBsysC/mbs_common/mbs_realtime/user_realtime.h

@@ -25,6 +25,18 @@ extern "C" {
 #endif
 #endif
 
+#ifdef WEBSOCKET
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+    void user_realtime_plot(MbsData* mbs_data);
+    void user_keyboard(MbsData* mbs_data, Simu_realtime *realtime, int keyevent, int state);
+#ifdef __cplusplus
+}
+#endif
+#endif
+
 #endif
 
 #ifdef __cplusplus

MBsysC/mbs_common/mbs_realtime/websocket/json.h

+/*
+  Copyright (C) 2011 Joseph A. Adams (joeyadams3.14159@gmail.com)
+  All rights reserved.
+
+  Permission is hereby granted, free of charge, to any person obtaining a copy
+  of this software and associated documentation files (the "Software"), to deal
+  in the Software without restriction, including without limitation the rights
+  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+  copies of the Software, and to permit persons to whom the Software is
+  furnished to do so, subject to the following conditions:
+
+  The above copyright notice and this permission notice shall be included in
+  all copies or substantial portions of the Software.
+
+  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+  THE SOFTWARE.
+*/
+
+#ifndef CCAN_JSON_H
+#define CCAN_JSON_H
+
+#include <stdbool.h>
+#include <stddef.h>
+
+typedef enum {
+	JSON_NULL,
+	JSON_BOOL,
+	JSON_STRING,
+	JSON_NUMBER,
+	JSON_ARRAY,
+	JSON_OBJECT,
+} JsonTag;
+
+typedef struct JsonNode JsonNode;
+
+struct JsonNode
+{
+	/* only if parent is an object or array (NULL otherwise) */
+	JsonNode *parent;
+	JsonNode *prev, *next;
+	
+	/* only if parent is an object (NULL otherwise) */
+	char *key; /* Must be valid UTF-8. */
+	
+	JsonTag tag;
+	union {
+		/* JSON_BOOL */
+		bool bool_;
+		
+		/* JSON_STRING */
+		char *string_; /* Must be valid UTF-8. */
+		
+		/* JSON_NUMBER */
+		double number_;
+		
+		/* JSON_ARRAY */
+		/* JSON_OBJECT */
+		struct {
+			JsonNode *head, *tail;
+		} children;
+	};
+};
+
+/*** Encoding, decoding, and validation ***/
+
+JsonNode   *json_decode         (const char *json);
+char       *json_encode         (const JsonNode *node);
+char       *json_encode_string  (const char *str);
+char       *json_stringify      (const JsonNode *node, const char *space);
+void        json_delete         (JsonNode *node);
+
+bool        json_validate       (const char *json);
+
+/*** Lookup and traversal ***/
+
+JsonNode   *json_find_element   (JsonNode *array, int index);
+JsonNode   *json_find_member    (JsonNode *object, const char *key);
+
+JsonNode   *json_first_child    (const JsonNode *node);
+
+#define json_foreach(i, object_or_array)            \
+	for ((i) = json_first_child(object_or_array);   \
+		 (i) != NULL;                               \
+		 (i) = (i)->next)
+
+/*** Construction and manipulation ***/
+
+JsonNode *json_mknull(void);
+JsonNode *json_mkbool(bool b);
+JsonNode *json_mkstring(const char *s);
+JsonNode *json_mknumber(double n);
+JsonNode *json_mkarray(void);
+JsonNode *json_mkobject(void);
+
+void json_append_element(JsonNode *array, JsonNode *element);
+void json_prepend_element(JsonNode *array, JsonNode *element);
+void json_append_member(JsonNode *object, const char *key, JsonNode *value);
+void json_prepend_member(JsonNode *object, const char *key, JsonNode *value);
+
+void json_remove_from_parent(JsonNode *node);
+
+/*** Debugging ***/
+
+/*
+ * Look for structure and encoding problems in a JsonNode or its descendents.
+ *
+ * If a problem is detected, return false, writing a description of the problem
+ * to errmsg (unless errmsg is NULL).
+ */
+bool json_check(const JsonNode *node, char errmsg[256]);
+
+#endif

MBsysC/mbs_common/mbs_realtime/websocket/visu_websocket.cc

+#ifdef WEBSOCKET
+
+#include <stdio.h> 
+#include <sys/socket.h> 
+#include <arpa/inet.h> 
+#include <unistd.h> 
+#include <string.h> 
+
+#include <iostream>
+#include <string>
+#include <sstream>
+#include <chrono>
+#include <thread>
+#include <sys/time.h>
+#include <ctime>
+
+#include "visu_websocket.h"
+#include "json.h"
+#include <fcntl.h>
+#include <poll.h>
+
+
+int sock = 0;
+char buffer[1024] = {0};
+bool is_connected = false;
+struct pollfd pfds[1]; // More if you want to monitor more
+bool pause_sim = false;
+struct sockaddr_in serv_addr; 
+
+#define PORT 65432 
+#define START_VIEWPOINT 0 ///< default initial viewpoint
+#define FRAMERATE 18.0
+
+using std::chrono::duration_cast;
+using std::chrono::milliseconds;
+using std::chrono::system_clock;
+using std::chrono::high_resolution_clock;
+
+/*! \brief input of the 3D window
+ */
+typedef struct VisuIn
+{
+    int nb_models; ///< number of models
+
+    int start_viewpoint; ///< initial viewpoint
+
+    int *nb_q; ///< number of joints for each model
+
+    double **q_vec; ///< initial joint positions
+
+    char **mbs_file; ///< mbs files to load
+
+    char *project_path;
+    
+    Realtime_visu *visu; ///< pointer to the "visu" structure
+
+} VisuIn;
+
+VisuIn *visu_in;
+
+
+
+/*! \brief initialize the communication with the webserver
+ */
+
+void init_websocket(Realtime_visu *visu, MbsData *mbs_data, int nb_models, int *nb_q, double **q_vec, char **mbs_file, char *project_path, int start_viewpoint){
+	
+    
+	// safety
+    if (nb_models < 1)
+    {
+        std::cout << "Nb_models for anim should be positive (not " << nb_models << ") !" << std::endl;
+        exit(EXIT_FAILURE);
+    }
+	
+	visu_in = new VisuIn;
+
+    visu_in->nb_models       = nb_models;
+    visu_in->start_viewpoint = start_viewpoint;
+    visu_in->nb_q            = nb_q;
+    visu_in->q_vec           = q_vec;
+    visu_in->mbs_file        = mbs_file;
+    visu_in->visu            = visu;
+    visu_in->project_path    = project_path;
+
+
+    // Create a socket connection (UDP) to the webserver
+    if ((sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) 
+    { 
+        printf("Socket creation error \n"); 
+        exit(EXIT_FAILURE);
+    } 
+
+    // Set port and IP:
+    serv_addr.sin_family = AF_INET;
+    serv_addr.sin_port = htons(PORT);
+    serv_addr.sin_addr.s_addr = inet_addr("127.0.0.1");
+
+    // Setup polling
+    pfds[0].fd = sock;         
+    pfds[0].events = POLLIN; // Tell me when ready to read 
+    
+    is_connected = true;
+
+    // Updade mbs data on the server
+    send_mbs_data();
+}
+
+/*! \brief update mbs data on the server
+ */
+void send_mbs_data(){
+
+    JsonNode *node;
+    char *msg;
+
+    // Create a JSON message
+    node = json_mkobject();
+
+    // Add mbs name and loading path
+    json_append_member(node, "mbs_name", json_mkstring(visu_in->mbs_file[0]));
+    json_append_member(node, "mbs_path", json_mkstring(visu_in->project_path));
+    
+    // Encode and send message to the server
+    msg = json_encode(node);
+    sendto(sock , msg, strlen(msg) , MSG_CONFIRM, (const struct sockaddr *) &serv_addr,  sizeof(serv_addr));
+}
+
+/*! \brief update the joint values for visualization
+ */
+void joints_update(Simu_realtime *realtime, int nb_models, int *nb_q, double **q_vec){
+	// variable declaration
+    int flag_viewpoint;
+    JsonNode *node, *array;
+    char *msg;
+
+	// Get all models
+    for(int i=0; i<nb_models; i++)
+    {
+    	// Create a JSON response with the joints updated values
+        node = json_mkobject();
+        array = json_mkarray();
+        for(int j=0; j<nb_q[i]; ++j){
+            json_append_element(array, json_mknumber(q_vec[i][j]));
+        }
+        json_append_member(node, "q_anim", array);
+        json_append_member(node, "speed", json_mknumber(realtime->real_simu_speed_factor));
+        
+        // Encode and send the message to the server
+        msg = json_encode(node);
+        sendto(sock , msg, strlen(msg) , MSG_CONFIRM, (const struct sockaddr *) &serv_addr,  sizeof(serv_addr) ); 
+    }
+
+}
+
+/*! \brief process messages from the realtime server
+ */
+void process_msg(Simu_realtime *realtime){
+    // variable declaration
+    int bytes_read;
+    socklen_t server_struct_length = sizeof(serv_addr);
+    char buf[4096] = {""};
+    int key, state, speed;
+
+    Realtime_visu *visu;
+    MbsData* mbs_data;
+
+    // get fields
+    visu     = realtime->ext->visu;
+    mbs_data = realtime->ext->mbs_data;
+
+    // Check if data is available
+    if(poll(pfds, 1, 0)>0){