Initial commit.

.gitignore

@@ -0,0 +1,10 @@
+lib
+build
+bin
+.project
+.cproject
+.autotools
+.settings
+.externalToolBuilders
+*.svg
+*.gv

CMakeLists.txt

@@ -0,0 +1,67 @@
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+PROJECT(ubx_rsg_fmpc)
+
+set(CMAKE_CXX_FLAGS "-Wall -Werror -fvisibility=hidden")
+set(CMAKE_CXX_COMPILER clang++ )
+set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") ## temporary resources, for UBX
+
+# Global
+set(INSTALL_LIB_TYPES_DIR lib/ubx/types CACHE PATH "Installation directory for types libraries")
+set(INSTALL_LIB_BLOCKS_DIR lib/ubx/blocks CACHE PATH "Installation directory for blocks libraries")
+set(INSTALL_BIN_APPS_DIR bin CACHE PATH "Installation directory for bin")
+set(INSTALL_INCLUDE_DIR include/ubx CACHE PATH "Installation directory for header files (types)")
+set(INSTALL_CMAKE_DIR  share/ubx/cmake CACHE PATH  "Installation directory for CMake files") # default ${DEF_INSTALL_CMAKE_DIR}
+
+# Make relative paths absolute
+foreach(p LIB_TYPES LIB_BLOCKS BIN INCLUDE CMAKE)
+  set(var INSTALL_${p}_DIR)
+  if(NOT IS_ABSOLUTE "${${var}}")
+    set(${var} "${CMAKE_INSTALL_PREFIX}/${${var}}")
+  endif()
+endforeach()
+
+##
+# Add uninstall target.
+##
+configure_file(
+    "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in"
+    "${CMAKE_CURRENT_BINARY_DIR}/cmake/cmake_uninstall.cmake"
+    IMMEDIATE @ONLY)
+
+add_custom_target(uninstall
+    "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake/cmake_uninstall.cmake"
+)
+
+##
+# Generate config package
+##
+set(CONF_INCLUDE_DIRS "${INSTALL_INCLUDE_DIR}")
+set(CONF_CMAKE_DIR "${INSTALL_CMAKE_DIR}")
+configure_file(
+    "${CMAKE_CURRENT_SOURCE_DIR}/cmake/ubx-rsg-fmpc-config.cmake.in"
+    "${CMAKE_CURRENT_BINARY_DIR}/cmake/ubx-rsg-fmpc-config.cmake" @ONLY
+)
+
+set (UBX_ROOT $ENV{UBX_ROOT} )
+if (NOT UBX_ROOT)
+ message( FATAL_ERROR "UBX_ROOT is not defined. CMake install willl abort." )
+endif()
+
+find_package(Ubx REQUIRED)
+
+include_directories(
+  ${UBX_INCLUDE_DIR}
+)
+
+# Compile library fmpcwmlib
+add_library(fmpcwmlib SHARED ..//ubx_rsg_fmpc/src/fmpc_world_model.cpp
+..//ubx_rsg_fmpc/modules/fmpcwmlib_module.c
+)
+set_target_properties(fmpcwmlib PROPERTIES PREFIX "")
+target_link_libraries(fmpcwmlib ${UBX_LIBRARIES})
+
+# Install fmpcwmlib
+install(TARGETS fmpcwmlib DESTINATION ${INSTALL_LIB_BLOCKS_DIR} EXPORT fmpcwmlib-block)
+set_property(TARGET fmpcwmlib PROPERTY INSTALL_RPATH_USE_LINK_PATH TRUE)
+install(EXPORT fmpcwmlib-block DESTINATION ${INSTALL_CMAKE_DIR})
+

cmake/FindUbx.cmake

