only publish markers when there are any

SemRoCo · Nov 28, 2024 · bdeb735 · bdeb735
1 parent 7be5744
commit bdeb735
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 1 deletion.
diff --git a/src/giskardpy_ros/ros1/ros_msg_visualization.py b/src/giskardpy_ros/ros1/ros_msg_visualization.py
@@ -160,7 +160,8 @@ def publish_markers(self, world_ns: str = 'world', collision_ns: str = 'collisio
                 self.clear_marker(world_ns)
                 marker_array.markers.extend(self.create_world_markers(name_space=world_ns))
             marker_array.markers.extend(self.create_collision_markers(name_space=collision_ns))
-            self.publisher.publish(marker_array)
+            if len(marker_array.markers) > 0:
+                self.publisher.publish(marker_array)
 
     def publish_trajectory_markers(self, trajectory: Trajectory, every_x: int = 10,
                                    start_alpha: float = 0.5, stop_alpha: float = 1.0,

diff --git a/test/test_integration_hsr.py b/test/test_integration_hsr.py
@@ -9,6 +9,7 @@
 
 from giskard_msgs.msg import LinkName, GiskardError
 from giskardpy.data_types.exceptions import EmptyProblemException
+from giskardpy.motion_graph.tasks.task import WEIGHT_ABOVE_CA
 from giskardpy_ros.configs.behavior_tree_config import StandAloneBTConfig
 from giskardpy_ros.configs.giskard import Giskard
 from giskardpy_ros.configs.iai_robots.hsr import HSRCollisionAvoidanceConfig, WorldWithHSRConfig, HSRStandaloneInterface
@@ -480,6 +481,27 @@ def test_collision_avoidance(self, zero_pose: HSRTestWrapper):
         zero_pose.set_joint_goal(js, add_monitor=False)
         zero_pose.execute()
 
+    #
+    # def test_avoid_collision_touch_hard_threshold(self, box_setup: HSRTestWrapper):
+    #     base_goal = PoseStamped()
+    #     base_goal.header.frame_id = box_setup.default_root
+    #     base_goal.pose.position.x = 0.2
+    #     base_goal.pose.orientation.z = 1
+    #     box_setup.teleport_base(base_goal)
+    #
+    #     box_setup.avoid_collision(min_distance=0.05, group1=box_setup.robot_name)
+    #     box_setup.allow_self_collision()
+    #
+    #     base_goal = PoseStamped()
+    #     base_goal.header.frame_id = 'base_footprint'
+    #     base_goal.pose.position.x = -0.3
+    #     base_goal.pose.orientation.w = 1
+    #     box_setup.set_cart_goal(base_goal, tip_link='base_footprint', root_link='map', weight=WEIGHT_ABOVE_CA)
+    #     box_setup.set_max_traj_length(30)
+    #     box_setup.execute(add_local_minimum_reached=False)
+    #     box_setup.check_cpi_geq(['base_link'], 0.048)
+    #     box_setup.check_cpi_leq(['base_link'], 0.07)
+
 
 class TestAddObject:
     def test_add(self, zero_pose):