Curly braces added

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "src/hal/components/cros"]
+	path = src/hal/components/cros
+	url = git@github.com:jjrbfi/cros.git

Readme.md

@@ -1,28 +1,48 @@
+<p align="center">
+  <a href="https://gitea.io/">
+     <img src="img/logo.png" alt="ROS + LinuxCNC" width="200"> 
 
-Hal_core is a lightweight hal environment.
-The installed size is approx 6.8Mib
+  </a>
+</p>
+<h1 align="center">ROS + LinuxCNC HAL</h1>
 
-A hal environment can be used as platform to run realtime applications like:
+Hal-core is a lightweight HAL environment from LinuxCNC.
+The installed size is approx 7Mib
 
-	motion controllers 
-	robots
-	cnc-machines 
-	parport, ethercat applications
-	research and development 
-	scientific projects
+### A hal environment can be used as platform to run realtime applications like:
 
-Packages needed for the installation (at the bottom):
+	Motion controllers 
+	Robots
+	CNC-machines 
+	Parport, EtherCAT applications
+	Research and development 
+	Scientific projects
 
-https://github.com/grotius-cnc/hal_core/tree/main/src#readme
+### Install hal-core:
 
-To install:
-
-	Install hal-core:
-	$ git clone https://github.com/grotius-cnc/hal_core.git /opt/hal-core
-	$ /opt/hal-core/./make
+	$ git clone --recursive https://github.com/jjrbfi/hal-core.git /opt/hal-core
+	$ cd /op/hal-core/
+	$ sudo ./make
+	$ sudo chown -R $USER:$USER /opt/hal-core/
 
-To run:
+### Before run:
+1. Modify **config/hal.yaml** file with your Hardware configuration and then run **hal_config.py** to create our .xml
+2. Run ROScore and keep it in the background with: ```roscore &```
+
 
-	$ /opt/hal-core/./runtest
+### To run:
 
-https://user-images.githubusercontent.com/44880102/129791198-ab705999-23ca-4004-a5f5-f0bd3357b47e.mp4
+	Setup HAL
+	$ ./runtest
+
+	Run ROS listener
+	$ halcmd -r
+
+
+### Requirements:
+```bash
+apt-get install -y build-essential 
+apt-get install -y libudev-dev
+apt-get install -y libboost-all-dev
+apt-get install -y libreadline-dev 
+```

make

@@ -5,12 +5,30 @@ chmod +x /opt/hal-core/runtest
 chmod +x /opt/hal-core/src/clean
 chmod +x /opt/hal-core/src/make
 chmod +x /opt/hal-core/src/configure
+chmod +x /opt/hal-core/scripts/halrun
+chmod +x /opt/hal-core/scripts/realtime
+
+# Compile cROS and copy shared library
+if [ -f /opt/hal-core/src/hal/components/cros/build/libcros.so ]; then
+	echo "Exist!"
+else
+  mkdir /opt/hal-core/src/hal/components/cros/build
+  cd /opt/hal-core/src/hal/components/cros/build && cmake ..
+  cd /opt/hal-core/src/hal/components/cros/build/ && make
+  ln -s /opt/hal-core/src/hal/components/cros/build/libcros.so /opt/hal-core/lib/
+  ln -s /opt/hal-core/src/hal/components/cros/include/ /opt/hal-core/src/cros
+fi
 
 # Compile hal-core
 cd /opt/hal-core/src/
 ./configure --disable-gtk --with-realtime=uspace
 ./make && sudo make setuid
 
+# Set user able to insert kernel modules
+chown 777 -R /opt/hal-core/bin/rtapi_app
+chown 777 -R /opt/hal-core/bin/module_helper
+chmod 777 /opt/hal-core/bin/rtapi_app
+chmod 777 /opt/hal-core/bin/module_helper 
 
 # Compile test component: 
 chmod +x /opt/hal-core/src/hal/components/test/make

runtest

@@ -1,10 +1,24 @@
 #!/usr/bin/bash
 
+# Startup hal-core
 cd /opt/hal-core/scripts/ && . ./rip-environment
 
-
 cd /opt/hal-core/bin
 
+halcmd stop
+halcmd loadrt threads name1=base-thread fp1=0 period1=1000000
+#name2=servo-thread period2=1
+
+# Unix command to load the ethercat .xml config
+/opt/hal-core/rtlib/./lcec_conf /opt/hal-core/rtlib/ethercat-conf.xml &
+halcmd loadrt lcec
+#halcmd loadrt test
+
+halcmd addf lcec.read-all base-thread
+halcmd addf lcec.write-all base-thread
+
+halcmd start
+
 halcmd show
 
 # To clean the hal environment :

src/hal/utils/halcmd_main.c

@@ -59,6 +59,19 @@
 #include <fnmatch.h>
 #include <search.h>
 
