Merge pull request #656 from RI-SE/fix_reset_started_flag

Reset started flag

atos/modules/EsminiAdapter/src/esminiadapter.cpp

@@ -242,7 +242,7 @@ void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr
 */
 void EsminiAdapter::onMonitorMessage(const Monitor::message_type::SharedPtr monr, uint32_t ATOSObjectId) {
 	if (auto idMapping = atosIdToEsminiId.find(ATOSObjectId); idMapping != atosIdToEsminiId.end()) {
-		reportObjectPosition(monr, idMapping->first); // Report object position to esmini
+		reportObjectPosition(monr, idMapping->second); // Report object position to esmini
 		SE_Step(); // Advance the "simulation world"-time
 	} 
 	else {

atos/modules/OpenScenarioGateway/openscenariogateway.py

@@ -55,6 +55,7 @@ def __init__(self):
 
         # ROS subscriptions/publishers
         self.init_ = self.create_subscription(Empty, "init", self.init_callback, 10)
+        self.arm_ = self.create_subscription(Empty, "arm", self.arm_callback, 10)
 
         self.story_board_element_sub_ = self.create_subscription(
             atos_interfaces.msg.StoryBoardElementStateChange,
@@ -93,6 +94,11 @@ def init_callback(self, msg):
             self.get_parameter(ACTIVE_OBJECT_NAME_PARAMETER).value
         )
 
+    def arm_callback(self, msg):
+        # Reset the started flag
+        for id in self.active_objects:
+            self.active_objects[id].started = False
+
     def story_board_element_state_change_callback(self, story_board_element):
         if (
             story_board_element.name in self.start_actions_to_obj_name.keys()
@@ -185,8 +191,6 @@ def update_active_scenario_objects(self, active_objects_name: List[str]):
                         obj.name, id
                     )
                 )
-                # Reset the started flag
-                obj.started = False
 
     def get_all_objects_in_scenario(self, scenario_file) -> List[ScenarioObject]:
         scenario = xosc.ParseOpenScenario(scenario_file)