From f9bb6eb667ede233c88376277045bd1cacb3e78b Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Sat, 3 Feb 2024 18:03:00 +0900 Subject: [PATCH] [rcleus] initial commit --- jsk_roseus/AMENT_IGNORE | 0 rcleus/CMakeLists.txt | 45 +++++++++++ rcleus/include/rcleus/rcleus.h | 100 ++++++++++++++++++++++++ rcleus/package.xml | 24 ++++++ rcleus/src/rcleus.cpp | 138 +++++++++++++++++++++++++++++++++ roseus/AMENT_IGNORE | 0 roseus_mongo/AMENT_IGNORE | 0 roseus_msgs/AMENT_IGNORE | 0 roseus_smach/AMENT_IGNORE | 0 roseus_tutorials/AMENT_IGNORE | 0 10 files changed, 307 insertions(+) create mode 100644 jsk_roseus/AMENT_IGNORE create mode 100644 rcleus/CMakeLists.txt create mode 100644 rcleus/include/rcleus/rcleus.h create mode 100644 rcleus/package.xml create mode 100644 rcleus/src/rcleus.cpp create mode 100644 roseus/AMENT_IGNORE create mode 100644 roseus_mongo/AMENT_IGNORE create mode 100644 roseus_msgs/AMENT_IGNORE create mode 100644 roseus_smach/AMENT_IGNORE create mode 100644 roseus_tutorials/AMENT_IGNORE diff --git a/jsk_roseus/AMENT_IGNORE b/jsk_roseus/AMENT_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/rcleus/CMakeLists.txt b/rcleus/CMakeLists.txt new file mode 100644 index 000000000..9f2ddce5f --- /dev/null +++ b/rcleus/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.8) +project(rcleus) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# Check EusLisp exists +set(ENV_EUSDIR $ENV{EUSDIR}) +if(ENV_EUSDIR) + set(EUSDIR $ENV_EUSDIR) +else() + set(EUSDIR /usr/share/euslisp) # for debian/ubuntu +endif() +if(EXISTS ${EUSDIR}) + message(STATUS "Found EusLisp: ${EUSDIR}") +else() + message(FATAL_ERROR "EusLisp not found") +endif() + +# Set EusLisp include directory +set(euslisp_INCLUDE_DIRS ${EUSDIR}/include) + +include_directories(include) + +ament_auto_add_library(rcleus SHARED + src/rcleus.cpp) +target_compile_options(rcleus + PRIVATE -O2 -DNDEBUG -DLinux -D_REENTRANT -DTHREADED -DPTHREAD -DX11R6_1 -Dx86_64 -Wno-comment) +target_include_directories(rcleus + PUBLIC ${euslisp_INCLUDE_DIRS}) + +# Testing +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/rcleus/include/rcleus/rcleus.h b/rcleus/include/rcleus/rcleus.h new file mode 100644 index 000000000..db3928462 --- /dev/null +++ b/rcleus/include/rcleus/rcleus.h @@ -0,0 +1,100 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/* + * rcleus.h + * Author: Yoshiki Obinata + */ + +#ifndef RCLEUS_H_ +#define RCLEUS_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Define the EusLisp types not to conflict with the C++ types +#define class eus_class +#define throw eus_throw +#define export eus_export +#include +#undef class +#undef throw +#undef export + +extern "C" { + pointer ___rcleus(context *ctx, int n, pointer *argv, pointer env); + void register_rcleus() { + char modname[] = "___rcleus"; + return add_module_initializer(modname, (pointer(*)())___rcleus); + } +} + +struct RcleusStaticData { + std::shared_ptr node; + std::shared_ptr rate; + // map> mapAdvertised; ///< advertised + // topics map> mapSubscribed; ///< + // subscribed topics map> + // mapServiced; ///< subscribed topics + // map mapTimered; ///< subscribed timers + // map> + // mapHandle; ///< for grouping nodehandle +}; + +inline void rcleusSignalHandler(int sig) { + // memoize for euslisp handler... + context *ctx = euscontexts[thr_self()]; + ctx->intsig = sig; +} + +// C implementations of rcleus +pointer RCLEUS(context *ctx, int n, pointer *argv); + +#endif // RCLEUS_H_ diff --git a/rcleus/package.xml b/rcleus/package.xml new file mode 100644 index 000000000..c3968517f --- /dev/null +++ b/rcleus/package.xml @@ -0,0 +1,24 @@ + + + + rcleus + 0.0.0 + ROS2 Client Library for EusLisp + Yoshiki Obinata + Yoshiki Obinata + BSD + + ament_cmake + + ament_cmake_auto + + rclcpp + rlwrap + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rcleus/src/rcleus.cpp b/rcleus/src/rcleus.cpp new file mode 100644 index 000000000..76f990bd3 --- /dev/null +++ b/rcleus/src/rcleus.cpp @@ -0,0 +1,138 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/* + * rcleus.cpp + * Author: Yoshiki Obinata + */ + +#include "rcleus/rcleus.h" + +static RcleusStaticData s_staticData; +static bool s_bInstalled = false; // flag to check if the rclcpp is initialized + +pointer RCLEUS(context *ctx, int n, pointer *argv) { + char name[256] = ""; + uint32_t options = 0; + int cargc = 0; + char *cargv[32]; + + if (s_bInstalled) { + RCLCPP_WARN(rclcpp::get_logger(name), "RCLEUS is already installed as %s", + name); + return (T); + } + + ckarg(3); + if (isstring(argv[0])) + strncpy(name, (char *)(argv[0]->c.str.chars), 255); + else + error(E_NOSTRING); + options = ckintval(argv[1]); + pointer p = argv[2]; + if (islist(p)) { + while (1) { + if (!iscons(p)) + break; + cargv[cargc] = (char *)((ccar(p))->c.str.chars); + cargc++; + p = ccdr(p); + } + } else + error(E_NOSEQ); + + // convert invalid node name charactors to _, we assume it is '-' + for (unsigned int i = 0; i < strlen(name); i++) + if (!(isalpha(name[i]) || isdigit(name[i]))) + name[i] = '_'; + + // TODO defkeyword + // K_ROSEUS_MD5SUM = defkeyword(ctx, "MD5SUM-"); + // K_ROSEUS_DATATYPE = defkeyword(ctx, "DATATYPE-"); + // K_ROSEUS_DEFINITION = defkeyword(ctx, "DEFINITION-"); + // K_ROSEUS_CONNECTION_HEADER = + // intern(ctx, "_CONNECTION-HEADER", 18, findpkg(makestring("ROS", 3))); + // K_ROSEUS_SERIALIZATION_LENGTH = defkeyword(ctx, "SERIALIZATION-LENGTH"); + // K_ROSEUS_SERIALIZE = defkeyword(ctx, "SERIALIZE"); + // K_ROSEUS_DESERIALIZE = defkeyword(ctx, "DESERIALIZE"); + // K_ROSEUS_GET = defkeyword(ctx, "GET"); + // K_ROSEUS_INIT = defkeyword(ctx, "INIT"); + // K_ROSEUS_REQUEST = defkeyword(ctx, "REQUEST"); + // K_ROSEUS_RESPONSE = defkeyword(ctx, "RESPONSE"); + // K_ROSEUS_GROUPNAME = defkeyword(ctx, "GROUPNAME"); + // K_ROSEUS_ONESHOT = defkeyword(ctx, "ONESHOT"); + // K_ROSEUS_LAST_EXPECTED = defkeyword(ctx, "LAST-EXPECTED"); + // K_ROSEUS_LAST_REAL = defkeyword(ctx, "LAST-REAL"); + // K_ROSEUS_CURRENT_EXPECTED = defkeyword(ctx, "CURRENT-EXPECTED"); + // K_ROSEUS_CURRENT_REAL = defkeyword(ctx, "CURRENT-REAL"); + // K_ROSEUS_LAST_DURATION = defkeyword(ctx, "LAST-DURATION"); + // K_ROSEUS_SEC = defkeyword(ctx, "SEC"); + // K_ROSEUS_NSEC = defkeyword(ctx, "NSEC"); + + // s_mapAdvertised.clear(); + // s_mapSubscribed.clear(); + // s_mapServiced.clear(); + // s_mapTimered.clear(); + // s_mapHandle.clear(); + + /* + set locale to none to let C RTL assume logging string can contain + non-ascii characters. Refer: + https://logging.apache.org/log4cxx/latest_stable/faq.html + */ + setlocale(LC_ALL, ""); + + try { + rclcpp::init(cargc, cargv); + s_staticData.node = std::make_shared(name); + } catch (const rclcpp::exceptions::RCLError &e) { + RCLCPP_ERROR(rclcpp::get_logger(name), "%s", e.what()); + error(E_MISMATCHARG); + return (NIL); + } + + // s_node.reset(new ros::NodeHandle()); + // s_rate.reset(new ros::Rate(50)); + + s_bInstalled = true; + + // install signal handler for sigint. DO NOT call unix:signal after + signal(SIGINT, rcleusSignalHandler); + return (T); +} + +pointer ___rcleus(context *ctx, int n, pointer *argv, pointer env) { + defun(ctx, "RCLEUS-RAW", argv[0], (pointer(*)())RCLEUS, ""); + return 0; +} // pointer ___rcleus(context *ctx, int n, pointer *argv, pointer env) diff --git a/roseus/AMENT_IGNORE b/roseus/AMENT_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/roseus_mongo/AMENT_IGNORE b/roseus_mongo/AMENT_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/roseus_msgs/AMENT_IGNORE b/roseus_msgs/AMENT_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/roseus_smach/AMENT_IGNORE b/roseus_smach/AMENT_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/roseus_tutorials/AMENT_IGNORE b/roseus_tutorials/AMENT_IGNORE new file mode 100644 index 000000000..e69de29bb