+// cROS
+#include "cros.h"
+#include <unistd.h>
+#include <errno.h>
+#include <signal.h>
+
+#define DIR_SEPARATOR_STR "/"
+#define ROS_MASTER_PORT 11311
+#define ROS_MASTER_ADDRESS "127.0.0.1"
+
+CrosNode *node; //! Pointer to object storing the ROS node. This object includes all the ROS node state variables
+static unsigned char exit_flag = 0; //! ROS node loop exit flag. When set to 1 the cRosNodeStart() function exits
+
 static int get_input(FILE *srcfile, char *buf, size_t bufsize);
 static void print_help_general(int showR);
 static int release_HAL_mutex(void);
@@ -71,10 +84,128 @@ static char *prompt_continue    = "halcmd+: ";
 
 #define MAX_EXTEND_LINES 20
 
+
+/***********************************************************************
+*                   cROS FUNCTION DEFINITIONS                         *
+************************************************************************/
+
+// This callback will be invoked when the subscriber receives a message
+static CallbackResponse callback_sub(cRosMessage *message, void* data_context)
+{
+    int retval;
+    char *tokens[MAX_TOK+1];
+
+    cRosMessageField *data_field = cRosMessageGetField(message, "data");
+    if(data_field != NULL)
+    {
+        ROS_INFO(node, "I heard: [%s]\n", data_field->data.as_string);
+        halcmd_startup(1);
+        // remove comments, do var substitution, and tokenise
+        retval = halcmd_preprocess_line(data_field->data.as_string, tokens);
+        // Run the command
+        retval = halcmd_parse_cmd(tokens);
+        return 0; // 0=success
+    }
+}
+
+struct sigaction old_int_signal_handler, old_term_signal_handler; //! Structures codifying the original handlers of SIGINT and SIGTERM signals (e.g. used when pressing Ctrl-C for the second time);
+
+// This callback function will be called when the main process receives a SIGINT or
+// SIGTERM signal.
+// Function set_signal_handler() should be called to set this function as the handler of
+// these signals
+static void exit_deamon_handler(int sig)
+{
+  printf("Signal %i received: exiting safely.\n", sig);
+  sigaction(SIGINT, &old_int_signal_handler, NULL);
+  sigaction(SIGTERM, &old_term_signal_handler, NULL);
+  exit_flag = 1; // Indicate the exit of cRosNodeStart loop (safe exit)
+}
+
+// Sets the signal handler functions of SIGINT and SIGTERM: exit_deamon_handler
+static int set_signal_handler(void)
+  {
+   int ret;
+   struct sigaction act;
+
+   memset (&act, '\0', sizeof(act));
+
+   act.sa_handler = exit_deamon_handler;
+   // If the signal handler is invoked while a system call or library function call is blocked,
+   // then the we want the call to be automatically restarted after the signal handler returns
+   // instead of making the call fail with the error EINTR.
+   act.sa_flags=SA_RESTART;
+   if(sigaction(SIGINT, &act, &old_int_signal_handler) == 0 && sigaction(SIGTERM, &act,  &old_term_signal_handler) == 0)
+      ret=0;
+   else
+     {
+      ret=errno;
+      printf("Error setting termination signal handler. errno=%d\n",ret);
+     }
+   return(ret);
+  }
+
+
 /***********************************************************************
 *                   LOCAL FUNCTION DEFINITIONS                         *
 ************************************************************************/
 
