changed default params

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py

@@ -45,7 +45,7 @@ def step(self) -> Path:
 
 
 class VisibilityPlanner(Planner):
-    def __init__(self, node: NodeWithConfig, buffer: tf2.Buffer) -> None:
+    def __init__(self, node: NodeWithConfig, buffer: tf2.BufferInterface) -> None:
         self.node = node
         self.buffer = buffer
         self.robots = []
@@ -75,7 +75,7 @@ def set_ball(self, ball: PoseWithCovarianceStamped) -> None:
         point.point = ball.pose.pose.position
         try:
             tf_point = self.buffer.transform(point, self.frame).point
-            self.ball = RoundObstacle(center=(tf_point.x, tf_point.y), radius=self.node.config.map.ball_diameter)
+            self.ball = RoundObstacle(center=(tf_point.x, tf_point.y), radius=self.node.config.map.ball_diameter / 2.0)
         except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
             self.ball = None
             self.node.get_logger().warn(str(e))
@@ -113,8 +113,8 @@ def step(self) -> Path:
         ).transform.translation
         start = (my_position.x, my_position.y)
         config = ObstacleMapConfig(dilate=self.node.config.map.inflation.dilate, num_vertices=12)
-        obstacles = self.robots
-        if self.ball_obstacle_active and self.ball is not None:
+        obstacles = self.robots.copy()
+        if self.ball is not None:
             obstacles.append(self.ball)
         omap = ObstacleMap(config, obstacles)
         path = omap.shortest_path(start, goal)
@@ -124,8 +124,11 @@ def map_to_pose(position):
             pose.pose.position.x = position[0]
             pose.pose.position.y = position[1]
             return pose
+
+        poses = list(map(map_to_pose, path))
+        poses.append(self.goal)
 
         return Path(
             header=Header(frame_id=self.frame, stamp=self.node.get_clock().now().to_msg()),
-            poses=list(map(map_to_pose, path)),
+            poses=poses,
         )

bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml

@@ -67,7 +67,7 @@ bitbots_path_planning:
     inflation:
       dilate:
         type: double
-        default_value: 0.25
+        default_value: 0.3
         description: 'The dilation value for the inflation'
         read_only: true
       blur:
@@ -79,7 +79,7 @@ bitbots_path_planning:
   controller:
     carrot_distance:
       type: int
-      default_value: 4
+      default_value: 1
       description: 'The distance to the carrot that we want to reach on the path'
       validation:
         bounds<>: [0, 100]
@@ -109,7 +109,7 @@ bitbots_path_planning:
         bounds<>: [0.0, 1.0]
     smoothing_tau:
       type: double
-      default_value: 1.0
+      default_value: 0.1
       description: 'This is the time constant of the exponential smoothing filter. The higher the value, the more smoothing is applied. It denotes the time after which a unit step function input signal reaches 63% (1-1/e) of its original strength. In other words, it denotes the time it takes for a new input to be 63% integrated into the output. It is update rate independent!'
       validation:
         bounds<>: [0.0, 1.0]