Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Handle negative observations / filter rework #480

Merged
merged 20 commits into from
Jun 22, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,6 @@
PoseStamped,
PoseWithCovarianceStamped,
TransformStamped,
TwistStamped,
TwistWithCovarianceStamped,
)
from rclpy.clock import ClockType
from rclpy.duration import Duration
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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",
Expand Down
17 changes: 3 additions & 14 deletions bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
3 changes: 0 additions & 3 deletions bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand All @@ -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",
Expand Down
2 changes: 1 addition & 1 deletion bitbots_misc/bitbots_containers/hlvs/Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 0 additions & 1 deletion bitbots_msgs/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
3 changes: 0 additions & 3 deletions bitbots_msgs/msg/PoseWithCertaintyStamped.msg

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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:
Expand Down
Original file line number Diff line number Diff line change
@@ -1,15 +1,14 @@
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
from rclpy.executors import MultiThreadedExecutor
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
Expand Down Expand Up @@ -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(),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 0 additions & 1 deletion bitbots_navigation/bitbots_path_planning/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
<maintainer email="git@flova.de">Florian Vahl</maintainer>
<license>MIT</license>

<depend>bitbots_msgs</depend>
<depend>bitbots_tf_buffer</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
Expand Down
6 changes: 0 additions & 6 deletions bitbots_world_model/bitbots_ball_filter/README.md

This file was deleted.

Loading
Loading