Migrate to Boost SML state machine

.clang-format

@@ -0,0 +1,2 @@
+DisableFormat: true
+SortIncludes: false

atos/modules/EsminiAdapter/src/esminiadapter.cpp

@@ -65,7 +65,7 @@ std::filesystem::path EsminiAdapter::getOpenDriveFile()
 {
 	std::filesystem::path odrFilePath;
 	if (SE_GetODRFilename() != nullptr) {
-		odrFilePath = std::filesystem::path(SE_GetODRFilename()); 
+		odrFilePath = std::filesystem::path(SE_GetODRFilename());
 		RCLCPP_INFO(me->get_logger(), "Got ODR file %s from scenario", odrFilePath.string().c_str());
 	}
 	else {
@@ -101,7 +101,7 @@ std::shared_ptr<EsminiAdapter> EsminiAdapter::instance() {
 		me->connectedObjectIdsSub = ROSChannels::ConnectedObjectIds::Sub(*me,&EsminiAdapter::onConnectedObjectIdsMessage);
 		me->exitSub = ROSChannels::Exit::Sub(*me,&EsminiAdapter::onStaticExitMessage);
 		me->stateChangeSub = ROSChannels::StateChange::Sub(*me,&EsminiAdapter::onStaticStateChangeMessage);
-		
+
 	}
 	return me;
 }
@@ -113,7 +113,7 @@ std::shared_ptr<EsminiAdapter> EsminiAdapter::instance() {
 void EsminiAdapter::onConnectedObjectIdsMessage(const ConnectedObjectIds::message_type::SharedPtr msg) {
 	for (uint32_t id : msg->ids) {
 		if (me->monrSubscribers.find(id) == me->monrSubscribers.end()){
-			me->monrSubscribers[id] = std::make_shared<Monitor::Sub>(*me, id, std::bind(&EsminiAdapter::onMonitorMessage, me, _1, id));	
+			me->monrSubscribers[id] = std::make_shared<Monitor::Sub>(*me, id, std::bind(&EsminiAdapter::onMonitorMessage, me, _1, id));
 		}
 	}
 }
@@ -122,7 +122,7 @@ void EsminiAdapter::onConnectedObjectIdsMessage(const ConnectedObjectIds::messag
  * @brief To ensure that EsminiAdapter follows the states, we execute actions only when going from IDLE to INITIALIZED,
  * from ARMED to RUNNING and from any state to ABORTING. We do this instead of subscribing to "/init", "/start", etc.,
  * because we only want to execute the actions once when changing to these states.
- * 
+ *
  * @param msg StateChange message
  */
 void EsminiAdapter::onStaticStateChangeMessage(const ROSChannels::StateChange::message_type::SharedPtr msg) {
@@ -163,7 +163,7 @@ void EsminiAdapter::fetchOSCFilePath()
 	if (done.get_future().wait_for(250ms) == std::future_status::timeout) {
 		RCLCPP_ERROR(me->get_logger(), "Failed to fetch open scenario file path");
 	}
-	
+
 }
 
 void EsminiAdapter::onStaticExitMessage(const ROSChannels::Exit::message_type::SharedPtr)
@@ -244,7 +244,7 @@ void EsminiAdapter::onMonitorMessage(const Monitor::message_type::SharedPtr monr
 	if (auto idMapping = atosIdToEsminiId.find(ATOSObjectId); idMapping != atosIdToEsminiId.end()) {
 		reportObjectPosition(monr, idMapping->second); // Report object position to esmini
 		SE_Step(); // Advance the "simulation world"-time
-	} 
+	}
 	else {
 		RCLCPP_WARN(me->get_logger(), "Received MONR message for object with ATOS Object ID %d, but no such object exists in the scenario", ATOSObjectId);
 	}
@@ -388,10 +388,10 @@ std::string EsminiAdapter::projStrFromGeoReference(RM_GeoReference& geoRef) {
 
 /*!
  * \brief Returns object states for each timestep by simulating the loaded scenario.
- *  The simulation is stopped if there is no vehicle movement and at least 
+ *  The simulation is stopped if there is no vehicle movement and at least
  * 	MIN_SCENARIO_TIME has passed or if more than MAX_SCENARIO_TIME has passed.
  *	Inspired by ScenarioGateway::WriteStatesToFile from esmini lib.
- *  
+ *
  * \param timeStep Time step to use for generating the trajectories
  * \param endTime End time of the simulation
  * \param states The return map of ids mapping to the respective object states at different timesteps
@@ -425,7 +425,7 @@ void EsminiAdapter::getObjectStates(
 		if (SE_GetQuitFlag() != 0) {
 			break;
 		}
-		
+
 		SE_StepDT(timeStep);
 		accumTime += timeStep;
 		for (int j = 0; j < SE_GetNumberOfObjects(); j++){
@@ -479,7 +479,7 @@ std::map<uint32_t, ATOS::Trajectory> EsminiAdapter::extractTrajectories(
 
 /*!
  * \brief Initialize the esmini simulator and perform subsequent setup tasks.
- * Can be called many times, each time the test is initialized. 
+ * Can be called many times, each time the test is initialized.
  */
 void EsminiAdapter::runEsminiSimulation()
 {
@@ -503,7 +503,7 @@ void EsminiAdapter::runEsminiSimulation()
 		throw std::runtime_error(std::string("Failed to initialize with odr file ").append(odrFile));
 	}
 
-	
+
 	// Call RM_GetOpenDriveGeoReference to get the RM_GeoReference struct
 	RM_GeoReference geoRef;
 	if (RM_GetOpenDriveGeoReference(&geoRef) == 0) {
@@ -517,13 +517,13 @@ void EsminiAdapter::runEsminiSimulation()
 			me->testOrigin.position.altitude = llh_0[2];
 			me->testOriginSet = true;
 
-			std::string projStringTo = "+proj=tmerc +lat_0=" + std::to_string(llh_0[0]) + 
-													" +lon_0=" + std::to_string(llh_0[1]) + 
+			std::string projStringTo = "+proj=tmerc +lat_0=" + std::to_string(llh_0[0]) +
+													" +lon_0=" + std::to_string(llh_0[1]) +
 													" +datum="+ toDatum + " +units=m +no_defs";
 
 			me->crsTransformation = std::make_shared<CRSTransformation>(projStringFrom, projStringTo);
 			me->applyTrajTransform = true;
-		} 
+		}
 		catch (std::exception& e) {
 			RCLCPP_ERROR(me->get_logger(), e.what());
 			return;
@@ -565,7 +565,7 @@ void EsminiAdapter::runEsminiSimulation()
 				me->atosObjectIdToTraj.emplace(atos_id, traj);
 				me->atosIdToEsminiId.emplace(atos_id, esminiId);
 				RCLCPP_INFO(me->get_logger(), "Extracted trajectory for object %s with size %d", objectName, traj.points.size());
-				
+
 				me->pathPublishers.emplace(atos_id, ROSChannels::Path::Pub(*me, atos_id));
 				me->pathPublishers.at(atos_id).publish(traj.toPath());
 				std::array<double,3> llh_0 = {me->testOrigin.position.latitude, me->testOrigin.position.longitude, me->testOrigin.position.altitude};
@@ -632,4 +632,4 @@ int EsminiAdapter::initializeModule() {
 		std::bind(&EsminiAdapter::onRequestTestOrigin, _1, _2));
 
 	return retval;
-}
+}

atos/modules/ObjectControl/CMakeLists.txt

@@ -46,22 +46,10 @@ add_executable(${OBJECT_CONTROL_TARGET}
 	${CMAKE_CURRENT_SOURCE_DIR}/src/objectlistener.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/src/objectconnection.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/src/channel.cpp
-	${CMAKE_CURRENT_SOURCE_DIR}/src/states/state.cpp
-	${CMAKE_CURRENT_SOURCE_DIR}/src/states/idle.cpp
-	${CMAKE_CURRENT_SOURCE_DIR}/src/states/initialized.cpp
-	${CMAKE_CURRENT_SOURCE_DIR}/src/states/connecting.cpp
-	${CMAKE_CURRENT_SOURCE_DIR}/src/states/ready.cpp
-	${CMAKE_CURRENT_SOURCE_DIR}/src/states/aborting.cpp
-	${CMAKE_CURRENT_SOURCE_DIR}/src/states/clearing.cpp
-	${CMAKE_CURRENT_SOURCE_DIR}/src/states/testlive.cpp
-	${CMAKE_CURRENT_SOURCE_DIR}/src/states/disarming.cpp
-	${CMAKE_CURRENT_SOURCE_DIR}/src/states/armed.cpp
-	${CMAKE_CURRENT_SOURCE_DIR}/src/states/done.cpp
-	${CMAKE_CURRENT_SOURCE_DIR}/src/states/remotecontrolled.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/src/objectcontrol.cpp
 )
 # Link project executable to util libraries
-target_link_libraries(${OBJECT_CONTROL_TARGET} 
+target_link_libraries(${OBJECT_CONTROL_TARGET}
 	${TIME_LIBRARY}
 	${COREUTILS_LIBRARY}
 	${FILESYSTEM_LIBRARY}
@@ -97,4 +85,3 @@ install(TARGETS ${OBJECT_CONTROL_TARGET}
 	ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
 	PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
 )
-

atos/modules/ObjectControl/inc/objectcontrol.hpp

@@ -12,12 +12,15 @@
 #include <memory>
 #include <unordered_map>
 
+#include "sml.hpp"
+
 #include "geographic_msgs/msg/geo_point.hpp"
 
 #include "module.hpp"
 #include "atosTime.h"
 #include "testobject.hpp"
 #include "objectlistener.hpp"
+#include "statemachine.hpp"
 #include "roschannels/commandchannels.hpp"
 #include "roschannels/monitorchannel.hpp"
 #include "roschannels/remotecontrolchannels.hpp"
@@ -38,45 +41,11 @@
 class ObjectControlState;
 class ObjectListener;
 
-namespace AbstractKinematics {
-	class Idle;
-	class Initialized;
-	class Connecting;
-	class Ready;
-	class Aborting;
-	class Clearing;
-	class Armed;
-	class TestLive;
-	class Disarming;
-	class Done;
-	class RemoteControlled;
-}
-
-namespace RelativeKinematics {
-	class Initialized;
-	class Connecting;
-	class Ready;
-	class Aborting;
-	class Clearing;
-	class Armed;
-	class TestLive;
-	class Disarming;
-	class Done;
-	class RemoteControlled;
-}
-
-namespace AbsoluteKinematics {
-	class Initialized;
-	class Connecting;
-	class Ready;
-	class Aborting;
-	class Clearing;
-	class Armed;
-	class TestLive;
-	class Disarming;
-	class Done;
-	class RemoteControlled;
-}
+enum class ControlMode : int {
+	AbsoluteKinematics,
+	RelativeKinematics,
+};
+
 
 /*!
  * \brief The ObjectControl class is intended as an overarching device
@@ -87,47 +56,10 @@ namespace AbsoluteKinematics {
 class ObjectControl : public Module
 {
 	friend class ObjectControlState;
-	friend class AbstractKinematics::Idle;
-	friend class AbstractKinematics::Initialized;
-	friend class AbstractKinematics::Connecting;
-	friend class AbstractKinematics::Ready;
-	friend class AbstractKinematics::Aborting;
-	friend class AbstractKinematics::Clearing;
-	friend class AbstractKinematics::Armed;
-	friend class AbstractKinematics::TestLive;
-	friend class AbstractKinematics::Disarming;
-	friend class AbstractKinematics::Done;
-	friend class AbstractKinematics::RemoteControlled;
-	friend class RelativeKinematics::Initialized;
-	friend class RelativeKinematics::Connecting;
-	friend class RelativeKinematics::Ready;
-	friend class RelativeKinematics::Aborting;
-	friend class RelativeKinematics::Clearing;
-	friend class RelativeKinematics::Armed;
-	friend class RelativeKinematics::TestLive;
-	friend class RelativeKinematics::Disarming;
-	friend class RelativeKinematics::Done;
-	friend class RelativeKinematics::RemoteControlled;
-	friend class AbsoluteKinematics::Initialized;
-	friend class AbsoluteKinematics::Connecting;
-	friend class AbsoluteKinematics::Ready;
-	friend class AbsoluteKinematics::Aborting;
-	friend class AbsoluteKinematics::Clearing;
-	friend class AbsoluteKinematics::Armed;
-	friend class AbsoluteKinematics::TestLive;
-	friend class AbsoluteKinematics::Disarming;
-	friend class AbsoluteKinematics::Done;
-	friend class AbsoluteKinematics::RemoteControlled;
-
 	friend class ObjectListener;
 
 public:
 	ObjectControl(std::shared_ptr<rclcpp::executors::MultiThreadedExecutor>);
-	typedef enum {
-		RELATIVE_KINEMATICS,	//!< Scenario executed relative to immobile VUT
-		ABSOLUTE_KINEMATICS		//!< Scenario executed relative to earth-fixed point
-	} ControlMode;
-
 
 	typedef struct {
 		unsigned int numberOfTargets;
@@ -212,6 +144,8 @@ class ObjectControl : public Module
 	void startControlSignalSubscriber();
 	void stopControlSignalSubscriber();
 
+	void setControlMode(ControlMode cm)	{ controlMode = cm; }
+
 private:
 	bool isResetting;
 	geographic_msgs::msg::GeoPoint origin_pos; //!< Test origin
@@ -240,8 +174,9 @@ class ObjectControl : public Module
 
 	using clock = std::chrono::steady_clock;
 
-	ControlMode controlMode;
-	ObjectControlState* state;					//!< State of module
+	ControlMode controlMode{ControlMode::AbsoluteKinematics};
+
+
 	std::map<uint32_t,std::shared_ptr<TestObject>> objects;		//!< List of configured test participants
 	std::map<uint32_t,ObjectListener> objectListeners;
 	std::map<uint16_t,std::function<void()>> storedActions;
@@ -294,6 +229,13 @@ class ObjectControl : public Module
 	rclcpp::Client<atos_interfaces::srv::GetObjectIp>::SharedPtr ipClient;	//!< Client to request object IPs
 	rclcpp::Client<atos_interfaces::srv::GetObjectReturnTrajectory>::SharedPtr returnTrajectoryClient;	//!< Client to request object return trajectory
 	rclcpp::Service<atos_interfaces::srv::GetObjectControlState>::SharedPtr stateService;	//!< Service to request object control state
+
+	state_machine::Logger sm_logger{get_logger(), stateChangePub, failurePub};
+	boost::sml::sm<state_machine::StateMachine,
+					boost::sml::logger<state_machine::Logger>,
+					boost::sml::thread_safe<std::recursive_mutex>> sm{this, sm_logger};
+
+public:
 	//! Connection methods
 	//! \brief Initiate a thread-based connection attempt. Threads are detached after start,
 	//!			and can be terminated by calling ::abortConnectionAttempt or setting ::connStopReqFuture.
@@ -361,7 +303,9 @@ class ObjectControl : public Module
 	void injectObjectData(const MonitorMessage& monr);
 	//! \brief TODO
 	OsiHandler::LocalObjectGroundTruth_t buildOSILocalGroundTruth(const MonitorMessage&) const;
-	
+
 	void publishScenarioInfoToJournal();
-};
 
+	OBCState_t stateAsNumber();
+	ControlCenterStatusType controlCenterStatus();
+};