Tune ball approach (#590)

Author: texhnolyze

bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -104,7 +104,7 @@ body_behavior:
     goalpost_safety_distance: 0.05
 
     # Distance at which the ball is first approached before the ball obstacle is deactivated and we approach closer for the kick
-    ball_far_approach_dist: 0.5
+    ball_far_approach_dist: 0.3
 
     # Range in which the ball far approach point is counted as reached
     ball_far_approach_position_thresh: 0.2
@@ -113,14 +113,14 @@ body_behavior:
     ball_reapproach_dist: 1.0
 
     # Distance at which the ball is normally approached
-    ball_approach_dist: 0.20
+    ball_approach_dist: 0.2                                                          
 
     # Angle at which the ball is normally approached again
     ball_reapproach_angle: 1.2
 
     # The position where the supporter robot should place itself in order to accept a pass
     pass_position_x: 0.1
-    pass_position_y: 1.0
+    pass_position_y: 1.0                                                                                                                                                                                                                                                                                                                                               
     supporter_max_x: 4.0
 
     # maximal angle of a ball kick

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py

@@ -69,10 +69,21 @@ def step(self, path: Path) -> tuple[Twist, PointStamped]:
             end_pose.position.y - current_pose.position.y, end_pose.position.x - current_pose.position.x
         )
 
-        # Calculate the distance from our current position to the final position of the global plan
-        distance = math.hypot(
-            end_pose.position.x - current_pose.position.x, end_pose.position.y - current_pose.position.y
-        )
+        if len(path.poses) < 3:
+            # Calculate the distance from our current position to the final position of the global plan
+            distance = math.hypot(
+                end_pose.position.x - current_pose.position.x, end_pose.position.y - current_pose.position.y
+            )
+        else:
+            # Calculate path length
+            distance = 0
+            a_position = current_pose.position
+            pose: PoseStamped
+            for pose in path.poses:
+                b_postion = pose.pose.position
+                distance += math.hypot(b_postion.x - a_position.x, b_postion.y - a_position.y)
+                a_position = b_postion
+            distance += math.hypot(end_pose.position.x - a_position.x, end_pose.position.y - a_position.y)
 
         # Calculate the translational walk velocity.
         # It considers the distance and breaks if we are close to the final position of the global plan

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()
 

bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml

@@ -66,7 +66,7 @@ bitbots_path_planning:
         read_only: true
       blur:
         type: int
-        default_value: 13
+        default_value: 11
         description: 'The blur value for the inflation'
         read_only: true
 
@@ -121,7 +121,7 @@ bitbots_path_planning:
         bounds<>: [0.0, 1.0]
     translation_slow_down_factor:
       type: double
-      default_value: 0.5
+      default_value: 1.0
       description: 'Clamped p gain of the translation controller'
       validation:
         bounds<>: [0.0, 1.0]

bitbots_world_model/bitbots_robot_filter/config/params.yaml

@@ -6,7 +6,7 @@ bitbots_robot_filter:
     robots_publish_topic: 'robots_relative_filtered'
 
     filter_frame: 'map'
-    robot_dummy_size: 0.4
+    robot_dummy_size: 0.6
     robot_merge_distance: 0.5
     robot_storage_time: 10e9
     team_data_timeout: 1e9