@@ -0,0 +1,62 @@
+# - Try to find UBX
+# Once done this will define
+#
+#  UBX_FOUND - if UBX was found
+#  UBX_INCLUDE_DIR - the UBX include directory (-I)
+#  UBX_LINK_DIRECTORIES - the UBX linker directories (-L)
+#  UBX_LIBRARIES - UBX libraries
+#
+# You can set an environment variable "UBX_ROOT" to help CMake to find the UBX library,
+# in case it is not installed in one of the standard paths.
+#
+
+FIND_PATH(UBX_INCLUDE_DIR NAMES ubx.h
+  PATHS
+  $ENV{UBX_ROOT}/src
+  ENV CPATH
+  /usr/include/
+  /usr/local/include/
+  NO_DEFAULT_PATH
+)
+
+FIND_LIBRARY(UBX_LIBRARY NAMES "ubx" 
+  PATHS
+  $ENV{UBX_ROOT}/src
+  $ENV{UBX_ROOT}/lib 
+  ENV LD_LIBRARY_PATH
+  ENV LIBRARY_PATH
+  /usr/lib
+  /usr/local/lib
+  NO_DEFAULT_PATH
+)
+
+IF(UBX_LIBRARY)
+  GET_FILENAME_COMPONENT(UBX_LINK_DIRECTORIES ${UBX_LIBRARY} PATH CACHE)
+ENDIF(UBX_LIBRARY)
+
+
+SET(UBX_LIBRARIES_TMP
+)
+
+
+IF(UBX_LIBRARY) 
+  SET(UBX_LIBRARIES
+    ${UBX_LIBRARY}
+    CACHE STRING "Microblx library"
+  ) 
+ENDIF(UBX_LIBRARY)
+
+
+
+
+
+include(FindPackageHandleStandardArgs)
+# handle the QUIETLY and REQUIRED arguments and set UBX_FOUND to TRUE
+# if all listed variables are TRUE
+find_package_handle_standard_args(UBX  DEFAULT_MSG
+                                  UBX_INCLUDE_DIR UBX_LINK_DIRECTORIES UBX_LIBRARIES)
+
+# show the UBX_INCLUDE_DIR and UBX_LIBRARIES variables only in the advanced view
+IF(UBX_FOUND)
+  MARK_AS_ADVANCED(UBX_INCLUDE_DIR UBX_LINK_DIRECTORIES UBX_LIBRARIES)
+ENDIF(UBX_FOUND)

cmake/cmake_uninstall.cmake.in

@@ -0,0 +1,22 @@
+if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+  message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+
+message("destination $ENV{DESTDIR}")
+file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
+string(REGEX REPLACE "\n" ";" files "${files}")
+foreach(file ${files})
+  message(STATUS "Uninstalling $ENV{DESTDIR}${file}")
+  if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}")
+    exec_program(
+      "@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\""
+      OUTPUT_VARIABLE rm_out
+      RETURN_VALUE rm_retval
+      )
+    if(NOT "${rm_retval}" STREQUAL 0)
+      message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}")
+    endif(NOT "${rm_retval}" STREQUAL 0)
+  else(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}")
+    message(STATUS "File $ENV{DESTDIR}${file} does not exist.")
+  endif(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}")
+endforeach(file)

cmake/ubx-rsg-fmpc-config.cmake.in

