compilation working

RI-SE · Sep 19, 2024 · fef8e4c · fef8e4c
1 parent 8d36a38
commit fef8e4c
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 31 deletions.
diff --git a/atos/modules/ObjectControl/inc/sm_impl.hpp b/atos/modules/ObjectControl/inc/sm_impl.hpp
@@ -7,46 +7,18 @@
 class SmImpl {
 public:
     // Events
-    struct initializeRequest {};
-    struct ev_stop {};
+    struct InitializeRequest {};
 
     // Guards
     constexpr static auto guard = [](ObjectControl* handler) { return true; };
 
     // Actions
     constexpr static auto clearScenarioAction = [](ObjectControl* handler) { handler->clearScenario(); };
 
-    constexpr static auto initializeRequest = [](ObjectControl* handler) { handler->initializeRequest(); };
-
-    void initializeRequest(
-		ObjectControl& handler) {
-	RCLCPP_INFO(handler.get_logger(), "Handling initialization request");
-	JournalRecordData(JOURNAL_RECORD_EVENT, "INIT received");
-	bool successful = handler.loadScenario(); // Reload objects on each initialize request. 
-	if (!successful) {
-		RCLCPP_ERROR(handler.get_logger(), "Failed to load scenario");
-		JournalRecordData(JOURNAL_RECORD_EVENT, "INIT failed");
-		return;
-	}
-	try {
-		auto anchorID = handler.getAnchorObjectID();
-		handler.transformScenarioRelativeTo(anchorID);
-		handler.controlMode = ObjectControl::RELATIVE_KINEMATICS;
-		setState(handler, new RelativeKinematics::Initialized);
-		RCLCPP_INFO(handler.get_logger(), "Relative control mode enabled");
-	} catch (std::invalid_argument&) {
-		handler.controlMode = ObjectControl::ABSOLUTE_KINEMATICS;
-		setState(handler, new AbsoluteKinematics::Initialized);
-		RCLCPP_INFO(handler.get_logger(), "Absolute control mode enabled");
-	}
-}
-    constexpr static auto ac_stop  = [](ObjectControl* handler) { handler->loadScenario(); };
-
     auto operator()() const noexcept {
         using namespace boost::sml;
         return make_transition_table(
-                 *state<AbstractKinematics::Idle> + event<initializeRequest> [ guard ] / clearScenarioAction = state<AbstractKinematics::Ready>
-                 , state<AbstractKinematics::Read>
+                 *state<AbstractKinematics::Idle> + event<InitializeRequest> [ guard ] / clearScenarioAction = state<AbstractKinematics::Initialized>
         );
     }
 };
diff --git a/atos/modules/ObjectControl/src/objectcontrol.cpp b/atos/modules/ObjectControl/src/objectcontrol.cpp
@@ -74,7 +74,7 @@ ObjectControl::ObjectControl(std::shared_ptr<rclcpp::executors::MultiThreadedExe
 	stateService = create_service<atos_interfaces::srv::GetObjectControlState>(ServiceNames::getObjectControlState,
 		std::bind(&ObjectControl::onRequestState, this, _1, _2));
 	//RCLCPP_ERROR(get_logger(), "State is initialized: %s", sm->is("Initialized"));
-	sm->process_event(SmImpl::initializeRequest());
+	sm->process_event(SmImpl::InitializeRequest());
 	RCLCPP_ERROR(get_logger(), "State is initialized: %u", sm->is(sml::state<AbstractKinematics::Idle>));
 	RCLCPP_ERROR(get_logger(), "State is initialized: %u", sm->is(sml::state<AbstractKinematics::Initialized>));