diff --git a/ComponentRosBaseFake/.classpath b/ComponentRosBaseFake/.classpath new file mode 100644 index 00000000..6d6fc9aa --- /dev/null +++ b/ComponentRosBaseFake/.classpath @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/ComponentRosBaseFake/.cproject b/ComponentRosBaseFake/.cproject new file mode 100644 index 00000000..d28f9605 --- /dev/null +++ b/ComponentRosBaseFake/.cproject @@ -0,0 +1,133 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ComponentRosBaseFake/.project b/ComponentRosBaseFake/.project new file mode 100644 index 00000000..0b2f4afa --- /dev/null +++ b/ComponentRosBaseFake/.project @@ -0,0 +1,43 @@ + + + ComponentRosBaseFake + + + CommBasicObjects + CommNavigationObjects + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + + + org.eclipse.jdt.core.javabuilder + + + + + org.eclipse.xtext.ui.shared.xtextBuilder + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + + org.eclipse.sirius.nature.modelingproject + org.eclipse.cdt.core.cnature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + org.smartmdsd.utils.natures.ComponentNature + org.eclipse.xtext.ui.shared.xtextNature + org.eclipse.jdt.core.javanature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + + diff --git a/ComponentRosBaseFake/ROS/src-gen/CMakeLists.txt b/ComponentRosBaseFake/ROS/src-gen/CMakeLists.txt new file mode 100644 index 00000000..b522b818 --- /dev/null +++ b/ComponentRosBaseFake/ROS/src-gen/CMakeLists.txt @@ -0,0 +1,39 @@ +#-------------------------------------------------------------------------- +# This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +# developed by the SeRoNet Project consortium: +# http:#www.seronet-projekt.de +# +# The ROS Mixed-Port Component is developed by: +# Service Robotics Research Center of Ulm University of Applied Sciences +# Fraunhofer Institute for Manufacturing Engineering and Automation IPA +# +# This code-generator uses infrastructure of the SmartMDSD Toolchain on +# which the SeRoNet Tooling is based on. +# +# CAUTION: +# This software is a preview for the purpose of technology demonstration. +# It is experimental and comes with no support. Use at your own risk. +# Please do not modify this file. It will be re-generated +# running the code generator. +#-------------------------------------------------------------------------- +# %Tag(FULLTEXT)% +cmake_minimum_required(VERSION 2.8.3) +project(ros_componentrosbasefake) + +find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs) + +catkin_package() + +LIST(APPEND LOCAL_INCLUDES ${catkin_INCLUDE_DIRS}) +LIST(APPEND LOCAL_INCLUDES ${CMAKE_CURRENT_SOURCE_DIR}) +LIST(APPEND LOCAL_INCLUDES ${CMAKE_CURRENT_SOURCE_DIR}/../src) +set(ROS_INCLUDES ${LOCAL_INCLUDES} PARENT_SCOPE) + +LIST(APPEND SRCS ${CMAKE_CURRENT_SOURCE_DIR}/ComponentRosBaseFakeRosPortExtension.cc) +LIST(APPEND SRCS ${CMAKE_CURRENT_SOURCE_DIR}/../src/ComponentRosBaseFakeRosPortCallbacks.cc) + +set(ROS_SRCS ${SRCS} PARENT_SCOPE) + +set(ROS_LIBS ${catkin_LIBRARIES} PARENT_SCOPE) + +# %EndTag(FULLTEXT)% diff --git a/ComponentRosBaseFake/ROS/src-gen/ComponentRosBaseFakeRosPortBaseClass.hh b/ComponentRosBaseFake/ROS/src-gen/ComponentRosBaseFakeRosPortBaseClass.hh new file mode 100644 index 00000000..c0a1a2bb --- /dev/null +++ b/ComponentRosBaseFake/ROS/src-gen/ComponentRosBaseFakeRosPortBaseClass.hh @@ -0,0 +1,33 @@ +//-------------------------------------------------------------------------- +// This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +// developed by the SeRoNet Project consortium: +// http://www.seronet-projekt.de +// +// The ROS Mixed-Port Component is developed by: +// Service Robotics Research Center of Ulm University of Applied Sciences +// Fraunhofer Institute for Manufacturing Engineering and Automation IPA +// +// This code-generator uses infrastructure of the SmartMDSD Toolchain on +// which the SeRoNet Tooling is based on. +// +// CAUTION: +// This software is a preview for the purpose of technology demonstration. +// It is experimental and comes with no support. Use at your own risk. +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef ROS_PORT_BASE_CLASS_H_ +#define ROS_PORT_BASE_CLASS_H_ + +#include + +class ComponentRosBaseFakeRosPortBaseClass { +public: + ComponentRosBaseFakeRosPortBaseClass() { }; + virtual ~ComponentRosBaseFakeRosPortBaseClass() { } + + ros::Publisher twist_pub; +}; + +#endif // ROS_PORT_BASE_CLASS_H_ diff --git a/ComponentRosBaseFake/ROS/src-gen/ComponentRosBaseFakeRosPortCallbacksInterface.hh b/ComponentRosBaseFake/ROS/src-gen/ComponentRosBaseFakeRosPortCallbacksInterface.hh new file mode 100644 index 00000000..8ab8abeb --- /dev/null +++ b/ComponentRosBaseFake/ROS/src-gen/ComponentRosBaseFakeRosPortCallbacksInterface.hh @@ -0,0 +1,34 @@ +//-------------------------------------------------------------------------- +// This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +// developed by the SeRoNet Project consortium: +// http://www.seronet-projekt.de +// +// The ROS Mixed-Port Component is developed by: +// Service Robotics Research Center of Ulm University of Applied Sciences +// Fraunhofer Institute for Manufacturing Engineering and Automation IPA +// +// This code-generator uses infrastructure of the SmartMDSD Toolchain on +// which the SeRoNet Tooling is based on. +// +// CAUTION: +// This software is a preview for the purpose of technology demonstration. +// It is experimental and comes with no support. Use at your own risk. +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef ROS_PORT_CALLBACKS_INTERFACE_H_ +#define ROS_PORT_CALLBACKS_INTERFACE_H_ + +#include + +#include + +class ComponentRosBaseFakeRosPortCallbacksInterface { +public: + ComponentRosBaseFakeRosPortCallbacksInterface() { }; + virtual ~ComponentRosBaseFakeRosPortCallbacksInterface() { } + +}; + +#endif // ROS_PORT_CALLBACKS_INTERFACE_H_ diff --git a/ComponentRosBaseFake/ROS/src-gen/ComponentRosBaseFakeRosPortExtension.cc b/ComponentRosBaseFake/ROS/src-gen/ComponentRosBaseFakeRosPortExtension.cc new file mode 100644 index 00000000..dba65fce --- /dev/null +++ b/ComponentRosBaseFake/ROS/src-gen/ComponentRosBaseFakeRosPortExtension.cc @@ -0,0 +1,71 @@ +//-------------------------------------------------------------------------- +// This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +// developed by the SeRoNet Project consortium: +// http://www.seronet-projekt.de +// +// The ROS Mixed-Port Component is developed by: +// Service Robotics Research Center of Ulm University of Applied Sciences +// Fraunhofer Institute for Manufacturing Engineering and Automation IPA +// +// This code-generator uses infrastructure of the SmartMDSD Toolchain on +// which the SeRoNet Tooling is based on. +// +// CAUTION: +// This software is a preview for the purpose of technology demonstration. +// It is experimental and comes with no support. Use at your own risk. +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#include "ComponentRosBaseFakeRosPortExtension.hh" + +// statically create a global PlainOpcUaComponentRosBaseFakeExtension instance +static ComponentRosBaseFakeRosPortExtension ros_port_extension; + +ComponentRosBaseFakeRosPortExtension::ComponentRosBaseFakeRosPortExtension() +: ComponentRosBaseFakeExtension("ComponentRosBaseFakeRosPortExtension") +{ + nh = 0; + callbacksPtr = 0; +} + +ComponentRosBaseFakeRosPortExtension::~ComponentRosBaseFakeRosPortExtension() +{ } + +void ComponentRosBaseFakeRosPortExtension::loadParameters(const SmartACE::SmartIniParameter ¶meter) +{ } + +void ComponentRosBaseFakeRosPortExtension::initialize(ComponentRosBaseFake *component, int argc, char* argv[]) +{ + ros::init(argc, argv, "ComponentRosBaseFake", ros::init_options::NoSigintHandler); + nh = new ros::NodeHandle(); + + callbacksPtr = new ComponentRosBaseFakeRosPortCallbacks(); + + component->rosPorts = this; + + twist_pub = nh->advertise("/base/twist_controller_seronet/command", 10); +} + +int ComponentRosBaseFakeRosPortExtension::onStartup() +{ + return startExtensionThread(); +} + +int ComponentRosBaseFakeRosPortExtension::extensionExecution() +{ + ros::spin(); + return 0; +} + +int ComponentRosBaseFakeRosPortExtension::onShutdown(const std::chrono::steady_clock::duration &timeoutTime) +{ + ros::shutdown(); + return stopExtensionThread(timeoutTime); +} + +void ComponentRosBaseFakeRosPortExtension::destroy() +{ + delete nh; + delete callbacksPtr; +} diff --git a/ComponentRosBaseFake/ROS/src-gen/ComponentRosBaseFakeRosPortExtension.hh b/ComponentRosBaseFake/ROS/src-gen/ComponentRosBaseFakeRosPortExtension.hh new file mode 100644 index 00000000..a59c611a --- /dev/null +++ b/ComponentRosBaseFake/ROS/src-gen/ComponentRosBaseFakeRosPortExtension.hh @@ -0,0 +1,55 @@ +//-------------------------------------------------------------------------- +// This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +// developed by the SeRoNet Project consortium: +// http://www.seronet-projekt.de +// +// The ROS Mixed-Port Component is developed by: +// Service Robotics Research Center of Ulm University of Applied Sciences +// Fraunhofer Institute for Manufacturing Engineering and Automation IPA +// +// This code-generator uses infrastructure of the SmartMDSD Toolchain on +// which the SeRoNet Tooling is based on. +// +// CAUTION: +// This software is a preview for the purpose of technology demonstration. +// It is experimental and comes with no support. Use at your own risk. +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef ROS_PORT_COMPONENT_EXTENSION_H_ +#define ROS_PORT_COMPONENT_EXTENSION_H_ + +#include "ComponentRosBaseFakeRosPortBaseClass.hh" +#include "ComponentRosBaseFakeRosPortCallbacks.hh" + +#include "ComponentRosBaseFakeExtension.hh" + +// include component's main class +#include "ComponentRosBaseFake.hh" + +class ComponentRosBaseFakeRosPortExtension : public ComponentRosBaseFakeExtension, public ComponentRosBaseFakeRosPortBaseClass +{ +private: + ros::NodeHandle *nh; + + ComponentRosBaseFakeRosPortCallbacks *callbacksPtr; + + virtual int extensionExecution() override; +public: + ComponentRosBaseFakeRosPortExtension(); + virtual ~ComponentRosBaseFakeRosPortExtension(); + + virtual void loadParameters(const SmartACE::SmartIniParameter ¶meter); + virtual void initialize(ComponentRosBaseFake *component, int argc, char* argv[]) override; + virtual int onStartup() override; + + inline ros::Publisher* getTwist_pubPtr() { + return &twist_pub; + } + + virtual int onShutdown(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)) override; + virtual void destroy() override; +}; + +#endif /* ROS_PORT_COMPONENT_EXTENSION_H_ */ diff --git a/ComponentRosBaseFake/ROS/src-gen/package.xml b/ComponentRosBaseFake/ROS/src-gen/package.xml new file mode 100644 index 00000000..0dc40a28 --- /dev/null +++ b/ComponentRosBaseFake/ROS/src-gen/package.xml @@ -0,0 +1,18 @@ + + ros_componentrosbasefake + 0.0.0 + The ros_componentrosbasefake package + + Apache 2.0 + + + + Jane Doe + Jane Doe + + + catkin + roscpp + geometry_msgs + + diff --git a/ComponentRosBaseFake/ROS/src/ComponentRosBaseFakeRosPortCallbacks.cc b/ComponentRosBaseFake/ROS/src/ComponentRosBaseFakeRosPortCallbacks.cc new file mode 100644 index 00000000..51d10c9e --- /dev/null +++ b/ComponentRosBaseFake/ROS/src/ComponentRosBaseFakeRosPortCallbacks.cc @@ -0,0 +1,29 @@ +//-------------------------------------------------------------------------- +// This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +// developed by the SeRoNet Project consortium: +// http://www.seronet-projekt.de +// +// The ROS Mixed-Port Component is developed by: +// Service Robotics Research Center of Ulm University of Applied Sciences +// Fraunhofer Institute for Manufacturing Engineering and Automation IPA +// +// This code-generator uses infrastructure of the SmartMDSD Toolchain on +// which the SeRoNet Tooling is based on. +// +// CAUTION: +// This software is a preview for the purpose of technology demonstration. +// It is experimental and comes with no support. Use at your own risk. +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#include "ComponentRosBaseFakeRosPortCallbacks.hh" + +// include component's main class +#include "ComponentRosBaseFake.hh" + +ComponentRosBaseFakeRosPortCallbacks::ComponentRosBaseFakeRosPortCallbacks() { } + +ComponentRosBaseFakeRosPortCallbacks::~ComponentRosBaseFakeRosPortCallbacks() { } + + diff --git a/ComponentRosBaseFake/ROS/src/ComponentRosBaseFakeRosPortCallbacks.hh b/ComponentRosBaseFake/ROS/src/ComponentRosBaseFakeRosPortCallbacks.hh new file mode 100644 index 00000000..8e3b58f2 --- /dev/null +++ b/ComponentRosBaseFake/ROS/src/ComponentRosBaseFakeRosPortCallbacks.hh @@ -0,0 +1,32 @@ +//-------------------------------------------------------------------------- +// This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +// developed by the SeRoNet Project consortium: +// http://www.seronet-projekt.de +// +// The ROS Mixed-Port Component is developed by: +// Service Robotics Research Center of Ulm University of Applied Sciences +// Fraunhofer Institute for Manufacturing Engineering and Automation IPA +// +// This code-generator uses infrastructure of the SmartMDSD Toolchain on +// which the SeRoNet Tooling is based on. +// +// CAUTION: +// This software is a preview for the purpose of technology demonstration. +// It is experimental and comes with no support. Use at your own risk. +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef ROS_PORT_CALLBACKS_H_ +#define ROS_PORT_CALLBACKS_H_ + +#include "ComponentRosBaseFakeRosPortCallbacksInterface.hh" + +class ComponentRosBaseFakeRosPortCallbacks : public ComponentRosBaseFakeRosPortCallbacksInterface { +public: + ComponentRosBaseFakeRosPortCallbacks(); + virtual ~ComponentRosBaseFakeRosPortCallbacks(); + +}; + +#endif // ROS_PORT_CALLBACKS_H_ diff --git a/ComponentRosBaseFake/model/ComponentRosBaseFake.component b/ComponentRosBaseFake/model/ComponentRosBaseFake.component new file mode 100644 index 00000000..81f8d00b --- /dev/null +++ b/ComponentRosBaseFake/model/ComponentRosBaseFake.component @@ -0,0 +1,14 @@ +ComponentDefinition ComponentRosBaseFake logo "/ComponentRosBaseFake/model/logo.png" +{ +Activity BaseFakeActivity + { + InputPortLink NavigationVelocityServiceIn { oversamplingOk undersamplingOk } + MixedPortROSLink twist_pub + DefaultTrigger PeriodicTimer 1.0 Hz + } + InputPort NavigationVelocityServiceIn implements CommNavigationObjects.NavigationVelocityService { + } + OutputPort BaseStateServiceOut implements CommBasicObjects.BaseStateService realizedBy BaseFakeActivity { + } + MixedPortROS twist_pub +} \ No newline at end of file diff --git a/ComponentRosBaseFake/model/ComponentRosBaseFakeComponentDefinition.jpg b/ComponentRosBaseFake/model/ComponentRosBaseFakeComponentDefinition.jpg new file mode 100644 index 00000000..cc93a27e Binary files /dev/null and b/ComponentRosBaseFake/model/ComponentRosBaseFakeComponentDefinition.jpg differ diff --git a/ComponentRosBaseFake/model/component.rosinterfacespool b/ComponentRosBaseFake/model/component.rosinterfacespool new file mode 100644 index 00000000..0733f295 --- /dev/null +++ b/ComponentRosBaseFake/model/component.rosinterfacespool @@ -0,0 +1,3 @@ +RosInterfacesPool { + RosPublisher twist_pub { topicName "/base/twist_controller_seronet/command" type "geometry_msgs.Twist" } +} \ No newline at end of file diff --git a/ComponentRosBaseFake/opcua-backend/src-gen/ComponentRosBaseFakeOpcUaBackendPortFactory.cc b/ComponentRosBaseFake/opcua-backend/src-gen/ComponentRosBaseFakeOpcUaBackendPortFactory.cc new file mode 100644 index 00000000..e7bf8455 --- /dev/null +++ b/ComponentRosBaseFake/opcua-backend/src-gen/ComponentRosBaseFakeOpcUaBackendPortFactory.cc @@ -0,0 +1,95 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#include "ComponentRosBaseFakeOpcUaBackendPortFactory.hh" + +// include all potentially required pattern implementations +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// include referenced CommunicationObject SeRoNetSDK self description implementations +#include "CommBasicObjectsOpcUa/CommBaseStateOpcUa.hh" +#include "CommBasicObjectsOpcUa/CommNavigationVelocityOpcUa.hh" + +// create a static instance of the OpcUaBackendPortFactory +static ComponentRosBaseFakeOpcUaBackendPortFactory OpcUaBackendPortFactory; + +ComponentRosBaseFakeOpcUaBackendPortFactory::ComponentRosBaseFakeOpcUaBackendPortFactory() +{ + componentImpl = 0; + ComponentRosBaseFake::instance()->addPortFactory("OpcUa_SeRoNet", this); +} + +ComponentRosBaseFakeOpcUaBackendPortFactory::~ComponentRosBaseFakeOpcUaBackendPortFactory() +{ } + +void ComponentRosBaseFakeOpcUaBackendPortFactory::initialize(ComponentRosBaseFake *component, int argc, char* argv[]) +{ + componentImpl = new SeRoNet::Utils::Component(component->connections.component.name); +} + +int ComponentRosBaseFakeOpcUaBackendPortFactory::onStartup() +{ + if (!component_thread.joinable()) { + component_thread = std::thread(&ComponentRosBaseFakeOpcUaBackendPortFactory::task_execution, this); + return 0; + } + return -1; +} + + +Smart::IPushServerPattern * ComponentRosBaseFakeOpcUaBackendPortFactory::createBaseStateServiceOut(const std::string &serviceName) +{ + return new SeRoNet::OPCUA::Server::PushServer(componentImpl, serviceName); +} + +Smart::ISendServerPattern * ComponentRosBaseFakeOpcUaBackendPortFactory::createNavigationVelocityServiceIn(const std::string &serviceName) +{ + return new SeRoNet::OPCUA::Server::SendServer(componentImpl, serviceName); +} + + +int ComponentRosBaseFakeOpcUaBackendPortFactory::task_execution() +{ + componentImpl->run(); + return 0; +} + +int ComponentRosBaseFakeOpcUaBackendPortFactory::onShutdown(const std::chrono::steady_clock::duration &timeoutTime) +{ + // stop component-internal infrastructure + componentImpl->stopRunning(); + // wait on component thread to exit + if (component_thread.joinable()) { + // FIXME: don't wait infinetly (use timeoutTime here) + component_thread.join(); + } + return 0; +} + +void ComponentRosBaseFakeOpcUaBackendPortFactory::destroy() +{ + // clean-up component's internally used resources + delete componentImpl; +} diff --git a/ComponentRosBaseFake/opcua-backend/src-gen/ComponentRosBaseFakeOpcUaBackendPortFactory.hh b/ComponentRosBaseFake/opcua-backend/src-gen/ComponentRosBaseFakeOpcUaBackendPortFactory.hh new file mode 100644 index 00000000..26007d41 --- /dev/null +++ b/ComponentRosBaseFake/opcua-backend/src-gen/ComponentRosBaseFakeOpcUaBackendPortFactory.hh @@ -0,0 +1,56 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef COMPONENTROSBASEFAKE_OPC_UA_BACKEND_PORTFACTORY_HH_ +#define COMPONENTROSBASEFAKE_OPC_UA_BACKEND_PORTFACTORY_HH_ + +// include the main component-definition class +#include "ComponentRosBaseFakePortFactoryInterface.hh" + +#include +#include + +// include SeRoNetSDK library +#include +#include + +class ComponentRosBaseFakeOpcUaBackendPortFactory: public ComponentRosBaseFakePortFactoryInterface +{ +private: + // internal component instance + SeRoNet::Utils::Component *componentImpl; + + // component thread + std::thread component_thread; + + // internal component thread method + int task_execution(); +public: + ComponentRosBaseFakeOpcUaBackendPortFactory(); + virtual ~ComponentRosBaseFakeOpcUaBackendPortFactory(); + + virtual void initialize(ComponentRosBaseFake *component, int argc, char* argv[]) override; + virtual int onStartup() override; + + + virtual Smart::IPushServerPattern * createBaseStateServiceOut(const std::string &serviceName) override; + virtual Smart::ISendServerPattern * createNavigationVelocityServiceIn(const std::string &serviceName) override; + + virtual int onShutdown(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)) override; + virtual void destroy() override; +}; + +#endif /* COMPONENTROSBASEFAKE_SERONET_SDK_PORTFACTORY_HH_ */ diff --git a/ComponentRosBaseFake/opcua-backend/src-gen/OpcUaBackend.cmake b/ComponentRosBaseFake/opcua-backend/src-gen/OpcUaBackend.cmake new file mode 100644 index 00000000..7a1f2e81 --- /dev/null +++ b/ComponentRosBaseFake/opcua-backend/src-gen/OpcUaBackend.cmake @@ -0,0 +1,12 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) + +FIND_PACKAGE(Open62541Cpp QUIET) +SET(SmartSoft_CD_API_DIR $ENV{SMART_ROOT_ACE}/modules) +FIND_PACKAGE(SeRoNetSDK QUIET) + +IF(SeRoNetSDK_FOUND) + FIND_PACKAGE(CommBasicObjectsOpcUa PATHS $ENV{SMART_ROOT_ACE}/modules) + SET(CMAKE_CXX_STANDARD 14) + INCLUDE_DIRECTORIES(${CMAKE_CURRENT_LIST_DIR}) + SET(OPC_UA_BACKEND_SRCS "${CMAKE_CURRENT_LIST_DIR}/ComponentRosBaseFakeOpcUaBackendPortFactory.cc") +ENDIF(SeRoNetSDK_FOUND) diff --git a/ComponentRosBaseFake/opcua-backend/src-gen/info.txt b/ComponentRosBaseFake/opcua-backend/src-gen/info.txt new file mode 100644 index 00000000..93cdd389 --- /dev/null +++ b/ComponentRosBaseFake/opcua-backend/src-gen/info.txt @@ -0,0 +1,17 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +// Generated with SmartMDSD Toolchain Version 3.12.0.201912041712 diff --git a/ComponentRosBaseFake/representations.aird b/ComponentRosBaseFake/representations.aird new file mode 100644 index 00000000..bdbb2f95 --- /dev/null +++ b/ComponentRosBaseFake/representations.aird @@ -0,0 +1,236 @@ + + + + model/ComponentRosBaseFake.component + model/component.rosinterfacespool + platform:/resource/CommNavigationObjects/model/CommNavigationObjects.services + platform:/resource/CommBasicObjects/model/CommBasicObjects.types + platform:/resource/CommNavigationObjects/model/CommNavigationObjects.types + platform:/plugin/org.xtext.base.stateMachine/componentLifecycle/ComponentLifecycle.automaton + platform:/resource/CommBasicObjects/model/CommBasicObjects.modes + platform:/resource/CommBasicObjects/model/CommBasicObjects.parameters + platform:/resource/CommBasicObjects/model/CommBasicObjects.services + platform:/resource/CommNavigationObjects/model/CommNavigationObjects.modes + platform:/resource/CommNavigationObjects/model/CommNavigationObjects.parameters + platform:/plugin/org.xtext.service.parameterDefinition/defaultParameterLib/defaultParameterLib.parameters + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + bold + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + bold + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ComponentRosBaseFake/smartsoft/CMakeLists.txt b/ComponentRosBaseFake/smartsoft/CMakeLists.txt new file mode 100644 index 00000000..4e90a44f --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/CMakeLists.txt @@ -0,0 +1,82 @@ +#-------------------------------------------------------------------------- +# Code generated by the SmartSoft MDSD Toolchain +# The SmartSoft Toolchain has been developed by: +# +# Service Robotics Research Center +# University of Applied Sciences Ulm +# Prittwitzstr. 10 +# 89075 Ulm (Germany) +# +# Information about the SmartSoft MDSD Toolchain is available at: +# www.servicerobotik-ulm.de +# +# This file is generated once. Modify this file to your needs. +# If you want the toolchain to re-generate this file, please +# delete it before running the code generator. +#-------------------------------------------------------------------------- + +CMAKE_MINIMUM_REQUIRED(VERSION 3.0) + +# find the SmartSoft CMake Macros (version 2 that uses CMake v3) +FIND_FILE(SMART_MACROS SmartMacros2.cmake PATHS $ENV{SMART_ROOT_ACE}/CMakeMacros /opt/smartSoftAce/CMakeMacros/) +INCLUDE(${SMART_MACROS}) + +# Create and configure a CMake project for a CommunicationObject +SMART_COMPONENT_PROJECT() + +# find SmartSoft package +#FIND_SMARTSOFT(3.0) +#FIXME: generalize this +FIND_PACKAGE(AceSmartSoft PATHS $ENV{SMART_ROOT_ACE}/modules) + +# generte doxygen documentation +SMART_COMPONENT_GENERATE_DOC() + +# automatically build CommunicationObject dependencies +SMART_COMPONENT_AUTOBUILD_DEPENDENCIES() + +################################################################################# +## here you can put all your project-specific compiler configurations ## +## using regular CMake commands (like FIND_PACKAGE, INCLUDE_DIRECTORIES, etc.) ## + +# you can add additional external libraries using regular CMake commands, e.g.: +#FIND_PACKAGE() + +INCLUDE(${CMAKE_CURRENT_LIST_DIR}/src-gen/MixedPortIncludes.cmake) + +# these are the regular includes (add your own includes, if needed) +INCLUDE_DIRECTORIES( + ${CMAKE_CURRENT_LIST_DIR}/src + ${CMAKE_CURRENT_LIST_DIR}/src-gen + ${CMAKE_CURRENT_LIST_DIR}/src-gen/params +) + +# collect the toolchain-generated source-files +FILE(GLOB_RECURSE SRC ${CMAKE_CURRENT_LIST_DIR}/src/*.cc) +FILE(GLOB_RECURSE SRC_GEN ${CMAKE_CURRENT_LIST_DIR}/src-gen/*.cc) +FILE(GLOB_RECURSE SRC_GEN_PARAMS ${CMAKE_CURRENT_LIST_DIR}/src-gen/params/*.cc) + +# collect your own (local) source-files here (if needed) +#FILE(GLOB_RECURSE USER_SRC ${CMAKE_CURRENT_LIST_DIR}/src/) + +# create the executable using collected source-files +ADD_EXECUTABLE(${PROJECT_NAME} ${SRC} ${SRC_GEN} ${SRC_GEN_PARAMS} ${FURTHER_SRCS}) # add ${USER_SRC} (if needed) + +# link your external libraries here (if there are any) +#TARGET_LINK_LIBRARIES(${PROJECT_NAME} ) + +INCLUDE(${CMAKE_CURRENT_LIST_DIR}/src-gen/MixedPortTargetConfigs.cmake) + +#INSTALL(FILES ${PROJECT_SOURCE_DIR}/src-gen/${PROJECT_NAME}.ini.template DESTINATION etc) + +## end user-code ## +################################################################################# + +# SmartSoftKernel is one of the default dependencies for each component +TARGET_LINK_LIBRARIES(${PROJECT_NAME} AceSmartSoftKernel) + +# automatically link to all dependent CommunicationObjects +SMART_COMPONENT_AUTOLINK_DEPENDENCIES() + +# automatically install the component target +SMART_COMPONENT_AUTOINSTALL() diff --git a/ComponentRosBaseFake/smartsoft/src-gen/BaseFakeActivityCore.cc b/ComponentRosBaseFake/smartsoft/src-gen/BaseFakeActivityCore.cc new file mode 100644 index 00000000..d5d1e63e --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/BaseFakeActivityCore.cc @@ -0,0 +1,121 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include "BaseFakeActivityCore.hh" +#include "BaseFakeActivity.hh" +#include "ComponentRosBaseFake.hh" + +//FIXME: use logging +//#include "smartGlobalLogger.hh" + +// include observers + + +BaseFakeActivityCore::BaseFakeActivityCore(Smart::IComponent *comp, const bool &useDefaultState) +: SmartACE::ManagedTask(comp) +, useDefaultState(useDefaultState) +, useLogging(false) +, taskLoggingId(0) +, currentUpdateCount(0) +, navigationVelocityServiceInStatus(Smart::SMART_DISCONNECTED) +, navigationVelocityServiceInObject() +{ +} + +BaseFakeActivityCore::~BaseFakeActivityCore() +{ +} + + +void BaseFakeActivityCore::notify_all_interaction_observers() { + std::unique_lock lock(interaction_observers_mutex); + // try dynamically down-casting this class to the derived class + // (we can do it safely here as we exactly know the derived class) + if(const BaseFakeActivity* baseFakeActivity = dynamic_cast(this)) { + for(auto it=interaction_observers.begin(); it!=interaction_observers.end(); it++) { + (*it)->on_update_from(baseFakeActivity); + } + } +} + +void BaseFakeActivityCore::attach_interaction_observer(BaseFakeActivityObserverInterface *observer) { + std::unique_lock lock(interaction_observers_mutex); + interaction_observers.push_back(observer); +} + +void BaseFakeActivityCore::detach_interaction_observer(BaseFakeActivityObserverInterface *observer) { + std::unique_lock lock(interaction_observers_mutex); + interaction_observers.remove(observer); +} + +int BaseFakeActivityCore::execute_protected_region() +{ + + // update of comm-objects must be within the protected region to prevent aged comm-object values + this->updateAllCommObjects(); + + if(useLogging == true) { + //FIXME: use logging + //Smart::LOGGER->log(taskLoggingId, getCurrentUpdateCount(), getPreviousCommObjId()); + } + + // this is the user code (should not internally use the state-pattern any more) + int retval = this->on_execute(); + + // notify all attached interaction observers + this->notify_all_interaction_observers(); + + // inform all associated tasks about a new update + this->trigger_all_tasks(); + + // increment current currentUpdateCount for the next iteration + currentUpdateCount++; + + return retval; +} + + +void BaseFakeActivityCore::updateAllCommObjects() +{ + navigationVelocityServiceInStatus = COMP->navigationVelocityServiceInInputTaskTrigger->getUpdate(navigationVelocityServiceInObject); + +} + + +// this method is meant to be used in derived classes +Smart::StatusCode BaseFakeActivityCore::baseStateServiceOutPut(CommBasicObjects::CommBaseState &baseStateServiceOutDataObject) +{ + Smart::StatusCode result = COMP->baseStateServiceOut->put(baseStateServiceOutDataObject); + if(useLogging == true) { + //FIXME: use logging + //Smart::LOGGER->log(pushLoggingId+1, getCurrentUpdateCount(), getPreviousCommObjId()); + } + return result; +} + +void BaseFakeActivityCore::triggerLogEntry(const int& idOffset) +{ + if(useLogging == true) { + int logId = taskLoggingId + 2*1 + idOffset; + //FIXME: use logging + //Smart::LOGGER->log(logId, getCurrentUpdateCount(), getPreviousCommObjId()); + } +} + +int BaseFakeActivityCore::getPreviousCommObjId() +{ + // this method needs to be overloaded and implemented in derived classes + return 0; +} diff --git a/ComponentRosBaseFake/smartsoft/src-gen/BaseFakeActivityCore.hh b/ComponentRosBaseFake/smartsoft/src-gen/BaseFakeActivityCore.hh new file mode 100644 index 00000000..93931f8c --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/BaseFakeActivityCore.hh @@ -0,0 +1,108 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _BASEFAKEACTIVITY_CORE_HH +#define _BASEFAKEACTIVITY_CORE_HH + +#include "aceSmartSoft.hh" + +// include upcall interface +#include "NavigationVelocityServiceInUpcallInterface.hh" + +// include communication-objects for output ports +#include + +// include all interaction-observer interfaces +#include + +#include + +class BaseFakeActivityCore +: public SmartACE::ManagedTask +, public Smart::TaskTriggerSubject +, public NavigationVelocityServiceInUpcallInterface +{ +private: + bool useDefaultState; + bool useLogging; + int taskLoggingId; + unsigned int currentUpdateCount; + + Smart::StatusCode navigationVelocityServiceInStatus; + CommBasicObjects::CommNavigationVelocity navigationVelocityServiceInObject; + + +protected: + virtual int execute_protected_region(); + + virtual void updateAllCommObjects(); + + virtual int getPreviousCommObjId(); + + void triggerLogEntry(const int& idOffset); + + + // overload and implement this method in derived classes to immediately get all incoming updates from NavigationVelocityServiceIn (as soon as they arrive) + virtual void on_NavigationVelocityServiceIn(const CommBasicObjects::CommNavigationVelocity &input) { + // no-op + } + + // this method can be safely used from the thread in derived classes + inline Smart::StatusCode navigationVelocityServiceInGetUpdate(CommBasicObjects::CommNavigationVelocity &navigationVelocityServiceInObject) const + { + // copy local object buffer and return the last status code + navigationVelocityServiceInObject = this->navigationVelocityServiceInObject; + return navigationVelocityServiceInStatus; + } + + // this method is meant to be used in derived classes + Smart::StatusCode baseStateServiceOutPut(CommBasicObjects::CommBaseState &baseStateServiceOutDataObject); + + +/** + * Implementation of the Subject part of an InteractionObserver + */ +private: + std::mutex interaction_observers_mutex; + std::list interaction_observers; +protected: + void notify_all_interaction_observers(); +public: + void attach_interaction_observer(BaseFakeActivityObserverInterface *observer); + void detach_interaction_observer(BaseFakeActivityObserverInterface *observer); + +public: + BaseFakeActivityCore(Smart::IComponent *comp, const bool &useDefaultState=true); + virtual ~BaseFakeActivityCore(); + + inline void setUpLogging(const int &taskNbr, const bool &useLogging=true) { + this->taskLoggingId = taskNbr; + this->useLogging = useLogging; + } + + inline bool isLoggingActive() const { + return useLogging; + } + + inline int getLoggingID() const { + return taskLoggingId; + } + + inline int getCurrentUpdateCount() const { + return currentUpdateCount; + } + +}; +#endif diff --git a/ComponentRosBaseFake/smartsoft/src-gen/BaseFakeActivityObserverInterface.hh b/ComponentRosBaseFake/smartsoft/src-gen/BaseFakeActivityObserverInterface.hh new file mode 100644 index 00000000..a4fa8531 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/BaseFakeActivityObserverInterface.hh @@ -0,0 +1,29 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _BASEFAKEACTIVITY_OBSERVER_INTERFACE_HH +#define _BASEFAKEACTIVITY_OBSERVER_INTERFACE_HH + +// forward declaration +class BaseFakeActivity; + +class BaseFakeActivityObserverInterface { +public: + virtual ~BaseFakeActivityObserverInterface() { } + + virtual void on_update_from(const BaseFakeActivity *subject) = 0; +}; + +#endif diff --git a/ComponentRosBaseFake/smartsoft/src-gen/ComponentProject.cmake b/ComponentRosBaseFake/smartsoft/src-gen/ComponentProject.cmake new file mode 100644 index 00000000..8b7757fc --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/ComponentProject.cmake @@ -0,0 +1,17 @@ +#-------------------------------------------------------------------------- +# Code generated by the SmartSoft MDSD Toolchain +# The SmartSoft Toolchain has been developed by: +# +# Service Robotics Research Center +# University of Applied Sciences Ulm +# Prittwitzstr. 10 +# 89075 Ulm (Germany) +# +# Information about the SmartSoft MDSD Toolchain is available at: +# www.servicerobotik-ulm.de +# +# Please do not modify this file. It will be re-generated +# running the code generator. +#-------------------------------------------------------------------------- + +PROJECT(ComponentRosBaseFake) diff --git a/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFake.cc b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFake.cc new file mode 100644 index 00000000..6443b948 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFake.cc @@ -0,0 +1,497 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include "ComponentRosBaseFake.hh" +#include "smartTimedTaskTrigger.h" +//FIXME: implement logging +//#include "smartGlobalLogger.hh" + +// the ace port-factory is used as a default port-mapping +#include "ComponentRosBaseFakeAcePortFactory.hh" + + +// initialize static singleton pointer to zero +ComponentRosBaseFake* ComponentRosBaseFake::_componentRosBaseFake = 0; + +// constructor +ComponentRosBaseFake::ComponentRosBaseFake() +{ + std::cout << "constructor of ComponentRosBaseFake\n"; + + // set all pointer members to NULL + baseFakeActivity = NULL; + baseFakeActivityTrigger = NULL; + baseStateServiceOut = NULL; + navigationVelocityServiceIn = NULL; + navigationVelocityServiceInInputTaskTrigger = NULL; + navigationVelocityServiceInUpcallManager = NULL; + //twist_pub = NULL; + stateChangeHandler = NULL; + stateSlave = NULL; + wiringSlave = NULL; + + // set default ini parameter values + connections.component.name = "ComponentRosBaseFake"; + connections.component.initialComponentMode = "Neutral"; + connections.component.defaultScheduler = "DEFAULT"; + connections.component.useLogger = false; + + connections.baseStateServiceOut.serviceName = "BaseStateServiceOut"; + connections.baseStateServiceOut.roboticMiddleware = "ACE_SmartSoft"; + connections.navigationVelocityServiceIn.serviceName = "NavigationVelocityServiceIn"; + connections.navigationVelocityServiceIn.roboticMiddleware = "ACE_SmartSoft"; + connections.baseFakeActivity.minActFreq = 0.0; + connections.baseFakeActivity.maxActFreq = 0.0; + connections.baseFakeActivity.trigger = "PeriodicTimer"; + connections.baseFakeActivity.periodicActFreq = 1.0; + // scheduling default parameters + connections.baseFakeActivity.scheduler = "DEFAULT"; + connections.baseFakeActivity.priority = -1; + connections.baseFakeActivity.cpuAffinity = -1; + + // initialize members of ComponentRosBaseFakeROSExtension + rosPorts = 0; + + // initialize members of OpcUaBackendComponentGeneratorExtension + + // initialize members of PlainOpcUaComponentRosBaseFakeExtension + +} + +void ComponentRosBaseFake::addPortFactory(const std::string &name, ComponentRosBaseFakePortFactoryInterface *portFactory) +{ + portFactoryRegistry[name] = portFactory; +} + +void ComponentRosBaseFake::addExtension(ComponentRosBaseFakeExtension *extension) +{ + componentExtensionRegistry[extension->getName()] = extension; +} + +SmartACE::SmartComponent* ComponentRosBaseFake::getComponentImpl() +{ + return dynamic_cast(portFactoryRegistry["ACE_SmartSoft"])->getComponentImpl(); +} + +/** + * Notify the component that setup/initialization is finished. + * You may call this function from anywhere in the component. + * + * Set component's internal lifecycle state automaton (if any) into + * Alive mode (from here on the component is ready to provide its services) + */ +void ComponentRosBaseFake::setStartupFinished() { + stateSlave->setWaitState("Alive"); + std::cout << "ComponentDefinition initialization/startup finished." << std::endl; +} + + + + +/** + * First connect ALL client ports contained in this component, then start all services: + * activate state, push, etc... + */ +Smart::StatusCode ComponentRosBaseFake::connectAndStartAllServices() { + Smart::StatusCode status = Smart::SMART_OK; + + return status; +} + +/** + * Start all tasks contained in this component. + */ +void ComponentRosBaseFake::startAllTasks() { + // start task BaseFakeActivity + if(connections.baseFakeActivity.scheduler != "DEFAULT") { + ACE_Sched_Params baseFakeActivity_SchedParams(ACE_SCHED_OTHER, ACE_THR_PRI_OTHER_DEF); + if(connections.baseFakeActivity.scheduler == "FIFO") { + baseFakeActivity_SchedParams.policy(ACE_SCHED_FIFO); + baseFakeActivity_SchedParams.priority(ACE_THR_PRI_FIFO_MIN); + } else if(connections.baseFakeActivity.scheduler == "RR") { + baseFakeActivity_SchedParams.policy(ACE_SCHED_RR); + baseFakeActivity_SchedParams.priority(ACE_THR_PRI_RR_MIN); + } + baseFakeActivity->start(baseFakeActivity_SchedParams, connections.baseFakeActivity.cpuAffinity); + } else { + baseFakeActivity->start(); + } +} + +/** + * Start all timers contained in this component + */ +void ComponentRosBaseFake::startAllTimers() { +} + + +Smart::TaskTriggerSubject* ComponentRosBaseFake::getInputTaskTriggerFromString(const std::string &client) +{ + if(client == "NavigationVelocityServiceIn") return navigationVelocityServiceInInputTaskTrigger; + + return NULL; +} + + +void ComponentRosBaseFake::init(int argc, char *argv[]) +{ + try { + Smart::StatusCode status; + + // load initial parameters from ini-file (if found) + loadParameter(argc, argv); + + + // initializations of ComponentRosBaseFakeROSExtension + + // initializations of OpcUaBackendComponentGeneratorExtension + + // initializations of PlainOpcUaComponentRosBaseFakeExtension + + + // initialize all registered port-factories + for(auto portFactory = portFactoryRegistry.begin(); portFactory != portFactoryRegistry.end(); portFactory++) + { + portFactory->second->initialize(this, argc, argv); + } + + // initialize all registered component-extensions + for(auto extension = componentExtensionRegistry.begin(); extension != componentExtensionRegistry.end(); extension++) + { + extension->second->initialize(this, argc, argv); + } + + ComponentRosBaseFakePortFactoryInterface *acePortFactory = portFactoryRegistry["ACE_SmartSoft"]; + if(acePortFactory == 0) { + std::cerr << "ERROR: acePortFactory NOT instantiated -> exit(-1)" << std::endl; + exit(-1); + } + + // this pointer is used for backwards compatibility (deprecated: should be removed as soon as all patterns, including coordination, are moved to port-factory) + SmartACE::SmartComponent *component = dynamic_cast(acePortFactory)->getComponentImpl(); + + std::cout << "ComponentDefinition ComponentRosBaseFake is named " << connections.component.name << std::endl; + + if(connections.component.useLogger == true) { + //FIXME: use logging + //Smart::LOGGER->openLogFileInFolder("data/"+connections.component.name); + //Smart::LOGGER->startLogging(); + } + + // create event-test handlers (if needed) + + // create server ports + // TODO: set minCycleTime from Ini-file + baseStateServiceOut = portFactoryRegistry[connections.baseStateServiceOut.roboticMiddleware]->createBaseStateServiceOut(connections.baseStateServiceOut.serviceName); + navigationVelocityServiceIn = portFactoryRegistry[connections.navigationVelocityServiceIn.roboticMiddleware]->createNavigationVelocityServiceIn(connections.navigationVelocityServiceIn.serviceName); + + // create client ports + + // create InputTaskTriggers and UpcallManagers + navigationVelocityServiceInInputTaskTrigger = new Smart::InputTaskTrigger(navigationVelocityServiceIn); + navigationVelocityServiceInUpcallManager = new NavigationVelocityServiceInUpcallManager(navigationVelocityServiceIn); + + // create input-handler + + // create request-handlers + + // create state pattern + stateChangeHandler = new SmartStateChangeHandler(); + stateSlave = new SmartACE::StateSlave(component, stateChangeHandler); + status = stateSlave->setUpInitialState(connections.component.initialComponentMode); + if (status != Smart::SMART_OK) std::cerr << status << "; failed setting initial ComponentMode: " << connections.component.initialComponentMode << std::endl; + // activate state slave + status = stateSlave->activate(); + if(status != Smart::SMART_OK) std::cerr << "ERROR: activate state" << std::endl; + + wiringSlave = new SmartACE::WiringSlave(component); + // add client port to wiring slave + + + + // create Task BaseFakeActivity + baseFakeActivity = new BaseFakeActivity(component); + // configure input-links + navigationVelocityServiceInUpcallManager->attach(baseFakeActivity); + // configure task-trigger (if task is configurable) + if(connections.baseFakeActivity.trigger == "PeriodicTimer") { + // create PeriodicTimerTrigger + int microseconds = 1000*1000 / connections.baseFakeActivity.periodicActFreq; + if(microseconds > 0) { + Smart::TimedTaskTrigger *triggerPtr = new Smart::TimedTaskTrigger(); + triggerPtr->attach(baseFakeActivity); + component->getTimerManager()->scheduleTimer(triggerPtr, (void *) 0, std::chrono::microseconds(microseconds), std::chrono::microseconds(microseconds)); + // store trigger in class member + baseFakeActivityTrigger = triggerPtr; + } else { + std::cerr << "ERROR: could not set-up Timer with cycle-time " << microseconds << " as activation source for Task BaseFakeActivity" << std::endl; + } + } else if(connections.baseFakeActivity.trigger == "DataTriggered") { + baseFakeActivityTrigger = getInputTaskTriggerFromString(connections.baseFakeActivity.inPortRef); + if(baseFakeActivityTrigger != NULL) { + baseFakeActivityTrigger->attach(baseFakeActivity, connections.baseFakeActivity.prescale); + } else { + std::cerr << "ERROR: could not set-up InPort " << connections.baseFakeActivity.inPortRef << " as activation source for Task BaseFakeActivity" << std::endl; + } + } else + { + // setup default task-trigger as PeriodicTimer + Smart::TimedTaskTrigger *triggerPtr = new Smart::TimedTaskTrigger(); + int microseconds = 1000*1000 / 1.0; + if(microseconds > 0) { + component->getTimerManager()->scheduleTimer(triggerPtr, (void *) 0, std::chrono::microseconds(microseconds), std::chrono::microseconds(microseconds)); + triggerPtr->attach(baseFakeActivity); + // store trigger in class member + baseFakeActivityTrigger = triggerPtr; + } else { + std::cerr << "ERROR: could not set-up Timer with cycle-time " << microseconds << " as activation source for Task BaseFakeActivity" << std::endl; + } + } + + + // link observers with subjects + } catch (const std::exception &ex) { + std::cerr << "Uncaught std exception" << ex.what() << std::endl; + } catch (...) { + std::cerr << "Uncaught exception" << std::endl; + } +} + +// run the component +void ComponentRosBaseFake::run() +{ + stateSlave->acquire("init"); + // startup all registered port-factories + for(auto portFactory = portFactoryRegistry.begin(); portFactory != portFactoryRegistry.end(); portFactory++) + { + portFactory->second->onStartup(); + } + + // startup all registered component-extensions + for(auto extension = componentExtensionRegistry.begin(); extension != componentExtensionRegistry.end(); extension++) + { + extension->second->onStartup(); + } + stateSlave->release("init"); + + // do not call this handler within the init state (see above) as this handler internally calls setStartupFinished() (this should be fixed in future) + compHandler.onStartup(); + + // this call blocks until the component is commanded to shutdown + stateSlave->acquire("shutdown"); + + // shutdown all registered component-extensions + for(auto extension = componentExtensionRegistry.begin(); extension != componentExtensionRegistry.end(); extension++) + { + extension->second->onShutdown(); + } + + // shutdown all registered port-factories + for(auto portFactory = portFactoryRegistry.begin(); portFactory != portFactoryRegistry.end(); portFactory++) + { + portFactory->second->onShutdown(); + } + + if(connections.component.useLogger == true) { + //FIXME: use logging + //Smart::LOGGER->stopLogging(); + } + + compHandler.onShutdown(); + + stateSlave->release("shutdown"); +} + +// clean-up component's resources +void ComponentRosBaseFake::fini() +{ + // unlink all observers + + // destroy all task instances + // unlink all UpcallManagers + navigationVelocityServiceInUpcallManager->detach(baseFakeActivity); + // unlink the TaskTrigger + if(baseFakeActivityTrigger != NULL){ + baseFakeActivityTrigger->detach(baseFakeActivity); + delete baseFakeActivity; + } + + // destroy all input-handler + + // destroy InputTaskTriggers and UpcallManagers + delete navigationVelocityServiceInInputTaskTrigger; + delete navigationVelocityServiceInUpcallManager; + + // destroy client ports + + // destroy server ports + delete baseStateServiceOut; + delete navigationVelocityServiceIn; + // destroy event-test handlers (if needed) + + // destroy request-handlers + + delete stateSlave; + // destroy state-change-handler + delete stateChangeHandler; + + // destroy all master/slave ports + delete wiringSlave; + + + // destroy all registered component-extensions + for(auto extension = componentExtensionRegistry.begin(); extension != componentExtensionRegistry.end(); extension++) + { + extension->second->destroy(); + } + + // destroy all registered port-factories + for(auto portFactory = portFactoryRegistry.begin(); portFactory != portFactoryRegistry.end(); portFactory++) + { + portFactory->second->destroy(); + } + + // destruction of ComponentRosBaseFakeROSExtension + + // destruction of OpcUaBackendComponentGeneratorExtension + + // destruction of PlainOpcUaComponentRosBaseFakeExtension + +} + +void ComponentRosBaseFake::loadParameter(int argc, char *argv[]) +{ + /* + Parameters can be specified via command line --filename= or -f + + With this parameter present: + - The component will look for the file in the current working directory, + a path relative to the current directory or any absolute path + - The component will use the default values if the file cannot be found + + With this parameter absent: + - .ini will be read from current working directory, if found there + - $SMART_ROOT/etc/.ini will be read otherwise + - Default values will be used if neither found in working directory or /etc + */ + SmartACE::SmartIniParameter parameter; + std::ifstream parameterfile; + bool parameterFileFound = false; + + // load parameters + try + { + // if paramfile is given as argument + if(parameter.tryAddFileFromArgs(argc,argv,"filename", 'f')) + { + parameterFileFound = true; + std::cout << "parameter file is loaded from an argv argument \n"; + } else if(parameter.searchFile("ComponentRosBaseFake.ini", parameterfile)) { + parameterFileFound = true; + std::cout << "load ComponentRosBaseFake.ini parameter file\n"; + parameter.addFile(parameterfile); + } else { + std::cout << "WARNING: ComponentRosBaseFake.ini parameter file not found! (using default values or command line arguments)\n"; + } + + // add command line arguments to allow overwriting of parameters + // from file + parameter.addCommandLineArgs(argc,argv,"component"); + + // initialize the naming service using the command line parameters parsed in the + // SmartIniParameter class. The naming service parameters are expected to be in + // the "component" parameter group. + SmartACE::NAMING::instance()->checkForHelpArg(argc,argv); + if(parameterFileFound) + { + if(SmartACE::NAMING::instance()->init(parameter.getAllParametersFromGroup("component")) != 0) { + // initialization of naming service failed + throw std::logic_error( " Service initialization failed!\nPossible causes could be:\n-> Erroneous configuration.\n-> Naming service not reachable.\n" ); + } + } else { + if(SmartACE::NAMING::instance()->init(argc, argv) != 0) { + // initialization of naming service failed + throw std::logic_error( " Service initialization failed!\nPossible causes could be:\n-> Erroneous configuration.\n-> Naming service not reachable.\n" ); + } + } + + // print all known parameters + // parameter.print(); + + //--- server port // client port // other parameter --- + // load parameter + parameter.getString("component", "name", connections.component.name); + parameter.getString("component", "initialComponentMode", connections.component.initialComponentMode); + if(parameter.checkIfParameterExists("component", "defaultScheduler")) { + parameter.getString("component", "defaultScheduler", connections.component.defaultScheduler); + } + if(parameter.checkIfParameterExists("component", "useLogger")) { + parameter.getBoolean("component", "useLogger", connections.component.useLogger); + } + + + // load parameters for server BaseStateServiceOut + parameter.getString("BaseStateServiceOut", "serviceName", connections.baseStateServiceOut.serviceName); + if(parameter.checkIfParameterExists("BaseStateServiceOut", "roboticMiddleware")) { + parameter.getString("BaseStateServiceOut", "roboticMiddleware", connections.baseStateServiceOut.roboticMiddleware); + } + // load parameters for server NavigationVelocityServiceIn + parameter.getString("NavigationVelocityServiceIn", "serviceName", connections.navigationVelocityServiceIn.serviceName); + if(parameter.checkIfParameterExists("NavigationVelocityServiceIn", "roboticMiddleware")) { + parameter.getString("NavigationVelocityServiceIn", "roboticMiddleware", connections.navigationVelocityServiceIn.roboticMiddleware); + } + + // load parameters for task BaseFakeActivity + parameter.getDouble("BaseFakeActivity", "minActFreqHz", connections.baseFakeActivity.minActFreq); + parameter.getDouble("BaseFakeActivity", "maxActFreqHz", connections.baseFakeActivity.maxActFreq); + parameter.getString("BaseFakeActivity", "triggerType", connections.baseFakeActivity.trigger); + if(connections.baseFakeActivity.trigger == "PeriodicTimer") { + parameter.getDouble("BaseFakeActivity", "periodicActFreqHz", connections.baseFakeActivity.periodicActFreq); + } else if(connections.baseFakeActivity.trigger == "DataTriggered") { + parameter.getString("BaseFakeActivity", "inPortRef", connections.baseFakeActivity.inPortRef); + parameter.getInteger("BaseFakeActivity", "prescale", connections.baseFakeActivity.prescale); + } + if(parameter.checkIfParameterExists("BaseFakeActivity", "scheduler")) { + parameter.getString("BaseFakeActivity", "scheduler", connections.baseFakeActivity.scheduler); + } + if(parameter.checkIfParameterExists("BaseFakeActivity", "priority")) { + parameter.getInteger("BaseFakeActivity", "priority", connections.baseFakeActivity.priority); + } + if(parameter.checkIfParameterExists("BaseFakeActivity", "cpuAffinity")) { + parameter.getInteger("BaseFakeActivity", "cpuAffinity", connections.baseFakeActivity.cpuAffinity); + } + + // load parameters for ComponentRosBaseFakeROSExtension + + // load parameters for OpcUaBackendComponentGeneratorExtension + + // load parameters for PlainOpcUaComponentRosBaseFakeExtension + + + // load parameters for all registered component-extensions + for(auto extension = componentExtensionRegistry.begin(); extension != componentExtensionRegistry.end(); extension++) + { + extension->second->loadParameters(parameter); + } + + + } catch (const SmartACE::IniParameterError & e) { + std::cerr << e.what() << std::endl; + } catch (const std::exception &ex) { + std::cerr << "Uncaught std::exception: " << ex.what() << std::endl; + } catch (...) { + std::cerr << "Uncaught exception" << std::endl; + } +} diff --git a/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFake.hh b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFake.hh new file mode 100644 index 00000000..a8ce84f0 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFake.hh @@ -0,0 +1,237 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _COMPONENTROSBASEFAKE_HH +#define _COMPONENTROSBASEFAKE_HH + +#include +#include +#include "aceSmartSoft.hh" +#include "smartQueryServerTaskTrigger_T.h" +#include "ComponentRosBaseFakeCore.hh" + +#include "ComponentRosBaseFakePortFactoryInterface.hh" +#include "ComponentRosBaseFakeExtension.hh" + +// forward declarations +class ComponentRosBaseFakePortFactoryInterface; +class ComponentRosBaseFakeExtension; + +// includes for ComponentRosBaseFakeROSExtension +#include "ComponentRosBaseFakeRosPortBaseClass.hh" + +// includes for OpcUaBackendComponentGeneratorExtension + +// includes for PlainOpcUaComponentRosBaseFakeExtension +// include plain OPC UA device clients +// include plain OPC UA status servers + + +// include communication objects +#include +#include +#include +#include + +// include tasks +#include "BaseFakeActivity.hh" +// include UpcallManagers +#include "NavigationVelocityServiceInUpcallManager.hh" + +// include input-handler(s) +// include request-handler(s) + +// include handler +#include "CompHandler.hh" + + +#include "SmartStateChangeHandler.hh" + +#define COMP ComponentRosBaseFake::instance() + +class ComponentRosBaseFake : public ComponentRosBaseFakeCore { +private: + static ComponentRosBaseFake *_componentRosBaseFake; + + // constructor + ComponentRosBaseFake(); + + // copy-constructor + ComponentRosBaseFake(const ComponentRosBaseFake& cc); + + // destructor + ~ComponentRosBaseFake() { }; + + // load parameter from ini file + void loadParameter(int argc, char* argv[]); + + // instantiate comp-handler + CompHandler compHandler; + + // helper method that maps a string-name to an according TaskTriggerSubject + Smart::TaskTriggerSubject* getInputTaskTriggerFromString(const std::string &client); + + // internal map storing the different port-creation factories (that internally map to specific middleware implementations) + std::map portFactoryRegistry; + + // internal map storing various extensions of this component class + std::map componentExtensionRegistry; + +public: + + // define tasks + Smart::TaskTriggerSubject* baseFakeActivityTrigger; + BaseFakeActivity *baseFakeActivity; + + // define input-ports + // InputPort NavigationVelocityServiceIn + Smart::ISendServerPattern *navigationVelocityServiceIn; + Smart::InputTaskTrigger *navigationVelocityServiceInInputTaskTrigger; + NavigationVelocityServiceInUpcallManager *navigationVelocityServiceInUpcallManager; + + // define request-ports + + // define input-handler + + // define output-ports + Smart::IPushServerPattern *baseStateServiceOut; + + // define answer-ports + + // define request-handlers + + // definitions of ComponentRosBaseFakeROSExtension + ComponentRosBaseFakeRosPortBaseClass *rosPorts; + + // definitions of OpcUaBackendComponentGeneratorExtension + + // definitions of PlainOpcUaComponentRosBaseFakeExtension + + + // define default slave ports + SmartACE::StateSlave *stateSlave; + SmartStateChangeHandler *stateChangeHandler; + SmartACE::WiringSlave *wiringSlave; + + + /// this method is used to register different PortFactory classes (one for each supported middleware framework) + void addPortFactory(const std::string &name, ComponentRosBaseFakePortFactoryInterface *portFactory); + + SmartACE::SmartComponent* getComponentImpl(); + + /// this method is used to register different component-extension classes + void addExtension(ComponentRosBaseFakeExtension *extension); + + /// this method allows to access the registered component-extensions (automatically converting to the actuall implementation type) + template + T* getExtension(const std::string &name) { + auto it = componentExtensionRegistry.find(name); + if(it != componentExtensionRegistry.end()) { + return dynamic_cast(it->second); + } + return 0; + } + + /// initialize component's internal members + void init(int argc, char *argv[]); + + /// execute the component's infrastructure + void run(); + + /// clean-up component's resources + void fini(); + + /// call this method to set the overall component into the Alive state (i.e. component is then ready to operate) + void setStartupFinished(); + + /// connect all component's client ports + Smart::StatusCode connectAndStartAllServices(); + + /// start all assocuated Activities + void startAllTasks(); + + /// start all associated timers + void startAllTimers(); + + + // return singleton instance + static ComponentRosBaseFake* instance() + { + if(_componentRosBaseFake == 0) { + _componentRosBaseFake = new ComponentRosBaseFake(); + } + return _componentRosBaseFake; + } + + static void deleteInstance() { + if(_componentRosBaseFake != 0) { + delete _componentRosBaseFake; + } + } + + // connections parameter + struct connections_struct + { + // component struct + struct component_struct + { + // the name of the component + std::string name; + std::string initialComponentMode; + std::string defaultScheduler; + bool useLogger; + } component; + + //--- task parameter --- + struct BaseFakeActivity_struct { + double minActFreq; + double maxActFreq; + std::string trigger; + // only one of the following two params is + // actually used at run-time according + // to the system config model + double periodicActFreq; + // or + std::string inPortRef; + int prescale; + // scheduling parameters + std::string scheduler; + int priority; + int cpuAffinity; + } baseFakeActivity; + + //--- upcall parameter --- + + //--- server port parameter --- + struct BaseStateServiceOut_struct { + std::string serviceName; + std::string roboticMiddleware; + } baseStateServiceOut; + struct NavigationVelocityServiceIn_struct { + std::string serviceName; + std::string roboticMiddleware; + } navigationVelocityServiceIn; + + //--- client port parameter --- + + // -- parameters for ComponentRosBaseFakeROSExtension + + // -- parameters for OpcUaBackendComponentGeneratorExtension + + // -- parameters for PlainOpcUaComponentRosBaseFakeExtension + + } connections; +}; +#endif diff --git a/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFake.ini.head b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFake.ini.head new file mode 100644 index 00000000..ec1ae723 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFake.ini.head @@ -0,0 +1,79 @@ +[smartsoft] +#-------------------------------------------------------------------------- +# Code generated by the SmartSoft MDSD Toolchain +# The SmartSoft Toolchain has been developed by: +# +# Service Robotics Research Center +# University of Applied Sciences Ulm +# Prittwitzstr. 10 +# 89075 Ulm (Germany) +# +# Information about the SmartSoft MDSD Toolchain is available at: +# www.servicerobotik-ulm.de +# +# Please do not modify this file. It will be re-generated +# running the code generator. +#-------------------------------------------------------------------------- + +########################## +# component parameters + +[component] + +# the name of the component for the naming service +name ComponentRosBaseFake + +# the initial ComponentMode +initialComponentMode Neutral +#defaultScheduler FIFO +#useLogger true + +########################## +# Task parameters + +[BaseFakeActivity] +minActFreqHz 0.0 +maxActFreqHz 0.0 +# setup default trigger as PeriodicTimer +triggerType PeriodicTimer +periodicActFreqHz 1.0 +# other trigger-types are: +#triggerType PeriodicTimer +#periodicActFreqHz 0.0 +# or alternatively: +#triggerType DataTriggered +#inPortRef +#prescale 1 +# optional scheduling parameters +# scheduler FIFO +# priority 0 +# cpuAffinity 0 + +########################## +# InputHandler parameters + +########################## +# server port parameter + +[BaseStateServiceOut] +serviceName BaseStateServiceOut +roboticMiddleware ACE_SmartSoft + +[NavigationVelocityServiceIn] +serviceName NavigationVelocityServiceIn +roboticMiddleware ACE_SmartSoft + +########################## +# client port parameter + +########################## +## parameters of ComponentRosBaseFakeROSExtension + +########################## +## parameters of OpcUaBackendComponentGeneratorExtension + +########################## +## parameters of PlainOpcUaComponentRosBaseFakeExtension + + + diff --git a/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeAcePortFactory.cc b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeAcePortFactory.cc new file mode 100644 index 00000000..34d9d6ff --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeAcePortFactory.cc @@ -0,0 +1,83 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#include "ComponentRosBaseFakeAcePortFactory.hh" + +// create a static instance of the default AcePortFactory +static ComponentRosBaseFakeAcePortFactory acePortFactory; + +ComponentRosBaseFakeAcePortFactory::ComponentRosBaseFakeAcePortFactory() +{ + componentImpl = 0; + ComponentRosBaseFake::instance()->addPortFactory("ACE_SmartSoft", this); +} + +ComponentRosBaseFakeAcePortFactory::~ComponentRosBaseFakeAcePortFactory() +{ } + +void ComponentRosBaseFakeAcePortFactory::initialize(ComponentRosBaseFake *component, int argc, char* argv[]) +{ + if(component->connections.component.defaultScheduler != "DEFAULT") { + ACE_Sched_Params sched_params(ACE_SCHED_OTHER, ACE_THR_PRI_OTHER_DEF); + if(component->connections.component.defaultScheduler == "FIFO") { + sched_params.policy(ACE_SCHED_FIFO); + sched_params.priority(ACE_THR_PRI_FIFO_MIN); + } else if(component->connections.component.defaultScheduler == "RR") { + sched_params.policy(ACE_SCHED_RR); + sched_params.priority(ACE_THR_PRI_RR_MIN); + } + // create new instance of the SmartSoft component with customized scheuling parameters + componentImpl = new ComponentRosBaseFakeImpl(component->connections.component.name, argc, argv, sched_params); + } else { + // create new instance of the SmartSoft component + componentImpl = new ComponentRosBaseFakeImpl(component->connections.component.name, argc, argv); + } +} + +int ComponentRosBaseFakeAcePortFactory::onStartup() +{ + return componentImpl->startComponentInfrastructure(); +} + + +Smart::IPushServerPattern * ComponentRosBaseFakeAcePortFactory::createBaseStateServiceOut(const std::string &serviceName) +{ + return new SmartACE::PushServer(componentImpl, serviceName); +} + +Smart::ISendServerPattern * ComponentRosBaseFakeAcePortFactory::createNavigationVelocityServiceIn(const std::string &serviceName) +{ + return new SmartACE::SendServer(componentImpl, serviceName); +} + + +SmartACE::SmartComponent* ComponentRosBaseFakeAcePortFactory::getComponentImpl() +{ + return componentImpl; +} + +int ComponentRosBaseFakeAcePortFactory::onShutdown(const std::chrono::steady_clock::duration &timeoutTime) +{ + componentImpl->stopComponentInfrastructure(timeoutTime); + return 0; +} + +void ComponentRosBaseFakeAcePortFactory::destroy() +{ + // clean-up component's internally used resources (internally used communication middleware) + componentImpl->cleanUpComponentResources(); + delete componentImpl; +} diff --git a/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeAcePortFactory.hh b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeAcePortFactory.hh new file mode 100644 index 00000000..6d0d6436 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeAcePortFactory.hh @@ -0,0 +1,48 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef COMPONENTROSBASEFAKE_ACE_PORTFACTORY_HH_ +#define COMPONENTROSBASEFAKE_ACE_PORTFACTORY_HH_ + +// include ACE/SmartSoft component implementation +#include "ComponentRosBaseFakeImpl.hh" + +// include the main component-definition class +#include "ComponentRosBaseFakePortFactoryInterface.hh" + +class ComponentRosBaseFakeAcePortFactory: public ComponentRosBaseFakePortFactoryInterface +{ +private: + ComponentRosBaseFakeImpl *componentImpl; +public: + ComponentRosBaseFakeAcePortFactory(); + virtual ~ComponentRosBaseFakeAcePortFactory(); + + virtual void initialize(ComponentRosBaseFake *component, int argc, char* argv[]) override; + virtual int onStartup() override; + + + virtual Smart::IPushServerPattern * createBaseStateServiceOut(const std::string &serviceName) override; + virtual Smart::ISendServerPattern * createNavigationVelocityServiceIn(const std::string &serviceName) override; + + // get a pointer to the internal component implementation + SmartACE::SmartComponent* getComponentImpl(); + + virtual int onShutdown(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)) override; + virtual void destroy() override; +}; + +#endif /* COMPONENTROSBASEFAKE_ACE_PORTFACTORY_HH_ */ diff --git a/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeDependencies.cmake.in b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeDependencies.cmake.in new file mode 100644 index 00000000..27f5ecca --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeDependencies.cmake.in @@ -0,0 +1,20 @@ +#-------------------------------------------------------------------------- +# Code generated by the SmartSoft MDSD Toolchain +# The SmartSoft Toolchain has been developed by: +# +# Service Robotics Research Center +# University of Applied Sciences Ulm +# Prittwitzstr. 10 +# 89075 Ulm (Germany) +# +# Information about the SmartSoft MDSD Toolchain is available at: +# www.servicerobotik-ulm.de +# +# Please do not modify this file. It will be re-generated +# running the code generator. +#-------------------------------------------------------------------------- + +# other CommunicationObject-dependencies are added here (if there are any) +LIST(APPEND ComponentRosBaseFake_DEPENDENCIES CommBasicObjects) + +LIST(REMOVE_DUPLICATES ComponentRosBaseFake_DEPENDENCIES) diff --git a/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeExtension.cc b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeExtension.cc new file mode 100644 index 00000000..b2c8e773 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeExtension.cc @@ -0,0 +1,62 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#include "ComponentRosBaseFakeExtension.hh" + +ComponentRosBaseFakeExtension::ComponentRosBaseFakeExtension(const std::string &name) +: extension_name(name) +{ + cancelled = false; + COMP->addExtension(this); +} + +ComponentRosBaseFakeExtension::~ComponentRosBaseFakeExtension() +{ } + +int ComponentRosBaseFakeExtension::startExtensionThread() +{ + cancelled = false; + // execute the task_execution() method in a new thread now + extension_future = std::async(std::launch::async, &ComponentRosBaseFakeExtension::extensionExecution, this); + return 0; +} + +int ComponentRosBaseFakeExtension::stopExtensionThread(const std::chrono::steady_clock::duration &timeoutTime) +{ + cancelled = true; + // wait on extension thread to exit + if (extension_future.wait_for(timeoutTime) == std::future_status::timeout) { + return -1; + } + return 0; +} + +void ComponentRosBaseFakeExtension::loadParameters(const SmartACE::SmartIniParameter ¶meter) +{ + // no-op +} + +int ComponentRosBaseFakeExtension::onStartup() +{ + // default implementation just starts the internal thread + return startExtensionThread(); +} + +int ComponentRosBaseFakeExtension::onShutdown(const std::chrono::steady_clock::duration &timeoutTime) +{ + // default implementation stops the internal thread waiting up to timeoutTime until it is stopped (or a timeout occurs) + return stopExtensionThread(timeoutTime); +} diff --git a/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeExtension.hh b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeExtension.hh new file mode 100644 index 00000000..2f42ec33 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeExtension.hh @@ -0,0 +1,65 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef COMPONENTROSBASEFAKE_EXTENSION_HH_ +#define COMPONENTROSBASEFAKE_EXTENSION_HH_ + +#include +#include +#include +#include + +// include component's main class +#include "ComponentRosBaseFake.hh" + +// forward declaration +class ComponentRosBaseFake; + +class ComponentRosBaseFakeExtension { +private: + std::string extension_name; + std::future extension_future; + +protected: + /// use this variable within extensionExecution() to check if the thread is commanded to shutdown + std::atomic cancelled; + + /// use this method to start internal extension thread + virtual int startExtensionThread(); + + /// implement this method in derived classes + virtual int extensionExecution() = 0; + + /// use this method to start internal extension thread + virtual int stopExtensionThread(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)); +public: + ComponentRosBaseFakeExtension(const std::string &name); + virtual ~ComponentRosBaseFakeExtension(); + + inline std::string getName() const { + return extension_name; + } + + virtual void loadParameters(const SmartACE::SmartIniParameter ¶meter); + virtual void initialize(ComponentRosBaseFake *component, int argc, char* argv[]) = 0; + virtual int onStartup(); + + virtual int onShutdown(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)); + virtual void destroy() = 0; + +}; + +#endif /* COMPONENTROSBASEFAKE_EXTENSION_HH_ */ diff --git a/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeImpl.cc b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeImpl.cc new file mode 100644 index 00000000..a80c4777 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeImpl.cc @@ -0,0 +1,48 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include "ComponentRosBaseFakeImpl.hh" + +ComponentRosBaseFakeImpl::ComponentRosBaseFakeImpl(const std::string &componentName, int & argc, char ** argv) +: SmartACE::SmartComponent(componentName, argc, argv) +{ } + +ComponentRosBaseFakeImpl::ComponentRosBaseFakeImpl(const std::string &componentName, int & argc, char ** argv, const ACE_Sched_Params &sched_params) +: SmartACE::SmartComponent(componentName, argc, argv, NULL, sched_params) +{ } + +ComponentRosBaseFakeImpl::~ComponentRosBaseFakeImpl() +{ } + +int ComponentRosBaseFakeImpl::startComponentInfrastructure(void) +{ + // open thread for managing all server-initiated-disconnects in this component + srvInitDiscHandler.start(); + + // if meanwile strg+c was called -> return immediatelly, without waiting on substate or reactor + if(!first_call_of_handle_signal) return -1; + + componentIsRunning = true; + return 0; +} + +void ComponentRosBaseFakeImpl::stopComponentInfrastructure(const std::chrono::steady_clock::duration &timeoutTime) +{ + this->signalSmartTasksToStop(timeoutTime); +} +void ComponentRosBaseFakeImpl::cleanUpComponentResources() +{ + this->cleanUpInternalResources(); +} diff --git a/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeImpl.hh b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeImpl.hh new file mode 100644 index 00000000..b09fd05f --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakeImpl.hh @@ -0,0 +1,33 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _COMPONENTROSBASEFAKEIMPL_HH +#define _COMPONENTROSBASEFAKEIMPL_HH + +#include +#include "aceSmartSoft.hh" + +class ComponentRosBaseFakeImpl : public SmartACE::SmartComponent { +public: + ComponentRosBaseFakeImpl(const std::string &componentName, int & argc, char ** argv); + ComponentRosBaseFakeImpl(const std::string &componentName, int & argc, char ** argv, const ACE_Sched_Params &sched_params); + virtual ~ComponentRosBaseFakeImpl(); + + int startComponentInfrastructure(); + void stopComponentInfrastructure(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)); + void cleanUpComponentResources(); +}; + +#endif diff --git a/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakePortFactoryInterface.hh b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakePortFactoryInterface.hh new file mode 100644 index 00000000..4f72497c --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/ComponentRosBaseFakePortFactoryInterface.hh @@ -0,0 +1,50 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef COMPONENTROSBASEFAKE_PORTFACTORYINTERFACE_HH_ +#define COMPONENTROSBASEFAKE_PORTFACTORYINTERFACE_HH_ + +// include communication objects +#include +#include +#include +#include + +#include + +// include component's main class +#include "ComponentRosBaseFake.hh" + +// forward declaration +class ComponentRosBaseFake; + +class ComponentRosBaseFakePortFactoryInterface { +public: + ComponentRosBaseFakePortFactoryInterface() { }; + virtual ~ComponentRosBaseFakePortFactoryInterface() { }; + + virtual void initialize(ComponentRosBaseFake *component, int argc, char* argv[]) = 0; + virtual int onStartup() = 0; + + + virtual Smart::IPushServerPattern * createBaseStateServiceOut(const std::string &serviceName) = 0; + virtual Smart::ISendServerPattern * createNavigationVelocityServiceIn(const std::string &serviceName) = 0; + + virtual int onShutdown(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)) = 0; + virtual void destroy() = 0; +}; + +#endif /* COMPONENTROSBASEFAKE_PORTFACTORYINTERFACE_HH_ */ diff --git a/ComponentRosBaseFake/smartsoft/src-gen/MixedPortIncludes.cmake b/ComponentRosBaseFake/smartsoft/src-gen/MixedPortIncludes.cmake new file mode 100644 index 00000000..546211bb --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/MixedPortIncludes.cmake @@ -0,0 +1,23 @@ +# includes for ComponentRosBaseFakeROSExtension +GET_FILENAME_COMPONENT(ROS_DIR "${PROJECT_SOURCE_DIR}/../ROS" REALPATH) +IF(EXISTS ${ROS_DIR}) +ADD_SUBDIRECTORY(${ROS_DIR}/src-gen/ ${PROJECT_BINARY_DIR}/ROS) +LIST(APPEND FURTHER_SRCS ${ROS_SRCS}) +INCLUDE_DIRECTORIES(${ROS_INCLUDES}) +ENDIF(EXISTS ${ROS_DIR}) + +# includes for OpcUaBackendComponentGeneratorExtension +GET_FILENAME_COMPONENT(OPC_UA_BACKEND_DIR "${PROJECT_SOURCE_DIR}/../opcua-backend" REALPATH) +IF(EXISTS ${OPC_UA_BACKEND_DIR}) + INCLUDE("${OPC_UA_BACKEND_DIR}/src-gen/OpcUaBackend.cmake") + LIST(APPEND FURTHER_SRCS ${OPC_UA_BACKEND_SRCS}) +ENDIF(EXISTS ${OPC_UA_BACKEND_DIR}) + +# includes for PlainOpcUaComponentRosBaseFakeExtension +GET_FILENAME_COMPONENT(PlainOPCUA_DIR "${PROJECT_SOURCE_DIR}/../plainOpcUa" REALPATH) +IF(EXISTS ${PlainOPCUA_DIR}) +INCLUDE("${PlainOPCUA_DIR}/src-gen/ComponentRosBaseFakePlainOpcUa.cmake") +LIST(APPEND FURTHER_SRCS ${PLAIN_OPCUA_SRCS}) +ENDIF(EXISTS ${PlainOPCUA_DIR}) + + diff --git a/ComponentRosBaseFake/smartsoft/src-gen/MixedPortTargetConfigs.cmake b/ComponentRosBaseFake/smartsoft/src-gen/MixedPortTargetConfigs.cmake new file mode 100644 index 00000000..9f070510 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/MixedPortTargetConfigs.cmake @@ -0,0 +1,17 @@ +# target configurations for ComponentRosBaseFakeROSExtension +IF(EXISTS ${ROS_DIR}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ROS_LIBS}) +ENDIF(EXISTS ${ROS_DIR}) + +# target configurations for OpcUaBackendComponentGeneratorExtension +IF(SeRoNetSDK_FOUND) +#TARGET_LINK_LIBRARIES(${PROJECT_NAME} SeRoNetSDK::SeRoNetSDK) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} CommBasicObjectsOpcUa) +ENDIF(SeRoNetSDK_FOUND) + +# target configurations for PlainOpcUaComponentRosBaseFakeExtension +IF(Open62541CppWrapper_FOUND) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} Open62541CppWrapper) +TARGET_COMPILE_DEFINITIONS(${PROJECT_NAME} PUBLIC HAS_OPCUA) +ENDIF(Open62541CppWrapper_FOUND) + diff --git a/ComponentRosBaseFake/smartsoft/src-gen/NavigationVelocityServiceInUpcallInterface.hh b/ComponentRosBaseFake/smartsoft/src-gen/NavigationVelocityServiceInUpcallInterface.hh new file mode 100644 index 00000000..c8e18d12 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/NavigationVelocityServiceInUpcallInterface.hh @@ -0,0 +1,28 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _NAVIGATIONVELOCITYSERVICEIN_UPCALL_INTERFACE_HH +#define _NAVIGATIONVELOCITYSERVICEIN_UPCALL_INTERFACE_HH + +#include "CommBasicObjects/CommNavigationVelocity.hh" + +class NavigationVelocityServiceInUpcallInterface { +public: + virtual ~NavigationVelocityServiceInUpcallInterface() { } + + virtual void on_NavigationVelocityServiceIn(const CommBasicObjects::CommNavigationVelocity &input) = 0; +}; + +#endif diff --git a/ComponentRosBaseFake/smartsoft/src-gen/NavigationVelocityServiceInUpcallManager.cc b/ComponentRosBaseFake/smartsoft/src-gen/NavigationVelocityServiceInUpcallManager.cc new file mode 100644 index 00000000..5973eedd --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/NavigationVelocityServiceInUpcallManager.cc @@ -0,0 +1,40 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include "NavigationVelocityServiceInUpcallManager.hh" + +NavigationVelocityServiceInUpcallManager::NavigationVelocityServiceInUpcallManager( + Smart::InputSubject *subject, + const int &prescaleFactor) + : Smart::IInputHandler(subject, prescaleFactor) +{ } +NavigationVelocityServiceInUpcallManager::~NavigationVelocityServiceInUpcallManager() +{ } + +void NavigationVelocityServiceInUpcallManager::notify_upcalls(const CommBasicObjects::CommNavigationVelocity &input) +{ + for(auto it=upcalls.begin(); it!=upcalls.end(); it++) { + (*it)->on_NavigationVelocityServiceIn(input); + } +} + +void NavigationVelocityServiceInUpcallManager::attach(NavigationVelocityServiceInUpcallInterface *upcall) +{ + upcalls.push_back(upcall); +} +void NavigationVelocityServiceInUpcallManager::detach(NavigationVelocityServiceInUpcallInterface *upcall) +{ + upcalls.remove(upcall); +} diff --git a/ComponentRosBaseFake/smartsoft/src-gen/NavigationVelocityServiceInUpcallManager.hh b/ComponentRosBaseFake/smartsoft/src-gen/NavigationVelocityServiceInUpcallManager.hh new file mode 100644 index 00000000..c69a852a --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/NavigationVelocityServiceInUpcallManager.hh @@ -0,0 +1,54 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _NAVIGATIONVELOCITYSERVICEIN_UPCALL_MANAGER_HH +#define _NAVIGATIONVELOCITYSERVICEIN_UPCALL_MANAGER_HH + +#include +#include "aceSmartSoft.hh" +#include "NavigationVelocityServiceInUpcallInterface.hh" + +/** NavigationVelocityServiceInUpcallManager connects input-handling with Upcall propagation + * + * This class implements an InputHandler for the InputPort NavigationVelocityServiceIn and propagates the handling + * of incoming data to all associated (i.e. attached) Upcalls. + */ +class NavigationVelocityServiceInUpcallManager +: public Smart::IInputHandler +{ +private: + // list of associated updalls + std::list upcalls; + + // call the on_NavigationVelocityServiceIn of all the attached NavigationVelocityServiceInUpcallInterfaces + void notify_upcalls(const CommBasicObjects::CommNavigationVelocity &input); + +protected: + virtual void handle_input(const CommBasicObjects::CommNavigationVelocity &input) { + // relay input-handling to all attached NavigationVelocityServiceInUpcallInterfaces + this->notify_upcalls(input); + } +public: + NavigationVelocityServiceInUpcallManager( + Smart::InputSubject *subject, + const int &prescaleFactor=1 + ); + virtual ~NavigationVelocityServiceInUpcallManager(); + + void attach(NavigationVelocityServiceInUpcallInterface *upcall); + void detach(NavigationVelocityServiceInUpcallInterface *upcall); +}; + +#endif diff --git a/ComponentRosBaseFake/smartsoft/src-gen/SmartStateChangeHandlerCore.hh b/ComponentRosBaseFake/smartsoft/src-gen/SmartStateChangeHandlerCore.hh new file mode 100644 index 00000000..b2214ca3 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/SmartStateChangeHandlerCore.hh @@ -0,0 +1,27 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _SMARTSTATECHANGEHANDLERCORE_HH +#define _SMARTSTATECHANGEHANDLERCORE_HH + +#include "aceSmartSoft.hh" + +class SmartStateChangeHandlerCore : public SmartACE::StateChangeHandler +{ +public: + void handleEnterState(const std::string & substate) throw() { }; + void handleQuitState(const std::string & substate) throw() { }; +}; +#endif diff --git a/ComponentRosBaseFake/smartsoft/src-gen/info.txt b/ComponentRosBaseFake/smartsoft/src-gen/info.txt new file mode 100644 index 00000000..93cdd389 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/info.txt @@ -0,0 +1,17 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +// Generated with SmartMDSD Toolchain Version 3.12.0.201912041712 diff --git a/ComponentRosBaseFake/smartsoft/src-gen/main.cc b/ComponentRosBaseFake/smartsoft/src-gen/main.cc new file mode 100644 index 00000000..6efed331 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src-gen/main.cc @@ -0,0 +1,32 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include +#include "ComponentRosBaseFake.hh" + +int main(int argc, char *argv[]) +{ + std::cout << "main...\n"; + // initialize component infrastructure (loading ini-file, creating ports, tasks, etc.) + ComponentRosBaseFake::instance()->init(argc, argv); + // run component infrastructure until the component is commanded to shutdown + ComponentRosBaseFake::instance()->run(); + // clean-up component's internal resources (deleting ports, tasks, etc.) + ComponentRosBaseFake::instance()->fini(); + // destroy the component's singleton + ComponentRosBaseFake::deleteInstance(); + std::cout << "... main() end. return 0." << std::endl; + return 0; +} diff --git a/ComponentRosBaseFake/smartsoft/src/BaseFakeActivity.cc b/ComponentRosBaseFake/smartsoft/src/BaseFakeActivity.cc new file mode 100644 index 00000000..1686971d --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src/BaseFakeActivity.cc @@ -0,0 +1,87 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#include "BaseFakeActivity.hh" +#include "ComponentRosBaseFake.hh" + +#include + +BaseFakeActivity::BaseFakeActivity(SmartACE::SmartComponent *comp) +: BaseFakeActivityCore(comp) +{ + std::cout << "constructor BaseFakeActivity\n"; +} +BaseFakeActivity::~BaseFakeActivity() +{ + std::cout << "destructor BaseFakeActivity\n"; +} + + +void BaseFakeActivity::on_NavigationVelocityServiceIn(const CommBasicObjects::CommNavigationVelocity &input) +{ + // upcall triggered from InputPort NavigationVelocityServiceIn + // - use a local mutex here, because this upcal is called asynchroneously from outside of this task + // - do not use longer blocking calls here since this upcall blocks the InputPort NavigationVelocityServiceIn + // - if you need to implement a long-running procedure, do so within the on_execute() method and in + // there, use the method navigationVelocityServiceInGetUpdate(input) to get a copy of the input object + + geometry_msgs::Twist twist_msg; + twist_msg.linear.x = input.get_vX(); + twist_msg.linear.y = input.get_vY(); + twist_msg.angular.z = input.get_omega(); + + std::cout << "publishing twist: \n" << twist_msg << std::endl; + COMP -> rosPorts -> twist_pub.publish(twist_msg); + //twist_pub.publish(twist); +} + +int BaseFakeActivity::on_entry() +{ + // do initialization procedures here, which are called once, each time the task is started + // it is possible to return != 0 (e.g. when initialization fails) then the task is not executed further + return 0; +} +int BaseFakeActivity::on_execute() +{ + // this method is called from an outside loop, + // hence, NEVER use an infinite loop (like "while(1)") here inside!!! + // also do not use blocking calls which do not result from smartsoft kernel + + // to get the incoming data, use this methods: + Smart::StatusCode status; + CommBasicObjects::CommNavigationVelocity navigationVelocityServiceInObject; + status = this->navigationVelocityServiceInGetUpdate(navigationVelocityServiceInObject); + if(status != Smart::SMART_OK) { + std::cerr << status << std::endl; + // return 0; + } else { + std::cout << "received: " << navigationVelocityServiceInObject << std::endl; + } + + std::cout << "Hello from BaseFakeActivity " << std::endl; + + CommBasicObjects::CommBaseState baseState; + status = this->baseStateServiceOutPut(baseState); + std::cout << "sent baseState :\n " << status << std::endl; + + // it is possible to return != 0 (e.g. when the task detects errors), then the outer loop breaks and the task stops + return 0; +} +int BaseFakeActivity::on_exit() +{ + // use this method to clean-up resources which are initialized in on_entry() and needs to be freed before the on_execute() can be called again + return 0; +} diff --git a/ComponentRosBaseFake/smartsoft/src/BaseFakeActivity.hh b/ComponentRosBaseFake/smartsoft/src/BaseFakeActivity.hh new file mode 100644 index 00000000..22f162c3 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src/BaseFakeActivity.hh @@ -0,0 +1,35 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#ifndef _BASEFAKEACTIVITY_HH +#define _BASEFAKEACTIVITY_HH + +#include "BaseFakeActivityCore.hh" + +class BaseFakeActivity : public BaseFakeActivityCore +{ +private: + virtual void on_NavigationVelocityServiceIn(const CommBasicObjects::CommNavigationVelocity &input); +public: + BaseFakeActivity(SmartACE::SmartComponent *comp); + virtual ~BaseFakeActivity(); + + virtual int on_entry(); + virtual int on_execute(); + virtual int on_exit(); +}; + +#endif diff --git a/ComponentRosBaseFake/smartsoft/src/CompHandler.cc b/ComponentRosBaseFake/smartsoft/src/CompHandler.cc new file mode 100644 index 00000000..b2320fca --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src/CompHandler.cc @@ -0,0 +1,55 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#include "CompHandler.hh" +#include "ComponentRosBaseFake.hh" + +#include + +// include communication objects +#include +#include + + +void CompHandler::onStartup() +{ + std::cout << "startup - put your startupCode in CompHandler::onStartup()!!!\n"; + + Smart::StatusCode status; + + // Start all services. If you need manual control, use the content of this function to + // connect and start each service individually, e.g: + // COMP->connectMyPortName("SmartExampleComponent", "examplePort"); + status = COMP->connectAndStartAllServices(); + + // Start all tasks. If you need manual control, use the content of this function to + // start each task individually. + COMP->startAllTasks(); + + // Start all timers. If you need manual control, use the content of this function to + // start each timer individually. + COMP->startAllTimers(); + + // Notify the component that setup/initialization is finished. + // You may move this function to any other place. + COMP->setStartupFinished(); +} + +void CompHandler::onShutdown() +{ + std::cout << "shutdown - put your cleanup code in CompHandler::onShutdown()!!!\n"; + +} diff --git a/ComponentRosBaseFake/smartsoft/src/CompHandler.hh b/ComponentRosBaseFake/smartsoft/src/CompHandler.hh new file mode 100644 index 00000000..6cf3e54f --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src/CompHandler.hh @@ -0,0 +1,27 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#ifndef _COMPHANDLER_HH +#define _COMPHANDLER_HH + +class CompHandler +{ +public: + void onStartup(); + void onShutdown(); +}; + +#endif diff --git a/ComponentRosBaseFake/smartsoft/src/ComponentRosBaseFakeCore.cc b/ComponentRosBaseFake/smartsoft/src/ComponentRosBaseFakeCore.cc new file mode 100644 index 00000000..824578d3 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src/ComponentRosBaseFakeCore.cc @@ -0,0 +1,23 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#include "ComponentRosBaseFakeCore.hh" + +// constructor +ComponentRosBaseFakeCore::ComponentRosBaseFakeCore() +{ + std::cout << "constructor ComponentRosBaseFakeCore\n"; +} diff --git a/ComponentRosBaseFake/smartsoft/src/ComponentRosBaseFakeCore.hh b/ComponentRosBaseFake/smartsoft/src/ComponentRosBaseFakeCore.hh new file mode 100644 index 00000000..43761f83 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src/ComponentRosBaseFakeCore.hh @@ -0,0 +1,31 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#ifndef _COMPONENTROSBASEFAKECORE_HH +#define _COMPONENTROSBASEFAKECORE_HH + +#include "aceSmartSoft.hh" +#include + +class ComponentRosBaseFakeCore +{ +private: + +public: + ComponentRosBaseFakeCore(); +}; + +#endif diff --git a/ComponentRosBaseFake/smartsoft/src/SmartStateChangeHandler.cc b/ComponentRosBaseFake/smartsoft/src/SmartStateChangeHandler.cc new file mode 100644 index 00000000..da212273 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src/SmartStateChangeHandler.cc @@ -0,0 +1,34 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#include "SmartStateChangeHandler.hh" +#include "ComponentRosBaseFake.hh" + +#include + +// Called when a substate is entered +void SmartStateChangeHandler::handleEnterState(const std::string & substate) throw() +{ + // change this code to your needs !!! + +} + +// Called when a substate is left +void SmartStateChangeHandler::handleQuitState(const std::string & substate) throw() +{ + // change this code to your needs !!! + +} diff --git a/ComponentRosBaseFake/smartsoft/src/SmartStateChangeHandler.hh b/ComponentRosBaseFake/smartsoft/src/SmartStateChangeHandler.hh new file mode 100644 index 00000000..89e804e3 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src/SmartStateChangeHandler.hh @@ -0,0 +1,30 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#ifndef _SMARTSTATECHANGEHANDLER_USER_HH +#define _SMARTSTATECHANGEHANDLER_USER_HH + +#include "aceSmartSoft.hh" + +#include "SmartStateChangeHandlerCore.hh" + +class SmartStateChangeHandler : public SmartStateChangeHandlerCore +{ + void handleEnterState(const std::string & substate) throw(); + void handleQuitState(const std::string & substate) throw(); +}; + +#endif diff --git a/ComponentRosBaseFake/smartsoft/src/predeploy.sh b/ComponentRosBaseFake/smartsoft/src/predeploy.sh new file mode 100644 index 00000000..be981936 --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src/predeploy.sh @@ -0,0 +1,20 @@ +#!/bin/bash + +# Use this variable to name files that are to be deployed to the data +# instance directory of +# the component (_data/) on the target host. +# +# Files can be relative to component project directory or absolute. +# Add one file or directory per line. +# +# Examples: +# +# DEPLOY_LIBRARIES="../bin/libMyLibrary.so" +# +# DEPLOY_COMPONENT_FILES=" +# $SMART_ROOT_ACE/myFILE +# $SMART_ROOT_ACE/myFILE2 +# " + +DEPLOY_LIBRARIES="" +DEPLOY_COMPONENT_FILES="" diff --git a/ComponentRosBaseFake/smartsoft/src/startstop-hooks.sh b/ComponentRosBaseFake/smartsoft/src/startstop-hooks.sh new file mode 100644 index 00000000..04393e2d --- /dev/null +++ b/ComponentRosBaseFake/smartsoft/src/startstop-hooks.sh @@ -0,0 +1,33 @@ +#!/bin/bash + +# This script provides methods to call custom commands pre/post of starting/stoping the component during launch on the device. +# This script is being executed on the target device where the component is running. +# For example the script can be used to start and stop the morse simulator automatically. + +case "$1" in + +pre-start) + echo "Triggering pre-start hooks FROM COMPONENT ComponentRosBaseFake ..." + # Insert commands you want to call prior to starting the components +;; + +post-start) + echo "Triggering post-start hooks FROM COMPONENT ComponentRosBaseFake ..." + # Insert commands you want to call after all components were started +;; + +pre-stop) + echo "Triggering pre-stop hooks FROM COMPONENT ComponentRosBaseFake ..." + # Insert commands you want to call before stopping all components +;; + +post-stop) + echo "Triggering post-stop hooks FROM COMPONENT ComponentRosBaseFake ..." + # Insert commands you want to call after all components were stopped +;; + +*) + echo "ERROR in $0: no such hook '$1'. Usage: $0 pre-start|post-start|pre-stop|post-stop" +;; + +esac diff --git a/ComponentRosDock/.classpath b/ComponentRosDock/.classpath new file mode 100644 index 00000000..6d6fc9aa --- /dev/null +++ b/ComponentRosDock/.classpath @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/ComponentRosDock/.cproject b/ComponentRosDock/.cproject new file mode 100644 index 00000000..ab067d64 --- /dev/null +++ b/ComponentRosDock/.cproject @@ -0,0 +1,133 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ComponentRosDock/.project b/ComponentRosDock/.project new file mode 100644 index 00000000..101d0755 --- /dev/null +++ b/ComponentRosDock/.project @@ -0,0 +1,43 @@ + + + ComponentRosDock + + + CommBasicObjects + CommNavigationObjects + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + + + org.eclipse.jdt.core.javabuilder + + + + + org.eclipse.xtext.ui.shared.xtextBuilder + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + + org.eclipse.sirius.nature.modelingproject + org.eclipse.cdt.core.cnature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + org.smartmdsd.utils.natures.ComponentNature + org.eclipse.xtext.ui.shared.xtextNature + org.eclipse.jdt.core.javanature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + + diff --git a/ComponentRosDock/README.md b/ComponentRosDock/README.md new file mode 100644 index 00000000..c4e29651 --- /dev/null +++ b/ComponentRosDock/README.md @@ -0,0 +1,23 @@ +### Docking + +#### Laserscan Units: +- start_angle: [rad] (-3.14 .. 3.14) +- resolution: [rad] +- distance_min: [m] +- distance_max: [m] + +#### How to use (for testing): +needs `ComponentRosBaseFake` + +- start `roscore` +- start Naming Service `cd $SMART_ROOT_ACE && ./startSmartSoftNamingService` +- start Base Fake `cd $SMART_ROOT_ACE && ./bin/ComponentRosBaseFake` +- start Docking `cd $SMART_ROOT_ACE && ./bin/ComponentRosDock` +- start Robot Console `cd $SMART_ROOT_ACE && ./bin/SmartRobotConsole` + +- use `State Client` (98) in Robot Console to activate **Docking** (when Docking is succeeded, Docking Activity is stopped) +- for another Docking process, use `State Client` (98) in Robot Console to activate **Neutral** before **Docking** can be activated again + +#### How to use (for running on Robot) + +- make sure the catkin workspace including mojin_seronet is sourced (check `$CMAKE_PREFIX_PATH` and `$ROS_PACKAGE_PATH`) diff --git a/ComponentRosDock/ROS/src-gen/CMakeLists.txt b/ComponentRosDock/ROS/src-gen/CMakeLists.txt new file mode 100644 index 00000000..57391fcb --- /dev/null +++ b/ComponentRosDock/ROS/src-gen/CMakeLists.txt @@ -0,0 +1,39 @@ +#-------------------------------------------------------------------------- +# This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +# developed by the SeRoNet Project consortium: +# http:#www.seronet-projekt.de +# +# The ROS Mixed-Port Component is developed by: +# Service Robotics Research Center of Ulm University of Applied Sciences +# Fraunhofer Institute for Manufacturing Engineering and Automation IPA +# +# This code-generator uses infrastructure of the SmartMDSD Toolchain on +# which the SeRoNet Tooling is based on. +# +# CAUTION: +# This software is a preview for the purpose of technology demonstration. +# It is experimental and comes with no support. Use at your own risk. +# Please do not modify this file. It will be re-generated +# running the code generator. +#-------------------------------------------------------------------------- +# %Tag(FULLTEXT)% +cmake_minimum_required(VERSION 2.8.3) +project(ros_componentrosdock) + +find_package(catkin REQUIRED COMPONENTS roscpp std_msgs std_msgs std_msgs sensor_msgs geometry_msgs std_msgs std_msgs) + +catkin_package() + +LIST(APPEND LOCAL_INCLUDES ${catkin_INCLUDE_DIRS}) +LIST(APPEND LOCAL_INCLUDES ${CMAKE_CURRENT_SOURCE_DIR}) +LIST(APPEND LOCAL_INCLUDES ${CMAKE_CURRENT_SOURCE_DIR}/../src) +set(ROS_INCLUDES ${LOCAL_INCLUDES} PARENT_SCOPE) + +LIST(APPEND SRCS ${CMAKE_CURRENT_SOURCE_DIR}/ComponentRosDockRosPortExtension.cc) +LIST(APPEND SRCS ${CMAKE_CURRENT_SOURCE_DIR}/../src/ComponentRosDockRosPortCallbacks.cc) + +set(ROS_SRCS ${SRCS} PARENT_SCOPE) + +set(ROS_LIBS ${catkin_LIBRARIES} PARENT_SCOPE) + +# %EndTag(FULLTEXT)% diff --git a/ComponentRosDock/ROS/src-gen/ComponentRosDockRosPortBaseClass.hh b/ComponentRosDock/ROS/src-gen/ComponentRosDockRosPortBaseClass.hh new file mode 100644 index 00000000..0ad8dd41 --- /dev/null +++ b/ComponentRosDock/ROS/src-gen/ComponentRosDockRosPortBaseClass.hh @@ -0,0 +1,39 @@ +//-------------------------------------------------------------------------- +// This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +// developed by the SeRoNet Project consortium: +// http://www.seronet-projekt.de +// +// The ROS Mixed-Port Component is developed by: +// Service Robotics Research Center of Ulm University of Applied Sciences +// Fraunhofer Institute for Manufacturing Engineering and Automation IPA +// +// This code-generator uses infrastructure of the SmartMDSD Toolchain on +// which the SeRoNet Tooling is based on. +// +// CAUTION: +// This software is a preview for the purpose of technology demonstration. +// It is experimental and comes with no support. Use at your own risk. +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef ROS_PORT_BASE_CLASS_H_ +#define ROS_PORT_BASE_CLASS_H_ + +#include + +class ComponentRosDockRosPortBaseClass { +public: + ComponentRosDockRosPortBaseClass() { }; + virtual ~ComponentRosDockRosPortBaseClass() { } + + ros::Publisher charging_pub; + ros::Publisher dock_action_goal; + ros::Subscriber dock_action_result; + ros::Publisher laser_pub; + ros::Subscriber twist_sub; + ros::Publisher undock_action_goal; + ros::Subscriber undock_action_result; +}; + +#endif // ROS_PORT_BASE_CLASS_H_ diff --git a/ComponentRosDock/ROS/src-gen/ComponentRosDockRosPortCallbacksInterface.hh b/ComponentRosDock/ROS/src-gen/ComponentRosDockRosPortCallbacksInterface.hh new file mode 100644 index 00000000..4f12875f --- /dev/null +++ b/ComponentRosDock/ROS/src-gen/ComponentRosDockRosPortCallbacksInterface.hh @@ -0,0 +1,46 @@ +//-------------------------------------------------------------------------- +// This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +// developed by the SeRoNet Project consortium: +// http://www.seronet-projekt.de +// +// The ROS Mixed-Port Component is developed by: +// Service Robotics Research Center of Ulm University of Applied Sciences +// Fraunhofer Institute for Manufacturing Engineering and Automation IPA +// +// This code-generator uses infrastructure of the SmartMDSD Toolchain on +// which the SeRoNet Tooling is based on. +// +// CAUTION: +// This software is a preview for the purpose of technology demonstration. +// It is experimental and comes with no support. Use at your own risk. +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef ROS_PORT_CALLBACKS_INTERFACE_H_ +#define ROS_PORT_CALLBACKS_INTERFACE_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include + +class ComponentRosDockRosPortCallbacksInterface { +public: + ComponentRosDockRosPortCallbacksInterface() { }; + virtual ~ComponentRosDockRosPortCallbacksInterface() { } + + virtual void dock_action_result_cb (const std_msgs::String::ConstPtr &msg) = 0; + + virtual void twist_sub_cb (const geometry_msgs::Twist::ConstPtr &msg) = 0; + + virtual void undock_action_result_cb (const std_msgs::String::ConstPtr &msg) = 0; + +}; + +#endif // ROS_PORT_CALLBACKS_INTERFACE_H_ diff --git a/ComponentRosDock/ROS/src-gen/ComponentRosDockRosPortExtension.cc b/ComponentRosDock/ROS/src-gen/ComponentRosDockRosPortExtension.cc new file mode 100644 index 00000000..315b0a07 --- /dev/null +++ b/ComponentRosDock/ROS/src-gen/ComponentRosDockRosPortExtension.cc @@ -0,0 +1,77 @@ +//-------------------------------------------------------------------------- +// This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +// developed by the SeRoNet Project consortium: +// http://www.seronet-projekt.de +// +// The ROS Mixed-Port Component is developed by: +// Service Robotics Research Center of Ulm University of Applied Sciences +// Fraunhofer Institute for Manufacturing Engineering and Automation IPA +// +// This code-generator uses infrastructure of the SmartMDSD Toolchain on +// which the SeRoNet Tooling is based on. +// +// CAUTION: +// This software is a preview for the purpose of technology demonstration. +// It is experimental and comes with no support. Use at your own risk. +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#include "ComponentRosDockRosPortExtension.hh" + +// statically create a global PlainOpcUaComponentRosDockExtension instance +static ComponentRosDockRosPortExtension ros_port_extension; + +ComponentRosDockRosPortExtension::ComponentRosDockRosPortExtension() +: ComponentRosDockExtension("ComponentRosDockRosPortExtension") +{ + nh = 0; + callbacksPtr = 0; +} + +ComponentRosDockRosPortExtension::~ComponentRosDockRosPortExtension() +{ } + +void ComponentRosDockRosPortExtension::loadParameters(const SmartACE::SmartIniParameter ¶meter) +{ } + +void ComponentRosDockRosPortExtension::initialize(ComponentRosDock *component, int argc, char* argv[]) +{ + ros::init(argc, argv, "ComponentRosDock", ros::init_options::NoSigintHandler); + nh = new ros::NodeHandle(); + + callbacksPtr = new ComponentRosDockRosPortCallbacks(); + + component->rosPorts = this; + + charging_pub = nh->advertise("/charging_seronet", 10); + dock_action_goal = nh->advertise("/docker_control/dock_seronet/goal", 10); + dock_action_result = nh->subscribe("/docker_control/dock_seronet/result", 10, &ComponentRosDockRosPortCallbacks::dock_action_result_cb, callbacksPtr); + laser_pub = nh->advertise("/scan_unified_seronet", 10); + twist_sub = nh->subscribe("/base/twist_controller/command", 10, &ComponentRosDockRosPortCallbacks::twist_sub_cb, callbacksPtr); + undock_action_goal = nh->advertise("/docker_control/undock_seronet/goal", 10); + undock_action_result = nh->subscribe("/docker_control/undock_seronet/result", 10, &ComponentRosDockRosPortCallbacks::undock_action_result_cb, callbacksPtr); +} + +int ComponentRosDockRosPortExtension::onStartup() +{ + return startExtensionThread(); +} + +int ComponentRosDockRosPortExtension::extensionExecution() +{ + ros::spin(); + return 0; +} + +int ComponentRosDockRosPortExtension::onShutdown(const std::chrono::steady_clock::duration &timeoutTime) +{ + ros::shutdown(); + return stopExtensionThread(timeoutTime); +} + +void ComponentRosDockRosPortExtension::destroy() +{ + delete nh; + delete callbacksPtr; +} diff --git a/ComponentRosDock/ROS/src-gen/ComponentRosDockRosPortExtension.hh b/ComponentRosDock/ROS/src-gen/ComponentRosDockRosPortExtension.hh new file mode 100644 index 00000000..bf4318fc --- /dev/null +++ b/ComponentRosDock/ROS/src-gen/ComponentRosDockRosPortExtension.hh @@ -0,0 +1,73 @@ +//-------------------------------------------------------------------------- +// This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +// developed by the SeRoNet Project consortium: +// http://www.seronet-projekt.de +// +// The ROS Mixed-Port Component is developed by: +// Service Robotics Research Center of Ulm University of Applied Sciences +// Fraunhofer Institute for Manufacturing Engineering and Automation IPA +// +// This code-generator uses infrastructure of the SmartMDSD Toolchain on +// which the SeRoNet Tooling is based on. +// +// CAUTION: +// This software is a preview for the purpose of technology demonstration. +// It is experimental and comes with no support. Use at your own risk. +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef ROS_PORT_COMPONENT_EXTENSION_H_ +#define ROS_PORT_COMPONENT_EXTENSION_H_ + +#include "ComponentRosDockRosPortBaseClass.hh" +#include "ComponentRosDockRosPortCallbacks.hh" + +#include "ComponentRosDockExtension.hh" + +// include component's main class +#include "ComponentRosDock.hh" + +class ComponentRosDockRosPortExtension : public ComponentRosDockExtension, public ComponentRosDockRosPortBaseClass +{ +private: + ros::NodeHandle *nh; + + ComponentRosDockRosPortCallbacks *callbacksPtr; + + virtual int extensionExecution() override; +public: + ComponentRosDockRosPortExtension(); + virtual ~ComponentRosDockRosPortExtension(); + + virtual void loadParameters(const SmartACE::SmartIniParameter ¶meter); + virtual void initialize(ComponentRosDock *component, int argc, char* argv[]) override; + virtual int onStartup() override; + + inline ros::Publisher* getCurrent_pubPtr() { + return &charging_pub; + } + inline ros::Publisher* getDock_action_goalPtr() { + return &dock_action_goal; + } + inline ros::Subscriber* getDock_action_resultPtr() { + return &dock_action_result; + } + inline ros::Publisher* getLaser_pubPtr() { + return &laser_pub; + } + inline ros::Subscriber* getTwist_subPtr() { + return &twist_sub; + } + inline ros::Publisher* getUndock_action_goalPtr() { + return &undock_action_goal; + } + inline ros::Subscriber* getUndock_action_resultPtr() { + return &undock_action_result; + } + + virtual int onShutdown(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)) override; + virtual void destroy() override; +}; + +#endif /* ROS_PORT_COMPONENT_EXTENSION_H_ */ diff --git a/ComponentRosDock/ROS/src-gen/package.xml b/ComponentRosDock/ROS/src-gen/package.xml new file mode 100644 index 00000000..64223623 --- /dev/null +++ b/ComponentRosDock/ROS/src-gen/package.xml @@ -0,0 +1,20 @@ + + ros_componentrosdock + 0.0.0 + The ros_componentrosdock package + + Apache 2.0 + + + + Jane Doe + Jane Doe + + + catkin + roscpp + geometry_msgs + sensor_msgs + std_msgs + + diff --git a/ComponentRosDock/ROS/src/ComponentRosDockRosPortCallbacks.cc b/ComponentRosDock/ROS/src/ComponentRosDockRosPortCallbacks.cc new file mode 100644 index 00000000..36442ecb --- /dev/null +++ b/ComponentRosDock/ROS/src/ComponentRosDockRosPortCallbacks.cc @@ -0,0 +1,50 @@ +//-------------------------------------------------------------------------- +// This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +// developed by the SeRoNet Project consortium: +// http://www.seronet-projekt.de +// +// The ROS Mixed-Port Component is developed by: +// Service Robotics Research Center of Ulm University of Applied Sciences +// Fraunhofer Institute for Manufacturing Engineering and Automation IPA +// +// This code-generator uses infrastructure of the SmartMDSD Toolchain on +// which the SeRoNet Tooling is based on. +// +// CAUTION: +// This software is a preview for the purpose of technology demonstration. +// It is experimental and comes with no support. Use at your own risk. +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#include "ComponentRosDockRosPortCallbacks.hh" + +// include component's main class +#include "ComponentRosDock.hh" + +ComponentRosDockRosPortCallbacks::ComponentRosDockRosPortCallbacks() { } + +ComponentRosDockRosPortCallbacks::~ComponentRosDockRosPortCallbacks() { } + +void ComponentRosDockRosPortCallbacks::dock_action_result_cb (const std_msgs::String::ConstPtr &msg) +{ + // for implementing this method, you can use the "COMP->" macro to access the component's class members + COMP->dockActivity->dock_action_result_cb(msg); + +} + +void ComponentRosDockRosPortCallbacks::twist_sub_cb (const geometry_msgs::Twist::ConstPtr &msg) +{ + // for implementing this method, you can use the "COMP->" macro to access the component's class members + COMP->forwardActivity->twist_sub_cb(msg); + +} + +void ComponentRosDockRosPortCallbacks::undock_action_result_cb (const std_msgs::String::ConstPtr &msg) +{ + // for implementing this method, you can use the "COMP->" macro to access the component's class members + COMP->undockActivity->undock_action_result_cb(msg); + +} + + diff --git a/ComponentRosDock/ROS/src/ComponentRosDockRosPortCallbacks.hh b/ComponentRosDock/ROS/src/ComponentRosDockRosPortCallbacks.hh new file mode 100644 index 00000000..ea4c9f29 --- /dev/null +++ b/ComponentRosDock/ROS/src/ComponentRosDockRosPortCallbacks.hh @@ -0,0 +1,38 @@ +//-------------------------------------------------------------------------- +// This file is generated by the SeRoNet Tooling. The SeRoNet Tooling is +// developed by the SeRoNet Project consortium: +// http://www.seronet-projekt.de +// +// The ROS Mixed-Port Component is developed by: +// Service Robotics Research Center of Ulm University of Applied Sciences +// Fraunhofer Institute for Manufacturing Engineering and Automation IPA +// +// This code-generator uses infrastructure of the SmartMDSD Toolchain on +// which the SeRoNet Tooling is based on. +// +// CAUTION: +// This software is a preview for the purpose of technology demonstration. +// It is experimental and comes with no support. Use at your own risk. +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef ROS_PORT_CALLBACKS_H_ +#define ROS_PORT_CALLBACKS_H_ + +#include "ComponentRosDockRosPortCallbacksInterface.hh" + +class ComponentRosDockRosPortCallbacks : public ComponentRosDockRosPortCallbacksInterface { +public: + ComponentRosDockRosPortCallbacks(); + virtual ~ComponentRosDockRosPortCallbacks(); + + virtual void dock_action_result_cb (const std_msgs::String::ConstPtr &msg) override; + + virtual void twist_sub_cb (const geometry_msgs::Twist::ConstPtr &msg) override; + + virtual void undock_action_result_cb (const std_msgs::String::ConstPtr &msg) override; + +}; + +#endif // ROS_PORT_CALLBACKS_H_ diff --git a/ComponentRosDock/model/ComponentRosDock.component b/ComponentRosDock/model/ComponentRosDock.component new file mode 100644 index 00000000..a30b41dd --- /dev/null +++ b/ComponentRosDock/model/ComponentRosDock.component @@ -0,0 +1,63 @@ +ComponentDefinition ComponentRosDock logo "/ComponentRosDock/model/logo.png" +{ + Activity DockActivity + { + InputPortLink BaseStateServiceIn { oversamplingOk undersamplingOk } + + MixedPortROSLink dock_action_goal + MixedPortROSLink dock_action_result + OperationModeBinding CoordinationPort.dock; + DefaultTrigger PeriodicTimer 1.0 Hz + } + + MixedPortROS twist_sub + + Activity UndockActivity + { + InputPortLink BaseStateServiceIn { oversamplingOk undersamplingOk } + + MixedPortROSLink undock_action_goal + MixedPortROSLink undock_action_result + + OperationModeBinding CoordinationPort.unDock; + DefaultTrigger PeriodicTimer 1.0 Hz + } + InputPort BaseStateServiceIn implements CommBasicObjects.BaseStateService { + } + InputPort LaserServiceIn implements CommBasicObjects.LaserService { + } + OutputPort NavigationVelocityServiceOut implements CommNavigationObjects.NavigationVelocityService realizedBy ForwardActivity { + } + + OutputPort RobotDockingEventServiceOut implements CommNavigationObjects.RobotDockingEventService realizedBy DockActivity { + } + + CoordinationSlavePort CoordinationPort implements CommNavigationObjects.RobotDockingCoordinationService + { + PrivateOperationMode dock; + PrivateOperationMode unDock; + + PublicOperationMode CommNavigationObjects.RobotDockingStates.Docking { activates ( dock ) } + PublicOperationMode CommNavigationObjects.RobotDockingStates.UnDocking { activates ( unDock ) } + + with dockingEvent realizedBy RobotDockingEventServiceOut + } + + MixedPortROS undock_action_goal + MixedPortROS dock_action_goal + MixedPortROS dock_action_result + + Activity ForwardActivity + { + MixedPortROSLink twist_sub + InputPortLink LaserServiceIn { oversamplingOk undersamplingOk } + MixedPortROSLink laser_pub + MixedPortROSLink charging_pub + DefaultTrigger PeriodicTimer 1.0 Hz + } + + ComponentParameters ComponentRosDock slave CoordinationPort +MixedPortROS undock_action_result + MixedPortROS laser_pub +MixedPortROS charging_pub +} \ No newline at end of file diff --git a/ComponentRosDock/model/ComponentRosDock.componentParameters b/ComponentRosDock/model/ComponentRosDock.componentParameters new file mode 100644 index 00000000..2e0400fb --- /dev/null +++ b/ComponentRosDock/model/ComponentRosDock.componentParameters @@ -0,0 +1,4 @@ +ComponentParameter ComponentRosDock component ComponentRosDock +{ + +} diff --git a/ComponentRosDock/model/ComponentRosDockComponentDefinition.jpg b/ComponentRosDock/model/ComponentRosDockComponentDefinition.jpg new file mode 100644 index 00000000..e303e75a Binary files /dev/null and b/ComponentRosDock/model/ComponentRosDockComponentDefinition.jpg differ diff --git a/ComponentRosDock/model/component.rosinterfacespool b/ComponentRosDock/model/component.rosinterfacespool new file mode 100644 index 00000000..27522ff6 --- /dev/null +++ b/ComponentRosDock/model/component.rosinterfacespool @@ -0,0 +1,9 @@ +RosInterfacesPool { + RosSubscriber twist_sub { topicName "/base/twist_controller/command" type "geometry_msgs.Twist" } + RosPublisher laser_pub { topicName "/scan_unified_seronet" type "sensor_msgs.LaserScan" } + RosPublisher charging_pub { topicName "/charging_seronet" type "std_msgs.Bool"} + RosPublisher dock_action_goal { topicName "/docker_control/dock_seronet/goal" type "std_msgs.String" } + RosPublisher undock_action_goal { topicName "/docker_control/undock_seronet/goal" type "std_msgs.String" } + RosSubscriber dock_action_result { topicName "/docker_control/dock_seronet/result" type "std_msgs.String" } + RosSubscriber undock_action_result { topicName "/docker_control/undock_seronet/result" type "std_msgs.String" } +} \ No newline at end of file diff --git a/ComponentRosDock/opcua-backend/src-gen/ComponentRosDockOpcUaBackendPortFactory.cc b/ComponentRosDock/opcua-backend/src-gen/ComponentRosDockOpcUaBackendPortFactory.cc new file mode 100644 index 00000000..61148f01 --- /dev/null +++ b/ComponentRosDock/opcua-backend/src-gen/ComponentRosDockOpcUaBackendPortFactory.cc @@ -0,0 +1,109 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#include "ComponentRosDockOpcUaBackendPortFactory.hh" + +// include all potentially required pattern implementations +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// include referenced CommunicationObject SeRoNetSDK self description implementations +#include "CommBasicObjectsOpcUa/CommBaseStateOpcUa.hh" +#include "CommBasicObjectsOpcUa/CommMobileLaserScanOpcUa.hh" +#include "CommBasicObjectsOpcUa/CommNavigationVelocityOpcUa.hh" +#include "CommNavigationObjectsOpcUa/CommDockingEventParameterOpcUa.hh" +#include "CommNavigationObjectsOpcUa/CommDockingEventResultOpcUa.hh" +#include "CommNavigationObjectsOpcUa/CommDockingEventStateOpcUa.hh" + +// create a static instance of the OpcUaBackendPortFactory +static ComponentRosDockOpcUaBackendPortFactory OpcUaBackendPortFactory; + +ComponentRosDockOpcUaBackendPortFactory::ComponentRosDockOpcUaBackendPortFactory() +{ + componentImpl = 0; + ComponentRosDock::instance()->addPortFactory("OpcUa_SeRoNet", this); +} + +ComponentRosDockOpcUaBackendPortFactory::~ComponentRosDockOpcUaBackendPortFactory() +{ } + +void ComponentRosDockOpcUaBackendPortFactory::initialize(ComponentRosDock *component, int argc, char* argv[]) +{ + componentImpl = new SeRoNet::Utils::Component(component->connections.component.name); +} + +int ComponentRosDockOpcUaBackendPortFactory::onStartup() +{ + if (!component_thread.joinable()) { + component_thread = std::thread(&ComponentRosDockOpcUaBackendPortFactory::task_execution, this); + return 0; + } + return -1; +} + +Smart::IPushClientPattern * ComponentRosDockOpcUaBackendPortFactory::createBaseStateServiceIn() +{ + return new SeRoNet::OPCUA::Client::PushClient(componentImpl); +} + +Smart::IPushClientPattern * ComponentRosDockOpcUaBackendPortFactory::createLaserServiceIn() +{ + return new SeRoNet::OPCUA::Client::PushClient(componentImpl); +} + +Smart::ISendClientPattern * ComponentRosDockOpcUaBackendPortFactory::createNavigationVelocityServiceOut() +{ + return new SeRoNet::OPCUA::Client::SendClient(componentImpl); +} + + +Smart::IEventServerPattern * ComponentRosDockOpcUaBackendPortFactory::createRobotDockingEventServiceOut(const std::string &serviceName, std::shared_ptr> robotDockingEventServiceOutEventTestHandler) +{ + return new SeRoNet::OPCUA::Server::EventServer(componentImpl, serviceName, robotDockingEventServiceOutEventTestHandler); +} + + +int ComponentRosDockOpcUaBackendPortFactory::task_execution() +{ + componentImpl->run(); + return 0; +} + +int ComponentRosDockOpcUaBackendPortFactory::onShutdown(const std::chrono::steady_clock::duration &timeoutTime) +{ + // stop component-internal infrastructure + componentImpl->stopRunning(); + // wait on component thread to exit + if (component_thread.joinable()) { + // FIXME: don't wait infinetly (use timeoutTime here) + component_thread.join(); + } + return 0; +} + +void ComponentRosDockOpcUaBackendPortFactory::destroy() +{ + // clean-up component's internally used resources + delete componentImpl; +} diff --git a/ComponentRosDock/opcua-backend/src-gen/ComponentRosDockOpcUaBackendPortFactory.hh b/ComponentRosDock/opcua-backend/src-gen/ComponentRosDockOpcUaBackendPortFactory.hh new file mode 100644 index 00000000..64b9ef3a --- /dev/null +++ b/ComponentRosDock/opcua-backend/src-gen/ComponentRosDockOpcUaBackendPortFactory.hh @@ -0,0 +1,58 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef COMPONENTROSDOCK_OPC_UA_BACKEND_PORTFACTORY_HH_ +#define COMPONENTROSDOCK_OPC_UA_BACKEND_PORTFACTORY_HH_ + +// include the main component-definition class +#include "ComponentRosDockPortFactoryInterface.hh" + +#include +#include + +// include SeRoNetSDK library +#include +#include + +class ComponentRosDockOpcUaBackendPortFactory: public ComponentRosDockPortFactoryInterface +{ +private: + // internal component instance + SeRoNet::Utils::Component *componentImpl; + + // component thread + std::thread component_thread; + + // internal component thread method + int task_execution(); +public: + ComponentRosDockOpcUaBackendPortFactory(); + virtual ~ComponentRosDockOpcUaBackendPortFactory(); + + virtual void initialize(ComponentRosDock *component, int argc, char* argv[]) override; + virtual int onStartup() override; + + virtual Smart::IPushClientPattern * createBaseStateServiceIn() override; + virtual Smart::IPushClientPattern * createLaserServiceIn() override; + virtual Smart::ISendClientPattern * createNavigationVelocityServiceOut() override; + + virtual Smart::IEventServerPattern * createRobotDockingEventServiceOut(const std::string &serviceName, std::shared_ptr> robotDockingEventServiceOutEventTestHandler) override; + + virtual int onShutdown(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)) override; + virtual void destroy() override; +}; + +#endif /* COMPONENTROSDOCK_SERONET_SDK_PORTFACTORY_HH_ */ diff --git a/ComponentRosDock/opcua-backend/src-gen/OpcUaBackend.cmake b/ComponentRosDock/opcua-backend/src-gen/OpcUaBackend.cmake new file mode 100644 index 00000000..636197fa --- /dev/null +++ b/ComponentRosDock/opcua-backend/src-gen/OpcUaBackend.cmake @@ -0,0 +1,13 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) + +FIND_PACKAGE(Open62541Cpp QUIET) +SET(SmartSoft_CD_API_DIR $ENV{SMART_ROOT_ACE}/modules) +FIND_PACKAGE(SeRoNetSDK QUIET) + +IF(SeRoNetSDK_FOUND) + FIND_PACKAGE(CommBasicObjectsOpcUa PATHS $ENV{SMART_ROOT_ACE}/modules) + FIND_PACKAGE(CommNavigationObjectsOpcUa PATHS $ENV{SMART_ROOT_ACE}/modules) + SET(CMAKE_CXX_STANDARD 14) + INCLUDE_DIRECTORIES(${CMAKE_CURRENT_LIST_DIR}) + SET(OPC_UA_BACKEND_SRCS "${CMAKE_CURRENT_LIST_DIR}/ComponentRosDockOpcUaBackendPortFactory.cc") +ENDIF(SeRoNetSDK_FOUND) diff --git a/ComponentRosDock/opcua-backend/src-gen/info.txt b/ComponentRosDock/opcua-backend/src-gen/info.txt new file mode 100644 index 00000000..93cdd389 --- /dev/null +++ b/ComponentRosDock/opcua-backend/src-gen/info.txt @@ -0,0 +1,17 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +// Generated with SmartMDSD Toolchain Version 3.12.0.201912041712 diff --git a/ComponentRosDock/representations.aird b/ComponentRosDock/representations.aird new file mode 100644 index 00000000..1981fea3 --- /dev/null +++ b/ComponentRosDock/representations.aird @@ -0,0 +1,1036 @@ + + + + model/ComponentRosDock.component + model/component.rosinterfacespool + platform:/resource/CommBasicObjects/model/CommBasicObjects.services + platform:/resource/CommBasicObjects/model/CommBasicObjects.types + platform:/plugin/org.xtext.base.stateMachine/componentLifecycle/ComponentLifecycle.automaton + platform:/resource/CommBasicObjects/model/CommBasicObjects.modes + platform:/plugin/org.xtext.service.parameterDefinition/defaultParameterLib/defaultParameterLib.parameters + platform:/resource/CommBasicObjects/model/CommBasicObjects.parameters + platform:/resource/CommNavigationObjects/model/CommNavigationObjects.services + platform:/resource/CommNavigationObjects/model/CommNavigationObjects.types + platform:/resource/CommNavigationObjects/model/CommNavigationObjects.modes + platform:/resource/CommNavigationObjects/model/CommNavigationObjects.parameters + model/ComponentRosDock.componentParameters + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + bold + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + bold + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + bold + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + bold + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + bold + + + + + + + + + + + + + + + + KEEP_LOCATION + KEEP_SIZE + KEEP_RATIO + + bold + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ComponentRosDock/smartsoft/CMakeLists.txt b/ComponentRosDock/smartsoft/CMakeLists.txt new file mode 100644 index 00000000..4e90a44f --- /dev/null +++ b/ComponentRosDock/smartsoft/CMakeLists.txt @@ -0,0 +1,82 @@ +#-------------------------------------------------------------------------- +# Code generated by the SmartSoft MDSD Toolchain +# The SmartSoft Toolchain has been developed by: +# +# Service Robotics Research Center +# University of Applied Sciences Ulm +# Prittwitzstr. 10 +# 89075 Ulm (Germany) +# +# Information about the SmartSoft MDSD Toolchain is available at: +# www.servicerobotik-ulm.de +# +# This file is generated once. Modify this file to your needs. +# If you want the toolchain to re-generate this file, please +# delete it before running the code generator. +#-------------------------------------------------------------------------- + +CMAKE_MINIMUM_REQUIRED(VERSION 3.0) + +# find the SmartSoft CMake Macros (version 2 that uses CMake v3) +FIND_FILE(SMART_MACROS SmartMacros2.cmake PATHS $ENV{SMART_ROOT_ACE}/CMakeMacros /opt/smartSoftAce/CMakeMacros/) +INCLUDE(${SMART_MACROS}) + +# Create and configure a CMake project for a CommunicationObject +SMART_COMPONENT_PROJECT() + +# find SmartSoft package +#FIND_SMARTSOFT(3.0) +#FIXME: generalize this +FIND_PACKAGE(AceSmartSoft PATHS $ENV{SMART_ROOT_ACE}/modules) + +# generte doxygen documentation +SMART_COMPONENT_GENERATE_DOC() + +# automatically build CommunicationObject dependencies +SMART_COMPONENT_AUTOBUILD_DEPENDENCIES() + +################################################################################# +## here you can put all your project-specific compiler configurations ## +## using regular CMake commands (like FIND_PACKAGE, INCLUDE_DIRECTORIES, etc.) ## + +# you can add additional external libraries using regular CMake commands, e.g.: +#FIND_PACKAGE() + +INCLUDE(${CMAKE_CURRENT_LIST_DIR}/src-gen/MixedPortIncludes.cmake) + +# these are the regular includes (add your own includes, if needed) +INCLUDE_DIRECTORIES( + ${CMAKE_CURRENT_LIST_DIR}/src + ${CMAKE_CURRENT_LIST_DIR}/src-gen + ${CMAKE_CURRENT_LIST_DIR}/src-gen/params +) + +# collect the toolchain-generated source-files +FILE(GLOB_RECURSE SRC ${CMAKE_CURRENT_LIST_DIR}/src/*.cc) +FILE(GLOB_RECURSE SRC_GEN ${CMAKE_CURRENT_LIST_DIR}/src-gen/*.cc) +FILE(GLOB_RECURSE SRC_GEN_PARAMS ${CMAKE_CURRENT_LIST_DIR}/src-gen/params/*.cc) + +# collect your own (local) source-files here (if needed) +#FILE(GLOB_RECURSE USER_SRC ${CMAKE_CURRENT_LIST_DIR}/src/) + +# create the executable using collected source-files +ADD_EXECUTABLE(${PROJECT_NAME} ${SRC} ${SRC_GEN} ${SRC_GEN_PARAMS} ${FURTHER_SRCS}) # add ${USER_SRC} (if needed) + +# link your external libraries here (if there are any) +#TARGET_LINK_LIBRARIES(${PROJECT_NAME} ) + +INCLUDE(${CMAKE_CURRENT_LIST_DIR}/src-gen/MixedPortTargetConfigs.cmake) + +#INSTALL(FILES ${PROJECT_SOURCE_DIR}/src-gen/${PROJECT_NAME}.ini.template DESTINATION etc) + +## end user-code ## +################################################################################# + +# SmartSoftKernel is one of the default dependencies for each component +TARGET_LINK_LIBRARIES(${PROJECT_NAME} AceSmartSoftKernel) + +# automatically link to all dependent CommunicationObjects +SMART_COMPONENT_AUTOLINK_DEPENDENCIES() + +# automatically install the component target +SMART_COMPONENT_AUTOINSTALL() diff --git a/ComponentRosDock/smartsoft/src-gen/BaseStateServiceInUpcallInterface.hh b/ComponentRosDock/smartsoft/src-gen/BaseStateServiceInUpcallInterface.hh new file mode 100644 index 00000000..7bd00151 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/BaseStateServiceInUpcallInterface.hh @@ -0,0 +1,28 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _BASESTATESERVICEIN_UPCALL_INTERFACE_HH +#define _BASESTATESERVICEIN_UPCALL_INTERFACE_HH + +#include "CommBasicObjects/CommBaseState.hh" + +class BaseStateServiceInUpcallInterface { +public: + virtual ~BaseStateServiceInUpcallInterface() { } + + virtual void on_BaseStateServiceIn(const CommBasicObjects::CommBaseState &input) = 0; +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/BaseStateServiceInUpcallManager.cc b/ComponentRosDock/smartsoft/src-gen/BaseStateServiceInUpcallManager.cc new file mode 100644 index 00000000..9a83f5ec --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/BaseStateServiceInUpcallManager.cc @@ -0,0 +1,40 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include "BaseStateServiceInUpcallManager.hh" + +BaseStateServiceInUpcallManager::BaseStateServiceInUpcallManager( + Smart::InputSubject *subject, + const int &prescaleFactor) + : Smart::IInputHandler(subject, prescaleFactor) +{ } +BaseStateServiceInUpcallManager::~BaseStateServiceInUpcallManager() +{ } + +void BaseStateServiceInUpcallManager::notify_upcalls(const CommBasicObjects::CommBaseState &input) +{ + for(auto it=upcalls.begin(); it!=upcalls.end(); it++) { + (*it)->on_BaseStateServiceIn(input); + } +} + +void BaseStateServiceInUpcallManager::attach(BaseStateServiceInUpcallInterface *upcall) +{ + upcalls.push_back(upcall); +} +void BaseStateServiceInUpcallManager::detach(BaseStateServiceInUpcallInterface *upcall) +{ + upcalls.remove(upcall); +} diff --git a/ComponentRosDock/smartsoft/src-gen/BaseStateServiceInUpcallManager.hh b/ComponentRosDock/smartsoft/src-gen/BaseStateServiceInUpcallManager.hh new file mode 100644 index 00000000..b8e9b030 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/BaseStateServiceInUpcallManager.hh @@ -0,0 +1,54 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _BASESTATESERVICEIN_UPCALL_MANAGER_HH +#define _BASESTATESERVICEIN_UPCALL_MANAGER_HH + +#include +#include "aceSmartSoft.hh" +#include "BaseStateServiceInUpcallInterface.hh" + +/** BaseStateServiceInUpcallManager connects input-handling with Upcall propagation + * + * This class implements an InputHandler for the InputPort BaseStateServiceIn and propagates the handling + * of incoming data to all associated (i.e. attached) Upcalls. + */ +class BaseStateServiceInUpcallManager +: public Smart::IInputHandler +{ +private: + // list of associated updalls + std::list upcalls; + + // call the on_BaseStateServiceIn of all the attached BaseStateServiceInUpcallInterfaces + void notify_upcalls(const CommBasicObjects::CommBaseState &input); + +protected: + virtual void handle_input(const CommBasicObjects::CommBaseState &input) { + // relay input-handling to all attached BaseStateServiceInUpcallInterfaces + this->notify_upcalls(input); + } +public: + BaseStateServiceInUpcallManager( + Smart::InputSubject *subject, + const int &prescaleFactor=1 + ); + virtual ~BaseStateServiceInUpcallManager(); + + void attach(BaseStateServiceInUpcallInterface *upcall); + void detach(BaseStateServiceInUpcallInterface *upcall); +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentProject.cmake b/ComponentRosDock/smartsoft/src-gen/ComponentProject.cmake new file mode 100644 index 00000000..172d0634 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentProject.cmake @@ -0,0 +1,17 @@ +#-------------------------------------------------------------------------- +# Code generated by the SmartSoft MDSD Toolchain +# The SmartSoft Toolchain has been developed by: +# +# Service Robotics Research Center +# University of Applied Sciences Ulm +# Prittwitzstr. 10 +# 89075 Ulm (Germany) +# +# Information about the SmartSoft MDSD Toolchain is available at: +# www.servicerobotik-ulm.de +# +# Please do not modify this file. It will be re-generated +# running the code generator. +#-------------------------------------------------------------------------- + +PROJECT(ComponentRosDock) diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentRosDock.cc b/ComponentRosDock/smartsoft/src-gen/ComponentRosDock.cc new file mode 100644 index 00000000..50fb7822 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentRosDock.cc @@ -0,0 +1,805 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include "ComponentRosDock.hh" +#include "smartTimedTaskTrigger.h" +//FIXME: implement logging +//#include "smartGlobalLogger.hh" + +// the ace port-factory is used as a default port-mapping +#include "ComponentRosDockAcePortFactory.hh" + +#include "RobotDockingEventServiceOutEventTestHandler.hh" + +// initialize static singleton pointer to zero +ComponentRosDock* ComponentRosDock::_componentRosDock = 0; + +// constructor +ComponentRosDock::ComponentRosDock() +{ + std::cout << "constructor of ComponentRosDock\n"; + + // set all pointer members to NULL + baseStateServiceIn = NULL; + baseStateServiceInInputTaskTrigger = NULL; + baseStateServiceInUpcallManager = NULL; + //componentRosDock = NULL; + //coordinationPort = NULL; + dockActivity = NULL; + dockActivityTrigger = NULL; + forwardActivity = NULL; + forwardActivityTrigger = NULL; + laserServiceIn = NULL; + laserServiceInInputTaskTrigger = NULL; + laserServiceInUpcallManager = NULL; + navigationVelocityServiceOut = NULL; + robotDockingEventServiceOut = NULL; + robotDockingEventServiceOutEventTestHandler = nullptr; + undockActivity = NULL; + undockActivityTrigger = NULL; + //charging_pub = NULL; + //dock_action_goal = NULL; + //dock_action_result = NULL; + //laser_pub = NULL; + //twist_sub = NULL; + //undock_action_goal = NULL; + //undock_action_result = NULL; + stateChangeHandler = NULL; + stateSlave = NULL; + wiringSlave = NULL; + param = NULL; + + // set default ini parameter values + connections.component.name = "ComponentRosDock"; + connections.component.initialComponentMode = "Neutral"; + connections.component.defaultScheduler = "DEFAULT"; + connections.component.useLogger = false; + + connections.robotDockingEventServiceOut.serviceName = "RobotDockingEventServiceOut"; + connections.robotDockingEventServiceOut.roboticMiddleware = "ACE_SmartSoft"; + connections.baseStateServiceIn.wiringName = "BaseStateServiceIn"; + connections.baseStateServiceIn.serverName = "unknown"; + connections.baseStateServiceIn.serviceName = "unknown"; + connections.baseStateServiceIn.interval = 1; + connections.baseStateServiceIn.roboticMiddleware = "ACE_SmartSoft"; + connections.laserServiceIn.wiringName = "LaserServiceIn"; + connections.laserServiceIn.serverName = "unknown"; + connections.laserServiceIn.serviceName = "unknown"; + connections.laserServiceIn.interval = 1; + connections.laserServiceIn.roboticMiddleware = "ACE_SmartSoft"; + connections.navigationVelocityServiceOut.initialConnect = false; + connections.navigationVelocityServiceOut.wiringName = "NavigationVelocityServiceOut"; + connections.navigationVelocityServiceOut.serverName = "unknown"; + connections.navigationVelocityServiceOut.serviceName = "unknown"; + connections.navigationVelocityServiceOut.interval = 1; + connections.navigationVelocityServiceOut.roboticMiddleware = "ACE_SmartSoft"; + connections.dockActivity.minActFreq = 0.0; + connections.dockActivity.maxActFreq = 0.0; + connections.dockActivity.trigger = "PeriodicTimer"; + connections.dockActivity.periodicActFreq = 1.0; + // scheduling default parameters + connections.dockActivity.scheduler = "DEFAULT"; + connections.dockActivity.priority = -1; + connections.dockActivity.cpuAffinity = -1; + connections.forwardActivity.minActFreq = 0.0; + connections.forwardActivity.maxActFreq = 0.0; + connections.forwardActivity.trigger = "PeriodicTimer"; + connections.forwardActivity.periodicActFreq = 1.0; + // scheduling default parameters + connections.forwardActivity.scheduler = "DEFAULT"; + connections.forwardActivity.priority = -1; + connections.forwardActivity.cpuAffinity = -1; + connections.undockActivity.minActFreq = 0.0; + connections.undockActivity.maxActFreq = 0.0; + connections.undockActivity.trigger = "PeriodicTimer"; + connections.undockActivity.periodicActFreq = 1.0; + // scheduling default parameters + connections.undockActivity.scheduler = "DEFAULT"; + connections.undockActivity.priority = -1; + connections.undockActivity.cpuAffinity = -1; + + // initialize members of ComponentRosDockROSExtension + rosPorts = 0; + + // initialize members of OpcUaBackendComponentGeneratorExtension + + // initialize members of PlainOpcUaComponentRosDockExtension + +} + +void ComponentRosDock::addPortFactory(const std::string &name, ComponentRosDockPortFactoryInterface *portFactory) +{ + portFactoryRegistry[name] = portFactory; +} + +void ComponentRosDock::addExtension(ComponentRosDockExtension *extension) +{ + componentExtensionRegistry[extension->getName()] = extension; +} + +SmartACE::SmartComponent* ComponentRosDock::getComponentImpl() +{ + return dynamic_cast(portFactoryRegistry["ACE_SmartSoft"])->getComponentImpl(); +} + +/** + * Notify the component that setup/initialization is finished. + * You may call this function from anywhere in the component. + * + * Set component's internal lifecycle state automaton (if any) into + * Alive mode (from here on the component is ready to provide its services) + */ +void ComponentRosDock::setStartupFinished() { + stateSlave->setWaitState("Alive"); + std::cout << "ComponentDefinition initialization/startup finished." << std::endl; +} + + +Smart::StatusCode ComponentRosDock::connectBaseStateServiceIn(const std::string &serverName, const std::string &serviceName) { + Smart::StatusCode status; + + std::cout << "connecting to: " << serverName << "; " << serviceName << std::endl; + status = baseStateServiceIn->connect(serverName, serviceName); + while(status != Smart::SMART_OK) + { + ACE_OS::sleep(ACE_Time_Value(0,500000)); + status = COMP->baseStateServiceIn->connect(serverName, serviceName); + } + std::cout << "connected.\n"; + baseStateServiceIn->subscribe(connections.baseStateServiceIn.interval); + return status; +} +Smart::StatusCode ComponentRosDock::connectLaserServiceIn(const std::string &serverName, const std::string &serviceName) { + Smart::StatusCode status; + + std::cout << "connecting to: " << serverName << "; " << serviceName << std::endl; + status = laserServiceIn->connect(serverName, serviceName); + while(status != Smart::SMART_OK) + { + ACE_OS::sleep(ACE_Time_Value(0,500000)); + status = COMP->laserServiceIn->connect(serverName, serviceName); + } + std::cout << "connected.\n"; + laserServiceIn->subscribe(connections.laserServiceIn.interval); + return status; +} +Smart::StatusCode ComponentRosDock::connectNavigationVelocityServiceOut(const std::string &serverName, const std::string &serviceName) { + Smart::StatusCode status; + + if(connections.navigationVelocityServiceOut.initialConnect == false) { + return Smart::SMART_OK; + } + std::cout << "connecting to: " << serverName << "; " << serviceName << std::endl; + status = navigationVelocityServiceOut->connect(serverName, serviceName); + while(status != Smart::SMART_OK) + { + ACE_OS::sleep(ACE_Time_Value(0,500000)); + status = COMP->navigationVelocityServiceOut->connect(serverName, serviceName); + } + std::cout << "connected.\n"; + return status; +} + + +/** + * First connect ALL client ports contained in this component, then start all services: + * activate state, push, etc... + */ +Smart::StatusCode ComponentRosDock::connectAndStartAllServices() { + Smart::StatusCode status = Smart::SMART_OK; + + status = connectBaseStateServiceIn(connections.baseStateServiceIn.serverName, connections.baseStateServiceIn.serviceName); + if(status != Smart::SMART_OK) return status; + status = connectLaserServiceIn(connections.laserServiceIn.serverName, connections.laserServiceIn.serviceName); + if(status != Smart::SMART_OK) return status; + status = connectNavigationVelocityServiceOut(connections.navigationVelocityServiceOut.serverName, connections.navigationVelocityServiceOut.serviceName); + if(status != Smart::SMART_OK) return status; + return status; +} + +/** + * Start all tasks contained in this component. + */ +void ComponentRosDock::startAllTasks() { + // start task DockActivity + if(connections.dockActivity.scheduler != "DEFAULT") { + ACE_Sched_Params dockActivity_SchedParams(ACE_SCHED_OTHER, ACE_THR_PRI_OTHER_DEF); + if(connections.dockActivity.scheduler == "FIFO") { + dockActivity_SchedParams.policy(ACE_SCHED_FIFO); + dockActivity_SchedParams.priority(ACE_THR_PRI_FIFO_MIN); + } else if(connections.dockActivity.scheduler == "RR") { + dockActivity_SchedParams.policy(ACE_SCHED_RR); + dockActivity_SchedParams.priority(ACE_THR_PRI_RR_MIN); + } + dockActivity->start(dockActivity_SchedParams, connections.dockActivity.cpuAffinity); + } else { + dockActivity->start(); + } + // start task ForwardActivity + if(connections.forwardActivity.scheduler != "DEFAULT") { + ACE_Sched_Params forwardActivity_SchedParams(ACE_SCHED_OTHER, ACE_THR_PRI_OTHER_DEF); + if(connections.forwardActivity.scheduler == "FIFO") { + forwardActivity_SchedParams.policy(ACE_SCHED_FIFO); + forwardActivity_SchedParams.priority(ACE_THR_PRI_FIFO_MIN); + } else if(connections.forwardActivity.scheduler == "RR") { + forwardActivity_SchedParams.policy(ACE_SCHED_RR); + forwardActivity_SchedParams.priority(ACE_THR_PRI_RR_MIN); + } + forwardActivity->start(forwardActivity_SchedParams, connections.forwardActivity.cpuAffinity); + } else { + forwardActivity->start(); + } + // start task UndockActivity + if(connections.undockActivity.scheduler != "DEFAULT") { + ACE_Sched_Params undockActivity_SchedParams(ACE_SCHED_OTHER, ACE_THR_PRI_OTHER_DEF); + if(connections.undockActivity.scheduler == "FIFO") { + undockActivity_SchedParams.policy(ACE_SCHED_FIFO); + undockActivity_SchedParams.priority(ACE_THR_PRI_FIFO_MIN); + } else if(connections.undockActivity.scheduler == "RR") { + undockActivity_SchedParams.policy(ACE_SCHED_RR); + undockActivity_SchedParams.priority(ACE_THR_PRI_RR_MIN); + } + undockActivity->start(undockActivity_SchedParams, connections.undockActivity.cpuAffinity); + } else { + undockActivity->start(); + } +} + +/** + * Start all timers contained in this component + */ +void ComponentRosDock::startAllTimers() { +} + + +Smart::TaskTriggerSubject* ComponentRosDock::getInputTaskTriggerFromString(const std::string &client) +{ + if(client == "BaseStateServiceIn") return baseStateServiceInInputTaskTrigger; + if(client == "LaserServiceIn") return laserServiceInInputTaskTrigger; + + return NULL; +} + + +void ComponentRosDock::init(int argc, char *argv[]) +{ + try { + Smart::StatusCode status; + + // load initial parameters from ini-file (if found) + loadParameter(argc, argv); + + // print out the actual parameters which are used to initialize the component + std::cout << " \nComponentDefinition Initial-Parameters:\n" << COMP->getParameters() << std::endl; + + // initializations of ComponentRosDockROSExtension + + // initializations of OpcUaBackendComponentGeneratorExtension + + // initializations of PlainOpcUaComponentRosDockExtension + + + // initialize all registered port-factories + for(auto portFactory = portFactoryRegistry.begin(); portFactory != portFactoryRegistry.end(); portFactory++) + { + portFactory->second->initialize(this, argc, argv); + } + + // initialize all registered component-extensions + for(auto extension = componentExtensionRegistry.begin(); extension != componentExtensionRegistry.end(); extension++) + { + extension->second->initialize(this, argc, argv); + } + + ComponentRosDockPortFactoryInterface *acePortFactory = portFactoryRegistry["ACE_SmartSoft"]; + if(acePortFactory == 0) { + std::cerr << "ERROR: acePortFactory NOT instantiated -> exit(-1)" << std::endl; + exit(-1); + } + + // this pointer is used for backwards compatibility (deprecated: should be removed as soon as all patterns, including coordination, are moved to port-factory) + SmartACE::SmartComponent *component = dynamic_cast(acePortFactory)->getComponentImpl(); + + std::cout << "ComponentDefinition ComponentRosDock is named " << connections.component.name << std::endl; + + if(connections.component.useLogger == true) { + //FIXME: use logging + //Smart::LOGGER->openLogFileInFolder("data/"+connections.component.name); + //Smart::LOGGER->startLogging(); + } + + // create event-test handlers (if needed) + robotDockingEventServiceOutEventTestHandler = std::make_shared(); + + // create server ports + // TODO: set minCycleTime from Ini-file + robotDockingEventServiceOutEventTestHandler = std::make_shared(); + robotDockingEventServiceOut = portFactoryRegistry[connections.robotDockingEventServiceOut.roboticMiddleware]->createRobotDockingEventServiceOut(connections.robotDockingEventServiceOut.serviceName, robotDockingEventServiceOutEventTestHandler); + + // create client ports + baseStateServiceIn = portFactoryRegistry[connections.baseStateServiceIn.roboticMiddleware]->createBaseStateServiceIn(); + laserServiceIn = portFactoryRegistry[connections.laserServiceIn.roboticMiddleware]->createLaserServiceIn(); + navigationVelocityServiceOut = portFactoryRegistry[connections.navigationVelocityServiceOut.roboticMiddleware]->createNavigationVelocityServiceOut(); + + // create InputTaskTriggers and UpcallManagers + baseStateServiceInInputTaskTrigger = new Smart::InputTaskTrigger(baseStateServiceIn); + baseStateServiceInUpcallManager = new BaseStateServiceInUpcallManager(baseStateServiceIn); + laserServiceInInputTaskTrigger = new Smart::InputTaskTrigger(laserServiceIn); + laserServiceInUpcallManager = new LaserServiceInUpcallManager(laserServiceIn); + + // create input-handler + + // create request-handlers + + // create state pattern + stateChangeHandler = new SmartStateChangeHandler(); + stateSlave = new SmartACE::StateSlave(component, stateChangeHandler); + if (stateSlave->defineStates("Docking" ,"dock") != Smart::SMART_OK) std::cerr << "ERROR: defining state combinaion Docking.dock" << std::endl; + if (stateSlave->defineStates("UnDocking" ,"unDock") != Smart::SMART_OK) std::cerr << "ERROR: defining state combinaion UnDocking.unDock" << std::endl; + status = stateSlave->setUpInitialState(connections.component.initialComponentMode); + if (status != Smart::SMART_OK) std::cerr << status << "; failed setting initial ComponentMode: " << connections.component.initialComponentMode << std::endl; + // activate state slave + status = stateSlave->activate(); + if(status != Smart::SMART_OK) std::cerr << "ERROR: activate state" << std::endl; + + wiringSlave = new SmartACE::WiringSlave(component); + // add client port to wiring slave + if(connections.baseStateServiceIn.roboticMiddleware == "ACE_SmartSoft") { + //FIXME: this must also work with other implementations + dynamic_cast*>(baseStateServiceIn)->add(wiringSlave, connections.baseStateServiceIn.wiringName); + } + if(connections.laserServiceIn.roboticMiddleware == "ACE_SmartSoft") { + //FIXME: this must also work with other implementations + dynamic_cast*>(laserServiceIn)->add(wiringSlave, connections.laserServiceIn.wiringName); + } + if(connections.navigationVelocityServiceOut.roboticMiddleware == "ACE_SmartSoft") { + //FIXME: this must also work with other implementations + dynamic_cast*>(navigationVelocityServiceOut)->add(wiringSlave, connections.navigationVelocityServiceOut.wiringName); + } + + // create parameter slave + param = new SmartACE::ParameterSlave(component, ¶mHandler); + + + // create Task DockActivity + dockActivity = new DockActivity(component); + // configure input-links + baseStateServiceInUpcallManager->attach(dockActivity); + // configure task-trigger (if task is configurable) + if(connections.dockActivity.trigger == "PeriodicTimer") { + // create PeriodicTimerTrigger + int microseconds = 1000*1000 / connections.dockActivity.periodicActFreq; + if(microseconds > 0) { + Smart::TimedTaskTrigger *triggerPtr = new Smart::TimedTaskTrigger(); + triggerPtr->attach(dockActivity); + component->getTimerManager()->scheduleTimer(triggerPtr, (void *) 0, std::chrono::microseconds(microseconds), std::chrono::microseconds(microseconds)); + // store trigger in class member + dockActivityTrigger = triggerPtr; + } else { + std::cerr << "ERROR: could not set-up Timer with cycle-time " << microseconds << " as activation source for Task DockActivity" << std::endl; + } + } else if(connections.dockActivity.trigger == "DataTriggered") { + dockActivityTrigger = getInputTaskTriggerFromString(connections.dockActivity.inPortRef); + if(dockActivityTrigger != NULL) { + dockActivityTrigger->attach(dockActivity, connections.dockActivity.prescale); + } else { + std::cerr << "ERROR: could not set-up InPort " << connections.dockActivity.inPortRef << " as activation source for Task DockActivity" << std::endl; + } + } else + { + // setup default task-trigger as PeriodicTimer + Smart::TimedTaskTrigger *triggerPtr = new Smart::TimedTaskTrigger(); + int microseconds = 1000*1000 / 1.0; + if(microseconds > 0) { + component->getTimerManager()->scheduleTimer(triggerPtr, (void *) 0, std::chrono::microseconds(microseconds), std::chrono::microseconds(microseconds)); + triggerPtr->attach(dockActivity); + // store trigger in class member + dockActivityTrigger = triggerPtr; + } else { + std::cerr << "ERROR: could not set-up Timer with cycle-time " << microseconds << " as activation source for Task DockActivity" << std::endl; + } + } + + // create Task ForwardActivity + forwardActivity = new ForwardActivity(component); + // configure input-links + laserServiceInUpcallManager->attach(forwardActivity); + // configure task-trigger (if task is configurable) + if(connections.forwardActivity.trigger == "PeriodicTimer") { + // create PeriodicTimerTrigger + int microseconds = 1000*1000 / connections.forwardActivity.periodicActFreq; + if(microseconds > 0) { + Smart::TimedTaskTrigger *triggerPtr = new Smart::TimedTaskTrigger(); + triggerPtr->attach(forwardActivity); + component->getTimerManager()->scheduleTimer(triggerPtr, (void *) 0, std::chrono::microseconds(microseconds), std::chrono::microseconds(microseconds)); + // store trigger in class member + forwardActivityTrigger = triggerPtr; + } else { + std::cerr << "ERROR: could not set-up Timer with cycle-time " << microseconds << " as activation source for Task ForwardActivity" << std::endl; + } + } else if(connections.forwardActivity.trigger == "DataTriggered") { + forwardActivityTrigger = getInputTaskTriggerFromString(connections.forwardActivity.inPortRef); + if(forwardActivityTrigger != NULL) { + forwardActivityTrigger->attach(forwardActivity, connections.forwardActivity.prescale); + } else { + std::cerr << "ERROR: could not set-up InPort " << connections.forwardActivity.inPortRef << " as activation source for Task ForwardActivity" << std::endl; + } + } else + { + // setup default task-trigger as PeriodicTimer + Smart::TimedTaskTrigger *triggerPtr = new Smart::TimedTaskTrigger(); + int microseconds = 1000*1000 / 1.0; + if(microseconds > 0) { + component->getTimerManager()->scheduleTimer(triggerPtr, (void *) 0, std::chrono::microseconds(microseconds), std::chrono::microseconds(microseconds)); + triggerPtr->attach(forwardActivity); + // store trigger in class member + forwardActivityTrigger = triggerPtr; + } else { + std::cerr << "ERROR: could not set-up Timer with cycle-time " << microseconds << " as activation source for Task ForwardActivity" << std::endl; + } + } + + // create Task UndockActivity + undockActivity = new UndockActivity(component); + // configure input-links + baseStateServiceInUpcallManager->attach(undockActivity); + // configure task-trigger (if task is configurable) + if(connections.undockActivity.trigger == "PeriodicTimer") { + // create PeriodicTimerTrigger + int microseconds = 1000*1000 / connections.undockActivity.periodicActFreq; + if(microseconds > 0) { + Smart::TimedTaskTrigger *triggerPtr = new Smart::TimedTaskTrigger(); + triggerPtr->attach(undockActivity); + component->getTimerManager()->scheduleTimer(triggerPtr, (void *) 0, std::chrono::microseconds(microseconds), std::chrono::microseconds(microseconds)); + // store trigger in class member + undockActivityTrigger = triggerPtr; + } else { + std::cerr << "ERROR: could not set-up Timer with cycle-time " << microseconds << " as activation source for Task UndockActivity" << std::endl; + } + } else if(connections.undockActivity.trigger == "DataTriggered") { + undockActivityTrigger = getInputTaskTriggerFromString(connections.undockActivity.inPortRef); + if(undockActivityTrigger != NULL) { + undockActivityTrigger->attach(undockActivity, connections.undockActivity.prescale); + } else { + std::cerr << "ERROR: could not set-up InPort " << connections.undockActivity.inPortRef << " as activation source for Task UndockActivity" << std::endl; + } + } else + { + // setup default task-trigger as PeriodicTimer + Smart::TimedTaskTrigger *triggerPtr = new Smart::TimedTaskTrigger(); + int microseconds = 1000*1000 / 1.0; + if(microseconds > 0) { + component->getTimerManager()->scheduleTimer(triggerPtr, (void *) 0, std::chrono::microseconds(microseconds), std::chrono::microseconds(microseconds)); + triggerPtr->attach(undockActivity); + // store trigger in class member + undockActivityTrigger = triggerPtr; + } else { + std::cerr << "ERROR: could not set-up Timer with cycle-time " << microseconds << " as activation source for Task UndockActivity" << std::endl; + } + } + + + // link observers with subjects + } catch (const std::exception &ex) { + std::cerr << "Uncaught std exception" << ex.what() << std::endl; + } catch (...) { + std::cerr << "Uncaught exception" << std::endl; + } +} + +// run the component +void ComponentRosDock::run() +{ + stateSlave->acquire("init"); + // startup all registered port-factories + for(auto portFactory = portFactoryRegistry.begin(); portFactory != portFactoryRegistry.end(); portFactory++) + { + portFactory->second->onStartup(); + } + + // startup all registered component-extensions + for(auto extension = componentExtensionRegistry.begin(); extension != componentExtensionRegistry.end(); extension++) + { + extension->second->onStartup(); + } + stateSlave->release("init"); + + // do not call this handler within the init state (see above) as this handler internally calls setStartupFinished() (this should be fixed in future) + compHandler.onStartup(); + + // this call blocks until the component is commanded to shutdown + stateSlave->acquire("shutdown"); + + // shutdown all registered component-extensions + for(auto extension = componentExtensionRegistry.begin(); extension != componentExtensionRegistry.end(); extension++) + { + extension->second->onShutdown(); + } + + // shutdown all registered port-factories + for(auto portFactory = portFactoryRegistry.begin(); portFactory != portFactoryRegistry.end(); portFactory++) + { + portFactory->second->onShutdown(); + } + + if(connections.component.useLogger == true) { + //FIXME: use logging + //Smart::LOGGER->stopLogging(); + } + + compHandler.onShutdown(); + + stateSlave->release("shutdown"); +} + +// clean-up component's resources +void ComponentRosDock::fini() +{ + // unlink all observers + + // destroy all task instances + // unlink all UpcallManagers + baseStateServiceInUpcallManager->detach(dockActivity); + // unlink the TaskTrigger + if(dockActivityTrigger != NULL){ + dockActivityTrigger->detach(dockActivity); + delete dockActivity; + } + // unlink all UpcallManagers + laserServiceInUpcallManager->detach(forwardActivity); + // unlink the TaskTrigger + if(forwardActivityTrigger != NULL){ + forwardActivityTrigger->detach(forwardActivity); + delete forwardActivity; + } + // unlink all UpcallManagers + baseStateServiceInUpcallManager->detach(undockActivity); + // unlink the TaskTrigger + if(undockActivityTrigger != NULL){ + undockActivityTrigger->detach(undockActivity); + delete undockActivity; + } + + // destroy all input-handler + + // destroy InputTaskTriggers and UpcallManagers + delete baseStateServiceInInputTaskTrigger; + delete baseStateServiceInUpcallManager; + delete laserServiceInInputTaskTrigger; + delete laserServiceInUpcallManager; + + // destroy client ports + delete baseStateServiceIn; + delete laserServiceIn; + delete navigationVelocityServiceOut; + + // destroy server ports + delete robotDockingEventServiceOut; + // destroy event-test handlers (if needed) + robotDockingEventServiceOutEventTestHandler; + + // destroy request-handlers + + delete stateSlave; + // destroy state-change-handler + delete stateChangeHandler; + + // destroy all master/slave ports + delete wiringSlave; + delete param; + + + // destroy all registered component-extensions + for(auto extension = componentExtensionRegistry.begin(); extension != componentExtensionRegistry.end(); extension++) + { + extension->second->destroy(); + } + + // destroy all registered port-factories + for(auto portFactory = portFactoryRegistry.begin(); portFactory != portFactoryRegistry.end(); portFactory++) + { + portFactory->second->destroy(); + } + + // destruction of ComponentRosDockROSExtension + + // destruction of OpcUaBackendComponentGeneratorExtension + + // destruction of PlainOpcUaComponentRosDockExtension + +} + +void ComponentRosDock::loadParameter(int argc, char *argv[]) +{ + /* + Parameters can be specified via command line --filename= or -f + + With this parameter present: + - The component will look for the file in the current working directory, + a path relative to the current directory or any absolute path + - The component will use the default values if the file cannot be found + + With this parameter absent: + - .ini will be read from current working directory, if found there + - $SMART_ROOT/etc/.ini will be read otherwise + - Default values will be used if neither found in working directory or /etc + */ + SmartACE::SmartIniParameter parameter; + std::ifstream parameterfile; + bool parameterFileFound = false; + + // load parameters + try + { + // if paramfile is given as argument + if(parameter.tryAddFileFromArgs(argc,argv,"filename", 'f')) + { + parameterFileFound = true; + std::cout << "parameter file is loaded from an argv argument \n"; + } else if(parameter.searchFile("ComponentRosDock.ini", parameterfile)) { + parameterFileFound = true; + std::cout << "load ComponentRosDock.ini parameter file\n"; + parameter.addFile(parameterfile); + } else { + std::cout << "WARNING: ComponentRosDock.ini parameter file not found! (using default values or command line arguments)\n"; + } + + // add command line arguments to allow overwriting of parameters + // from file + parameter.addCommandLineArgs(argc,argv,"component"); + + // initialize the naming service using the command line parameters parsed in the + // SmartIniParameter class. The naming service parameters are expected to be in + // the "component" parameter group. + SmartACE::NAMING::instance()->checkForHelpArg(argc,argv); + if(parameterFileFound) + { + if(SmartACE::NAMING::instance()->init(parameter.getAllParametersFromGroup("component")) != 0) { + // initialization of naming service failed + throw std::logic_error( " Service initialization failed!\nPossible causes could be:\n-> Erroneous configuration.\n-> Naming service not reachable.\n" ); + } + } else { + if(SmartACE::NAMING::instance()->init(argc, argv) != 0) { + // initialization of naming service failed + throw std::logic_error( " Service initialization failed!\nPossible causes could be:\n-> Erroneous configuration.\n-> Naming service not reachable.\n" ); + } + } + + // print all known parameters + // parameter.print(); + + //--- server port // client port // other parameter --- + // load parameter + parameter.getString("component", "name", connections.component.name); + parameter.getString("component", "initialComponentMode", connections.component.initialComponentMode); + if(parameter.checkIfParameterExists("component", "defaultScheduler")) { + parameter.getString("component", "defaultScheduler", connections.component.defaultScheduler); + } + if(parameter.checkIfParameterExists("component", "useLogger")) { + parameter.getBoolean("component", "useLogger", connections.component.useLogger); + } + + // load parameters for client BaseStateServiceIn + parameter.getString("BaseStateServiceIn", "serviceName", connections.baseStateServiceIn.serviceName); + parameter.getString("BaseStateServiceIn", "serverName", connections.baseStateServiceIn.serverName); + parameter.getString("BaseStateServiceIn", "wiringName", connections.baseStateServiceIn.wiringName); + parameter.getInteger("BaseStateServiceIn", "interval", connections.baseStateServiceIn.interval); + if(parameter.checkIfParameterExists("BaseStateServiceIn", "roboticMiddleware")) { + parameter.getString("BaseStateServiceIn", "roboticMiddleware", connections.baseStateServiceIn.roboticMiddleware); + } + // load parameters for client LaserServiceIn + parameter.getString("LaserServiceIn", "serviceName", connections.laserServiceIn.serviceName); + parameter.getString("LaserServiceIn", "serverName", connections.laserServiceIn.serverName); + parameter.getString("LaserServiceIn", "wiringName", connections.laserServiceIn.wiringName); + parameter.getInteger("LaserServiceIn", "interval", connections.laserServiceIn.interval); + if(parameter.checkIfParameterExists("LaserServiceIn", "roboticMiddleware")) { + parameter.getString("LaserServiceIn", "roboticMiddleware", connections.laserServiceIn.roboticMiddleware); + } + // load parameters for client NavigationVelocityServiceOut + parameter.getBoolean("NavigationVelocityServiceOut", "initialConnect", connections.navigationVelocityServiceOut.initialConnect); + parameter.getString("NavigationVelocityServiceOut", "serviceName", connections.navigationVelocityServiceOut.serviceName); + parameter.getString("NavigationVelocityServiceOut", "serverName", connections.navigationVelocityServiceOut.serverName); + parameter.getString("NavigationVelocityServiceOut", "wiringName", connections.navigationVelocityServiceOut.wiringName); + if(parameter.checkIfParameterExists("NavigationVelocityServiceOut", "roboticMiddleware")) { + parameter.getString("NavigationVelocityServiceOut", "roboticMiddleware", connections.navigationVelocityServiceOut.roboticMiddleware); + } + + // load parameters for server RobotDockingEventServiceOut + parameter.getString("RobotDockingEventServiceOut", "serviceName", connections.robotDockingEventServiceOut.serviceName); + if(parameter.checkIfParameterExists("RobotDockingEventServiceOut", "roboticMiddleware")) { + parameter.getString("RobotDockingEventServiceOut", "roboticMiddleware", connections.robotDockingEventServiceOut.roboticMiddleware); + } + + // load parameters for task DockActivity + parameter.getDouble("DockActivity", "minActFreqHz", connections.dockActivity.minActFreq); + parameter.getDouble("DockActivity", "maxActFreqHz", connections.dockActivity.maxActFreq); + parameter.getString("DockActivity", "triggerType", connections.dockActivity.trigger); + if(connections.dockActivity.trigger == "PeriodicTimer") { + parameter.getDouble("DockActivity", "periodicActFreqHz", connections.dockActivity.periodicActFreq); + } else if(connections.dockActivity.trigger == "DataTriggered") { + parameter.getString("DockActivity", "inPortRef", connections.dockActivity.inPortRef); + parameter.getInteger("DockActivity", "prescale", connections.dockActivity.prescale); + } + if(parameter.checkIfParameterExists("DockActivity", "scheduler")) { + parameter.getString("DockActivity", "scheduler", connections.dockActivity.scheduler); + } + if(parameter.checkIfParameterExists("DockActivity", "priority")) { + parameter.getInteger("DockActivity", "priority", connections.dockActivity.priority); + } + if(parameter.checkIfParameterExists("DockActivity", "cpuAffinity")) { + parameter.getInteger("DockActivity", "cpuAffinity", connections.dockActivity.cpuAffinity); + } + // load parameters for task ForwardActivity + parameter.getDouble("ForwardActivity", "minActFreqHz", connections.forwardActivity.minActFreq); + parameter.getDouble("ForwardActivity", "maxActFreqHz", connections.forwardActivity.maxActFreq); + parameter.getString("ForwardActivity", "triggerType", connections.forwardActivity.trigger); + if(connections.forwardActivity.trigger == "PeriodicTimer") { + parameter.getDouble("ForwardActivity", "periodicActFreqHz", connections.forwardActivity.periodicActFreq); + } else if(connections.forwardActivity.trigger == "DataTriggered") { + parameter.getString("ForwardActivity", "inPortRef", connections.forwardActivity.inPortRef); + parameter.getInteger("ForwardActivity", "prescale", connections.forwardActivity.prescale); + } + if(parameter.checkIfParameterExists("ForwardActivity", "scheduler")) { + parameter.getString("ForwardActivity", "scheduler", connections.forwardActivity.scheduler); + } + if(parameter.checkIfParameterExists("ForwardActivity", "priority")) { + parameter.getInteger("ForwardActivity", "priority", connections.forwardActivity.priority); + } + if(parameter.checkIfParameterExists("ForwardActivity", "cpuAffinity")) { + parameter.getInteger("ForwardActivity", "cpuAffinity", connections.forwardActivity.cpuAffinity); + } + // load parameters for task UndockActivity + parameter.getDouble("UndockActivity", "minActFreqHz", connections.undockActivity.minActFreq); + parameter.getDouble("UndockActivity", "maxActFreqHz", connections.undockActivity.maxActFreq); + parameter.getString("UndockActivity", "triggerType", connections.undockActivity.trigger); + if(connections.undockActivity.trigger == "PeriodicTimer") { + parameter.getDouble("UndockActivity", "periodicActFreqHz", connections.undockActivity.periodicActFreq); + } else if(connections.undockActivity.trigger == "DataTriggered") { + parameter.getString("UndockActivity", "inPortRef", connections.undockActivity.inPortRef); + parameter.getInteger("UndockActivity", "prescale", connections.undockActivity.prescale); + } + if(parameter.checkIfParameterExists("UndockActivity", "scheduler")) { + parameter.getString("UndockActivity", "scheduler", connections.undockActivity.scheduler); + } + if(parameter.checkIfParameterExists("UndockActivity", "priority")) { + parameter.getInteger("UndockActivity", "priority", connections.undockActivity.priority); + } + if(parameter.checkIfParameterExists("UndockActivity", "cpuAffinity")) { + parameter.getInteger("UndockActivity", "cpuAffinity", connections.undockActivity.cpuAffinity); + } + + // load parameters for ComponentRosDockROSExtension + + // load parameters for OpcUaBackendComponentGeneratorExtension + + // load parameters for PlainOpcUaComponentRosDockExtension + + + // load parameters for all registered component-extensions + for(auto extension = componentExtensionRegistry.begin(); extension != componentExtensionRegistry.end(); extension++) + { + extension->second->loadParameters(parameter); + } + + paramHandler.loadParameter(parameter); + + } catch (const SmartACE::IniParameterError & e) { + std::cerr << e.what() << std::endl; + } catch (const std::exception &ex) { + std::cerr << "Uncaught std::exception: " << ex.what() << std::endl; + } catch (...) { + std::cerr << "Uncaught exception" << std::endl; + } +} diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentRosDock.hh b/ComponentRosDock/smartsoft/src-gen/ComponentRosDock.hh new file mode 100644 index 00000000..c420d72a --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentRosDock.hh @@ -0,0 +1,324 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _COMPONENTROSDOCK_HH +#define _COMPONENTROSDOCK_HH + +#include +#include +#include "aceSmartSoft.hh" +#include "smartQueryServerTaskTrigger_T.h" +#include "ComponentRosDockCore.hh" + +#include "ComponentRosDockPortFactoryInterface.hh" +#include "ComponentRosDockExtension.hh" + +// forward declarations +class ComponentRosDockPortFactoryInterface; +class ComponentRosDockExtension; + +// includes for ComponentRosDockROSExtension +#include "ComponentRosDockRosPortBaseClass.hh" + +// includes for OpcUaBackendComponentGeneratorExtension + +// includes for PlainOpcUaComponentRosDockExtension +// include plain OPC UA device clients +// include plain OPC UA status servers + + +// include communication objects +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// include tasks +#include "DockActivity.hh" +#include "ForwardActivity.hh" +#include "UndockActivity.hh" +// include UpcallManagers +#include "BaseStateServiceInUpcallManager.hh" +#include "LaserServiceInUpcallManager.hh" + +// include input-handler(s) +// include request-handler(s) + +// include handler +#include "CompHandler.hh" + +#include "ParameterStateStruct.hh" +#include "ParameterUpdateHandler.hh" + +#include "SmartStateChangeHandler.hh" + +#define COMP ComponentRosDock::instance() + +class ComponentRosDock : public ComponentRosDockCore { +private: + static ComponentRosDock *_componentRosDock; + + // constructor + ComponentRosDock(); + + // copy-constructor + ComponentRosDock(const ComponentRosDock& cc); + + // destructor + ~ComponentRosDock() { }; + + // load parameter from ini file + void loadParameter(int argc, char* argv[]); + + // instantiate comp-handler + CompHandler compHandler; + + // helper method that maps a string-name to an according TaskTriggerSubject + Smart::TaskTriggerSubject* getInputTaskTriggerFromString(const std::string &client); + + // internal map storing the different port-creation factories (that internally map to specific middleware implementations) + std::map portFactoryRegistry; + + // internal map storing various extensions of this component class + std::map componentExtensionRegistry; + +public: + ParameterStateStruct getGlobalState() const + { + return paramHandler.getGlobalState(); + } + + ParameterStateStruct getParameters() const + { + return paramHandler.getGlobalState(); + } + + // define tasks + Smart::TaskTriggerSubject* dockActivityTrigger; + DockActivity *dockActivity; + Smart::TaskTriggerSubject* forwardActivityTrigger; + ForwardActivity *forwardActivity; + Smart::TaskTriggerSubject* undockActivityTrigger; + UndockActivity *undockActivity; + + // define input-ports + // InputPort BaseStateServiceIn + Smart::IPushClientPattern *baseStateServiceIn; + Smart::InputTaskTrigger *baseStateServiceInInputTaskTrigger; + BaseStateServiceInUpcallManager *baseStateServiceInUpcallManager; + // InputPort LaserServiceIn + Smart::IPushClientPattern *laserServiceIn; + Smart::InputTaskTrigger *laserServiceInInputTaskTrigger; + LaserServiceInUpcallManager *laserServiceInUpcallManager; + + // define request-ports + + // define input-handler + + // define output-ports + Smart::ISendClientPattern *navigationVelocityServiceOut; + Smart::IEventServerPattern *robotDockingEventServiceOut; + std::shared_ptr> robotDockingEventServiceOutEventTestHandler; + + // define answer-ports + + // define request-handlers + + // definitions of ComponentRosDockROSExtension + ComponentRosDockRosPortBaseClass *rosPorts; + + // definitions of OpcUaBackendComponentGeneratorExtension + + // definitions of PlainOpcUaComponentRosDockExtension + + + // define default slave ports + SmartACE::StateSlave *stateSlave; + SmartStateChangeHandler *stateChangeHandler; + SmartACE::WiringSlave *wiringSlave; + ParamUpdateHandler paramHandler; + SmartACE::ParameterSlave *param; + + + /// this method is used to register different PortFactory classes (one for each supported middleware framework) + void addPortFactory(const std::string &name, ComponentRosDockPortFactoryInterface *portFactory); + + SmartACE::SmartComponent* getComponentImpl(); + + /// this method is used to register different component-extension classes + void addExtension(ComponentRosDockExtension *extension); + + /// this method allows to access the registered component-extensions (automatically converting to the actuall implementation type) + template + T* getExtension(const std::string &name) { + auto it = componentExtensionRegistry.find(name); + if(it != componentExtensionRegistry.end()) { + return dynamic_cast(it->second); + } + return 0; + } + + /// initialize component's internal members + void init(int argc, char *argv[]); + + /// execute the component's infrastructure + void run(); + + /// clean-up component's resources + void fini(); + + /// call this method to set the overall component into the Alive state (i.e. component is then ready to operate) + void setStartupFinished(); + + /// connect all component's client ports + Smart::StatusCode connectAndStartAllServices(); + + /// start all assocuated Activities + void startAllTasks(); + + /// start all associated timers + void startAllTimers(); + + Smart::StatusCode connectBaseStateServiceIn(const std::string &serverName, const std::string &serviceName); + Smart::StatusCode connectLaserServiceIn(const std::string &serverName, const std::string &serviceName); + Smart::StatusCode connectNavigationVelocityServiceOut(const std::string &serverName, const std::string &serviceName); + + // return singleton instance + static ComponentRosDock* instance() + { + if(_componentRosDock == 0) { + _componentRosDock = new ComponentRosDock(); + } + return _componentRosDock; + } + + static void deleteInstance() { + if(_componentRosDock != 0) { + delete _componentRosDock; + } + } + + // connections parameter + struct connections_struct + { + // component struct + struct component_struct + { + // the name of the component + std::string name; + std::string initialComponentMode; + std::string defaultScheduler; + bool useLogger; + } component; + + //--- task parameter --- + struct DockActivity_struct { + double minActFreq; + double maxActFreq; + std::string trigger; + // only one of the following two params is + // actually used at run-time according + // to the system config model + double periodicActFreq; + // or + std::string inPortRef; + int prescale; + // scheduling parameters + std::string scheduler; + int priority; + int cpuAffinity; + } dockActivity; + struct ForwardActivity_struct { + double minActFreq; + double maxActFreq; + std::string trigger; + // only one of the following two params is + // actually used at run-time according + // to the system config model + double periodicActFreq; + // or + std::string inPortRef; + int prescale; + // scheduling parameters + std::string scheduler; + int priority; + int cpuAffinity; + } forwardActivity; + struct UndockActivity_struct { + double minActFreq; + double maxActFreq; + std::string trigger; + // only one of the following two params is + // actually used at run-time according + // to the system config model + double periodicActFreq; + // or + std::string inPortRef; + int prescale; + // scheduling parameters + std::string scheduler; + int priority; + int cpuAffinity; + } undockActivity; + + //--- upcall parameter --- + + //--- server port parameter --- + struct RobotDockingEventServiceOut_struct { + std::string serviceName; + std::string roboticMiddleware; + } robotDockingEventServiceOut; + + //--- client port parameter --- + struct BaseStateServiceIn_struct { + std::string serverName; + std::string serviceName; + std::string wiringName; + long interval; + std::string roboticMiddleware; + } baseStateServiceIn; + struct LaserServiceIn_struct { + std::string serverName; + std::string serviceName; + std::string wiringName; + long interval; + std::string roboticMiddleware; + } laserServiceIn; + struct NavigationVelocityServiceOut_struct { + bool initialConnect; + std::string serverName; + std::string serviceName; + std::string wiringName; + long interval; + std::string roboticMiddleware; + } navigationVelocityServiceOut; + + // -- parameters for ComponentRosDockROSExtension + + // -- parameters for OpcUaBackendComponentGeneratorExtension + + // -- parameters for PlainOpcUaComponentRosDockExtension + + } connections; +}; +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentRosDock.ini.head b/ComponentRosDock/smartsoft/src-gen/ComponentRosDock.ini.head new file mode 100644 index 00000000..b276be34 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentRosDock.ini.head @@ -0,0 +1,133 @@ +[smartsoft] +#-------------------------------------------------------------------------- +# Code generated by the SmartSoft MDSD Toolchain +# The SmartSoft Toolchain has been developed by: +# +# Service Robotics Research Center +# University of Applied Sciences Ulm +# Prittwitzstr. 10 +# 89075 Ulm (Germany) +# +# Information about the SmartSoft MDSD Toolchain is available at: +# www.servicerobotik-ulm.de +# +# Please do not modify this file. It will be re-generated +# running the code generator. +#-------------------------------------------------------------------------- + +########################## +# component parameters + +[component] + +# the name of the component for the naming service +name ComponentRosDock + +# the initial ComponentMode +initialComponentMode Neutral +#defaultScheduler FIFO +#useLogger true + +########################## +# Task parameters + +[DockActivity] +minActFreqHz 0.0 +maxActFreqHz 0.0 +# setup default trigger as PeriodicTimer +triggerType PeriodicTimer +periodicActFreqHz 1.0 +# other trigger-types are: +#triggerType PeriodicTimer +#periodicActFreqHz 0.0 +# or alternatively: +#triggerType DataTriggered +#inPortRef +#prescale 1 +# optional scheduling parameters +# scheduler FIFO +# priority 0 +# cpuAffinity 0 + +[ForwardActivity] +minActFreqHz 0.0 +maxActFreqHz 0.0 +# setup default trigger as PeriodicTimer +triggerType PeriodicTimer +periodicActFreqHz 1.0 +# other trigger-types are: +#triggerType PeriodicTimer +#periodicActFreqHz 0.0 +# or alternatively: +#triggerType DataTriggered +#inPortRef +#prescale 1 +# optional scheduling parameters +# scheduler FIFO +# priority 0 +# cpuAffinity 0 + +[UndockActivity] +minActFreqHz 0.0 +maxActFreqHz 0.0 +# setup default trigger as PeriodicTimer +triggerType PeriodicTimer +periodicActFreqHz 1.0 +# other trigger-types are: +#triggerType PeriodicTimer +#periodicActFreqHz 0.0 +# or alternatively: +#triggerType DataTriggered +#inPortRef +#prescale 1 +# optional scheduling parameters +# scheduler FIFO +# priority 0 +# cpuAffinity 0 + +########################## +# InputHandler parameters + +########################## +# server port parameter + +[RobotDockingEventServiceOut] +serviceName RobotDockingEventServiceOut +roboticMiddleware ACE_SmartSoft + +########################## +# client port parameter + +[BaseStateServiceIn] +serverName unknown +serviceName unknown +wiringName BaseStateServiceIn +interval 1 +roboticMiddleware ACE_SmartSoft + +[LaserServiceIn] +serverName unknown +serviceName unknown +wiringName LaserServiceIn +interval 1 +roboticMiddleware ACE_SmartSoft + +[NavigationVelocityServiceOut] +initialConnect false +serverName unknown +serviceName unknown +wiringName NavigationVelocityServiceOut +interval 1 +roboticMiddleware ACE_SmartSoft + +########################## +## parameters of ComponentRosDockROSExtension + +########################## +## parameters of OpcUaBackendComponentGeneratorExtension + +########################## +## parameters of PlainOpcUaComponentRosDockExtension + + + diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentRosDock.ini.template b/ComponentRosDock/smartsoft/src-gen/ComponentRosDock.ini.template new file mode 100644 index 00000000..329c0ff3 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentRosDock.ini.template @@ -0,0 +1,144 @@ +[smartsoft] +#-------------------------------------------------------------------------- +# Code generated by the SmartSoft MDSD Toolchain +# The SmartSoft Toolchain has been developed by: +# +# Service Robotics Research Center +# University of Applied Sciences Ulm +# Prittwitzstr. 10 +# 89075 Ulm (Germany) +# +# Information about the SmartSoft MDSD Toolchain is available at: +# www.servicerobotik-ulm.de +# +# Please do not modify this file. It will be re-generated +# running the code generator. +#-------------------------------------------------------------------------- + +########################## +# component parameters + +[component] + +# the name of the component for the naming service +name ComponentRosDock + +# the initial ComponentMode +initialComponentMode Neutral +#defaultScheduler FIFO +#useLogger true + +########################## +# Task parameters + +[DockActivity] +minActFreqHz 0.0 +maxActFreqHz 0.0 +# setup default trigger as PeriodicTimer +triggerType PeriodicTimer +periodicActFreqHz 1.0 +# other trigger-types are: +#triggerType PeriodicTimer +#periodicActFreqHz 0.0 +# or alternatively: +#triggerType DataTriggered +#inPortRef +#prescale 1 +# optional scheduling parameters +# scheduler FIFO +# priority 0 +# cpuAffinity 0 + +[ForwardActivity] +minActFreqHz 0.0 +maxActFreqHz 0.0 +# setup default trigger as PeriodicTimer +triggerType PeriodicTimer +periodicActFreqHz 1.0 +# other trigger-types are: +#triggerType PeriodicTimer +#periodicActFreqHz 0.0 +# or alternatively: +#triggerType DataTriggered +#inPortRef +#prescale 1 +# optional scheduling parameters +# scheduler FIFO +# priority 0 +# cpuAffinity 0 + +[UndockActivity] +minActFreqHz 0.0 +maxActFreqHz 0.0 +# setup default trigger as PeriodicTimer +triggerType PeriodicTimer +periodicActFreqHz 1.0 +# other trigger-types are: +#triggerType PeriodicTimer +#periodicActFreqHz 0.0 +# or alternatively: +#triggerType DataTriggered +#inPortRef +#prescale 1 +# optional scheduling parameters +# scheduler FIFO +# priority 0 +# cpuAffinity 0 + +########################## +# InputHandler parameters + +########################## +# server port parameter + +[RobotDockingEventServiceOut] +serviceName RobotDockingEventServiceOut +roboticMiddleware ACE_SmartSoft + +########################## +# client port parameter + +[BaseStateServiceIn] +serverName unknown +serviceName unknown +wiringName BaseStateServiceIn +interval 1 +roboticMiddleware ACE_SmartSoft + +[LaserServiceIn] +serverName unknown +serviceName unknown +wiringName LaserServiceIn +interval 1 +roboticMiddleware ACE_SmartSoft + +[NavigationVelocityServiceOut] +initialConnect false +serverName unknown +serviceName unknown +wiringName NavigationVelocityServiceOut +interval 1 +roboticMiddleware ACE_SmartSoft + +########################## +## parameters of ComponentRosDockROSExtension + +########################## +## parameters of OpcUaBackendComponentGeneratorExtension + +########################## +## parameters of PlainOpcUaComponentRosDockExtension + + + +##################### +# internal parameters +##################### + +##################### +# extended parameters +##################### + +##################### +# instance parameters +##################### diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentRosDockAcePortFactory.cc b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockAcePortFactory.cc new file mode 100644 index 00000000..eefccfe5 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockAcePortFactory.cc @@ -0,0 +1,93 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#include "ComponentRosDockAcePortFactory.hh" + +// create a static instance of the default AcePortFactory +static ComponentRosDockAcePortFactory acePortFactory; + +ComponentRosDockAcePortFactory::ComponentRosDockAcePortFactory() +{ + componentImpl = 0; + ComponentRosDock::instance()->addPortFactory("ACE_SmartSoft", this); +} + +ComponentRosDockAcePortFactory::~ComponentRosDockAcePortFactory() +{ } + +void ComponentRosDockAcePortFactory::initialize(ComponentRosDock *component, int argc, char* argv[]) +{ + if(component->connections.component.defaultScheduler != "DEFAULT") { + ACE_Sched_Params sched_params(ACE_SCHED_OTHER, ACE_THR_PRI_OTHER_DEF); + if(component->connections.component.defaultScheduler == "FIFO") { + sched_params.policy(ACE_SCHED_FIFO); + sched_params.priority(ACE_THR_PRI_FIFO_MIN); + } else if(component->connections.component.defaultScheduler == "RR") { + sched_params.policy(ACE_SCHED_RR); + sched_params.priority(ACE_THR_PRI_RR_MIN); + } + // create new instance of the SmartSoft component with customized scheuling parameters + componentImpl = new ComponentRosDockImpl(component->connections.component.name, argc, argv, sched_params); + } else { + // create new instance of the SmartSoft component + componentImpl = new ComponentRosDockImpl(component->connections.component.name, argc, argv); + } +} + +int ComponentRosDockAcePortFactory::onStartup() +{ + return componentImpl->startComponentInfrastructure(); +} + +Smart::IPushClientPattern * ComponentRosDockAcePortFactory::createBaseStateServiceIn() +{ + return new SmartACE::PushClient(componentImpl); +} + +Smart::IPushClientPattern * ComponentRosDockAcePortFactory::createLaserServiceIn() +{ + return new SmartACE::PushClient(componentImpl); +} + +Smart::ISendClientPattern * ComponentRosDockAcePortFactory::createNavigationVelocityServiceOut() +{ + return new SmartACE::SendClient(componentImpl); +} + + +Smart::IEventServerPattern * ComponentRosDockAcePortFactory::createRobotDockingEventServiceOut(const std::string &serviceName, std::shared_ptr> robotDockingEventServiceOutEventTestHandler) +{ + return new SmartACE::EventServer(componentImpl, serviceName, robotDockingEventServiceOutEventTestHandler); +} + + +SmartACE::SmartComponent* ComponentRosDockAcePortFactory::getComponentImpl() +{ + return componentImpl; +} + +int ComponentRosDockAcePortFactory::onShutdown(const std::chrono::steady_clock::duration &timeoutTime) +{ + componentImpl->stopComponentInfrastructure(timeoutTime); + return 0; +} + +void ComponentRosDockAcePortFactory::destroy() +{ + // clean-up component's internally used resources (internally used communication middleware) + componentImpl->cleanUpComponentResources(); + delete componentImpl; +} diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentRosDockAcePortFactory.hh b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockAcePortFactory.hh new file mode 100644 index 00000000..0fee08cb --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockAcePortFactory.hh @@ -0,0 +1,50 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef COMPONENTROSDOCK_ACE_PORTFACTORY_HH_ +#define COMPONENTROSDOCK_ACE_PORTFACTORY_HH_ + +// include ACE/SmartSoft component implementation +#include "ComponentRosDockImpl.hh" + +// include the main component-definition class +#include "ComponentRosDockPortFactoryInterface.hh" + +class ComponentRosDockAcePortFactory: public ComponentRosDockPortFactoryInterface +{ +private: + ComponentRosDockImpl *componentImpl; +public: + ComponentRosDockAcePortFactory(); + virtual ~ComponentRosDockAcePortFactory(); + + virtual void initialize(ComponentRosDock *component, int argc, char* argv[]) override; + virtual int onStartup() override; + + virtual Smart::IPushClientPattern * createBaseStateServiceIn() override; + virtual Smart::IPushClientPattern * createLaserServiceIn() override; + virtual Smart::ISendClientPattern * createNavigationVelocityServiceOut() override; + + virtual Smart::IEventServerPattern * createRobotDockingEventServiceOut(const std::string &serviceName, std::shared_ptr> robotDockingEventServiceOutEventTestHandler) override; + + // get a pointer to the internal component implementation + SmartACE::SmartComponent* getComponentImpl(); + + virtual int onShutdown(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)) override; + virtual void destroy() override; +}; + +#endif /* COMPONENTROSDOCK_ACE_PORTFACTORY_HH_ */ diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentRosDockDependencies.cmake.in b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockDependencies.cmake.in new file mode 100644 index 00000000..296c55a5 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockDependencies.cmake.in @@ -0,0 +1,21 @@ +#-------------------------------------------------------------------------- +# Code generated by the SmartSoft MDSD Toolchain +# The SmartSoft Toolchain has been developed by: +# +# Service Robotics Research Center +# University of Applied Sciences Ulm +# Prittwitzstr. 10 +# 89075 Ulm (Germany) +# +# Information about the SmartSoft MDSD Toolchain is available at: +# www.servicerobotik-ulm.de +# +# Please do not modify this file. It will be re-generated +# running the code generator. +#-------------------------------------------------------------------------- + +# other CommunicationObject-dependencies are added here (if there are any) +LIST(APPEND ComponentRosDock_DEPENDENCIES CommBasicObjects) +LIST(APPEND ComponentRosDock_DEPENDENCIES CommNavigationObjects) + +LIST(REMOVE_DUPLICATES ComponentRosDock_DEPENDENCIES) diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentRosDockExtension.cc b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockExtension.cc new file mode 100644 index 00000000..d1c9bd95 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockExtension.cc @@ -0,0 +1,62 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#include "ComponentRosDockExtension.hh" + +ComponentRosDockExtension::ComponentRosDockExtension(const std::string &name) +: extension_name(name) +{ + cancelled = false; + COMP->addExtension(this); +} + +ComponentRosDockExtension::~ComponentRosDockExtension() +{ } + +int ComponentRosDockExtension::startExtensionThread() +{ + cancelled = false; + // execute the task_execution() method in a new thread now + extension_future = std::async(std::launch::async, &ComponentRosDockExtension::extensionExecution, this); + return 0; +} + +int ComponentRosDockExtension::stopExtensionThread(const std::chrono::steady_clock::duration &timeoutTime) +{ + cancelled = true; + // wait on extension thread to exit + if (extension_future.wait_for(timeoutTime) == std::future_status::timeout) { + return -1; + } + return 0; +} + +void ComponentRosDockExtension::loadParameters(const SmartACE::SmartIniParameter ¶meter) +{ + // no-op +} + +int ComponentRosDockExtension::onStartup() +{ + // default implementation just starts the internal thread + return startExtensionThread(); +} + +int ComponentRosDockExtension::onShutdown(const std::chrono::steady_clock::duration &timeoutTime) +{ + // default implementation stops the internal thread waiting up to timeoutTime until it is stopped (or a timeout occurs) + return stopExtensionThread(timeoutTime); +} diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentRosDockExtension.hh b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockExtension.hh new file mode 100644 index 00000000..0ffb2756 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockExtension.hh @@ -0,0 +1,65 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef COMPONENTROSDOCK_EXTENSION_HH_ +#define COMPONENTROSDOCK_EXTENSION_HH_ + +#include +#include +#include +#include + +// include component's main class +#include "ComponentRosDock.hh" + +// forward declaration +class ComponentRosDock; + +class ComponentRosDockExtension { +private: + std::string extension_name; + std::future extension_future; + +protected: + /// use this variable within extensionExecution() to check if the thread is commanded to shutdown + std::atomic cancelled; + + /// use this method to start internal extension thread + virtual int startExtensionThread(); + + /// implement this method in derived classes + virtual int extensionExecution() = 0; + + /// use this method to start internal extension thread + virtual int stopExtensionThread(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)); +public: + ComponentRosDockExtension(const std::string &name); + virtual ~ComponentRosDockExtension(); + + inline std::string getName() const { + return extension_name; + } + + virtual void loadParameters(const SmartACE::SmartIniParameter ¶meter); + virtual void initialize(ComponentRosDock *component, int argc, char* argv[]) = 0; + virtual int onStartup(); + + virtual int onShutdown(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)); + virtual void destroy() = 0; + +}; + +#endif /* COMPONENTROSDOCK_EXTENSION_HH_ */ diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentRosDockImpl.cc b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockImpl.cc new file mode 100644 index 00000000..2e901d2f --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockImpl.cc @@ -0,0 +1,48 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include "ComponentRosDockImpl.hh" + +ComponentRosDockImpl::ComponentRosDockImpl(const std::string &componentName, int & argc, char ** argv) +: SmartACE::SmartComponent(componentName, argc, argv) +{ } + +ComponentRosDockImpl::ComponentRosDockImpl(const std::string &componentName, int & argc, char ** argv, const ACE_Sched_Params &sched_params) +: SmartACE::SmartComponent(componentName, argc, argv, NULL, sched_params) +{ } + +ComponentRosDockImpl::~ComponentRosDockImpl() +{ } + +int ComponentRosDockImpl::startComponentInfrastructure(void) +{ + // open thread for managing all server-initiated-disconnects in this component + srvInitDiscHandler.start(); + + // if meanwile strg+c was called -> return immediatelly, without waiting on substate or reactor + if(!first_call_of_handle_signal) return -1; + + componentIsRunning = true; + return 0; +} + +void ComponentRosDockImpl::stopComponentInfrastructure(const std::chrono::steady_clock::duration &timeoutTime) +{ + this->signalSmartTasksToStop(timeoutTime); +} +void ComponentRosDockImpl::cleanUpComponentResources() +{ + this->cleanUpInternalResources(); +} diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentRosDockImpl.hh b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockImpl.hh new file mode 100644 index 00000000..de7fb062 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockImpl.hh @@ -0,0 +1,33 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _COMPONENTROSDOCKIMPL_HH +#define _COMPONENTROSDOCKIMPL_HH + +#include +#include "aceSmartSoft.hh" + +class ComponentRosDockImpl : public SmartACE::SmartComponent { +public: + ComponentRosDockImpl(const std::string &componentName, int & argc, char ** argv); + ComponentRosDockImpl(const std::string &componentName, int & argc, char ** argv, const ACE_Sched_Params &sched_params); + virtual ~ComponentRosDockImpl(); + + int startComponentInfrastructure(); + void stopComponentInfrastructure(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)); + void cleanUpComponentResources(); +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/ComponentRosDockPortFactoryInterface.hh b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockPortFactoryInterface.hh new file mode 100644 index 00000000..85851c92 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ComponentRosDockPortFactoryInterface.hh @@ -0,0 +1,60 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +#ifndef COMPONENTROSDOCK_PORTFACTORYINTERFACE_HH_ +#define COMPONENTROSDOCK_PORTFACTORYINTERFACE_HH_ + +// include communication objects +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +// include component's main class +#include "ComponentRosDock.hh" + +// forward declaration +class ComponentRosDock; + +class ComponentRosDockPortFactoryInterface { +public: + ComponentRosDockPortFactoryInterface() { }; + virtual ~ComponentRosDockPortFactoryInterface() { }; + + virtual void initialize(ComponentRosDock *component, int argc, char* argv[]) = 0; + virtual int onStartup() = 0; + + virtual Smart::IPushClientPattern * createBaseStateServiceIn() = 0; + virtual Smart::IPushClientPattern * createLaserServiceIn() = 0; + virtual Smart::ISendClientPattern * createNavigationVelocityServiceOut() = 0; + + virtual Smart::IEventServerPattern * createRobotDockingEventServiceOut(const std::string &serviceName, std::shared_ptr> robotDockingEventServiceOutEventTestHandler) = 0; + + virtual int onShutdown(const std::chrono::steady_clock::duration &timeoutTime=std::chrono::seconds(2)) = 0; + virtual void destroy() = 0; +}; + +#endif /* COMPONENTROSDOCK_PORTFACTORYINTERFACE_HH_ */ diff --git a/ComponentRosDock/smartsoft/src-gen/DockActivityCore.cc b/ComponentRosDock/smartsoft/src-gen/DockActivityCore.cc new file mode 100644 index 00000000..a13f080d --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/DockActivityCore.cc @@ -0,0 +1,134 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include "DockActivityCore.hh" +#include "DockActivity.hh" +#include "ComponentRosDock.hh" + +//FIXME: use logging +//#include "smartGlobalLogger.hh" + +// include observers + + +DockActivityCore::DockActivityCore(Smart::IComponent *comp, const bool &useDefaultState) +: SmartACE::ManagedTask(comp) +, useDefaultState(useDefaultState) +, useLogging(false) +, taskLoggingId(0) +, currentUpdateCount(0) +, baseStateServiceInStatus(Smart::SMART_DISCONNECTED) +, baseStateServiceInObject() +{ +} + +DockActivityCore::~DockActivityCore() +{ +} + +void DockActivityCore::dock_action_result_cb (const std_msgs::String::ConstPtr &msg) { + // implement this method +} + +void DockActivityCore::notify_all_interaction_observers() { + std::unique_lock lock(interaction_observers_mutex); + // try dynamically down-casting this class to the derived class + // (we can do it safely here as we exactly know the derived class) + if(const DockActivity* dockActivity = dynamic_cast(this)) { + for(auto it=interaction_observers.begin(); it!=interaction_observers.end(); it++) { + (*it)->on_update_from(dockActivity); + } + } +} + +void DockActivityCore::attach_interaction_observer(DockActivityObserverInterface *observer) { + std::unique_lock lock(interaction_observers_mutex); + interaction_observers.push_back(observer); +} + +void DockActivityCore::detach_interaction_observer(DockActivityObserverInterface *observer) { + std::unique_lock lock(interaction_observers_mutex); + interaction_observers.remove(observer); +} + +int DockActivityCore::execute_protected_region() +{ + if(useDefaultState) { + Smart::StatusCode status = COMP->stateSlave->acquire("dock"); + if(status != Smart::SMART_OK) { + std::cerr << "DockActivityCore: ERROR acquiring state active: " << status << std::endl; + return 0; + } + } + + // update of comm-objects must be within the protected region to prevent aged comm-object values + this->updateAllCommObjects(); + + if(useLogging == true) { + //FIXME: use logging + //Smart::LOGGER->log(taskLoggingId, getCurrentUpdateCount(), getPreviousCommObjId()); + } + + // this is the user code (should not internally use the state-pattern any more) + int retval = this->on_execute(); + + // notify all attached interaction observers + this->notify_all_interaction_observers(); + + // inform all associated tasks about a new update + this->trigger_all_tasks(); + + // increment current currentUpdateCount for the next iteration + currentUpdateCount++; + + if(useDefaultState) { + COMP->stateSlave->release("dock"); + } + return retval; +} + + +void DockActivityCore::updateAllCommObjects() +{ + baseStateServiceInStatus = COMP->baseStateServiceInInputTaskTrigger->getUpdate(baseStateServiceInObject); + +} + + +// this method is meant to be used in derived classes +Smart::StatusCode DockActivityCore::robotDockingEventServiceOutPut(CommNavigationObjects::CommDockingEventState &eventState) +{ + Smart::StatusCode result = COMP->robotDockingEventServiceOut->put(eventState); + if(useLogging == true) { + //FIXME: use logging + //Smart::LOGGER->log(pushLoggingId+1, getCurrentUpdateCount(), getPreviousCommObjId()); + } + return result; +} + +void DockActivityCore::triggerLogEntry(const int& idOffset) +{ + if(useLogging == true) { + int logId = taskLoggingId + 2*2 + idOffset; + //FIXME: use logging + //Smart::LOGGER->log(logId, getCurrentUpdateCount(), getPreviousCommObjId()); + } +} + +int DockActivityCore::getPreviousCommObjId() +{ + // this method needs to be overloaded and implemented in derived classes + return 0; +} diff --git a/ComponentRosDock/smartsoft/src-gen/DockActivityCore.hh b/ComponentRosDock/smartsoft/src-gen/DockActivityCore.hh new file mode 100644 index 00000000..39d66eeb --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/DockActivityCore.hh @@ -0,0 +1,112 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _DOCKACTIVITY_CORE_HH +#define _DOCKACTIVITY_CORE_HH + +#include "aceSmartSoft.hh" + +// include upcall interface +#include "BaseStateServiceInUpcallInterface.hh" + +// include communication-objects for output ports +#include +#include +#include + +// include all interaction-observer interfaces +#include + +#include +#include + +class DockActivityCore +: public SmartACE::ManagedTask +, public Smart::TaskTriggerSubject +, public BaseStateServiceInUpcallInterface +{ +private: + bool useDefaultState; + bool useLogging; + int taskLoggingId; + unsigned int currentUpdateCount; + + Smart::StatusCode baseStateServiceInStatus; + CommBasicObjects::CommBaseState baseStateServiceInObject; + + +protected: + virtual int execute_protected_region(); + + virtual void updateAllCommObjects(); + + virtual int getPreviousCommObjId(); + + void triggerLogEntry(const int& idOffset); + + + // overload and implement this method in derived classes to immediately get all incoming updates from BaseStateServiceIn (as soon as they arrive) + virtual void on_BaseStateServiceIn(const CommBasicObjects::CommBaseState &input) { + // no-op + } + + // this method can be safely used from the thread in derived classes + inline Smart::StatusCode baseStateServiceInGetUpdate(CommBasicObjects::CommBaseState &baseStateServiceInObject) const + { + // copy local object buffer and return the last status code + baseStateServiceInObject = this->baseStateServiceInObject; + return baseStateServiceInStatus; + } + + // this method is meant to be used in derived classes + Smart::StatusCode robotDockingEventServiceOutPut(CommNavigationObjects::CommDockingEventState &eventState); + + +/** + * Implementation of the Subject part of an InteractionObserver + */ +private: + std::mutex interaction_observers_mutex; + std::list interaction_observers; +protected: + void notify_all_interaction_observers(); +public: + void attach_interaction_observer(DockActivityObserverInterface *observer); + void detach_interaction_observer(DockActivityObserverInterface *observer); + +public: + DockActivityCore(Smart::IComponent *comp, const bool &useDefaultState=true); + virtual ~DockActivityCore(); + + inline void setUpLogging(const int &taskNbr, const bool &useLogging=true) { + this->taskLoggingId = taskNbr; + this->useLogging = useLogging; + } + + inline bool isLoggingActive() const { + return useLogging; + } + + inline int getLoggingID() const { + return taskLoggingId; + } + + inline int getCurrentUpdateCount() const { + return currentUpdateCount; + } + + void dock_action_result_cb (const std_msgs::String::ConstPtr &msg); +}; +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/DockActivityObserverInterface.hh b/ComponentRosDock/smartsoft/src-gen/DockActivityObserverInterface.hh new file mode 100644 index 00000000..cf720ff0 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/DockActivityObserverInterface.hh @@ -0,0 +1,29 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _DOCKACTIVITY_OBSERVER_INTERFACE_HH +#define _DOCKACTIVITY_OBSERVER_INTERFACE_HH + +// forward declaration +class DockActivity; + +class DockActivityObserverInterface { +public: + virtual ~DockActivityObserverInterface() { } + + virtual void on_update_from(const DockActivity *subject) = 0; +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/ForwardActivityCore.cc b/ComponentRosDock/smartsoft/src-gen/ForwardActivityCore.cc new file mode 100644 index 00000000..edcbf4e3 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ForwardActivityCore.cc @@ -0,0 +1,124 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include "ForwardActivityCore.hh" +#include "ForwardActivity.hh" +#include "ComponentRosDock.hh" + +//FIXME: use logging +//#include "smartGlobalLogger.hh" + +// include observers + + +ForwardActivityCore::ForwardActivityCore(Smart::IComponent *comp, const bool &useDefaultState) +: SmartACE::ManagedTask(comp) +, useDefaultState(useDefaultState) +, useLogging(false) +, taskLoggingId(0) +, currentUpdateCount(0) +, laserServiceInStatus(Smart::SMART_DISCONNECTED) +, laserServiceInObject() +{ +} + +ForwardActivityCore::~ForwardActivityCore() +{ +} + +void ForwardActivityCore::twist_sub_cb (const geometry_msgs::Twist::ConstPtr &msg) { + // implement this method +} + +void ForwardActivityCore::notify_all_interaction_observers() { + std::unique_lock lock(interaction_observers_mutex); + // try dynamically down-casting this class to the derived class + // (we can do it safely here as we exactly know the derived class) + if(const ForwardActivity* forwardActivity = dynamic_cast(this)) { + for(auto it=interaction_observers.begin(); it!=interaction_observers.end(); it++) { + (*it)->on_update_from(forwardActivity); + } + } +} + +void ForwardActivityCore::attach_interaction_observer(ForwardActivityObserverInterface *observer) { + std::unique_lock lock(interaction_observers_mutex); + interaction_observers.push_back(observer); +} + +void ForwardActivityCore::detach_interaction_observer(ForwardActivityObserverInterface *observer) { + std::unique_lock lock(interaction_observers_mutex); + interaction_observers.remove(observer); +} + +int ForwardActivityCore::execute_protected_region() +{ + + // update of comm-objects must be within the protected region to prevent aged comm-object values + this->updateAllCommObjects(); + + if(useLogging == true) { + //FIXME: use logging + //Smart::LOGGER->log(taskLoggingId, getCurrentUpdateCount(), getPreviousCommObjId()); + } + + // this is the user code (should not internally use the state-pattern any more) + int retval = this->on_execute(); + + // notify all attached interaction observers + this->notify_all_interaction_observers(); + + // inform all associated tasks about a new update + this->trigger_all_tasks(); + + // increment current currentUpdateCount for the next iteration + currentUpdateCount++; + + return retval; +} + + +void ForwardActivityCore::updateAllCommObjects() +{ + laserServiceInStatus = COMP->laserServiceInInputTaskTrigger->getUpdate(laserServiceInObject); + +} + + +// this method is meant to be used in derived classes +Smart::StatusCode ForwardActivityCore::navigationVelocityServiceOutPut(CommBasicObjects::CommNavigationVelocity &navigationVelocityServiceOutDataObject) +{ + Smart::StatusCode result = COMP->navigationVelocityServiceOut->send(navigationVelocityServiceOutDataObject); + if(useLogging == true) { + //FIXME: use logging + //Smart::LOGGER->log(pushLoggingId+1, getCurrentUpdateCount(), getPreviousCommObjId()); + } + return result; +} + +void ForwardActivityCore::triggerLogEntry(const int& idOffset) +{ + if(useLogging == true) { + int logId = taskLoggingId + 2*2 + idOffset; + //FIXME: use logging + //Smart::LOGGER->log(logId, getCurrentUpdateCount(), getPreviousCommObjId()); + } +} + +int ForwardActivityCore::getPreviousCommObjId() +{ + // this method needs to be overloaded and implemented in derived classes + return 0; +} diff --git a/ComponentRosDock/smartsoft/src-gen/ForwardActivityCore.hh b/ComponentRosDock/smartsoft/src-gen/ForwardActivityCore.hh new file mode 100644 index 00000000..7d5cab78 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ForwardActivityCore.hh @@ -0,0 +1,111 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _FORWARDACTIVITY_CORE_HH +#define _FORWARDACTIVITY_CORE_HH + +#include "aceSmartSoft.hh" + +// include upcall interface +#include "LaserServiceInUpcallInterface.hh" + +// include communication-objects for output ports +#include + +// include all interaction-observer interfaces +#include + +#include +#include +#include + +class ForwardActivityCore +: public SmartACE::ManagedTask +, public Smart::TaskTriggerSubject +, public LaserServiceInUpcallInterface +{ +private: + bool useDefaultState; + bool useLogging; + int taskLoggingId; + unsigned int currentUpdateCount; + + Smart::StatusCode laserServiceInStatus; + CommBasicObjects::CommMobileLaserScan laserServiceInObject; + + +protected: + virtual int execute_protected_region(); + + virtual void updateAllCommObjects(); + + virtual int getPreviousCommObjId(); + + void triggerLogEntry(const int& idOffset); + + + // overload and implement this method in derived classes to immediately get all incoming updates from LaserServiceIn (as soon as they arrive) + virtual void on_LaserServiceIn(const CommBasicObjects::CommMobileLaserScan &input) { + // no-op + } + + // this method can be safely used from the thread in derived classes + inline Smart::StatusCode laserServiceInGetUpdate(CommBasicObjects::CommMobileLaserScan &laserServiceInObject) const + { + // copy local object buffer and return the last status code + laserServiceInObject = this->laserServiceInObject; + return laserServiceInStatus; + } + + // this method is meant to be used in derived classes + Smart::StatusCode navigationVelocityServiceOutPut(CommBasicObjects::CommNavigationVelocity &navigationVelocityServiceOutDataObject); + + +/** + * Implementation of the Subject part of an InteractionObserver + */ +private: + std::mutex interaction_observers_mutex; + std::list interaction_observers; +protected: + void notify_all_interaction_observers(); +public: + void attach_interaction_observer(ForwardActivityObserverInterface *observer); + void detach_interaction_observer(ForwardActivityObserverInterface *observer); + +public: + ForwardActivityCore(Smart::IComponent *comp, const bool &useDefaultState=true); + virtual ~ForwardActivityCore(); + + inline void setUpLogging(const int &taskNbr, const bool &useLogging=true) { + this->taskLoggingId = taskNbr; + this->useLogging = useLogging; + } + + inline bool isLoggingActive() const { + return useLogging; + } + + inline int getLoggingID() const { + return taskLoggingId; + } + + inline int getCurrentUpdateCount() const { + return currentUpdateCount; + } + + void twist_sub_cb (const geometry_msgs::Twist::ConstPtr &msg); +}; +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/ForwardActivityObserverInterface.hh b/ComponentRosDock/smartsoft/src-gen/ForwardActivityObserverInterface.hh new file mode 100644 index 00000000..b25eb1cf --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/ForwardActivityObserverInterface.hh @@ -0,0 +1,29 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _FORWARDACTIVITY_OBSERVER_INTERFACE_HH +#define _FORWARDACTIVITY_OBSERVER_INTERFACE_HH + +// forward declaration +class ForwardActivity; + +class ForwardActivityObserverInterface { +public: + virtual ~ForwardActivityObserverInterface() { } + + virtual void on_update_from(const ForwardActivity *subject) = 0; +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/LaserServiceInUpcallInterface.hh b/ComponentRosDock/smartsoft/src-gen/LaserServiceInUpcallInterface.hh new file mode 100644 index 00000000..fd7a2516 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/LaserServiceInUpcallInterface.hh @@ -0,0 +1,28 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _LASERSERVICEIN_UPCALL_INTERFACE_HH +#define _LASERSERVICEIN_UPCALL_INTERFACE_HH + +#include "CommBasicObjects/CommMobileLaserScan.hh" + +class LaserServiceInUpcallInterface { +public: + virtual ~LaserServiceInUpcallInterface() { } + + virtual void on_LaserServiceIn(const CommBasicObjects::CommMobileLaserScan &input) = 0; +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/LaserServiceInUpcallManager.cc b/ComponentRosDock/smartsoft/src-gen/LaserServiceInUpcallManager.cc new file mode 100644 index 00000000..c0a27866 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/LaserServiceInUpcallManager.cc @@ -0,0 +1,40 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include "LaserServiceInUpcallManager.hh" + +LaserServiceInUpcallManager::LaserServiceInUpcallManager( + Smart::InputSubject *subject, + const int &prescaleFactor) + : Smart::IInputHandler(subject, prescaleFactor) +{ } +LaserServiceInUpcallManager::~LaserServiceInUpcallManager() +{ } + +void LaserServiceInUpcallManager::notify_upcalls(const CommBasicObjects::CommMobileLaserScan &input) +{ + for(auto it=upcalls.begin(); it!=upcalls.end(); it++) { + (*it)->on_LaserServiceIn(input); + } +} + +void LaserServiceInUpcallManager::attach(LaserServiceInUpcallInterface *upcall) +{ + upcalls.push_back(upcall); +} +void LaserServiceInUpcallManager::detach(LaserServiceInUpcallInterface *upcall) +{ + upcalls.remove(upcall); +} diff --git a/ComponentRosDock/smartsoft/src-gen/LaserServiceInUpcallManager.hh b/ComponentRosDock/smartsoft/src-gen/LaserServiceInUpcallManager.hh new file mode 100644 index 00000000..cdb377b4 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/LaserServiceInUpcallManager.hh @@ -0,0 +1,54 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _LASERSERVICEIN_UPCALL_MANAGER_HH +#define _LASERSERVICEIN_UPCALL_MANAGER_HH + +#include +#include "aceSmartSoft.hh" +#include "LaserServiceInUpcallInterface.hh" + +/** LaserServiceInUpcallManager connects input-handling with Upcall propagation + * + * This class implements an InputHandler for the InputPort LaserServiceIn and propagates the handling + * of incoming data to all associated (i.e. attached) Upcalls. + */ +class LaserServiceInUpcallManager +: public Smart::IInputHandler +{ +private: + // list of associated updalls + std::list upcalls; + + // call the on_LaserServiceIn of all the attached LaserServiceInUpcallInterfaces + void notify_upcalls(const CommBasicObjects::CommMobileLaserScan &input); + +protected: + virtual void handle_input(const CommBasicObjects::CommMobileLaserScan &input) { + // relay input-handling to all attached LaserServiceInUpcallInterfaces + this->notify_upcalls(input); + } +public: + LaserServiceInUpcallManager( + Smart::InputSubject *subject, + const int &prescaleFactor=1 + ); + virtual ~LaserServiceInUpcallManager(); + + void attach(LaserServiceInUpcallInterface *upcall); + void detach(LaserServiceInUpcallInterface *upcall); +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/MixedPortIncludes.cmake b/ComponentRosDock/smartsoft/src-gen/MixedPortIncludes.cmake new file mode 100644 index 00000000..a6c46aa3 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/MixedPortIncludes.cmake @@ -0,0 +1,23 @@ +# includes for ComponentRosDockROSExtension +GET_FILENAME_COMPONENT(ROS_DIR "${PROJECT_SOURCE_DIR}/../ROS" REALPATH) +IF(EXISTS ${ROS_DIR}) +ADD_SUBDIRECTORY(${ROS_DIR}/src-gen/ ${PROJECT_BINARY_DIR}/ROS) +LIST(APPEND FURTHER_SRCS ${ROS_SRCS}) +INCLUDE_DIRECTORIES(${ROS_INCLUDES}) +ENDIF(EXISTS ${ROS_DIR}) + +# includes for OpcUaBackendComponentGeneratorExtension +GET_FILENAME_COMPONENT(OPC_UA_BACKEND_DIR "${PROJECT_SOURCE_DIR}/../opcua-backend" REALPATH) +IF(EXISTS ${OPC_UA_BACKEND_DIR}) + INCLUDE("${OPC_UA_BACKEND_DIR}/src-gen/OpcUaBackend.cmake") + LIST(APPEND FURTHER_SRCS ${OPC_UA_BACKEND_SRCS}) +ENDIF(EXISTS ${OPC_UA_BACKEND_DIR}) + +# includes for PlainOpcUaComponentRosDockExtension +GET_FILENAME_COMPONENT(PlainOPCUA_DIR "${PROJECT_SOURCE_DIR}/../plainOpcUa" REALPATH) +IF(EXISTS ${PlainOPCUA_DIR}) +INCLUDE("${PlainOPCUA_DIR}/src-gen/ComponentRosDockPlainOpcUa.cmake") +LIST(APPEND FURTHER_SRCS ${PLAIN_OPCUA_SRCS}) +ENDIF(EXISTS ${PlainOPCUA_DIR}) + + diff --git a/ComponentRosDock/smartsoft/src-gen/MixedPortTargetConfigs.cmake b/ComponentRosDock/smartsoft/src-gen/MixedPortTargetConfigs.cmake new file mode 100644 index 00000000..35bfea5a --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/MixedPortTargetConfigs.cmake @@ -0,0 +1,18 @@ +# target configurations for ComponentRosDockROSExtension +IF(EXISTS ${ROS_DIR}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ROS_LIBS}) +ENDIF(EXISTS ${ROS_DIR}) + +# target configurations for OpcUaBackendComponentGeneratorExtension +IF(SeRoNetSDK_FOUND) +#TARGET_LINK_LIBRARIES(${PROJECT_NAME} SeRoNetSDK::SeRoNetSDK) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} CommBasicObjectsOpcUa) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} CommNavigationObjectsOpcUa) +ENDIF(SeRoNetSDK_FOUND) + +# target configurations for PlainOpcUaComponentRosDockExtension +IF(Open62541CppWrapper_FOUND) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} Open62541CppWrapper) +TARGET_COMPILE_DEFINITIONS(${PROJECT_NAME} PUBLIC HAS_OPCUA) +ENDIF(Open62541CppWrapper_FOUND) + diff --git a/ComponentRosDock/smartsoft/src-gen/RobotDockingEventServiceOutEventTestHandlerCore.hh b/ComponentRosDock/smartsoft/src-gen/RobotDockingEventServiceOutEventTestHandlerCore.hh new file mode 100644 index 00000000..14a1321a --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/RobotDockingEventServiceOutEventTestHandlerCore.hh @@ -0,0 +1,28 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _ROBOTDOCKINGEVENTSERVICEOUT_EVENT_TEST_HANDLER_CORE_HH +#define _ROBOTDOCKINGEVENTSERVICEOUT_EVENT_TEST_HANDLER_CORE_HH + +#include "aceSmartSoft.hh" + +#include +#include +#include + +class RobotDockingEventServiceOutEventTestHandlerCore : public SmartACE::EventTestHandler +{ +}; +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/SmartStateChangeHandlerCore.hh b/ComponentRosDock/smartsoft/src-gen/SmartStateChangeHandlerCore.hh new file mode 100644 index 00000000..b2214ca3 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/SmartStateChangeHandlerCore.hh @@ -0,0 +1,27 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _SMARTSTATECHANGEHANDLERCORE_HH +#define _SMARTSTATECHANGEHANDLERCORE_HH + +#include "aceSmartSoft.hh" + +class SmartStateChangeHandlerCore : public SmartACE::StateChangeHandler +{ +public: + void handleEnterState(const std::string & substate) throw() { }; + void handleQuitState(const std::string & substate) throw() { }; +}; +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/UndockActivityCore.cc b/ComponentRosDock/smartsoft/src-gen/UndockActivityCore.cc new file mode 100644 index 00000000..e22611d2 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/UndockActivityCore.cc @@ -0,0 +1,124 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include "UndockActivityCore.hh" +#include "UndockActivity.hh" +#include "ComponentRosDock.hh" + +//FIXME: use logging +//#include "smartGlobalLogger.hh" + +// include observers + + +UndockActivityCore::UndockActivityCore(Smart::IComponent *comp, const bool &useDefaultState) +: SmartACE::ManagedTask(comp) +, useDefaultState(useDefaultState) +, useLogging(false) +, taskLoggingId(0) +, currentUpdateCount(0) +, baseStateServiceInStatus(Smart::SMART_DISCONNECTED) +, baseStateServiceInObject() +{ +} + +UndockActivityCore::~UndockActivityCore() +{ +} + +void UndockActivityCore::undock_action_result_cb (const std_msgs::String::ConstPtr &msg) { + // implement this method +} + +void UndockActivityCore::notify_all_interaction_observers() { + std::unique_lock lock(interaction_observers_mutex); + // try dynamically down-casting this class to the derived class + // (we can do it safely here as we exactly know the derived class) + if(const UndockActivity* undockActivity = dynamic_cast(this)) { + for(auto it=interaction_observers.begin(); it!=interaction_observers.end(); it++) { + (*it)->on_update_from(undockActivity); + } + } +} + +void UndockActivityCore::attach_interaction_observer(UndockActivityObserverInterface *observer) { + std::unique_lock lock(interaction_observers_mutex); + interaction_observers.push_back(observer); +} + +void UndockActivityCore::detach_interaction_observer(UndockActivityObserverInterface *observer) { + std::unique_lock lock(interaction_observers_mutex); + interaction_observers.remove(observer); +} + +int UndockActivityCore::execute_protected_region() +{ + if(useDefaultState) { + Smart::StatusCode status = COMP->stateSlave->acquire("unDock"); + if(status != Smart::SMART_OK) { + std::cerr << "UndockActivityCore: ERROR acquiring state active: " << status << std::endl; + return 0; + } + } + + // update of comm-objects must be within the protected region to prevent aged comm-object values + this->updateAllCommObjects(); + + if(useLogging == true) { + //FIXME: use logging + //Smart::LOGGER->log(taskLoggingId, getCurrentUpdateCount(), getPreviousCommObjId()); + } + + // this is the user code (should not internally use the state-pattern any more) + int retval = this->on_execute(); + + // notify all attached interaction observers + this->notify_all_interaction_observers(); + + // inform all associated tasks about a new update + this->trigger_all_tasks(); + + // increment current currentUpdateCount for the next iteration + currentUpdateCount++; + + if(useDefaultState) { + COMP->stateSlave->release("unDock"); + } + return retval; +} + + +void UndockActivityCore::updateAllCommObjects() +{ + baseStateServiceInStatus = COMP->baseStateServiceInInputTaskTrigger->getUpdate(baseStateServiceInObject); + +} + + + +void UndockActivityCore::triggerLogEntry(const int& idOffset) +{ + if(useLogging == true) { + int logId = taskLoggingId + 2*2 + idOffset; + //FIXME: use logging + //Smart::LOGGER->log(logId, getCurrentUpdateCount(), getPreviousCommObjId()); + } +} + +int UndockActivityCore::getPreviousCommObjId() +{ + // this method needs to be overloaded and implemented in derived classes + return 0; +} diff --git a/ComponentRosDock/smartsoft/src-gen/UndockActivityCore.hh b/ComponentRosDock/smartsoft/src-gen/UndockActivityCore.hh new file mode 100644 index 00000000..905d1c00 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/UndockActivityCore.hh @@ -0,0 +1,107 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _UNDOCKACTIVITY_CORE_HH +#define _UNDOCKACTIVITY_CORE_HH + +#include "aceSmartSoft.hh" + +// include upcall interface +#include "BaseStateServiceInUpcallInterface.hh" + +// include communication-objects for output ports + +// include all interaction-observer interfaces +#include + +#include +#include + +class UndockActivityCore +: public SmartACE::ManagedTask +, public Smart::TaskTriggerSubject +, public BaseStateServiceInUpcallInterface +{ +private: + bool useDefaultState; + bool useLogging; + int taskLoggingId; + unsigned int currentUpdateCount; + + Smart::StatusCode baseStateServiceInStatus; + CommBasicObjects::CommBaseState baseStateServiceInObject; + + +protected: + virtual int execute_protected_region(); + + virtual void updateAllCommObjects(); + + virtual int getPreviousCommObjId(); + + void triggerLogEntry(const int& idOffset); + + + // overload and implement this method in derived classes to immediately get all incoming updates from BaseStateServiceIn (as soon as they arrive) + virtual void on_BaseStateServiceIn(const CommBasicObjects::CommBaseState &input) { + // no-op + } + + // this method can be safely used from the thread in derived classes + inline Smart::StatusCode baseStateServiceInGetUpdate(CommBasicObjects::CommBaseState &baseStateServiceInObject) const + { + // copy local object buffer and return the last status code + baseStateServiceInObject = this->baseStateServiceInObject; + return baseStateServiceInStatus; + } + + + +/** + * Implementation of the Subject part of an InteractionObserver + */ +private: + std::mutex interaction_observers_mutex; + std::list interaction_observers; +protected: + void notify_all_interaction_observers(); +public: + void attach_interaction_observer(UndockActivityObserverInterface *observer); + void detach_interaction_observer(UndockActivityObserverInterface *observer); + +public: + UndockActivityCore(Smart::IComponent *comp, const bool &useDefaultState=true); + virtual ~UndockActivityCore(); + + inline void setUpLogging(const int &taskNbr, const bool &useLogging=true) { + this->taskLoggingId = taskNbr; + this->useLogging = useLogging; + } + + inline bool isLoggingActive() const { + return useLogging; + } + + inline int getLoggingID() const { + return taskLoggingId; + } + + inline int getCurrentUpdateCount() const { + return currentUpdateCount; + } + + void undock_action_result_cb (const std_msgs::String::ConstPtr &msg); +}; +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/UndockActivityObserverInterface.hh b/ComponentRosDock/smartsoft/src-gen/UndockActivityObserverInterface.hh new file mode 100644 index 00000000..7aa02629 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/UndockActivityObserverInterface.hh @@ -0,0 +1,29 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _UNDOCKACTIVITY_OBSERVER_INTERFACE_HH +#define _UNDOCKACTIVITY_OBSERVER_INTERFACE_HH + +// forward declaration +class UndockActivity; + +class UndockActivityObserverInterface { +public: + virtual ~UndockActivityObserverInterface() { } + + virtual void on_update_from(const UndockActivity *subject) = 0; +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/info.txt b/ComponentRosDock/smartsoft/src-gen/info.txt new file mode 100644 index 00000000..93cdd389 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/info.txt @@ -0,0 +1,17 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- + +// Generated with SmartMDSD Toolchain Version 3.12.0.201912041712 diff --git a/ComponentRosDock/smartsoft/src-gen/main.cc b/ComponentRosDock/smartsoft/src-gen/main.cc new file mode 100644 index 00000000..f1e43307 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/main.cc @@ -0,0 +1,32 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include +#include "ComponentRosDock.hh" + +int main(int argc, char *argv[]) +{ + std::cout << "main...\n"; + // initialize component infrastructure (loading ini-file, creating ports, tasks, etc.) + ComponentRosDock::instance()->init(argc, argv); + // run component infrastructure until the component is commanded to shutdown + ComponentRosDock::instance()->run(); + // clean-up component's internal resources (deleting ports, tasks, etc.) + ComponentRosDock::instance()->fini(); + // destroy the component's singleton + ComponentRosDock::deleteInstance(); + std::cout << "... main() end. return 0." << std::endl; + return 0; +} diff --git a/ComponentRosDock/smartsoft/src-gen/params/ComponentRosDock.ini.bottom b/ComponentRosDock/smartsoft/src-gen/params/ComponentRosDock.ini.bottom new file mode 100644 index 00000000..88f8f5de --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/params/ComponentRosDock.ini.bottom @@ -0,0 +1,11 @@ +##################### +# internal parameters +##################### + +##################### +# extended parameters +##################### + +##################### +# instance parameters +##################### diff --git a/ComponentRosDock/smartsoft/src-gen/params/ParameterStateStructCore.hh b/ComponentRosDock/smartsoft/src-gen/params/ParameterStateStructCore.hh new file mode 100644 index 00000000..65fa94eb --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/params/ParameterStateStructCore.hh @@ -0,0 +1,83 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _PARAMETERSTATESTRUCTCORE_HH +#define _PARAMETERSTATESTRUCTCORE_HH + +#include "aceSmartSoft.hh" + +#include + +// forward declaration (in order to define validateCOMMIT(ParameterStateStruct) which is implemented in derived class) +class ParameterStateStruct; + +class ParameterStateStructCore +{ + friend class ParamUpdateHandler; +public: + + /////////////////////////////////////////// + // Internal params + /////////////////////////////////////////// + + + /////////////////////////////////////////// + // External params + /////////////////////////////////////////// + + + /////////////////////////////////////////// + // Instance params + /////////////////////////////////////////// + + +protected: + + // Internal params + + // External params + + // Instance params (encapsulated in a wrapper class for each instantiated parameter repository) + + + void setContent(const ParameterStateStructCore &commit) { + // External params + + } + + // special trigger method (user upcall) called before updating parameter global state + virtual SmartACE::ParamResponseType handleCOMMIT(const ParameterStateStruct &commitState) = 0; +public: + ParameterStateStructCore() { } + virtual ~ParameterStateStructCore() { } + + // internal param getters + + // external param getters + + // repo wrapper class getter(s) + + // helper method to easily implement output stream in derived classes + void to_ostream(std::ostream &os = std::cout) const + { + // Internal params + + // External params + + // Instance params (encapsulated in a wrapper class for each instantiated parameter repository) + } +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src-gen/params/ParameterUpdateHandler.cc b/ComponentRosDock/smartsoft/src-gen/params/ParameterUpdateHandler.cc new file mode 100644 index 00000000..cb2ce492 --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/params/ParameterUpdateHandler.cc @@ -0,0 +1,106 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#include "ParameterUpdateHandler.hh" + +#include "ComponentRosDock.hh" + +SmartACE::CommParameterResponse ParamUpdateHandler::handleParameter(const SmartACE::CommParameterRequest& request) +{ + SmartACE::CommParameterResponse answer; + + std::string tag = request.getTag(); + std::cout<<"PARAMETER: "<globalState; +} + + +void ParamUpdateHandler::loadParameter(SmartACE::SmartIniParameter ¶meter) +{ + /* + Parameters can be specified via command line -filename= + + With this parameter present: + - The component will look for the file in the current working directory, + a path relative to the current directory or any absolute path + - The component will use the default values if the file cannot be found + + With this parameter absent: + - .ini will be read from current working directory, if found there + - $SMART_ROOT/etc/.ini will be read otherwise + - Default values will be used if neither found in working directory or /etc + */ + + // load parameters + try + { + // print all known parameters + parameter.print(); + + // + // load internal parameters (if any) + // + + // + // load extended parameters (if any) + // + + // + // load instance parameters (if a parameter definition was instantiated in the model) + // + + } catch (const SmartACE::IniParameterError & e) + { + std::cerr << "Exception from parameter handling: " << e << std::endl; + } catch (const std::exception &ex) + { + std::cerr << "Uncaught std:: exception" << ex.what() << std::endl; + } catch (...) + { + std::cerr << "Uncaught exception" << std::endl; + } +} diff --git a/ComponentRosDock/smartsoft/src-gen/params/ParameterUpdateHandler.hh b/ComponentRosDock/smartsoft/src-gen/params/ParameterUpdateHandler.hh new file mode 100644 index 00000000..92e1ab8b --- /dev/null +++ b/ComponentRosDock/smartsoft/src-gen/params/ParameterUpdateHandler.hh @@ -0,0 +1,36 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// Please do not modify this file. It will be re-generated +// running the code generator. +//-------------------------------------------------------------------------- +#ifndef _PARAMUPDATEHANDLER_HH_ +#define _PARAMUPDATEHANDLER_HH_ + +#include "aceSmartSoft.hh" +#include "ParameterStateStruct.hh" + +class ParamUpdateHandler: public SmartACE::ParameterUpdateHandler { +public: + SmartACE::CommParameterResponse handleParameter(const SmartACE::CommParameterRequest& request); + + ParameterStateStruct getGlobalState() const; + + void loadParameter(SmartACE::SmartIniParameter ¶meter); + +protected: + //local globale state stuff + ParameterStateStruct globalState,commitState; + mutable SmartACE::SmartMutex globalStateLock; +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src/CompHandler.cc b/ComponentRosDock/smartsoft/src/CompHandler.cc new file mode 100644 index 00000000..85c9e8c5 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/CompHandler.cc @@ -0,0 +1,56 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#include "CompHandler.hh" +#include "ComponentRosDock.hh" + +#include + +// include communication objects + + +void CompHandler::onStartup() +{ + std::cout << "startup - put your startupCode in CompHandler::onStartup()!!!\n"; + + Smart::StatusCode status; + + // Start all services. If you need manual control, use the content of this function to + // connect and start each service individually, e.g: + // COMP->connectMyPortName("SmartExampleComponent", "examplePort"); +// status = COMP->connectAndStartAllServices(); + status = COMP->connectBaseStateServiceIn(COMP->connections.baseStateServiceIn.serverName, COMP->connections.baseStateServiceIn.serviceName); + status = COMP->connectLaserServiceIn(COMP->connections.laserServiceIn.serverName, COMP->connections.laserServiceIn.serviceName); + status = COMP->connectNavigationVelocityServiceOut(COMP->connections.navigationVelocityServiceOut.serverName, COMP->connections.navigationVelocityServiceOut.serviceName); + + // Start all tasks. If you need manual control, use the content of this function to + // start each task individually. + COMP->startAllTasks(); + + // Start all timers. If you need manual control, use the content of this function to + // start each timer individually. + COMP->startAllTimers(); + + // Notify the component that setup/initialization is finished. + // You may move this function to any other place. + COMP->setStartupFinished(); +} + +void CompHandler::onShutdown() +{ + std::cout << "shutdown - put your cleanup code in CompHandler::onShutdown()!!!\n"; + +} diff --git a/ComponentRosDock/smartsoft/src/CompHandler.hh b/ComponentRosDock/smartsoft/src/CompHandler.hh new file mode 100644 index 00000000..6cf3e54f --- /dev/null +++ b/ComponentRosDock/smartsoft/src/CompHandler.hh @@ -0,0 +1,27 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#ifndef _COMPHANDLER_HH +#define _COMPHANDLER_HH + +class CompHandler +{ +public: + void onStartup(); + void onShutdown(); +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src/ComponentRosDockCore.cc b/ComponentRosDock/smartsoft/src/ComponentRosDockCore.cc new file mode 100644 index 00000000..61904510 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/ComponentRosDockCore.cc @@ -0,0 +1,23 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#include "ComponentRosDockCore.hh" + +// constructor +ComponentRosDockCore::ComponentRosDockCore() +{ + std::cout << "constructor ComponentRosDockCore\n"; +} diff --git a/ComponentRosDock/smartsoft/src/ComponentRosDockCore.hh b/ComponentRosDock/smartsoft/src/ComponentRosDockCore.hh new file mode 100644 index 00000000..f1332441 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/ComponentRosDockCore.hh @@ -0,0 +1,31 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#ifndef _COMPONENTROSDOCKCORE_HH +#define _COMPONENTROSDOCKCORE_HH + +#include "aceSmartSoft.hh" +#include + +class ComponentRosDockCore +{ +private: + +public: + ComponentRosDockCore(); +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src/DockActivity.cc b/ComponentRosDock/smartsoft/src/DockActivity.cc new file mode 100644 index 00000000..16bb78e1 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/DockActivity.cc @@ -0,0 +1,123 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#include "DockActivity.hh" +#include "ComponentRosDock.hh" + +#include + +DockActivity::DockActivity(SmartACE::SmartComponent *comp) +: DockActivityCore(comp) +{ + std::cout << "constructor DockActivity\n"; +} +DockActivity::~DockActivity() +{ + std::cout << "destructor DockActivity\n"; +} + +void DockActivity::dock() +{ + std::unique_lock lck (mtx); + + std_msgs::String dock_goal; + dock_goal.data = "station_charger"; + + std::cout << "publishing dock goal: " << dock_goal << std::endl; + COMP -> rosPorts -> dock_action_goal.publish(dock_goal); + docking = true; +} + +void DockActivity::dock_action_result_cb(const std_msgs::String::ConstPtr &msg) +{ + std::unique_lock lck (mtx); + + std::string::size_type loc = msg->data.find( "successful", 0 ); + if( loc != std::string::npos ) + { + std::cout << "docking succeeded: " << msg->data << std::endl; + + CommNavigationObjects::CommDockingEventState eventState; + eventState.setNewState(CommNavigationObjects::DockingEventType::DOCKING_DONE); + COMP->robotDockingEventServiceOut->put(eventState); + } + else + { + std::cout << "docking did not succeed: " << msg->data << std::endl; + + CommNavigationObjects::CommDockingEventState eventState; + eventState.setNewState(CommNavigationObjects::DockingEventType::DOCKING_ERROR); + COMP->robotDockingEventServiceOut->put(eventState); + } + + this->stop(); + std::cout << "docking false" << std::endl; + docking = false; +} + +void DockActivity::on_BaseStateServiceIn(const CommBasicObjects::CommBaseState &input) +{ + // upcall triggered from InputPort BaseStateServiceIn + // - use a local mutex here, because this upcal is called asynchroneously from outside of this task + // - do not use longer blocking calls here since this upcall blocks the InputPort BaseStateServiceIn + // - if you need to implement a long-running procedure, do so within the on_execute() method and in + // there, use the method baseStateServiceInGetUpdate(input) to get a copy of the input object + //std::cout << "base state in: " << input << std::endl; + +} + +int DockActivity::on_entry() +{ + std::cout << "entry DockActivity\n"; + + // do initialization procedures here, which are called once, each time the task is started + // it is possible to return != 0 (e.g. when initialization fails) then the task is not executed further + return 0; +} +int DockActivity::on_execute() +{ + // this method is called from an outside loop, + // hence, NEVER use an infinite loop (like "while(1)") here inside!!! + // also do not use blocking calls which do not result from smartsoft kernel + + // to get the incoming data, use this methods: +// Smart::StatusCode status; +// CommBasicObjects::CommBaseState baseStateServiceInObject; +// status = this->baseStateServiceInGetUpdate(baseStateServiceInObject); +// if(status != Smart::SMART_OK) { +// std::cerr << status << std::endl; +// return 0; +// } else { +// std::cout << "received: " << baseStateServiceInObject << std::endl; +// } + + if (!docking) + { + std::cout << "Dock! " << std::endl; + dock(); + } + else + std::cout << "Waiting for docking finished." << std::endl; + // it is possible to return != 0 (e.g. when the task detects errors), then the outer loop breaks and the task stops + return 0; +} +int DockActivity::on_exit() +{ + std::cout << "exit DockActivity\n"; + + // use this method to clean-up resources which are initialized in on_entry() and needs to be freed before the on_execute() can be called again + return 0; +} diff --git a/ComponentRosDock/smartsoft/src/DockActivity.hh b/ComponentRosDock/smartsoft/src/DockActivity.hh new file mode 100644 index 00000000..f9a43456 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/DockActivity.hh @@ -0,0 +1,45 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#ifndef _DOCKACTIVITY_HH +#define _DOCKACTIVITY_HH + +#include "DockActivityCore.hh" + +#include + +class DockActivity : public DockActivityCore +{ +private: + std::mutex mtx; + bool docking = false; + + virtual void on_BaseStateServiceIn(const CommBasicObjects::CommBaseState &input); + + void dock(); + +public: + DockActivity(SmartACE::SmartComponent *comp); + virtual ~DockActivity(); + + void dock_action_result_cb(const std_msgs::String::ConstPtr &msg); + + virtual int on_entry(); + virtual int on_execute(); + virtual int on_exit(); +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src/ForwardActivity.cc b/ComponentRosDock/smartsoft/src/ForwardActivity.cc new file mode 100644 index 00000000..75795689 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/ForwardActivity.cc @@ -0,0 +1,135 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#include +#include "ComponentRosDock.hh" + +#include + +ForwardActivity::ForwardActivity(SmartACE::SmartComponent *comp) +: ForwardActivityCore(comp) +{ + std::cout << "constructor ForwardActivity\n"; +} +ForwardActivity::~ForwardActivity() +{ + std::cout << "destructor ForwardActivity\n"; +} + +void ForwardActivity::on_BaseStateServiceIn(const CommBasicObjects::CommBaseState &input) +{ + // upcall triggered from InputPort BaseStateServiceIn + // - use a local mutex here, because this upcal is called asynchroneously from outside of this task + // - do not use longer blocking calls here since this upcall blocks the InputPort BaseStateServiceIn + // - if you need to implement a long-running procedure, do so within the on_execute() method and in + // there, use the method baseStateServiceInGetUpdate(input) to get a copy of the input object + // std::cout << "received base state: " << input << std::endl; + + std_msgs::Bool charging; + charging.data = (input.get_battery_state().getCurrent() > 0) && (bool)input.get_battery_state().getExternalPower(); + COMP -> rosPorts -> charging_pub.publish(charging); + +} + +void ForwardActivity::on_LaserServiceIn(const CommBasicObjects::CommMobileLaserScan &input) +{ + // upcall triggered from InputPort LaserServiceIn + // - use a local mutex here, because this upcal is called asynchroneously from outside of this task + // - do not use longer blocking calls here since this upcall blocks the InputPort LaserServiceIn + // - if you need to implement a long-running procedure, do so within the on_execute() method and in + // there, use the method laserServiceInGetUpdate(input) to get a copy of the input object + + //std::cout << "received laserscan: " << input << std::endl; + unsigned int scan_size = input.get_scan_size(); + unsigned int length_unit = input.get_scan_length_unit(); + + sensor_msgs::LaserScan laserscan; + + laserscan.header.frame_id = "base_link"; + laserscan.header.stamp.sec = input.get_scan_time_stamp().get_seconds(); + laserscan.header.stamp.nsec = input.get_scan_time_stamp().get_microseconds()*1000; + // angles are supposed to be in rad (-3.14 .. 3.14) + laserscan.angle_min = input.get_scan_start_angle(); + laserscan.angle_max = input.get_scan_start_angle() + input.get_scan_resolution() * scan_size; + laserscan.angle_increment = input.get_scan_resolution(); + laserscan.range_min = input.get_min_distance() / length_unit; + laserscan.range_max = input.get_max_distance() / length_unit; + + std::vector ranges; + std::vector intensities; + for (unsigned int i = 0; i < scan_size; i++) { + ranges.push_back(input.get_scan_distance(i, 1) / length_unit); + intensities.push_back(input.get_scan_intensity(i)); + } + laserscan.ranges = ranges; + laserscan.intensities = intensities; + + //std::cout << "publishing laserscan: " << laserscan << std::endl; + COMP -> rosPorts -> laser_pub.publish(laserscan); +} + +void ForwardActivity::twist_sub_cb(const geometry_msgs::Twist::ConstPtr &msg) +{ + CommBasicObjects::CommNavigationVelocity comNavVel; + + comNavVel.set_vX(msg->linear.x); + comNavVel.set_vY(msg->linear.y); + comNavVel.set_omega(msg->angular.z); +// std::cout << "Velocity x: " << msg->linear.x << std::endl; +// std::cout << "velocity y: " << msg->linear.y << std::endl; +// std::cout << "turnrate :" << msg->angular.z << std::endl; + + Smart::StatusCode status_nav = this->navigationVelocityServiceOutPut(comNavVel); + if(status_nav != Smart::SMART_OK) + { + std::cerr << status_nav << std::endl; + std::cout << "Some Error in the Conection as status is not ok " << std::endl; + } + else + { + //std::cout << "Updating Velocity " << comNavVel << std::endl; + } +} + +int ForwardActivity::on_entry() +{ + std::cout << "entry ForwardActivity " << std::endl; + + // do initialization procedures here, which are called once, each time the task is started + // it is possible to return != 0 (e.g. when initialization fails) then the task is not executed further + return 0; +} +int ForwardActivity::on_execute() +{ + // this method is called from an outside loop, + // hence, NEVER use an infinite loop (like "while(1)") here inside!!! + // also do not use blocking calls which do not result from smartsoft kernel + + // to get the incoming data, use this methods: + Smart::StatusCode status; + + //std::cout << "Hello from ForwardActivity " << std::endl; + + // it is possible to return != 0 (e.g. when the task detects errors), then the outer loop breaks and the task stops + return 0; +} +int ForwardActivity::on_exit() +{ + std::cout << "exit ForwardActivity " << std::endl; + + // use this method to clean-up resources which are initialized in on_entry() and needs to be freed before the on_execute() can be called again + return 0; +} diff --git a/ComponentRosDock/smartsoft/src/ForwardActivity.hh b/ComponentRosDock/smartsoft/src/ForwardActivity.hh new file mode 100644 index 00000000..fe698f70 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/ForwardActivity.hh @@ -0,0 +1,42 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#ifndef _TWISTACTIVITY_HH +#define _TWISTACTIVITY_HH + +#include "ForwardActivityCore.hh" + +#include + + +class ForwardActivity : public ForwardActivityCore +{ +private: +public: + ForwardActivity(SmartACE::SmartComponent *comp); + virtual ~ForwardActivity(); + + virtual void on_LaserServiceIn(const CommBasicObjects::CommMobileLaserScan &input); + virtual void on_BaseStateServiceIn(const CommBasicObjects::CommBaseState &input); + + void twist_sub_cb(const geometry_msgs::Twist::ConstPtr &msg); + + virtual int on_entry(); + virtual int on_execute(); + virtual int on_exit(); +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src/ParameterStateStruct.cc b/ComponentRosDock/smartsoft/src/ParameterStateStruct.cc new file mode 100644 index 00000000..06cd2df9 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/ParameterStateStruct.cc @@ -0,0 +1,32 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- + +#include "ParameterStateStruct.hh" + +#include "ComponentRosDock.hh" + +SmartACE::ParamResponseType ParameterStateStruct::handleCOMMIT(const ParameterStateStruct &commitState) { + // implement any consistency checks here which ensure that the incoming parameter meets components + // internal constraints. If the current parameter violates any consistency checks, return + // SmartACE::ParamResponseType::INVALID, which will result in this commitState to be rejected (not + // copied into the globalState) and the corresponding response type is communicated back to the + // ParameterMaster. Be aware, that you should avoid blocking calls here. If you need blocking + // calls, use an active trigger in combination with commit. + return SmartACE::ParamResponseType::OK; +} + +// implement your custom getter methods here diff --git a/ComponentRosDock/smartsoft/src/ParameterStateStruct.hh b/ComponentRosDock/smartsoft/src/ParameterStateStruct.hh new file mode 100644 index 00000000..b69b64c5 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/ParameterStateStruct.hh @@ -0,0 +1,48 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- + +#ifndef _PARAMETERSTATESTRUCT_HH +#define _PARAMETERSTATESTRUCT_HH + +#include "ParameterStateStructCore.hh" + +class ParameterStateStruct: public ParameterStateStructCore +{ +public: + ParameterStateStruct() { } + virtual ~ParameterStateStruct() { } + + // implement this method to check whether the incoming parameters + // provided by the commit state are consistent, in case the ParameterResponseType + // is not OK, the commit parameter is NOT copied into the global state. + // HINT: avoid using blocking calls from within this handler, use Triggers for that instead. + virtual SmartACE::ParamResponseType handleCOMMIT(const ParameterStateStruct &commitState); + + // define your own getter methods here + // (to provide conversion functions and/or more convenient getter methods) + // these own getter methods will be accessible through "COMP->getParameters().yourOwnMethod()" + // inside of your methods use the getter methods from the parent class ParameterStateStructCore + // (they provide parameter values from the global state only, which are available after the last commit) +}; + +inline std::ostream &operator<<(std::ostream &os, const ParameterStateStruct &pss) +{ + pss.to_ostream(os); + return os; +} + +#endif diff --git a/ComponentRosDock/smartsoft/src/RobotDockingEventServiceOutEventTestHandler.cc b/ComponentRosDock/smartsoft/src/RobotDockingEventServiceOutEventTestHandler.cc new file mode 100644 index 00000000..58b4d314 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/RobotDockingEventServiceOutEventTestHandler.cc @@ -0,0 +1,46 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#include "RobotDockingEventServiceOutEventTestHandler.hh" + +bool RobotDockingEventServiceOutEventTestHandler::testEvent( + CommNavigationObjects::CommDockingEventParameter &p, + CommNavigationObjects::CommDockingEventResult &r, + const CommNavigationObjects::CommDockingEventState &s +) throw() { + // fire all events (without filtering) in the default implementation + // implement your own (specific) event-filtering code using the event-parameter as input + // true means that the current event will be fired to the according client + // false means that the current event is ignored (it will not be communicated to the according client) + + bool result; + CommNavigationObjects::DockingEventType oldState, newState; + + oldState = p.getOldState(); + newState = s.getNewState(); + if ((oldState!=newState)) + { + p.setOldState(newState); + r.setState(newState); + result = true; + } + else + { + result = false; + } + + return result; +} diff --git a/ComponentRosDock/smartsoft/src/RobotDockingEventServiceOutEventTestHandler.hh b/ComponentRosDock/smartsoft/src/RobotDockingEventServiceOutEventTestHandler.hh new file mode 100644 index 00000000..f1f6c242 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/RobotDockingEventServiceOutEventTestHandler.hh @@ -0,0 +1,31 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#ifndef _ROBOTDOCKINGEVENTSERVICEOUT_EVENT_TEST_HANDLER_USER_HH +#define _ROBOTDOCKINGEVENTSERVICEOUT_EVENT_TEST_HANDLER_USER_HH + +#include "RobotDockingEventServiceOutEventTestHandlerCore.hh" + +class RobotDockingEventServiceOutEventTestHandler : public RobotDockingEventServiceOutEventTestHandlerCore +{ +public: + virtual bool testEvent( + CommNavigationObjects::CommDockingEventParameter &p, + CommNavigationObjects::CommDockingEventResult &r, + const CommNavigationObjects::CommDockingEventState &s + ) throw(); +}; +#endif diff --git a/ComponentRosDock/smartsoft/src/SmartStateChangeHandler.cc b/ComponentRosDock/smartsoft/src/SmartStateChangeHandler.cc new file mode 100644 index 00000000..44925d8d --- /dev/null +++ b/ComponentRosDock/smartsoft/src/SmartStateChangeHandler.cc @@ -0,0 +1,54 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#include "SmartStateChangeHandler.hh" +#include "ComponentRosDock.hh" + +#include + +// Called when a substate is entered +void SmartStateChangeHandler::handleEnterState(const std::string & substate) throw() +{ + // change this code to your needs !!! + std::cout << "enter state: " << substate << std::endl; +} + +// Called when a substate is left +void SmartStateChangeHandler::handleQuitState(const std::string & substate) throw() +{ + // change this code to your needs !!! + std::cout << "quit state: " << substate << std::endl; + + // restart Activities, because they're being stopped after successful (un)dock + if (substate == "dock") + { + std::cout << "restart dock " << std::endl; + CommNavigationObjects::CommDockingEventState eventState; + eventState.setNewState(CommNavigationObjects::DockingEventType::DOCKING_NOT_DONE); + COMP->robotDockingEventServiceOut->put(eventState); + + COMP->dockActivity->start(); + } + if (substate == "unDock") + { + std::cout << "restart undock " << std::endl; + CommNavigationObjects::CommDockingEventState eventState; + eventState.setNewState(CommNavigationObjects::DockingEventType::UN_DOCKING_NOT_DONE); + COMP->robotDockingEventServiceOut->put(eventState); + + COMP->undockActivity->start(); + } +} diff --git a/ComponentRosDock/smartsoft/src/SmartStateChangeHandler.hh b/ComponentRosDock/smartsoft/src/SmartStateChangeHandler.hh new file mode 100644 index 00000000..89e804e3 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/SmartStateChangeHandler.hh @@ -0,0 +1,30 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#ifndef _SMARTSTATECHANGEHANDLER_USER_HH +#define _SMARTSTATECHANGEHANDLER_USER_HH + +#include "aceSmartSoft.hh" + +#include "SmartStateChangeHandlerCore.hh" + +class SmartStateChangeHandler : public SmartStateChangeHandlerCore +{ + void handleEnterState(const std::string & substate) throw(); + void handleQuitState(const std::string & substate) throw(); +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src/UndockActivity.cc b/ComponentRosDock/smartsoft/src/UndockActivity.cc new file mode 100644 index 00000000..33e8a744 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/UndockActivity.cc @@ -0,0 +1,119 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#include "UndockActivity.hh" +#include "ComponentRosDock.hh" + +#include + +UndockActivity::UndockActivity(SmartACE::SmartComponent *comp) +: UndockActivityCore(comp) +{ + std::cout << "constructor UndockActivity\n"; +} +UndockActivity::~UndockActivity() +{ + std::cout << "destructor UndockActivity\n"; +} + +void UndockActivity::undock() +{ + std::unique_lock lck (mtx); + + std_msgs::String undock_goal; + undock_goal.data = "station_charger"; + + std::cout << "publishing undock goal: " << undock_goal << std::endl; + COMP -> rosPorts -> undock_action_goal.publish(undock_goal); + undocking = true; +} + +void UndockActivity::undock_action_result_cb(const std_msgs::String::ConstPtr &msg) +{ + std::unique_lock lck (mtx); + + std::string::size_type loc = msg->data.find( "successful", 0 ); + if( loc != std::string::npos ) + { + std::cout << "undocking succeeded: " << msg->data << std::endl; + CommNavigationObjects::CommDockingEventState eventState; + eventState.setNewState(CommNavigationObjects::DockingEventType::UN_DOCKING_DONE); + COMP->robotDockingEventServiceOut->put(eventState); + + } + else + { + std::cout << "undocking did not succeed: " << msg->data << std::endl; + CommNavigationObjects::CommDockingEventState eventState; + eventState.setNewState(CommNavigationObjects::DockingEventType::UN_DOCKING_ERROR); + COMP->robotDockingEventServiceOut->put(eventState); + } + + this->stop(); + std::cout << "undocking false" << std::endl; + undocking = false; +} +void UndockActivity::on_BaseStateServiceIn(const CommBasicObjects::CommBaseState &input) +{ + // upcall triggered from InputPort BaseStateServiceIn + // - use a local mutex here, because this upcal is called asynchroneously from outside of this task + // - do not use longer blocking calls here since this upcall blocks the InputPort BaseStateServiceIn + // - if you need to implement a long-running procedure, do so within the on_execute() method and in + // there, use the method baseStateServiceInGetUpdate(input) to get a copy of the input object +} + +int UndockActivity::on_entry() +{ + std::cout << "entry UndockActivity\n"; + + // do initialization procedures here, which are called once, each time the task is started + // it is possible to return != 0 (e.g. when initialization fails) then the task is not executed further + return 0; +} +int UndockActivity::on_execute() +{ + // this method is called from an outside loop, + // hence, NEVER use an infinite loop (like "while(1)") here inside!!! + // also do not use blocking calls which do not result from smartsoft kernel + + // to get the incoming data, use this methods: + Smart::StatusCode status; + CommBasicObjects::CommBaseState baseStateServiceInObject; + status = this->baseStateServiceInGetUpdate(baseStateServiceInObject); + if(status != Smart::SMART_OK) { + std::cerr << status << std::endl; + // return 0; + } else { + //std::cout << "received: " << baseStateServiceInObject << std::endl; + } + + if (!undocking) + { + std::cout << "Undock! " << std::endl; + undock(); + } + else + std::cout << "Waiting for undocking finished." << std::endl; + // it is possible to return != 0 (e.g. when the task detects errors), then the outer loop breaks and the task stops + return 0; +} +int UndockActivity::on_exit() +{ + std::cout << "exit UndockActivity\n"; + + // use this method to clean-up resources which are initialized in on_entry() and needs to be freed before the on_execute() can be called again + return 0; +} diff --git a/ComponentRosDock/smartsoft/src/UndockActivity.hh b/ComponentRosDock/smartsoft/src/UndockActivity.hh new file mode 100644 index 00000000..b7cdc41f --- /dev/null +++ b/ComponentRosDock/smartsoft/src/UndockActivity.hh @@ -0,0 +1,44 @@ +//-------------------------------------------------------------------------- +// Code generated by the SmartSoft MDSD Toolchain +// The SmartSoft Toolchain has been developed by: +// +// Service Robotics Research Center +// University of Applied Sciences Ulm +// Prittwitzstr. 10 +// 89075 Ulm (Germany) +// +// Information about the SmartSoft MDSD Toolchain is available at: +// www.servicerobotik-ulm.de +// +// This file is generated once. Modify this file to your needs. +// If you want the toolchain to re-generate this file, please +// delete it before running the code generator. +//-------------------------------------------------------------------------- +#ifndef _UNDOCKACTIVITY_HH +#define _UNDOCKACTIVITY_HH + +#include "UndockActivityCore.hh" + +#include + +class UndockActivity : public UndockActivityCore +{ +private: + std::mutex mtx; + bool undocking; + + virtual void on_BaseStateServiceIn(const CommBasicObjects::CommBaseState &input); + virtual void undock(); + +public: + UndockActivity(SmartACE::SmartComponent *comp); + virtual ~UndockActivity(); + + void undock_action_result_cb(const std_msgs::String::ConstPtr &msg); + + virtual int on_entry(); + virtual int on_execute(); + virtual int on_exit(); +}; + +#endif diff --git a/ComponentRosDock/smartsoft/src/predeploy.sh b/ComponentRosDock/smartsoft/src/predeploy.sh new file mode 100644 index 00000000..be981936 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/predeploy.sh @@ -0,0 +1,20 @@ +#!/bin/bash + +# Use this variable to name files that are to be deployed to the data +# instance directory of +# the component (_data/) on the target host. +# +# Files can be relative to component project directory or absolute. +# Add one file or directory per line. +# +# Examples: +# +# DEPLOY_LIBRARIES="../bin/libMyLibrary.so" +# +# DEPLOY_COMPONENT_FILES=" +# $SMART_ROOT_ACE/myFILE +# $SMART_ROOT_ACE/myFILE2 +# " + +DEPLOY_LIBRARIES="" +DEPLOY_COMPONENT_FILES="" diff --git a/ComponentRosDock/smartsoft/src/startstop-hooks.sh b/ComponentRosDock/smartsoft/src/startstop-hooks.sh new file mode 100755 index 00000000..45eaa575 --- /dev/null +++ b/ComponentRosDock/smartsoft/src/startstop-hooks.sh @@ -0,0 +1,39 @@ +#!/bin/bash + +# This script provides methods to call custom commands pre/post of starting/stoping the component during launch on the device. +# This script is being executed on the target device where the component is running. +# For example the script can be used to start and stop the morse simulator automatically. + +case "$1" in + +pre-start) + echo "Triggering pre-start hooks FROM COMPONENT ComponentRosDock ..." + # Insert commands you want to call prior to starting the components + + # check if mojin_seronet is sourced + if [[ $ROS_PACKAGE_PATH != *"mojin_seronet"* ]]; then + echo "\nMake sure the catkin workspace including mojin_seronet is sourced! Current package path is: $ROS_PACKAGE_PATH \n" + fi + roslaunch seronet_dock seronet_dock.launch +;; + +post-start) + echo "Triggering post-start hooks FROM COMPONENT ComponentRosDock ..." + # Insert commands you want to call after all components were started +;; + +pre-stop) + echo "Triggering pre-stop hooks FROM COMPONENT ComponentRosDock ..." + # Insert commands you want to call before stopping all components +;; + +post-stop) + echo "Triggering post-stop hooks FROM COMPONENT ComponentRosDock ..." + # Insert commands you want to call after all components were stopped +;; + +*) + echo "ERROR in $0: no such hook '$1'. Usage: $0 pre-start|post-start|pre-stop|post-stop" +;; + +esac