+int cros_main(){
+    //char path[4097]; // We need to tell our node where to find the .msg files that we'll be using
+    char *path="/opt/hal-core/src/hal/components/cros/samples/rosdb/";
+    const char *node_name;
+    int subidx; // Index (identifier) of the created subscriber
+    cRosErrCodePack err_cod;
+
+    node_name="/listener"; // Default node name if no command-line parameters are specified
+    //getcwd(path, sizeof(path));
+    //strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1);
+
+    printf("Using the following path for message definitions: %s\n", path);
+    // Create a new node and tell it to connect to roscore in the usual place
+    node = cRosNodeCreate(node_name, "127.0.0.1", ROS_MASTER_ADDRESS, ROS_MASTER_PORT, path);
+    if( node == NULL )
+    {
+        printf("cRosNodeCreate() failed; is this program already being run?");
+        return EXIT_FAILURE;
+    }
+
+    err_cod = cRosWaitPortOpen(ROS_MASTER_ADDRESS, ROS_MASTER_PORT, 0);
+    if(err_cod != CROS_SUCCESS_ERR_PACK)
+    {
+        cRosPrintErrCodePack(err_cod, "Port %s:%hu cannot be opened: ROS Master does not seems to be running", ROS_MASTER_ADDRESS, ROS_MASTER_PORT);
+        return EXIT_FAILURE;
+    }
+
+    // Create a subscriber to topic /chatter of type "std_msgs/String" and supply a callback for received messages (callback_sub)
+    err_cod = cRosApiRegisterSubscriber(node, "/chatter", "std_msgs/String", callback_sub, NULL, NULL, 0, &subidx);
+    if(err_cod != CROS_SUCCESS_ERR_PACK)
+    {
+        cRosPrintErrCodePack(err_cod, "cRosApiRegisterSubscriber() failed; did you run this program one directory above 'rosdb'?");
+        cRosNodeDestroy( node );
+        return EXIT_FAILURE;
+    }
+
+    ROS_INFO(node, "Node %s created with XMLRPC port: %i, TCPROS port: %i and RPCROS port: %i\n", node->name, node->xmlrpc_port, node->tcpros_port, node->rpcros_port);
+
+    // Function exit_deamon_handler() will be called when Ctrl-C is pressed or kill is executed
+    set_signal_handler();
+
+    // Run the main loop until exit_flag is 1
+    err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag );
+    if(err_cod != CROS_SUCCESS_ERR_PACK)
+        cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code");
+
+    // Free memory and unregister
+    err_cod=cRosNodeDestroy( node );
+    if(err_cod != CROS_SUCCESS_ERR_PACK)
+    {
+        cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unregistering from ROS master");
+        return EXIT_FAILURE;
+    }
+    return EXIT_SUCCESS;;
+}
+
 int main(int argc, char **argv)
 {
     int c, fd;
@@ -97,7 +228,7 @@ int main(int argc, char **argv)
     keep_going = 0;
     /* start parsing the command line, options first */
     while(1) {
-        c = getopt(argc, argv, "+RCfi:kqQsvVhe");
+        c = getopt(argc, argv, "+RCfri:kqQsvVhe");
         if(c == -1) break;
         switch(c) {
             case 'R':
@@ -147,6 +278,9 @@ int main(int argc, char **argv)
 	    case 'f':
                 filemode = 1;
 		break;
+        case 'r':
+                cros_main();
+                break;
 	    case 'C':
                 cl = getenv("COMP_LINE");
                 cw = getenv("COMP_POINT");
@@ -276,7 +410,6 @@ int main(int argc, char **argv)
             if (!extend_ct) { elineptr = (char*)raw_buf; }
             extend_ct = 0;
             if (prompt == prompt_continue) { prompt = prompt_interactive; }
-
 	    /* remove comments, do var substitution, and tokenise */
 	    retval = halcmd_preprocess_line(elineptr, tokens);
 	    if(echo_mode) {
@@ -390,6 +523,7 @@ static void print_help_general(int showR)
     printf("  -e             echo the commands from stdin to stderr\n");
     printf("  -f [filename]  Read commands from 'filename', not command\n");
     printf("                 line.  If no filename, read from stdin.\n");
+    printf("  -r             Start ROS listener (roscore have to be running)\n");
 #ifndef NO_INI
     printf("  -i filename    Open .ini file 'filename', allow commands\n");
     printf("                 to get their values from ini file.\n");
@@ -445,6 +579,7 @@ static int get_input(FILE *srcfile, char *buf, size_t bufsize) {
 }
 #endif
 
+
 void halcmd_output(const char *format, ...) {
     va_list ap;
     va_start(ap, format);

src/make

@@ -35,7 +35,7 @@ gcc -c -I. -Irtapi -Ihal -Os -fwrapv -g -Wall -DULAPI -std=gnu99 -fgnu89-inline
         -MP -MD -MF "objects/hal/utils/halcmd_commands.d" -MT "objects/hal/utils/halcmd_commands.o" \
         hal/utils/halcmd_commands.c -o objects/hal/utils/halcmd_commands.o
 # Compiling hal/utils/halcmd_main.c
-gcc -c -I. -Irtapi -Ihal -Os -fwrapv -g -Wall -DULAPI -std=gnu99 -fgnu89-inline -Werror=implicit-function-declaration -g -O2 \
+gcc -c -I. -Icros -Irtapi -Ihal -Os -fwrapv -g -Wall -DULAPI -std=gnu99 -fgnu89-inline -Werror=implicit-function-declaration -g -O2 \
         -MP -MD -MF "objects/hal/utils/halcmd_main.d" -MT "objects/hal/utils/halcmd_main.o" \
         hal/utils/halcmd_main.c -o objects/hal/utils/halcmd_main.o
 # Compiling hal/utils/halcmd_completion.c
@@ -54,7 +54,7 @@ gcc -c -I. -Irtapi -Ihal -Os -fwrapv -g -Wall -DULAPI -std=gnu99 -fgnu89-inline
 # Creating shared library libhalcore.so.0
 gcc -L/opt/hal-core/lib -Wl,-rpath,/opt/hal-core/lib -Wl,-soname,libhalcore.so.0 -shared -o ../lib/libhalcore.so.0 objects/hal/hal_lib.o objects/rtapi/uspace_ulapi.o -pthread -lrt 
 # Linking halcmd
-gcc -L/opt/hal-core/lib -Wl,-rpath,/opt/hal-core/lib -o ../bin/halcmd objects/hal/utils/halcmd.o objects/hal/utils/halcmd_commands.o objects/hal/utils/halcmd_main.o objects/hal/utils/halcmd_completion.o ../lib/libhalcore.so.0 -lreadline
+gcc -L/opt/hal-core/lib -Wl,-rpath,/opt/hal-core/lib -o ../bin/halcmd objects/hal/utils/halcmd.o objects/hal/utils/halcmd_commands.o objects/hal/utils/halcmd_main.o objects/hal/utils/halcmd_completion.o ../lib/libcros.so ../lib/libhalcore.so.0 -lreadline
 ln -sf libhalcore.so.0 ../lib/libhalcore.so
 
 # Compiling module_helper/module_helper.c