addressed most comments for the PR

src/giskardpy/goals/pointing.py

@@ -59,7 +59,10 @@ def __init__(self,
             self.tip_V_pointing_axis.vector.z = 1
 
         root_T_tip = god_map.world.compose_fk_expression(self.root, self.tip)
-        root_P_goal_point = cas.Point3(self.root_P_goal_point)
+        root_P_goal_point = symbol_manager.get_expr(f'god_map.motion_goal_manager.motion_goals[\'{str(self)}\']'
+                                                    f'.root_P_goal_point',
+                                                    input_type_hint=PointStamped,
+                                                    output_type_hint=cas.Point3)
         root_P_goal_point.reference_frame = self.root
         tip_V_pointing_axis = cas.Vector3(self.tip_V_pointing_axis)
 

src/giskardpy/python_interface/python_interface.py

@@ -40,7 +40,8 @@
     PayloadAlternator
 from giskardpy.utils.utils import kwargs_to_json, get_all_classes_in_package
 from giskardpy.goals.feature_functions import AlignPerpendicular, HeightGoal, AngleGoal, DistanceGoal
-from giskardpy.motion_graph.monitors.feature_monitors import PerpendicularMonitor, AngleMonitor, HeightMonitor, DistanceMonitor
+from giskardpy.motion_graph.monitors.feature_monitors import PerpendicularMonitor, AngleMonitor, HeightMonitor, \
+    DistanceMonitor
 from giskard_msgs.msg import ExecutionState
 
 
@@ -2056,19 +2057,15 @@ def execute(self, wait: bool = True) -> MoveResult:
         :param wait: this function blocks if wait=True
         :return: result from giskard
         """
-        result = self._send_action_goal(MoveGoal.EXECUTE, wait)
-        self.last_execution_state = result.execution_state
-        return result
+        return self._send_action_goal(MoveGoal.EXECUTE, wait)
 
     def projection(self, wait: bool = True) -> MoveResult:
         """
         Plans, but doesn't execute the goal. Useful, if you just want to look at the planning ghost.
         :param wait: this function blocks if wait=True
         :return: result from Giskard
         """
-        result = self._send_action_goal(MoveGoal.PROJECTION, wait)
-        self.last_execution_state = result.execution_state
-        return result
+        return self._send_action_goal(MoveGoal.PROJECTION, wait)
 
     def _send_action_goal(self, goal_type: int, wait: bool = True) -> Optional[MoveResult]:
         """
@@ -2082,7 +2079,9 @@ def _send_action_goal(self, goal_type: int, wait: bool = True) -> Optional[MoveR
         goal.type = goal_type
         if wait:
             self._client.send_goal_and_wait(goal)
-            return self._client.get_result()
+            result = self._client.get_result()
+            self.last_execution_state = result
+            return result
         else:
             self._client.send_goal(goal, feedback_cb=self._feedback_cb)
 
@@ -2117,7 +2116,15 @@ def get_result(self, timeout: rospy.Duration = rospy.Duration()) -> MoveResult:
     def _feedback_cb(self, msg: MoveFeedback):
         self.last_feedback = msg
 
-    def get_end_motion_reason(self, move_result: MoveResult = None, show_all=False):
+    def get_end_motion_reason(self, move_result: Optional[MoveResult] = None, show_all: bool = False) -> Dict[
+        str, bool]:
+        """
+        Analyzes a MoveResult msg to return a list of all monitors that hindered the EndMotion Monitors from becoming active.
+        Uses the last received MoveResult msg from execute() or projection() when not explicitly given.
+        :param move_result: the move_result msg to analyze
+        :param show_all: returns the state of all monitors when show_all==True
+        :return: Dict with monitor name as key and True or False as value
+        """
         if not move_result and not self.last_execution_state:
             raise Exception('No MoveResult available to analyze')
         elif not move_result:
@@ -2126,44 +2133,37 @@ def get_end_motion_reason(self, move_result: MoveResult = None, show_all=False):
             execution_state = move_result.execution_state
 
         result = {}
+        if show_all:
+            return {monitor.name: state for monitor, state in
+                    zip(execution_state.monitors, execution_state.monitor_state)}
 
-        endMotion_idx = 0
-        cancelMotion_idx = 0
-        idx = 0
-        for monitor in execution_state.monitors:
-            if monitor.monitor_class == 'CancelMotion':
-                cancelMotion_idx = idx
-            if monitor.monitor_class == 'EndMotion':
-                endMotion_idx = idx
-            idx += 1
-
-        if execution_state.monitor_state[endMotion_idx] == 1:
-            # the end motion was successful
-            return result
+        failedEndMotion_ids = []
+        for idx, monitor in enumerate(execution_state.monitors):
+            if monitor.monitor_class == 'EndMotion' and execution_state.monitor_state[idx] == 0:
+                failedEndMotion_ids.append(idx)
 
-        if show_all:
-            for monitor, state in zip(execution_state.monitors, execution_state.monitor_state):
-                result[monitor.name] = state
+        if len(failedEndMotion_ids) == 0:
+            # the end motion was successful
             return result
 
         def search_for_monitor_values_in_start_condition(start_condition: str):
             res = []
             for monitor, state in zip(execution_state.monitors, execution_state.monitor_state):
-                if monitor.name in start_condition and state == 0:
+                if f'\'{monitor.name}\'' in start_condition and state == 0:
                     res.append(monitor)
             return res
 
-        start_condition = execution_state.monitors[endMotion_idx].start_condition
-        false_monitors = search_for_monitor_values_in_start_condition(start_condition=start_condition)
-        idx = 0
-        # repeatedly search for all inactive monitors in all start_conditions directly
-        # connected to the endMotion start_condition
-        while idx < len(false_monitors):
-            if false_monitors[idx].start_condition != '1.0':
-                false_monitors.extend(search_for_monitor_values_in_start_condition(false_monitors[idx].start_condition))
-            idx += 1
-
-        for mon in false_monitors:
-            result[mon.name] = False
+        for endMotion_idx in failedEndMotion_ids:
+            start_condition = execution_state.monitors[endMotion_idx].start_condition
+            false_monitors = search_for_monitor_values_in_start_condition(start_condition=start_condition)
+            # repeatedly search for all inactive monitors in all start_conditions directly
+            # connected to the endMotion start_condition
+            for idx, false_monitor in enumerate(false_monitors):
+                if false_monitors[idx].start_condition != '1.0':
+                    false_monitors.extend(
+                        search_for_monitor_values_in_start_condition(false_monitor.start_condition))
+
+            for mon in false_monitors:
+                result[mon.name] = False
 
         return result

test/test_integration_pr2.py

@@ -4862,7 +4862,39 @@ def test_get_end_motion_reason_convoluted(self, zero_pose: PR2TestWrapper):
         assert len(reason) == 3 and list(reason.keys())[0] == 'M2 DistanceMonitor' \
                and list(reason.keys())[2] == 'M0 sleep1' and list(reason.keys())[1] == 'M1 sleep2'
 
-# kernprof -lv py.test -s test/test_integration_pr2.py
+    def test_multiple_end_motion_monitors(self, zero_pose: PR2TestWrapper):
+        goal_point = PointStamped()
+        goal_point.header.frame_id = 'map'
+        goal_point.point = Point(2, 2, 2)
+        controlled_point = PointStamped()
+        controlled_point.header.frame_id = zero_pose.r_tip
+
+        mon_sleep1 = zero_pose.monitors.add_sleep(10, name='sleep1')
+        mon_sleep2 = zero_pose.monitors.add_sleep(10, start_condition=mon_sleep1, name='sleep2')
+        mon_distance = zero_pose.monitors.add_distance(root_link='map', tip_link=zero_pose.r_tip,
+                                                       reference_point=goal_point,
+                                                       tip_point=controlled_point, lower_limit=0, upper_limit=0,
+                                                       start_condition=mon_sleep2)
+        zero_pose.motion_goals.add_distance(root_link='base_link', tip_link=zero_pose.r_tip, reference_point=goal_point,
+                                            tip_point=controlled_point, lower_limit=0, upper_limit=0)
+
+        mon_trajectory = zero_pose.monitors.add_max_trajectory_length(max_trajectory_length=1)
+        zero_pose.monitors.add_cancel_motion(mon_trajectory, error_message='stop motion')
+        zero_pose.monitors.add_end_motion(mon_distance)
+
+        mon_sleep3 = zero_pose.monitors.add_sleep(20, name='sleep3')
+        mon_sleep4 = zero_pose.monitors.add_sleep(20, start_condition=mon_sleep3, name='sleep4')
+        zero_pose.monitors.add_end_motion(mon_sleep4)
+
+        result = zero_pose.execute(expected_error_code=GiskardError.MAX_TRAJECTORY_LENGTH,
+                                   add_local_minimum_reached=False)
+        reason = zero_pose.get_end_motion_reason(move_result=result)
+        print(reason)
+        assert len(reason) == 5 and list(reason.keys())[0] == 'M2 DistanceMonitor' \
+               and list(reason.keys())[1] == 'M1 sleep2' and list(reason.keys())[2] == 'M0 sleep1' and \
+               list(reason.keys())[3] == 'M7 sleep4' and list(reason.keys())[4] == 'M6 sleep3'
+
+    # kernprof -lv py.test -s test/test_integration_pr2.py
 # time: [1-9][1-9]*.[1-9]* s
 # import pytest
 # pytest.main(['-s', __file__ + '::TestManipulability::test_manip1'])