@@ -0,0 +1,18 @@
+# - Config file for the ubx_rsg_fmpc package
+# It defines the following variables
+#  UBX_RSG_FMPC_TYPES_INCLUDE_DIRS - include directories for ubx_rsg_fmpc_types
+#  UBX_RSG_FMPC_TYPES_LIBRARIES    - libraries to link against
+
+# Compute paths
+#get_filename_component(UBX_RSG_FMPC_TYPES_CMAKE_DIR "${UBX_RSG_FMPC_TYPES_CMAKE_DIR}" PATH)
+set(UBX_RSG_FMPC_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
+set(UBX_RSG_FMPC_TYPES_CMAKE_DIR "@CONF_CMAKE_DIR@")
+
+
+# Our library dependencies (contains definitions for IMPORTED targets)
+if(NOT TARGET ubx_rsg_fmpc AND NOT UBX_RSG_FMPC_TYPES_BINARY_DIR)
+  include("${UBX_RSG_FMPC_TYPES_CMAKE_DIR}/ubx-rsg-fmpc-types-targets.cmake")
+endif()
+
+# These are IMPORTED targets created by ubx-rsg-fmpc-types-targets.cmake
+set(UBX_RSG_FMPC_TYPES_LIBRARIES ubx_rsg_fmpc)

models/fmpc_world_model.blx

@@ -0,0 +1,26 @@
+return block
+{
+      name="fmpc_world_model",
+      meta_data="Adapter block that connects a Robot Scene Graph (RSG) with a Fast Model Predictive Controller (FMPC).",
+      port_cache=true,
+
+      --types = {
+
+      --},
+
+     configurations = {
+	{ name="wm_handle", type_name = "struct rsg_wm_handle", doc="Handle to the world wodel instance. This parameter is mandatory."},
+	{ name="use_mockup", type_name = "bool", doc="If true an internal mockup is used rather than a real system. To be used for testing."},
+      },
+
+      ports = {
+	 { name="fmpc_virtual_fence", out_type_name="float", out_data_len=4, doc="AABB bounding box to constrain the FMPC. [Ax, Ay, Bx, By]" },
+	 { name="fmpc_obstacle", out_type_name="float", out_data_len=3, doc="Obstacle represented as a circle with the following parameters: [x,y,r]" },
+	 { name="fmpc_goal_pose", out_type_name="float", out_data_len=2, doc="A new goal setpoint represented as a point: [x,y]. Theta (rotation) is not supported." },
+	 { name="fmpc_robot_pose", in_type_name="float", in_data_len=2, doc="The current pose of the robot represented as a point: [x,y]. Theta (rotation) is not supported." },
+      },
+
+      operations = { start=true, stop=true, step=true },
+
+      cpp=true
+}

models/fmpc_world_model_package.pkg

@@ -0,0 +1,21 @@
+return pkg
+{
+  name="ubx_rsg_fmpc",
+  path="../",
+
+  --dependencies = {
+
+  --},
+
+  --types = {
+
+  --},
+
+  blocks = {
+    { name="fmpc_world_model", file="examples/fmpc_world_model.blx", src_dir="src" },
+  },
+
+  modules = { 
+    { name="fmpcwmlib", blocks={"fmpc_world_model"} },
+  },  
+}

modules/fmpcwmlib_module.c

@@ -0,0 +1,22 @@
+/*
+ * module function declaration for library fmpcwmlib header (autogenerated)
+ */
+
+#include "fmpcwmlib_module.h"
+
+int fmpcwmlib_mod_init(ubx_node_info_t* ni)
+{
+  int ret;
+  ret=fmpc_world_model_mod_init(ni);
+  if(ret!=0)
+    goto out;
+
+out:
+   return ret;
+}
+
+void fmpcwmlib_mod_cleanup(ubx_node_info_t* ni)
+{
+  fmpc_world_model_mod_cleanup(ni);
+}
+

modules/fmpcwmlib_module.h

@@ -0,0 +1,18 @@
+/*
+ * module function declaration for library fmpcwmlib header (autogenerated)
+ */
+
+#include <ubx.h>
+
+// Initialization and cleanup function declaration
+int fmpcwmlib_mod_init(ubx_node_info_t* ni);
+void fmpcwmlib_mod_cleanup(ubx_node_info_t* ni);
+
+
+/* declare module init and cleanup functions, so that the ubx core can
+ * find these when the module is loaded/unloaded.
+ * Please edit your license in macro LICENSE_SPDX */
+UBX_MODULE_INIT(fmpcwmlib_mod_init)
+UBX_MODULE_CLEANUP(fmpcwmlib_mod_cleanup)
+UBX_MODULE_LICENSE_SPDX(GPL-2.0+)
+

src/fmpc_world_model.cpp

@@ -0,0 +1,62 @@
+#include "fmpc_world_model.hpp"
+
+/* define a structure for holding the block local state. By assigning an
+ * instance of this struct to the block private_data pointer (see init), this
+ * information becomes accessible within the hook functions.
+ */
+struct fmpc_world_model_info
+{
+        /* add custom block local data here */
+
+        /* this is to have fast access to ports for reading and writing, without
+         * needing a hash table lookup */
+        struct fmpc_world_model_port_cache ports;
+};
+
+/* init */
+int fmpc_world_model_init(ubx_block_t *b)
+{
+        int ret = -1;
+        struct fmpc_world_model_info *inf;
+
+        /* allocate memory for the block local state */
+        if ((inf = (struct fmpc_world_model_info*)calloc(1, sizeof(struct fmpc_world_model_info)))==NULL) {
+                ERR("fmpc_world_model: failed to alloc memory");
+                ret=EOUTOFMEM;
+                goto out;
+        }
+        b->private_data=inf;
+        update_port_cache(b, &inf->ports);
+        ret=0;
+out:
+        return ret;
+}
+
+/* start */
+int fmpc_world_model_start(ubx_block_t *b)
+{
+        /* struct fmpc_world_model_info *inf = (struct fmpc_world_model_info*) b->private_data; */
+        int ret = 0;
+        return ret;
+}
+
+/* stop */
+void fmpc_world_model_stop(ubx_block_t *b)
+{
+        /* struct fmpc_world_model_info *inf = (struct fmpc_world_model_info*) b->private_data; */
+}
+
+/* cleanup */
+void fmpc_world_model_cleanup(ubx_block_t *b)
+{
+        free(b->private_data);
+}
+
+/* step */
+void fmpc_world_model_step(ubx_block_t *b)
+{
+        /*
+        struct fmpc_world_model_info *inf = (struct fmpc_world_model_info*) b->private_data;
+        */
+}
+