listen to ball obstacle active topic

Author: cl03

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py

@@ -38,6 +38,7 @@ def __init__(self, node: Node, buffer: tf2.Buffer) -> None:
         self.config_inflation_blur: int = self.node.config.map.inflation.blur
         self.config_inflation_dialation: int = self.node.config.map.inflation.dialate
         self.config_obstacle_value: int = self.node.config.map.obstacle_value
+        self.ball_obstacle_active: bool = True
 
     def set_ball(self, ball: PoseWithCovarianceStamped) -> None:
         """
@@ -60,7 +61,7 @@ def _render_balls(self) -> None:
             cv2.circle(
                 self.map,
                 self.to_map_space(ball.x, ball.y)[::-1],
-                round(self.config_ball_diameter * self.resolution),
+                round(self.config_ball_diameter / 2 * self.resolution),
                 self.config_obstacle_value,
                 -1,
             )
@@ -140,10 +141,17 @@ def update(self) -> None:
         Regenerates the costmap based on the ball and robot buffer
         """
         self.clear()
-        self._render_balls()
+        if self.ball_obstacle_active:
+            self._render_balls()
         self._render_robots()
         self.inflate()
 
+    def avoid_ball(self, state: bool) -> None:
+        """
+        Activates or deactivates the ball obstacle
+        """
+        self.ball_obstacle_active = state
+
     def get_map(self) -> np.ndarray:
         """
         Returns the costmap as an numpy array

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py

@@ -7,7 +7,7 @@
 from rclpy.duration import Duration
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
-from std_msgs.msg import Empty
+from std_msgs.msg import Bool, Empty
 
 from bitbots_path_planning.controller import Controller
 from bitbots_path_planning.map import Map
@@ -58,6 +58,13 @@ def __init__(self) -> None:
             5,
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
+        self.create_subscription(
+            Bool,
+            "ball_obstacle_active",
+            lambda msg: self.map.avoid_ball(msg.data),
+            5,
+            callback_group=MutuallyExclusiveCallbackGroup(),
+        )
 
         # Publisher
         self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 1)
@@ -106,7 +113,7 @@ def main(args=None):
     node = PathPlanning()
 
     # choose number of threads by number of callback_groups + 1 for simulation time
-    ex = MultiThreadedExecutor(num_threads=7)
+    ex = MultiThreadedExecutor(num_threads=8)
     ex.add_node(node)
     ex.spin()