From af3428f20856834258ed677b4393cbca8fc84d36 Mon Sep 17 00:00:00 2001 From: Sebastian Blumenthal Date: Tue, 4 Nov 2014 14:01:42 +0100 Subject: [PATCH] Initial commit. --- .gitignore | 10 +++ CMakeLists.txt | 67 ++++++++++++++++++ cmake/FindUbx.cmake | 62 +++++++++++++++++ cmake/cmake_uninstall.cmake.in | 22 ++++++ cmake/ubx-rsg-fmpc-config.cmake.in | 18 +++++ models/fmpc_world_model.blx | 26 +++++++ models/fmpc_world_model_package.pkg | 21 ++++++ modules/fmpcwmlib_module.c | 22 ++++++ modules/fmpcwmlib_module.h | 18 +++++ src/fmpc_world_model.cpp | 62 +++++++++++++++++ src/fmpc_world_model.hpp | 104 ++++++++++++++++++++++++++++ src/fmpc_world_model.usc | 17 +++++ 12 files changed, 449 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 cmake/FindUbx.cmake create mode 100644 cmake/cmake_uninstall.cmake.in create mode 100644 cmake/ubx-rsg-fmpc-config.cmake.in create mode 100644 models/fmpc_world_model.blx create mode 100644 models/fmpc_world_model_package.pkg create mode 100644 modules/fmpcwmlib_module.c create mode 100644 modules/fmpcwmlib_module.h create mode 100644 src/fmpc_world_model.cpp create mode 100644 src/fmpc_world_model.hpp create mode 100644 src/fmpc_world_model.usc diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..23978d1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,10 @@ +lib +build +bin +.project +.cproject +.autotools +.settings +.externalToolBuilders +*.svg +*.gv diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..3f39527 --- /dev/null +++ b/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}) + diff --git a/cmake/FindUbx.cmake b/cmake/FindUbx.cmake new file mode 100644 index 0000000..48b1468 --- /dev/null +++ b/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) diff --git a/cmake/cmake_uninstall.cmake.in b/cmake/cmake_uninstall.cmake.in new file mode 100644 index 0000000..cef9dc5 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/cmake/ubx-rsg-fmpc-config.cmake.in b/cmake/ubx-rsg-fmpc-config.cmake.in new file mode 100644 index 0000000..bb70c25 --- /dev/null +++ b/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) diff --git a/models/fmpc_world_model.blx b/models/fmpc_world_model.blx new file mode 100644 index 0000000..5b5c5dd --- /dev/null +++ b/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 +} diff --git a/models/fmpc_world_model_package.pkg b/models/fmpc_world_model_package.pkg new file mode 100644 index 0000000..87d75b3 --- /dev/null +++ b/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"} }, + }, +} diff --git a/modules/fmpcwmlib_module.c b/modules/fmpcwmlib_module.c new file mode 100644 index 0000000..b827bc0 --- /dev/null +++ b/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); +} + diff --git a/modules/fmpcwmlib_module.h b/modules/fmpcwmlib_module.h new file mode 100644 index 0000000..744bbe9 --- /dev/null +++ b/modules/fmpcwmlib_module.h @@ -0,0 +1,18 @@ +/* + * module function declaration for library fmpcwmlib header (autogenerated) + */ + +#include + +// 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+) + diff --git a/src/fmpc_world_model.cpp b/src/fmpc_world_model.cpp new file mode 100644 index 0000000..e26021c --- /dev/null +++ b/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; + */ +} + diff --git a/src/fmpc_world_model.hpp b/src/fmpc_world_model.hpp new file mode 100644 index 0000000..956223e --- /dev/null +++ b/src/fmpc_world_model.hpp @@ -0,0 +1,104 @@ +/* + * fmpc_world_model microblx function block (autogenerated, don't edit) + */ + +#include + +/* includes types and type metadata */ + + +/* block meta information */ +char fmpc_world_model_meta[] = + " { doc=''," + " real-time=true," + "}"; + +/* declaration of block configuration */ +ubx_config_t fmpc_world_model_config[] = { + { .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." }, + { NULL }, +}; + +/* declaration port block ports */ +ubx_port_t fmpc_world_model_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." }, + { NULL }, +}; + +/* declare a struct port_cache */ +struct fmpc_world_model_port_cache { + ubx_port_t* fmpc_virtual_fence; + ubx_port_t* fmpc_obstacle; + ubx_port_t* fmpc_goal_pose; + ubx_port_t* fmpc_robot_pose; +}; + +/* declare a helper function to update the port cache this is necessary + * because the port ptrs can change if ports are dynamically added or + * removed. This function should hence be called after all + * initialization is done, i.e. typically in 'start' + */ +static void update_port_cache(ubx_block_t *b, struct fmpc_world_model_port_cache *pc) +{ + pc->fmpc_virtual_fence = ubx_port_get(b, "fmpc_virtual_fence"); + pc->fmpc_obstacle = ubx_port_get(b, "fmpc_obstacle"); + pc->fmpc_goal_pose = ubx_port_get(b, "fmpc_goal_pose"); + pc->fmpc_robot_pose = ubx_port_get(b, "fmpc_robot_pose"); +} + + +/* for each port type, declare convenience functions to read/write from ports */ +//def_write_arr_fun(write_fmpc_virtual_fence_4, float, 4) +//def_write_arr_fun(write_fmpc_obstacle_3, float, 3) +//def_write_arr_fun(write_fmpc_goal_pose_2, float, 2) +//def_read_arr_fun(read_fmpc_robot_pose_2, float, 2) + +/* block operation forward declarations */ +int fmpc_world_model_init(ubx_block_t *b); +int fmpc_world_model_start(ubx_block_t *b); +void fmpc_world_model_stop(ubx_block_t *b); +void fmpc_world_model_cleanup(ubx_block_t *b); +void fmpc_world_model_step(ubx_block_t *b); + + +/* put everything together */ +ubx_block_t fmpc_world_model_block = { + .name = "fmpc_world_model", + .type = BLOCK_TYPE_COMPUTATION, + .meta_data = fmpc_world_model_meta, + .configs = fmpc_world_model_config, + .ports = fmpc_world_model_ports, + + /* ops */ + .init = fmpc_world_model_init, + .start = fmpc_world_model_start, + .stop = fmpc_world_model_stop, + .cleanup = fmpc_world_model_cleanup, + .step = fmpc_world_model_step, +}; + + +/* fmpc_world_model module init and cleanup functions */ +int fmpc_world_model_mod_init(ubx_node_info_t* ni) +{ + DBG(" "); + int ret = -1; + + if(ubx_block_register(ni, &fmpc_world_model_block) != 0) + goto out; + + ret=0; +out: + return ret; +} + +void fmpc_world_model_mod_cleanup(ubx_node_info_t *ni) +{ + DBG(" "); + ubx_block_unregister(ni, "fmpc_world_model"); +} + diff --git a/src/fmpc_world_model.usc b/src/fmpc_world_model.usc new file mode 100644 index 0000000..c30469b --- /dev/null +++ b/src/fmpc_world_model.usc @@ -0,0 +1,17 @@ +-- a minimal blockdiagram to start the block + +return bd.system +{ + imports = { + "std_types/stdtypes/stdtypes.so", + "std_blocks/ptrig/ptrig.so", + "std_blocks/lfds_buffers/lfds_cyclic.so", + "std_blocks/logging/file_logger.so", + "blocks/fmpcwmlib.so", + }, + + blocks = { + { name="fmpc_world_model_1", type="fmpc_world_model" }, + }, +} +