debug expr traj can be published for vector and points

src/giskardpy/configs/behavior_tree_config.py

@@ -113,6 +113,9 @@ def add_trajectory_plotter(self, normalize_position: bool = False, wait: bool =
     def add_trajectory_visualizer(self):
         self.tree.cleanup_control_loop.add_visualize_trajectory()
 
+    def add_debug_trajectory_visualizer(self):
+        self.tree.cleanup_control_loop.add_debug_visualize_trajectory()
+
     def add_debug_trajectory_plotter(self, normalize_position: bool = False, wait: bool = False):
         """
         Plots debug expressions defined in goals.
@@ -225,6 +228,7 @@ def setup(self):
         if self.debug_mode:
             self.add_trajectory_plotter(wait=True)
             # self.add_trajectory_visualizer()
+            self.add_debug_trajectory_visualizer()
             self.add_debug_trajectory_plotter(wait=True)
             self.add_debug_marker_publisher()
         # self.add_debug_marker_publisher()

src/giskardpy/goals/feature_functions.py

@@ -44,6 +44,7 @@ def __init__(self,
                                                                   color=ColorRGBA(r=1, g=0, b=0, a=1))
         elif isinstance(controlled_feature, Vector3Stamped):
             self.root_V_controlled_feature = root_T_tip.dot(cas.Vector3(tip_controlled_feature))
+            self.root_V_controlled_feature.vis_frame = controlled_feature.header.frame_id
             god_map.debug_expression_manager.add_debug_expression('root_V_controlled_feature',
                                                                   self.root_V_controlled_feature,
                                                                   color=ColorRGBA(r=1, g=0, b=0, a=1))
@@ -55,6 +56,7 @@ def __init__(self,
                                                                   color=ColorRGBA(r=0, g=1, b=0, a=1))
         if isinstance(reference_feature, Vector3Stamped):
             self.root_V_reference_feature = cas.Vector3(root_reference_feature)
+            self.root_V_reference_feature.vis_frame = controlled_feature.header.frame_id
             god_map.debug_expression_manager.add_debug_expression('root_V_reference_feature',
                                                                   self.root_V_reference_feature,
                                                                   color=ColorRGBA(r=0, g=1, b=0, a=1))

src/giskardpy/model/ros_msg_visualization.py

@@ -192,6 +192,33 @@ def compute_alpha(i):
                     marker_array.markers.extend(deepcopy(markers))
         self.publisher.publish(marker_array)
 
+    def publish_debug_trajectory(self,
+                                 debug_expressions: Dict[PrefixName, Union[cas.TransMatrix,
+                                 cas.Point3,
+                                 cas.Vector3,
+                                 cas.Quaternion]],
+                                 raw_debug_trajectory: List[Dict[PrefixName, np.ndarray]],
+                                 every_x: int = 10,
+                                 start_alpha: float = 0.5, stop_alpha: float = 1.0,
+                                 namespace: str = 'debug_trajectory') -> None:
+        self.clear_marker(namespace)
+        marker_array = MarkerArray()
+
+        def compute_alpha(i):
+            if i < 0 or i >= len(raw_debug_trajectory):
+                raise ValueError("Index i is out of range")
+            return start_alpha + i * (stop_alpha - start_alpha) / (len(raw_debug_trajectory) - 1)
+
+        for point_id, point in enumerate(raw_debug_trajectory):
+            if point_id % every_x == 0 or point_id == len(raw_debug_trajectory) - 1:
+                markers = self.debug_state_to_vectors_markers(debug_expressions=debug_expressions,
+                                                              debug_values=point,
+                                                              marker_id_offset=len(marker_array.markers))
+                for m in markers:
+                    m.color.a = compute_alpha(point_id)
+                marker_array.markers.extend(deepcopy(markers))
+        self.publisher.publish(marker_array)
+
     def clear_marker(self, ns: str):
         msg = MarkerArray()
         for i in self.marker_ids.values():
@@ -205,11 +232,12 @@ def clear_marker(self, ns: str):
 
     def debug_state_to_vectors_markers(self,
                                        debug_expressions: Dict[PrefixName, Union[cas.TransMatrix,
-                                                                                 cas.Point3,
-                                                                                 cas.Vector3,
-                                                                                 cas.Quaternion]],
+                                       cas.Point3,
+                                       cas.Vector3,
+                                       cas.Quaternion]],
                                        debug_values: Dict[PrefixName, np.ndarray],
-                                       width: float = 0.05) -> List[Marker]:
+                                       width: float = 0.05,
+                                       marker_id_offset: int = 0) -> List[Marker]:
         ms = []
         color_counter = 0
         for (name, expr), (_, value) in zip(debug_expressions.items(), debug_values.items()):
@@ -229,8 +257,8 @@ def debug_state_to_vectors_markers(self,
                 mx = Marker()
                 mx.action = mx.ADD
                 mx.header.frame_id = self.tf_root
-                mx.ns = f'debug{name}'
-                mx.id = 0
+                mx.ns = f'debug/{name}'
+                mx.id = 0 + marker_id_offset
                 mx.type = mx.CYLINDER
                 mx.pose.position.x = map_P_d[0][0] + map_V_x_offset[0]
                 mx.pose.position.y = map_P_d[1][0] + map_V_x_offset[1]
@@ -249,8 +277,8 @@ def debug_state_to_vectors_markers(self,
                 my = Marker()
                 my.action = my.ADD
                 my.header.frame_id = self.tf_root
-                my.ns = f'debug{name}'
-                my.id = 1
+                my.ns = f'debug/{name}'
+                my.id = 1+ marker_id_offset
                 my.type = my.CYLINDER
                 my.pose.position.x = map_P_d[0][0] + map_V_y_offset[0]
                 my.pose.position.y = map_P_d[1][0] + map_V_y_offset[1]
@@ -269,8 +297,8 @@ def debug_state_to_vectors_markers(self,
                 mz = Marker()
                 mz.action = mz.ADD
                 mz.header.frame_id = self.tf_root
-                mz.ns = f'debug{name}'
-                mz.id = 2
+                mz.ns = f'debug/{name}'
+                mz.id = 2+ marker_id_offset
                 mz.type = mz.CYLINDER
                 mz.pose.position.x = map_P_d[0][0] + map_V_z_offset[0]
                 mz.pose.position.y = map_P_d[1][0] + map_V_z_offset[1]
@@ -285,7 +313,7 @@ def debug_state_to_vectors_markers(self,
                 m = Marker()
                 m.action = m.ADD
                 m.ns = f'debug/{name}'
-                m.id = 0
+                m.id = 0+ marker_id_offset
                 m.header.frame_id = self.tf_root
                 m.pose.orientation.w = 1
                 if isinstance(expr, cas.Vector3):

src/giskardpy/tree/behaviors/debug_marker_publisher.py

@@ -11,15 +11,15 @@
 
 import giskardpy.casadi_wrapper as w
 from giskardpy.god_map import god_map
+from giskardpy.model.ros_msg_visualization import ROSMsgVisualization
 from giskardpy.tree.behaviors.plugin import GiskardBehavior
-from giskardpy.utils.decorators import record_time
+from giskardpy.utils.decorators import record_time, catch_and_raise_to_blackboard
 from giskardpy.utils.tfwrapper import normalize_quaternion_msg, np_to_kdl, point_to_kdl, kdl_to_point, \
     quaternion_to_kdl, transform_to_kdl, kdl_to_transform_stamped
 
 
 class DebugMarkerPublisher(GiskardBehavior):
 
-
     @profile
     def __init__(self, name: str = 'debug marker', tf_topic: str = '/tf', map_frame: Optional[str] = None):
         super().__init__(name)
@@ -53,3 +53,25 @@ def update(self):
             ms.markers.extend(markers)
             self.marker_pub.publish(ms)
         return Status.SUCCESS
+
+
+class DebugMarkerPublisherTrajectory(GiskardBehavior):
+    @profile
+    def __init__(self,
+                 name: Optional[str] = None,
+                 ensure_publish: bool = False):
+        super().__init__(name)
+        self.ensure_publish = ensure_publish
+        self.every_x = 10
+
+    @catch_and_raise_to_blackboard
+    @record_time
+    @profile
+    def update(self):
+        debug_exprs = god_map.debug_expression_manager.debug_expressions
+        if len(debug_exprs) > 0:
+            debug_traj = god_map.debug_expression_manager._raw_debug_trajectory
+            god_map.ros_visualizer.publish_debug_trajectory(debug_expressions=debug_exprs,
+                                                            raw_debug_trajectory=debug_traj,
+                                                            every_x=self.every_x)
+        return Status.SUCCESS

src/giskardpy/tree/branches/clean_up_control_loop.py

@@ -1,6 +1,7 @@
 from py_trees import Sequence
 
 from giskardpy.tree.behaviors.append_zero_velocity import SetZeroVelocity
+from giskardpy.tree.behaviors.debug_marker_publisher import DebugMarkerPublisherTrajectory
 from giskardpy.tree.behaviors.delete_monitors_behaviors import DeleteMonitors
 from giskardpy.tree.behaviors.goal_cleanup import GoalCleanUp
 from giskardpy.tree.behaviors.log_trajectory import LogTrajPlugin
@@ -39,6 +40,9 @@ def add_plot_debug_trajectory(self, normalize_position: bool = False, wait: bool
     def add_visualize_trajectory(self):
         self.add_child(VisualizeTrajectory())
 
+    def add_debug_visualize_trajectory(self):
+        self.add_child(DebugMarkerPublisherTrajectory())
+
     def add_plot_gantt_chart(self):
         self.insert_child(PlotGanttChart(), 0)