Plugin Webots Controller added

This plugin allows to use "Menge" to control humans
in the robot simulator Webots.
Vice versa, robots from Webots can be perceived by the agents
 in Menge as an agent, so that Menge humans can avoid the robot.

projects/g++/Plugins/CMakeLists.txt

@@ -41,5 +41,6 @@ ADD_SUBDIRECTORY(AgtKaramouzas)
 ADD_SUBDIRECTORY(AgtZanlungo)
 ADD_SUBDIRECTORY(StressGas)
 #ADD_SUBDIRECTORY(Sample)
+ADD_SUBDIRECTORY(WebotsController)
 
 set( CMAKE_INSTALL_ALWAYS true )

projects/g++/Plugins/WebotsController/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 2.8)
+
+project(WebotsControllerPlugin)
+
+add_definitions( -DWEBOTS_EXPORT )
+
+# Link with the Webots controller library.
+link_directories($ENV{WEBOTS_HOME}/lib/controller)
+set (WEBOTS_LIBRARIES ${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX} ${CMAKE_SHARED_LIBRARY_PREFIX}CppController${CMAKE_SHARED_LIBRARY_SUFFIX})
+include_directories($ENV{WEBOTS_HOME}/include/controller/c $ENV{WEBOTS_HOME}/include/controller/cpp)
+
+
+file(
+	GLOB_RECURSE
+	source_files
+	${PLUGIN_SOURCE_DIR}/WebotsController/*.cpp
+	${PLUGIN_SOURCE_DIR}/WebotsController/*.h
+)
+
+add_library(
+	webotsController
+	SHARED
+	${source_files}
+)
+
+target_link_libraries (webotsController mengeCore ${WEBOTS_LIBRARIES} )
+

src/Menge/MengeCore/Agents/AgentInitializer.cpp

@@ -3,7 +3,7 @@
 License
 
 Menge
-Copyright © and trademark ™ 2012-14 University of North Carolina at Chapel Hill.
+Copyright � and trademark � 2012-14 University of North Carolina at Chapel Hill.
 All rights reserved.
 
 Permission to use, copy, modify, and distribute this software and its documentation
@@ -65,6 +65,7 @@ const size_t CLASS = 0;                  ///< The default class
 const float PRIORITY = 0.f;              ///< The default priority
 const float MAX_ANGLE_VEL = TWOPI;       ///< The default maximum angular velocity
 const size_t OBSTACLE_SET = 0xFFFFFFFF;  ///< The default obstacle set (all obstacles)
+const size_t EXTERNAL = 0;
 
 ////////////////////////////////////////////////////////////////
 
@@ -86,6 +87,7 @@ AgentInitializer::AgentInitializer() {
   _obstacleSet = OBSTACLE_SET;
   _priority = PRIORITY;
   _class = CLASS;
+  _external = EXTERNAL;
 }
 
 ////////////////////////////////////////////////////////////////
@@ -101,6 +103,7 @@ AgentInitializer::AgentInitializer(const AgentInitializer& init) {
   _obstacleSet = init._obstacleSet;
   _priority = init._priority;
   _class = init._class;
+  _external = init._external;
 
   std::vector<BFSM::VelModifier*>::const_iterator vItr = init._velModifiers.begin();
   for (; vItr != init._velModifiers.end(); ++vItr) {
@@ -143,6 +146,7 @@ void AgentInitializer::setDefaults() {
   _priority = PRIORITY;
   _obstacleSet = OBSTACLE_SET;
   _class = CLASS;
+  _external = EXTERNAL;
 }
 
 ////////////////////////////////////////////////////////////////
@@ -196,6 +200,7 @@ bool AgentInitializer::setProperties(BaseAgent* agent) {
   agent->_obstacleSet = _obstacleSet;
   agent->_priority = _priority;
   agent->_class = _class;
+  agent->_external = _external;
 
   std::vector<BFSM::VelModifier*>::iterator vItr = _velModifiers.begin();
   for (; vItr != _velModifiers.end(); ++vItr) {
@@ -236,6 +241,8 @@ AgentInitializer::ParseResult AgentInitializer::setFromXMLAttribute(const ::std:
     result = constSizet(_class, value);
   } else if (paramName == "priority") {
     result = constFloat(_priority, value);
+  } else if (paramName == "external") {
+    result = constSizet(_external, value);
   }
 
   if (result == FAILURE) {

src/Menge/MengeCore/Agents/AgentInitializer.h

@@ -390,6 +390,11 @@ class MENGE_API AgentInitializer {
    */
   size_t _class;
 
+  /*!
+   @brief    Specifies whether the agent is controlled externally or whether Menge should control the agent.
+   */
+  size_t _external;
+
   /*!
    @brief    Velocity Modifiers to be applied to this class
    */

src/Menge/MengeCore/Agents/BaseAgent.cpp

@@ -3,7 +3,7 @@
 License
 
 Menge
-Copyright © and trademark ™ 2012-14 University of North Carolina at Chapel Hill.
+Copyright � and trademark � 2012-14 University of North Carolina at Chapel Hill.
 All rights reserved.
 
 Permission to use, copy, modify, and distribute this software and its documentation
@@ -65,6 +65,7 @@ BaseAgent::BaseAgent() {
   _nearAgents.clear();
   _nearObstacles.clear();
   _class = 0;
+  _external = 0;
   _obstacleSet = 0xFFFFFFFF;
   _priority = 0.f;
   _id = 0;

src/Menge/MengeCore/Agents/BaseAgent.h

@@ -262,6 +262,13 @@ class MENGE_API BaseAgent : public ProximityQuery {
    */
   size_t _class;
 
+  /*!
+   @brief    Specifies whether the agent is controlled externally (_external != 0) or whether Menge should control the agent (_external == 0).
+
+   */
+  size_t _external;
+
+
   /*!
    @brief    A mask indicating the obstacles with compatible ids which this agent can see.
 

src/Menge/MengeCore/Agents/SimulatorBase.h

@@ -187,12 +187,14 @@ void SimulatorBase<Agent>::doStep() {
 #pragma omp parallel for
   for (int i = 0; i < AGT_COUNT; ++i) {
     computeNeighbors(&(_agents[i]));
-    _agents[i].computeNewVelocity();
+    if(!_agents[i]._external)
+      _agents[i].computeNewVelocity();
   }
 
 #pragma omp parallel for
   for (int i = 0; i < AGT_COUNT; ++i) {
-    _agents[i].update(TIME_STEP);
+    if(!_agents[i]._external)
+      _agents[i].update(TIME_STEP);
   }
 
   _globalTime += TIME_STEP;

src/Menge/MengeCore/BFSM/FSM.cpp

@@ -3,7 +3,7 @@
 License
 
 Menge
-Copyright © and trademark ™ 2012-14 University of North Carolina at Chapel Hill.
+Copyright � and trademark � 2012-14 University of North Carolina at Chapel Hill.
 All rights reserved.
 
 Permission to use, copy, modify, and distribute this software and its documentation
@@ -274,7 +274,9 @@ bool FSM::doStep() {
     Agents::BaseAgent* agt = this->_sim->getAgent(a);
     try {
       advance(agt);
-      this->computePrefVelocity(agt);
+      if (!agt->_external){
+        this->computePrefVelocity(agt);
+      }
     } catch (StateException& e) {
       logger << Logger::ERR_MSG << e.what() << "\n";
       ++exceptionCount;

src/Plugins/WebotsController/WebotsControllerConfig.h

@@ -0,0 +1,66 @@
+/*
+ Menge Crowd Simulation Framework
+
+ Copyright and trademark 2012-17 University of North Carolina at Chapel Hill
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+ or
+    LICENSE.txt in the root of the Menge repository.
+
+ Any questions or comments should be sent to the authors menge@cs.unc.edu
+
+ <http://gamma.cs.unc.edu/Menge/>
+*/
+
+/*!
+ @file		WebotsControllerConfig.h
+ @brief		Configures the shared library aspect of the includes.
+ */
+
+#ifndef __WEBOTS_CONTROLLER_CONFIG_H__
+#define __WEBOTS_CONTROLLER_CONFIG_H__
+
+
+// Now set up compiler-dependent export/import symbols
+
+#if defined(_MSC_VER)
+
+  #if defined(WEBOTS_STATICLIB )
+    #define WEBOTS_API
+  #else
+    #if defined( WEBOTS_EXPORT )
+      // We are building the DLL, export the symbols tagged like this
+      #define WEBOTS_API __declspec(dllexport)
+    #else
+      // If we are consuming the DLL, import the symbols tagged like this
+      #define WEBOTS_API __declspec(dllimport)
+    #endif
+  #endif
+
+#elif defined(__GNUC__)
+
+  #if defined( WEBOTS_STATICLIB )
+    #define WEBOTS_API
+  #else
+    #if defined( WEBOTS_EXPORT )
+      #define WEBOTS_API __attribute__ ((visibility ("default")))
+    #else
+      // If you use -fvisibility=hidden in GCC, exception handling and RTTI
+      // would break if visibility wasn't set during export _and_ import
+      // because GCC would immediately forget all type infos encountered.
+      // See http://gcc.gnu.org/wiki/Visibility
+      #define WEBOTS_API __attribute__ ((visibility ("default")))
+    #endif
+  #endif
+
+#else
+
+  #error Unknown compiler, please implement shared library macros
+
+#endif	// Compiler-detection
+
+#endif // __SAMPLE_CONFIG_H__

src/Plugins/WebotsController/WebotsControllerPlugin.cpp

@@ -0,0 +1,79 @@
+/*
+
+License
+
+Menge
+Copyright � and trademark � 2012-14 University of North Carolina at Chapel Hill. 
+All rights reserved.
+
+Permission to use, copy, modify, and distribute this software and its documentation 
+for educational, research, and non-profit purposes, without fee, and without a 
+written agreement is hereby granted, provided that the above copyright notice, 
+this paragraph, and the following four paragraphs appear in all copies.
+
+This software program and documentation are copyrighted by the University of North 
+Carolina at Chapel Hill. The software program and documentation are supplied "as is," 
+without any accompanying services from the University of North Carolina at Chapel 
+Hill or the authors. The University of North Carolina at Chapel Hill and the 
+authors do not warrant that the operation of the program will be uninterrupted 
+or error-free. The end-user understands that the program was developed for research 
+purposes and is advised not to rely exclusively on the program for any reason.
+
+IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE AUTHORS 
+BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL 
+DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS 
+DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE 
+AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY 
+DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 
+OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY WARRANTY 
+OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND 
+THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS HAVE NO OBLIGATIONS 
+TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
+
+Any questions or comments should be sent to the authors {menge,geom}@cs.unc.edu
+
+*/
+
+/*!
+ *	@file		SamplePlugin.cpp  
+ *	@brief		Plugin for extended elements.
+ */
+
+#include "WebotsControllerConfig.h"
+#include "WebotsControllerTask.h"
+
+
+#include "MengeCore/PluginEngine/CorePluginEngine.h"
+
+using Menge::PluginEngine::CorePluginEngine;
+
+extern "C" {
+/*!
+ *	@brief		Retrieves the name of the plug-in.
+ *
+ *	@returns	The name of the plug in.
+ */
+WEBOTS_API const char * getName() {return "webots controller";}
+
+/*!
+ *	@brief		Description of the plug-in.
+ *
+ *	@returns	A description of the plugin.
+ */
+WEBOTS_API const char * getDescription() {
+	return	"Webots controller:\n"\
+			"\tToDO\n"
+			"\tToDO\n";
+}
+
+/*!
+ *	@brief		Registers the plug-in with the PluginEngine
+ *
+ *	@param		engine		A pointer to the plugin engine.
+ */
+WEBOTS_API void registerCorePlugin(CorePluginEngine* engine) {
+  engine->registerTaskFactory(new WebotsController::WebotsControllerTaskFactory());
+}
+}