homeos_ws/src/hos_cosgi_launcher/src/hos_cosgi_launcher.cpp

#include "hos_core_service_impl.h"

#include <cstdlib>
#include "ros/ros.h"
#include <apr_general.h>
#include <apr_strings.h>
extern "C" { // celix does not provide C++ ready headers
	//#include "hos_celix/celix_errno.h"
	#include "hos_celix/framework.h"
	//#include "hos_celix/bundle_context.h"
	#include "hos_celix/linked_list_iterator.h"
}
#include <exception>
#include <sstream>
#include <boost/format.hpp>
#include <signal.h>
#include <string>


using namespace hos::cosgi;


std::function<void(int)> signalHandler;

int main(int argc, char **argv) {

	bool apr_initialized = false;
	apr_pool_t* memoryPool = nullptr;
	framework_pt framework = nullptr;
	bundle_pt fwBundle = nullptr;
	properties_pt properties = nullptr;

	int loop_frequency = 100;
	std::string autostart_bundles;

	try {
		// Initialize ROS
		ros::init(argc, argv, "hos_osgi_launcher", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
		// Create public node-handle
		ros::NodeHandle rosPublicNodeHandle;
		// Create private node-handle
		ros::NodeHandle rosPrivateNodeHandle("~");
		if (!ros::ok()) {
			throw std::runtime_error(boost::format("Could not initialize ROS.").str());
		}
		ROS_INFO("ROS initialized");

		// Retrieve ROS parameter
		rosPrivateNodeHandle.getParam("loop_frequency", loop_frequency);
		rosPrivateNodeHandle.getParam("autostart_bundles", autostart_bundles);

		// Initialize APR and APR memory pool
		apr_status_t astatus = apr_initialize();
		if (astatus != APR_SUCCESS) {
			throw std::runtime_error((boost::format("Could not initialize APR (%1%).") % astatus).str());
		}
		apr_initialized = true;
		astatus = apr_pool_create(&memoryPool, nullptr);
		if (astatus != APR_SUCCESS) {
			memoryPool = nullptr;
			throw std::runtime_error((boost::format("Could not create APR memory pool (%1%).") % astatus).str());
		}
		ROS_INFO("APR initialized");

		// Create celix properties
		properties = properties_create();

		//properties = properties_load((char*)"config.properties");
		if (properties == nullptr) {
			throw std::runtime_error(boost::format("Could not create celix properties.").str());
		}
		properties_store(properties, (char*)"config.properties", nullptr);

		// Create and initialize celix framework
		celix_status_t cstatus = framework_create(&framework, memoryPool, properties);
		if (cstatus != CELIX_SUCCESS) {
			framework = nullptr;
			throw std::runtime_error((boost::format("Could not create celix framework (%1%).") % cstatus).str());
		}
		cstatus = fw_init(framework);
		if (cstatus != CELIX_SUCCESS) {
			framework = nullptr;
			throw std::runtime_error((boost::format("Could not initialize celix framework (%1%).") % cstatus).str());
		}
		ROS_INFO("Celix initialized");

		// Get and start framework bundle
		cstatus = framework_getFrameworkBundle(framework, &fwBundle);
		if (cstatus != CELIX_SUCCESS) {
			fwBundle = nullptr;
			throw std::runtime_error((boost::format("Could not find framework bundle (%1%).") % cstatus).str());
		}
		cstatus = bundle_start(fwBundle);
		if (cstatus != CELIX_SUCCESS) {
			fwBundle = nullptr;
			throw std::runtime_error((boost::format("Could not start framework bundle (%1%).") % cstatus).str());
		}
		ROS_INFO("Framework bundle started");

		// Create bundle context
		bundle_context_pt context = nullptr;
		bundle_getContext(fwBundle, &context);
		// Create and register hos-core-service
		HOSCoreServiceImpl hosCoreService(rosPublicNodeHandle, rosPrivateNodeHandle);
		service_registration_pt reg = nullptr;
		bundleContext_registerService(context, (char*)HOSCoreService::SERVICE_NAME, static_cast<HOSCoreService*>(&hosCoreService), nullptr, &reg);
		ROS_INFO("HOS core service registered");


		// Auto-start bundles
		// #### Copied from celix launcher ####
	    apr_pool_t *pool = nullptr;
	    cstatus = apr_pool_create(&pool, memoryPool);
	    if (cstatus == APR_SUCCESS) {
			ROS_INFO("Starting auto-start bundles");
			char* autoStart;
			// ROS param overrides config file
			if (!autostart_bundles.empty()) {
				autoStart = (char*)autostart_bundles.c_str();
			} else {
				autoStart = properties_get(properties, (char*)"cosgi.auto.start.1");
			}
			ROS_INFO_STREAM("Autostart bundles: " << autoStart);
			char delims[] = " ";
			char *result = nullptr;
			linked_list_pt bundles;
			array_list_pt installed = nullptr;
			bundle_pt bundle = nullptr;
			bundle_context_pt context = nullptr;
			linked_list_iterator_pt iter = nullptr;
			unsigned int i;

			if (linkedList_create(&bundles) != CELIX_SUCCESS) {
				ROS_ERROR("Could not create linked list.");
				exit(1);
			}
			result = strtok(autoStart, delims);
			while (result != nullptr) {
				char * location = apr_pstrdup(memoryPool, result);
				linkedList_addElement(bundles, location);
				result = strtok(nullptr, delims);
			}
			// First install all bundles
			// Afterwards start them
			arrayList_create(&installed);
			framework_getFrameworkBundle(framework, &bundle);
			bundle_getContext(bundle, &context);
			iter = linkedListIterator_create(bundles, 0);
			while (linkedListIterator_hasNext(iter)) {
				bundle_pt current = nullptr;
				char *location = (char *) linkedListIterator_next(iter);
				if (bundleContext_installBundle(context, location, &current) == CELIX_SUCCESS) {
					// Only add bundle if it is installed correctly
					arrayList_add(installed, current);
					ROS_INFO("Installed bundle %p from %s", current, location);
				} else {
					ROS_ERROR("Could not install bundle from %s", location);
				}
				linkedListIterator_remove(iter);
			}
			linkedListIterator_destroy(iter);

			for (i = 0; i < arrayList_size(installed); i++) {
				bundle_pt bundle = (bundle_pt) arrayList_get(installed, i);
				if (bundle_startWithOptions(bundle, 0) == CELIX_SUCCESS) {
					ROS_INFO("Started bundle %p", bundle);
				} else {
					ROS_ERROR("Could not start bundle %p", bundle);
				}
			}

			arrayList_destroy(installed);
			apr_pool_destroy(pool);
		// #### End Copied from celix launcher ####
		} else {
			throw std::runtime_error((boost::format("Could not create temporary APR memory pool.") % cstatus).str());
		}

		// main loop
		ROS_INFO("Entering main loop");
		signalHandler = [&](int signum) {
			hosCoreService.requestShutdown();
		};
		signal(SIGINT, [](int signum) { signalHandler(signum); });
		ros::Rate loop_rate(loop_frequency);
		while (ros::ok()) {
			ros::spinOnce();
			loop_rate.sleep();
		}
		ROS_INFO("Exiting main loop");

	} catch (std::exception &ex) {
		std::stringstream ss;
		ss  << "ERROR: " << ex.what() << std::endl;
		ROS_ERROR_STREAM(ss.str());
		std::cerr << ss.str();
	}

	// Deinitialize celix frameork
	if (fwBundle != nullptr) {
		bundle_stop(fwBundle);
		framework_waitForStop(framework);
	}
	if (framework != nullptr) {
		framework_destroy(framework);
	}
	if (properties != nullptr) {
		properties_destroy(properties);
	}
	ROS_INFO("Celix Framework is shut down");

	// Deinitialize APR
	if (memoryPool != nullptr) {
		apr_pool_destroy(memoryPool);
	}
	if (apr_initialized) {
		apr_terminate();
	}
	ROS_INFO("APR Framework is shut down");

	ROS_INFO("Exiting ...");
	return 0;
}