working example

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

Hide file tree

Showing 3 changed files with 33 additions and 4 deletions.
diff --git a/atos/modules/ObjectControl/inc/sm_impl.hpp b/atos/modules/ObjectControl/inc/sm_impl.hpp
@@ -15,13 +15,38 @@ class SmImpl {
 
     // 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::Initialized>
-                , "Driving"_s    + event<ev_stop>     / ac_stop     =    "Idle"_s
+                 *state<AbstractKinematics::Idle> + event<initializeRequest> [ guard ] / clearScenarioAction = state<AbstractKinematics::Ready>
+                 , state<AbstractKinematics::Read>
         );
     }
 };
diff --git a/atos/modules/ObjectControl/src/objectcontrol.cpp b/atos/modules/ObjectControl/src/objectcontrol.cpp
@@ -73,6 +73,10 @@ ObjectControl::ObjectControl(std::shared_ptr<rclcpp::executors::MultiThreadedExe
 	returnTrajectoryClient = create_client<atos_interfaces::srv::GetObjectReturnTrajectory>(ServiceNames::getObjectReturnTrajectory);
 	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());
+	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>));
 
 	// Set the initial state
 	this->state = static_cast<ObjectControlState*>(new AbstractKinematics::Idle);

diff --git a/modules/ObjectControl/inc/sm_impl.hpp b/modules/ObjectControl/inc/sm_impl.hpp
@@ -20,8 +20,8 @@ class SmImpl {
     auto operator()() const noexcept {
         using namespace boost::sml;
         return make_transition_table(
-                 *state<AbstractKinematics::Idle> + event<initializeRequest> [ guard ] / clearScenarioAction = state<AbstractKinematics::Initialized>
-                , "Driving"_s    + event<ev_stop>     / ac_stop     =    "Idle"_ss
+                 *"idle"_s + event<initializeRequest> [ guard ] / clearScenarioAction = "initialized"_s
+                , "Driving"_s    + event<ev_stop>     / ac_stop     =    "Idle"_s
         );
     }
 };