diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py
index 42b07cda8..482d58426 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py
+++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py
@@ -11,8 +11,6 @@
PoseStamped,
PoseWithCovarianceStamped,
TransformStamped,
- TwistStamped,
- TwistWithCovarianceStamped,
)
from rclpy.clock import ClockType
from rclpy.duration import Duration
@@ -49,14 +47,7 @@ def __init__(self, blackboard: "BodyBlackboard"):
self.ball_teammate.header.stamp = Time(clock_type=ClockType.ROS_TIME).to_msg()
self.ball_teammate.header.frame_id = self.map_frame
self.ball_lost_time = Duration(seconds=self._blackboard.node.get_parameter("body.ball_lost_time").value)
- self.ball_twist_map: Optional[TwistStamped] = None
self.ball_filtered: Optional[PoseWithCovarianceStamped] = None
- self.ball_twist_lost_time = Duration(
- seconds=self._blackboard.node.get_parameter("body.ball_twist_lost_time").value
- )
- self.ball_twist_precision_threshold = get_parameter_dict(
- self._blackboard.node, "body.ball_twist_precision_threshold"
- )
self.reset_ball_filter = self._blackboard.node.create_client(Trigger, "ball_filter_reset")
self.counter: int = 0
@@ -90,7 +81,6 @@ def __init__(self, blackboard: "BodyBlackboard"):
# Publisher for visualization in RViZ
self.ball_publisher = self._blackboard.node.create_publisher(PointStamped, "debug/viz_ball", 1)
- self.ball_twist_publisher = self._blackboard.node.create_publisher(TwistStamped, "debug/ball_twist", 1)
self.used_ball_pub = self._blackboard.node.create_publisher(PointStamped, "debug/used_ball", 1)
self.which_ball_pub = self._blackboard.node.create_publisher(Header, "debug/which_ball_is_used", 1)
@@ -238,46 +228,6 @@ def ball_filtered_callback(self, msg: PoseWithCovarianceStamped):
except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
self._blackboard.node.get_logger().warn(str(e))
- def recent_ball_twist_available(self) -> bool:
- if self.ball_twist_map is None:
- return False
- return self._blackboard.node.get_clock().now() - self.ball_twist_map.header.stamp < self.ball_twist_lost_time
-
- def ball_twist_callback(self, msg: TwistWithCovarianceStamped):
- x_sdev = msg.twist.covariance[0] # position 0,0 in a 6x6-matrix
- y_sdev = msg.twist.covariance[7] # position 1,1 in a 6x6-matrix
- if (
- x_sdev > self.ball_twist_precision_threshold["x_sdev"]
- or y_sdev > self.ball_twist_precision_threshold["y_sdev"]
- ):
- return
- if msg.header.frame_id != self.map_frame:
- try:
- # point (0,0,0)
- point_a = PointStamped()
- point_a.header = msg.header
- # linear velocity vector
- point_b = PointStamped()
- point_b.header = msg.header
- point_b.point.x = msg.twist.twist.linear.x
- point_b.point.y = msg.twist.twist.linear.y
- point_b.point.z = msg.twist.twist.linear.z
- # transform start and endpoint of velocity vector
- point_a = self._blackboard.tf_buffer.transform(point_a, self.map_frame, timeout=Duration(seconds=1.0))
- point_b = self._blackboard.tf_buffer.transform(point_b, self.map_frame, timeout=Duration(seconds=1.0))
- # build new twist using transform vector
- self.ball_twist_map = TwistStamped(header=msg.header)
- self.ball_twist_map.header.frame_id = self.map_frame
- self.ball_twist_map.twist.linear.x = point_b.point.x - point_a.point.x
- self.ball_twist_map.twist.linear.y = point_b.point.y - point_a.point.y
- self.ball_twist_map.twist.linear.z = point_b.point.z - point_a.point.z
- except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
- self._blackboard.node.get_logger().warn(str(e))
- else:
- self.ball_twist_map = TwistStamped(header=msg.header, twist=msg.twist.twist)
- if self.ball_twist_map is not None:
- self.ball_twist_publisher.publish(self.ball_twist_map)
-
def forget_ball(self, own: bool = True, team: bool = True, reset_ball_filter: bool = True) -> None:
"""
Forget that we and the best teammate saw a ball, optionally reset the ball filter
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py
index 85e332ee9..e8c77d3b8 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py
@@ -5,7 +5,7 @@
from bitbots_tf_buffer import Buffer
from dynamic_stack_decider.dsd import DSD
from game_controller_hl_interfaces.msg import GameState
-from geometry_msgs.msg import PoseWithCovarianceStamped, Twist, TwistWithCovarianceStamped
+from geometry_msgs.msg import PoseWithCovarianceStamped, Twist
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
from rclpy.duration import Duration
from rclpy.executors import MultiThreadedExecutor
@@ -75,13 +75,6 @@ def __init__(self, node: Node):
qos_profile=1,
callback_group=MutuallyExclusiveCallbackGroup(),
)
- node.create_subscription(
- TwistWithCovarianceStamped,
- node.get_parameter("body.ball_movement_subscribe_topic").get_parameter_value().string_value,
- blackboard.world_model.ball_twist_callback,
- qos_profile=1,
- callback_group=MutuallyExclusiveCallbackGroup(),
- )
node.create_subscription(
Twist,
"cmd_vel",
diff --git a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml
index 652762cc3..6356fdbf7 100644
--- a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml
+++ b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml
@@ -3,7 +3,7 @@
# Data older than this is seen as non existent
team_data_timeout: 2
# minimal confidence to regard a ball of a team mate as valid
- ball_max_covariance: 0.5
+ ball_max_covariance: 2.0
body:
roles:
@@ -71,18 +71,10 @@
# it is in a reachable area of the robot
ball_close_distance: 1.5
- # the maximal allowed standard deviation of the ball twist.
- ball_twist_precision_threshold:
- x_sdev: 0.3
- y_sdev: 0.3
-
- # the duration after which a ball_twist is considered irrelevant.
- ball_twist_lost_time: 2
-
# the maximal allowed standard deviation of the ball position.
ball_position_precision_threshold:
- x_sdev: 0.5
- y_sdev: 0.5
+ x_sdev: 2.5
+ y_sdev: 2.5
# An area in which the ball can be kicked
# defined by min/max x/y values in meters which represent ball positions relative to base_footprint
@@ -157,9 +149,6 @@
# Angle at which the ball is normally approached again
ball_reapproach_angle: 1.2
- # topics the behavior subscribes to
- ball_movement_subscribe_topic: 'ball_relative_movement'
-
# 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
diff --git a/bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py b/bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py
index 9eb9e6215..1107483b5 100644
--- a/bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py
+++ b/bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py
@@ -12,8 +12,6 @@
"/audio/audio",
"/ball_obstacle_active",
"/ball_position_relative_filtered",
- "/ball_relative_filtered",
- "/ball_relative_movement",
"/balls_relative",
"/camera/camera_info",
"/camera/image_to_record",
@@ -23,7 +21,6 @@
"/cop_r",
"/core/power_switch_status",
"/debug/approach_point",
- "/debug/ball_twist",
"/debug/dsd/body_behavior/dsd_current_action",
"/debug/dsd/body_behavior/dsd_stack",
"/debug/dsd/body_behavior/dsd_tree",
diff --git a/bitbots_misc/bitbots_containers/hlvs/Dockerfile b/bitbots_misc/bitbots_containers/hlvs/Dockerfile
index 71ef58d4c..c51e74415 100644
--- a/bitbots_misc/bitbots_containers/hlvs/Dockerfile
+++ b/bitbots_misc/bitbots_containers/hlvs/Dockerfile
@@ -42,7 +42,7 @@ RUN sudo rosdep init
# Add some requirements already here so that they are cached
#RUN python3 -m pip install -U pip && \
-# pip3 install -U PyYAML construct defusedxml filterpy matplotlib numpy opencv-python \
+# pip3 install -U PyYAML construct defusedxml matplotlib numpy opencv-python \
# protobuf psutil pytorchyolo setuptools sklearn transforms3d
ADD --chown=robot:robot https://raw.githubusercontent.com/bit-bots/bitbots_main/master/requirements/common.txt src/requirements_common.txt
diff --git a/bitbots_msgs/CMakeLists.txt b/bitbots_msgs/CMakeLists.txt
index 2488d45d1..61884419f 100644
--- a/bitbots_msgs/CMakeLists.txt
+++ b/bitbots_msgs/CMakeLists.txt
@@ -33,7 +33,6 @@ rosidl_generate_interfaces(
"msg/NetworkInterface.msg"
"msg/PoseWithCertainty.msg"
"msg/PoseWithCertaintyArray.msg"
- "msg/PoseWithCertaintyStamped.msg"
"msg/RobotControlState.msg"
"msg/RobotRelative.msg"
"msg/RobotRelativeArray.msg"
diff --git a/bitbots_msgs/msg/PoseWithCertaintyStamped.msg b/bitbots_msgs/msg/PoseWithCertaintyStamped.msg
deleted file mode 100644
index 2b4eea88f..000000000
--- a/bitbots_msgs/msg/PoseWithCertaintyStamped.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-# This expresses an estimated pose detection with a reference coordinate frame and timestamp
-std_msgs/Header header
-bitbots_msgs/PoseWithCertainty pose
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py
index ab151d8b1..6a2b225d1 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py
@@ -6,9 +6,7 @@
from nav_msgs.msg import OccupancyGrid
from rclpy.node import Node
from ros2_numpy import msgify, numpify
-from tf2_geometry_msgs import PointStamped
-
-from bitbots_msgs.msg import PoseWithCertaintyStamped
+from tf2_geometry_msgs import PointStamped, PoseWithCovarianceStamped
class Map:
@@ -33,13 +31,13 @@ def __init__(self, node: Node, buffer: tf2.Buffer) -> None:
self.config_inflation_dialation: int = self.node.declare_parameter("map.inflation.dialte", 3).value
self.config_obstacle_value: int = self.node.declare_parameter("map.obstacle_value", 50).value
- def set_ball(self, ball: PoseWithCertaintyStamped) -> None:
+ def set_ball(self, ball: PoseWithCovarianceStamped) -> None:
"""
Adds a given ball to the ball buffer
"""
point = PointStamped()
point.header.frame_id = ball.header.frame_id
- point.point = ball.pose.pose.pose.position
+ point.point = ball.pose.pose.position
try:
self.ball_buffer = [self.buffer.transform(point, self.frame).point]
except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
index 1d56d091b..214a48f17 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
@@ -1,7 +1,7 @@
import rclpy
import soccer_vision_3d_msgs.msg as sv3dm
from bitbots_tf_buffer import Buffer
-from geometry_msgs.msg import PointStamped, PoseStamped, Twist
+from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Twist
from nav_msgs.msg import OccupancyGrid, Path
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
from rclpy.duration import Duration
@@ -9,7 +9,6 @@
from rclpy.node import Node
from std_msgs.msg import Empty
-from bitbots_msgs.msg import PoseWithCertaintyStamped
from bitbots_path_planning.controller import Controller
from bitbots_path_planning.map import Map
from bitbots_path_planning.planner import Planner
@@ -37,8 +36,8 @@ def __init__(self) -> None:
# Subscriber
self.create_subscription(
- PoseWithCertaintyStamped,
- self.declare_parameter("map.ball_update_topic", "ball_relative_filtered").value,
+ PoseWithCovarianceStamped,
+ self.declare_parameter("map.ball_update_topic", "ball_position_relative_filtered").value,
self.map.set_ball,
5,
callback_group=MutuallyExclusiveCallbackGroup(),
diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml
index 455a8aaac..e804b5488 100644
--- a/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml
+++ b/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml
@@ -9,7 +9,7 @@ bitbots_path_planning:
size:
x: 11.0
y: 8.0
- ball_update_topic: ball_relative_filtered
+ ball_update_topic: ball_position_relative_filtered
robot_update_topic: robots_relative_filtered
ball_diameter: 0.13
obstacle_value: 50
diff --git a/bitbots_navigation/bitbots_path_planning/package.xml b/bitbots_navigation/bitbots_path_planning/package.xml
index 6c06f70c1..88c5f04e2 100644
--- a/bitbots_navigation/bitbots_path_planning/package.xml
+++ b/bitbots_navigation/bitbots_path_planning/package.xml
@@ -7,7 +7,6 @@
Florian Vahl
MIT
- bitbots_msgs
bitbots_tf_buffer
geometry_msgs
nav_msgs
diff --git a/bitbots_world_model/bitbots_ball_filter/README.md b/bitbots_world_model/bitbots_ball_filter/README.md
deleted file mode 100644
index 32d43d848..000000000
--- a/bitbots_world_model/bitbots_ball_filter/README.md
+++ /dev/null
@@ -1,6 +0,0 @@
-# bitbots_ball_filter
-
-ball_filter uses `filterpy` to create a kalmanfilter
-to reduce noise when getting the
-ball position from a ros node. It publishes the
-estimated position and velocity.
\ No newline at end of file
diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py
index 882ff6af4..e90ffff6d 100755
--- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py
+++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py
@@ -1,57 +1,34 @@
#! /usr/bin/env python3
-import math
from typing import Optional, Tuple
import numpy as np
import rclpy
import tf2_ros as tf2
from bitbots_tf_buffer import Buffer
-from filterpy.common import Q_discrete_white_noise
-from filterpy.kalman import KalmanFilter
-from geometry_msgs.msg import Point, PoseWithCovarianceStamped, TwistWithCovarianceStamped
+from geometry_msgs.msg import Point, PoseWithCovarianceStamped
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
from rclpy.duration import Duration
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
+from rclpy.time import Time
+from ros2_numpy import msgify, numpify
+from sensor_msgs.msg import CameraInfo
from soccer_vision_3d_msgs.msg import Ball, BallArray
from std_msgs.msg import Header
from std_srvs.srv import Trigger
-from tf2_geometry_msgs import PointStamped
+from tf2_geometry_msgs import PointStamped, PoseStamped
from bitbots_ball_filter.ball_filter_parameters import bitbots_ball_filter as parameters
-from bitbots_msgs.msg import PoseWithCertaintyStamped
-
-
-class BallWrapper:
- def __init__(self, position: Point, header: Header, confidence: float):
- self.position = position
- self.header = header
- self.confidence = confidence
- self.processed = False
-
- def flag_processed(self) -> None:
- self.processed = True
-
- def get_processed(self) -> bool:
- return self.processed
-
- def get_header(self) -> Header:
- return self.header
-
- def get_position(self) -> Point:
- return self.position
-
- def get_position_tuple(self) -> Tuple[float, float]:
- return self.position.x, self.position.y
-
- def get_confidence(self) -> float:
- return self.confidence
class BallFilter(Node):
+ ball_state_position: np.ndarray
+ ball_state_covariance: np.ndarray
+ config: parameters.Params
+
def __init__(self) -> None:
"""
- creates Kalmanfilter and subscribes to messages which are needed
+ creates filter and subscribes to messages which are needed
"""
super().__init__("ball_filter")
self.logger = self.get_logger()
@@ -59,42 +36,39 @@ def __init__(self) -> None:
# Setup dynamic reconfigure config
self.param_listener = parameters.ParamListener(self)
- self.kf: Optional[KalmanFilter] = None
- self.last_ball_measurement: Optional[BallWrapper] = None
- self.reset_requested = False
-
+ # Initialize parameters
self.update_params()
+ self.logger.info(f"Using frame '{self.config.filter.frame}' for ball filtering")
- self.logger.info(f"Using frame '{self.config.filter_frame}' for ball filtering")
+ self.camera_info: Optional[CameraInfo] = None
+
+ # Initialize state
+ self.reset_ball()
# publishes positions of ball
self.ball_pose_publisher = self.create_publisher(
- PoseWithCovarianceStamped, self.config.ball_position_publish_topic, 1
+ PoseWithCovarianceStamped, self.config.ros.ball_position_publish_topic, 1
)
- # publishes velocity of ball
- self.ball_movement_publisher = self.create_publisher(
- TwistWithCovarianceStamped, self.config.ball_movement_publish_topic, 1
- )
-
- # publishes ball
- self.ball_publisher = self.create_publisher(PoseWithCertaintyStamped, self.config.ball_publish_topic, 1)
-
# Create callback group
self.callback_group = MutuallyExclusiveCallbackGroup()
# setup subscriber
- self.subscriber = self.create_subscription(
+ self.ball_subscriber = self.create_subscription(
BallArray,
- self.config.ball_subscribe_topic,
+ self.config.ros.ball_subscribe_topic,
self.ball_callback,
2,
callback_group=self.callback_group,
)
+ self.camera_info_subscriber = self.create_subscription(
+ CameraInfo, self.config.ros.camera_info_subscribe_topic, self.camera_info_callback, 1
+ )
+
self.reset_service = self.create_service(
Trigger,
- self.config.ball_filter_reset_service_name,
+ self.config.ros.ball_filter_reset_service_name,
self.reset_filter_cb,
callback_group=self.callback_group,
)
@@ -102,73 +76,77 @@ def __init__(self) -> None:
self.filter_timer = self.create_timer(
self.filter_time_step, self.filter_step, callback_group=self.callback_group
)
- self.logger.debug("Ball filter initialized")
+
+ def reset_ball(self) -> None:
+ self.ball_state_position = np.zeros(3)
+ self.ball_state_covariance = np.eye(3) * 1000
def reset_filter_cb(self, req, response) -> Tuple[bool, str]:
self.logger.info("Resetting bitbots ball filter...")
- # Reset filter
- self.reset_requested = True
+ self.reset_ball()
response.success = True
return response
+ def camera_info_callback(self, msg: CameraInfo):
+ """Updates the camera intrinsics"""
+ self.camera_info = msg
+
def ball_callback(self, msg: BallArray) -> None:
- if msg.balls: # Balls exist
- # If we have a kalman filter, we use it's estimate to ignore balls that are too far away (false positives)
+ """handles incoming ball detections form a frame captured by the camera"""
+
+ # Keep track if we have updated the measurement
+ # We might not have a measurement if the ball is not visible
+ # We also filter out balls that are too far away from the filter's estimate
+ ball_measurement_updated: bool = False
+
+ # Do filtering, transform, ... if we have a ball
+ if msg.balls: # Balls exist in the frame
+ # Ignore balls that are too far away (false positives)
# The ignore distance is calculated using the filter's covariance and a factor
# This way false positives are ignored if we already have a good estimate
- if self.kf is not None:
- ignore_threshold_x = math.sqrt(self.kf.P[0, 0]) * self.config.ignore_measurement_threshold
- ignore_threshold_y = math.sqrt(self.kf.P[0, 0]) * self.config.ignore_measurement_threshold
-
- # Filter out balls that are too far away from the filter's estimate
- filtered_balls: list[
- tuple[Ball, PointStamped, float]
- ] = [] # Store, original ball in base_footprint frame, transformed ball in filter frame , distance to filter estimate
- ball: Ball
- for ball in msg.balls:
- ball_transform = self._get_transform(msg.header, ball.center)
- if ball_transform:
- distance_x = ball_transform.point.x - self.kf.x[0]
- distance_y = ball_transform.point.y - self.kf.x[1]
- if abs(distance_x) < ignore_threshold_x and abs(distance_y) < ignore_threshold_y:
- filtered_balls.append((ball, ball_transform, math.hypot(distance_x, distance_y)))
- else:
- filtered_balls = [(ball, self._get_transform(msg.header, ball.center), 0) for ball in msg.balls]
+ ignore_threshold_x, ignore_threshold_y, _ = (
+ np.diag(self.ball_state_covariance) * self.config.filter.tracking.ignore_measurement_threshold
+ )
+
+ # Filter out balls that are too far away from the filter's estimate
+ filtered_balls: list[
+ tuple[Ball, PointStamped, float]
+ ] = [] # Store, original ball in base_footprint frame, transformed ball in filter frame , distance to filter estimate
+ ball: Ball
+ for ball in msg.balls:
+ # Bring ball detection into the frame of the filter and decouple the ball from the robots movement
+ ball_transform = self._get_transform(msg.header, ball.center)
+ # This checks if the ball can be transformed to the filter frame
+ if ball_transform:
+ # Check if the ball is close enough to the filter's estimate
+ diff = numpify(ball_transform.point) - self.ball_state_position
+ if abs(diff[0]) < ignore_threshold_x and abs(diff[1]) < ignore_threshold_y:
+ # Store the ball relative to the robot, the ball in the filter frame and the distance to the filter estimate
+ filtered_balls.append((ball, ball_transform, np.linalg.norm(diff)))
# Select the ball with closest distance to the filter estimate
- # Return if no ball was found
ball_msg, ball_measurement_map, _ = min(filtered_balls, key=lambda x: x[2], default=(None, None, 0))
- if ball_measurement_map is None:
- return
+ # Only update the measurement if we have a ball that is close enough to the filter's estimate
+ if ball_measurement_map is not None:
+ # Estimate the covariance of the measurement
+ # Calculate the distance from the robot to the ball
+ distance = np.linalg.norm(numpify(ball_msg.center))
+ # Calculate the covariance of the measurement based on the distance and the filter's configuration
+ covariance = np.eye(3) * (
+ self.config.filter.covariance.measurement_uncertainty
+ + (distance**2) * self.config.filter.covariance.distance_factor
+ )
+ covariance[2, 2] = 0.0 # Ignore z-axis
- # Store the ball measurement
- self.last_ball_measurement = BallWrapper(
- ball_measurement_map.point, ball_measurement_map.header, ball_msg.confidence.confidence
- )
+ # Store the ball measurement
+ self.ball_state_position = numpify(ball_measurement_map.point)
+ self.ball_state_covariance = covariance
+ ball_measurement_updated = True
- # Initialize filter if not already done
- # We do this here, because we need the ball measurement to initialize the filter
- if self.kf is None:
- self.init_filter(*self.last_ball_measurement.get_position_tuple())
-
- # Calculate distance from the robot to the ball and update measurement noise
- assert msg.header.frame_id == "base_footprint", "Ball frame_id is not 'base_footprint'!"
- robot_ball_delta = math.hypot(ball_msg.center.x, ball_msg.center.y)
- self.update_measurement_noise(robot_ball_delta)
-
- def _get_closest_ball_to_previous_prediction(self, ball_array: BallArray) -> Optional[Ball]:
- closest_distance = math.inf
- closest_ball_msg = ball_array.balls[0]
- for ball_msg in ball_array.balls:
- ball_transform = self._get_transform(ball_array.header, ball_msg.center)
- if ball_transform and self.last_ball_measurement:
- distance = math.dist(
- (ball_transform.point.x, ball_transform.point.y),
- self.last_ball_measurement.get_position_tuple(),
- )
- if distance < closest_distance:
- closest_ball_msg = ball_msg
- return closest_ball_msg
+ # If we did not get a ball measurement, we can check if we should have seen the ball
+ # And increase the covariance if we did not see the ball
+ if not ball_measurement_updated and self.is_estimate_in_fov(msg.header):
+ self.ball_state_covariance *= np.eye(3) * self.config.filter.covariance.negative_observation.value
def _get_transform(
self, header: Header, point: Point, frame: Optional[str] = None, timeout: float = 0.3
@@ -178,7 +156,7 @@ def _get_transform(
"""
if frame is None:
- frame = self.config.filter_frame
+ frame = self.config.filter.frame
point_stamped = PointStamped()
point_stamped.header = header
@@ -189,18 +167,53 @@ def _get_transform(
except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
self.logger.warning(str(e))
- def update_measurement_noise(self, distance: float) -> None:
- self.kf.R = np.eye(2) * (self.config.measurement_certainty + self.config.noise_increment_factor * distance**2)
-
def update_params(self) -> None:
"""
Updates parameters from dynamic reconfigure
"""
self.config = self.param_listener.get_params()
- self.filter_time_step = 1.0 / self.config.filter_rate
- self.filter_reset_duration = Duration(seconds=self.config.filter_reset_time)
- if self.kf:
- self.setup_filter()
+ self.filter_time_step = 1.0 / self.config.filter.rate
+
+ def is_estimate_in_fov(self, header: Header) -> bool:
+ """
+ Calculates if a ball should be currently visible
+ """
+ # Check if we got a camera info to do this stuff
+ if self.camera_info is None:
+ self.logger.info("No camera info received. Not checking if the ball is currently visible.")
+ return False
+
+ # Build a pose
+ ball_pose = PoseStamped()
+ ball_pose.header.frame_id = self.config.filter.frame
+ ball_pose.header.stamp = header.stamp
+ ball_pose.pose.position = msgify(Point, self.ball_state_position)
+
+ # Transform to camera frame
+ try:
+ ball_in_camera_optical_frame = self.tf_buffer.transform(
+ ball_pose, self.camera_info.header.frame_id, timeout=Duration(nanoseconds=0.5 * (10**9))
+ )
+ except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
+ self.logger.warning(str(e))
+ return False
+
+ # Check if the ball is in front of the camera
+ if ball_in_camera_optical_frame.pose.position.z < 0:
+ return False
+
+ # Quick math to get the pixel
+ p = numpify(ball_in_camera_optical_frame.pose.position)
+ k = np.reshape(self.camera_info.k, (3, 3))
+ pixel = np.matmul(k, p)
+ pixel = pixel * (1 / pixel[2])
+
+ # Make sure that the transformed pixel is on the sensor and not too close to the border
+ border_fraction = self.config.filter.covariance.negative_observation.ignore_border
+ border_px = np.array([self.camera_info.width, self.camera_info.height]) / 2 * border_fraction
+ in_fov_horizontal = border_px[0] < pixel[0] <= self.camera_info.width - border_px[0]
+ in_fov_vertical = border_px[1] < pixel[1] <= self.camera_info.height - border_px[1]
+ return in_fov_horizontal and in_fov_vertical
def filter_step(self) -> None:
"""
@@ -216,132 +229,22 @@ def filter_step(self) -> None:
self.param_listener.refresh_dynamic_parameters()
self.update_params()
- # Reset filter if requested
- if self.reset_requested:
- self.kf = None
- self.reset_requested = False
- self.logger.info("Filter reset")
-
- # Early exit if filter is not initialized
- if self.kf is None:
- huge_cov_mat = np.eye(4) * 100
- self.publish_data(np.zeros((4,)), huge_cov_mat)
- return
-
- # Reset filer,if last measurement is too old
- age = self.get_clock().now() - rclpy.time.Time.from_msg(self.last_ball_measurement.get_header().stamp)
- if age > self.filter_reset_duration:
- self.kf = None
- self.logger.info(
- f"Reset filter! Reason: Latest ball is too old {age} > {self.filter_reset_duration} (filter_reset_duration)"
- )
- return
-
- # Predict next state
- self.kf.predict()
- # Update filter with new measurement if available
- if not self.last_ball_measurement.get_processed():
- self.last_ball_measurement.flag_processed()
- self.kf.update(self.last_ball_measurement.get_position_tuple())
- else:
- self.kf.update(None)
- self.publish_data(*self.kf.get_update())
-
- def init_filter(self, x: float, y: float) -> None:
- """
- Initializes kalman filter at given position
-
- :param x: start x position of the ball
- :param y: start y position of the ball
- """
- self.logger.info(f"Initializing filter at position ({x}, {y})")
-
- # Create Kalman filter
- self.kf = KalmanFilter(dim_x=4, dim_z=2, dim_u=0)
-
- # initial value of position(x,y) of the ball and velocity
- self.kf.x = np.array([x, y, 0, 0])
-
- # multiplying by the initial uncertainty
- self.kf.P = np.eye(4) * 1000
-
- # setup the other matrices, that can also be updated without reinitializing the filter
- self.setup_filter()
-
- def setup_filter(self) -> None:
- """
- Sets up the kalman filter with
- the different matrices
- """
- # Models the friction as an exponential decay alpha from the time constant tau (velocity_decay_time)
- # It is defined in a time-step independent way
- exponent_in_s = -self.filter_time_step / self.config.velocity_decay_time
- velocity_factor = 1 - math.exp(exponent_in_s)
-
- # transition matrix
- self.kf.F = np.array(
- [
- [1.0, 0.0, 1.0, 0.0],
- [0.0, 1.0, 0.0, 1.0],
- [0.0, 0.0, 1 - velocity_factor, 0.0],
- [0.0, 0.0, 0.0, 1 - velocity_factor],
- ]
- )
- # measurement function
- self.kf.H = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0]])
-
- # assigning measurement noise
- self.kf.R = np.eye(2) * self.config.measurement_certainty
-
- # assigning process noise
- self.kf.Q = Q_discrete_white_noise(
- dim=2, dt=self.filter_time_step, var=self.config.process_noise_variance, block_size=2, order_by_dim=False
- )
-
- def publish_data(self, state_vec: np.array, cov_mat: np.array) -> None:
- """
- Publishes ball position and velocity to ros nodes
- :param state_vec: current state of kalmanfilter
- :param cov_mat: current covariance matrix
- """
- header = Header()
- header.frame_id = self.config.filter_frame
- header.stamp = rclpy.time.Time.to_msg(self.get_clock().now())
-
- # position
- point_msg = Point(x=float(state_vec[0]), y=float(state_vec[1]))
-
- # covariance
- pos_covariance = np.zeros((6, 6))
- pos_covariance[:2, :2] = cov_mat[:2, :2]
+ # Increase covariance
+ self.ball_state_covariance[:2, :2] += np.eye(2) * self.config.filter.covariance.process_noise
+ # Build message
pose_msg = PoseWithCovarianceStamped()
- pose_msg.header = header
- pose_msg.pose.pose.position = point_msg
- pose_msg.pose.covariance = pos_covariance.reshape(-1)
+ pose_msg.header = Header(
+ stamp=Time.to_msg(self.get_clock().now()),
+ frame_id=self.config.filter.frame,
+ )
+ pose_msg.pose.pose.position = msgify(Point, self.ball_state_position)
+ covariance = np.zeros((6, 6))
+ covariance[:3, :3] = self.ball_state_covariance
+ pose_msg.pose.covariance = covariance.flatten()
pose_msg.pose.pose.orientation.w = 1.0
self.ball_pose_publisher.publish(pose_msg)
- # velocity
- movement_msg = TwistWithCovarianceStamped()
- movement_msg.header = header
- movement_msg.twist.twist.linear.x = float(state_vec[2] * self.config.filter_rate)
- movement_msg.twist.twist.linear.y = float(state_vec[3] * self.config.filter_rate)
- vel_covariance = np.eye(6)
- vel_covariance[:2, :2] = cov_mat[2:, 2:]
- movement_msg.twist.covariance = vel_covariance.reshape(-1)
- self.ball_movement_publisher.publish(movement_msg)
-
- # ball
- ball_msg = PoseWithCertaintyStamped()
- ball_msg.header = header
- ball_msg.pose.pose.pose.position = point_msg
- ball_msg.pose.pose.covariance = pos_covariance.reshape(-1)
- ball_msg.pose.confidence = float(
- self.last_ball_measurement.get_confidence() if self.last_ball_measurement else 0.0
- )
- self.ball_publisher.publish(ball_msg)
-
def main(args=None) -> None:
rclpy.init(args=args)
diff --git a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml
index f26bf4996..e29757d35 100644
--- a/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml
+++ b/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml
@@ -1,87 +1,87 @@
bitbots_ball_filter:
- ball_subscribe_topic:
- type: string
- default_value: 'balls_relative'
- read_only: true
- description: 'Topic to subscribe to for ball detections'
-
- ball_position_publish_topic:
- type: string
- default_value: 'ball_position_relative_filtered'
- read_only: true
- description: 'Topic to publish the filtered ball position'
-
- ball_movement_publish_topic:
- type: string
- default_value: 'ball_relative_movement'
- read_only: true
- description: 'Topic to publish the filtered ball movement'
-
- ball_publish_topic:
- type: string
- default_value: 'ball_relative_filtered'
- read_only: true
- description: 'Topic to publish the filtered ball'
-
- ball_filter_reset_service_name:
- type: string
- default_value: 'ball_filter_reset'
- read_only: true
- description: 'Service to reset the ball filter'
-
- filter_frame:
- type: string
- default_value: 'map'
- description: 'Frame to filter the ball in'
- validation:
- one_of<>: [['map', 'odom']]
+ ros:
+ camera_info_subscribe_topic:
+ type: string
+ default_value: 'camera/camera_info'
+ description: 'Topic of the camera info'
+ read_only: True
- filter_rate:
- type: int
- default_value: 62
- description: 'Filtering rate in Hz'
- read_only: true
- validation:
- bounds<>: [0, 100]
+ ball_subscribe_topic:
+ type: string
+ default_value: 'balls_relative'
+ read_only: true
+ description: 'Topic to subscribe to for ball detections'
- velocity_decay_time:
- type: double
- default_value: 1.96
- description: 'Duration (in seconds) after which, the velocity of the ball is reduced by 1-1/e (63%) due to friction'
- validation:
- bounds<>: [0.0001, 5.0]
-
- process_noise_variance:
- type: double
- default_value: 0.001
- description: 'Noise which is added to the estimated position without new measurements'
- validation:
- bounds<>: [0.0, 1.0]
+ ball_position_publish_topic:
+ type: string
+ default_value: 'ball_position_relative_filtered'
+ read_only: true
+ description: 'Topic to publish the filtered ball position'
- measurement_certainty:
- type: double
- default_value: 0.02
- description: 'Ball measurement certainty in filter'
- validation:
- bounds<>: [0.0, 1.0]
-
- filter_reset_time:
- type: int
- default_value: 20
- description: 'Max ball not seen time in Seconds'
- validation:
- bounds<>: [0, 100]
+ ball_filter_reset_service_name:
+ type: string
+ default_value: 'ball_filter_reset'
+ read_only: true
+ description: 'Service to reset the ball filter'
+
+ filter:
+ frame:
+ type: string
+ default_value: 'map'
+ description: 'Frame to filter the ball in'
+ read_only: true
+ validation:
+ one_of<>: [['map', 'odom']]
+
+ rate:
+ type: int
+ default_value: 62
+ description: 'Filtering rate in Hz'
+ read_only: true
+ validation:
+ bounds<>: [0, 100]
+
+ covariance:
+ process_noise:
+ type: double
+ default_value: 0.005
+ description: 'Noise which is added to the estimated position without new measurements'
+ validation:
+ bounds<>: [0.0, 1.0]
+
+ measurement_uncertainty:
+ type: double
+ default_value: 0.5
+ description: 'Ball measurement certainty in filter'
+ validation:
+ bounds<>: [0.0, 1.0]
+
+ distance_factor:
+ type: double
+ default_value: 0.02
+ description: 'Factor to increase the noise if the ball measurement is further away. This also depends on the reference distance.'
+ validation:
+ bounds<>: [0.0, 1.0]
- noise_increment_factor:
- type: double
- default_value: 0.2
- description: 'Factor to increase the noise if the ball is further away. This also depends on the reference distance.'
- validation:
- bounds<>: [0.0, 1.0]
+ negative_observation:
+ value:
+ type: double
+ default_value: 1.5
+ description: 'Value by which the covariance is increased if we have a negative observation, meaning we did not see the ball if we should have seen it.'
+ validation:
+ bounds<>: [0.0, 10.0]
+
+ ignore_border:
+ type: double
+ default_value: 0.1
+ description: 'Border as a fraction of the image height and width. We do not consider the negative observation if they are close to the border of the image, as we might not see the ball there, when it is partially occluded.'
+ validation:
+ bounds<>: [0.0, 1.0]
- ignore_measurement_threshold:
- type: double
- default_value: 3.0
- description: 'We calculate a distance around our filter state over which we ignore measurements. This threshold is based on the covariance of the filter scaled by this factor. This is used to ignore false positives if we already have a good estimation.'
- validation:
- bounds<>: [0.0, 10.0]
+ tracking:
+ ignore_measurement_threshold:
+ type: double
+ default_value: 6.0
+ description: 'We calculate a distance around our filter state over which we ignore measurements. This threshold is based on the covariance of the filter scaled by this factor. This is used to ignore false positives if we already have a good estimation.'
+ validation:
+ bounds<>: [0.0, 10.0]
diff --git a/bitbots_world_model/bitbots_ball_filter/package.xml b/bitbots_world_model/bitbots_ball_filter/package.xml
index f864ed94c..7c3cf3f1b 100644
--- a/bitbots_world_model/bitbots_ball_filter/package.xml
+++ b/bitbots_world_model/bitbots_ball_filter/package.xml
@@ -12,19 +12,16 @@
Hamburg Bit-Bots
-
-
rosidl_default_generators
rosidl_default_runtime
- bitbots_msgs
bitbots_tf_buffer
+ generate_parameter_library
geometry_msgs
rclpy
+ ros2_numpy
std_msgs
std_srvs
tf2_geometry_msgs
- generate_parameter_library
-
diff --git a/requirements/common.txt b/requirements/common.txt
index c99c26c19..e4f93bdd0 100644
--- a/requirements/common.txt
+++ b/requirements/common.txt
@@ -1,5 +1,4 @@
# This file includes common dependencies for all environments
-filterpy
pip
transforms3d==0.4.1
git+https://github.com/Flova/pyastar2d