diff --git a/.gitignore b/.gitignore
index 1d019f1b1..3d75bbd0c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -227,3 +227,5 @@ doku/*
# Workspace git status file from the deploy tool
**/workspace_status.json
+
+.pytest_cache/
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 7168aec42..be0dde9f6 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -222,11 +222,9 @@
"ros.distro": "iron",
"search.useIgnoreFiles": false,
"python.autoComplete.extraPaths": [
- "/opt/openrobots/lib/python3.10/site-packages",
"/opt/ros/iron/lib/python3.10/site-packages"
],
"python.analysis.extraPaths": [
- "/opt/openrobots/lib/python3.10/site-packages",
"/opt/ros/iron/lib/python3.10/site-packages"
],
"cmake.configureOnOpen": false,
diff --git a/Makefile b/Makefile
index 375fe05b5..92abcbe43 100644
--- a/Makefile
+++ b/Makefile
@@ -17,7 +17,8 @@ install-no-root: pull-init update-no-root
pip:
# Install and upgrade pip dependencies
- pip install --upgrade -r requirements/dev.txt --user
+ pip install --upgrade pip --user
+ pip install --upgrade -r requirements/dev.txt --user -v
pre-commit:
# Install pre-commit hooks for all submodules that have a .pre-commit-config.yaml file
diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py
index 30754da64..8f9eb2d92 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py
+++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py
@@ -166,8 +166,17 @@ def get_ball_goal(self, target: BallGoalType, distance: float, side_offset: floa
elif ball_x > self._blackboard.world_model.field_length / 2 - 0.2:
goal_angle = math.pi + np.copysign(math.pi / 4, ball_y)
- goal_x = ball_x - math.cos(goal_angle) * distance + math.sin(goal_angle) * side_offset
- goal_y = ball_y - math.sin(goal_angle) * distance + math.cos(goal_angle) * side_offset
+ # We don't want to walk into the ball, so we add an offset to stop before the ball
+ approach_offset_x = math.cos(goal_angle) * distance
+ approach_offset_y = math.sin(goal_angle) * distance
+
+ # We also want to kick the ball with one foot instead of the center between the feet
+ side_offset_x = math.cos(goal_angle - math.pi / 2) * side_offset
+ side_offset_y = math.sin(goal_angle - math.pi / 2) * side_offset
+
+ # Calculate the goal position (has nothing to do with the soccer goal)
+ goal_x = ball_x - approach_offset_x + side_offset_x
+ goal_y = ball_y - approach_offset_y + side_offset_y
ball_point = (goal_x, goal_y, goal_angle, self._blackboard.map_frame)
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to.py
index 72068ebbd..ec6682eff 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to.py
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to.py
@@ -1,4 +1,5 @@
import math
+from typing import Optional
import numpy as np
import tf2_ros as tf2
@@ -17,7 +18,7 @@ def __init__(self, blackboard, dsd, parameters):
self.point = float(parameters.get("x", 0)), float(parameters.get("y", 0)), float(parameters.get("t", 0))
self.threshold = float(parameters.get("threshold", 0.1))
self.first = True
- self.odom_goal_pose: PoseStamped = None
+ self.goal_pose: Optional[PoseStamped] = None
def perform(self, reevaluate=False):
if self.first:
@@ -32,18 +33,18 @@ def perform(self, reevaluate=False):
goal_pose.pose.orientation = quat_from_yaw(math.radians(self.point[2]))
try:
- self.odom_goal_pose = self.blackboard.tf_buffer.transform(
- goal_pose, self.blackboard.odom_frame, timeout=Duration(seconds=0.5)
+ self.goal_pose = self.blackboard.tf_buffer.transform(
+ goal_pose, self.blackboard.map_frame, timeout=Duration(seconds=0.5)
)
- self.blackboard.pathfinding.publish(self.odom_goal_pose)
+ self.blackboard.pathfinding.publish(self.goal_pose)
except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
self.blackboard.node.get_logger().warning("Could not transform goal pose: " + str(e))
self.first = False
else:
- current_position = self.blackboard.world_model.get_current_position(self.blackboard.odom_frame)
- if self.odom_goal_pose is not None and current_position is not None:
+ current_position = self.blackboard.world_model.get_current_position()
+ if self.goal_pose is not None and current_position is not None:
position = np.array(current_position[:2])
- goal = np.array([self.odom_goal_pose.pose.position.x, self.odom_goal_pose.pose.position.y])
+ goal = np.array([self.goal_pose.pose.position.x, self.goal_pose.pose.position.y])
distance = np.linalg.norm(goal - position)
if distance < self.threshold:
self.pop()
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py
index ebb24ec83..27108054a 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py
@@ -2,7 +2,6 @@
from bitbots_blackboard.capsules.pathfinding_capsule import BallGoalType
from dynamic_stack_decider.abstract_action_element import AbstractActionElement
from geometry_msgs.msg import Vector3
-from rclpy.duration import Duration
from std_msgs.msg import ColorRGBA
from visualization_msgs.msg import Marker
@@ -23,7 +22,7 @@ def __init__(self, blackboard, dsd, parameters):
self.blocking = parameters.get("blocking", True)
self.distance = parameters.get("distance", self.blackboard.config["ball_approach_dist"])
# Offset so we kick the ball with one foot instead of the center between the feet
- self.side_offset = parameters.get("side_offset", 0.08)
+ self.side_offset = parameters.get("side_offset", 0.05)
def perform(self, reevaluate=False):
pose_msg = self.blackboard.pathfinding.get_ball_goal(self.target, self.distance, self.side_offset)
@@ -31,6 +30,7 @@ def perform(self, reevaluate=False):
approach_marker = Marker()
approach_marker.pose.position.x = self.distance
+ approach_marker.pose.position.y = self.side_offset
approach_marker.type = Marker.SPHERE
approach_marker.action = Marker.MODIFY
approach_marker.id = 1
@@ -40,7 +40,6 @@ def perform(self, reevaluate=False):
color.b = 1.0
color.a = 1.0
approach_marker.color = color
- approach_marker.lifetime = Duration(seconds=0.5).to_msg()
scale = Vector3(x=0.2, y=0.2, z=0.2)
approach_marker.scale = scale
approach_marker.header.stamp = self.blackboard.node.get_clock().now().to_msg()
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/turn.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/turn.py
index 63ae26d99..cadece562 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/turn.py
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/turn.py
@@ -64,7 +64,7 @@ def __init__(self, blackboard, dsd, parameters):
def perform(self, reevaluate=False):
# Increase the rotation speed if we are not at max speed
- if abs(self.current_rotation_vel) < self.max_speed:
+ if abs(self.current_rotation_vel) < abs(self.max_speed):
self.current_rotation_vel += math.copysign(0.05, self.max_speed)
# Create the cmd_vel message
@@ -81,3 +81,13 @@ def perform(self, reevaluate=False):
# Check if the duration is over
if (self.blackboard.node.get_clock().now() - self.start_time).nanoseconds / 1e9 > self.duration:
self.pop()
+
+
+class TurnLastSeenBallSide(Turn):
+ """
+ Turns to the side where the ball was last seen for a given duration.
+ """
+
+ def __init__(self, blackboard, dsd, parameters):
+ super().__init__(blackboard, dsd, parameters)
+ self.max_speed = math.copysign(abs(self.max_speed), self.blackboard.world_model.get_ball_position_uv()[1])
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd
index 2fa525d7e..e5a6d1036 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd
@@ -1,6 +1,6 @@
#SearchBall
$DoOnce
- NOT_DONE --> @ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @Turn + duration:15, @GoToAbsolutePositionFieldFraction + x:0.5 + blocking:false
+ NOT_DONE --> @ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @TurnLastSeenBallSide + duration:15, @GoToAbsolutePositionFieldFraction + x:0.5 + blocking:false
DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.5 + latch:true
NO --> @LookAtFieldFeatures, @GoToAbsolutePositionFieldFraction + x:0.5
YES --> $DoOnce
diff --git a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml
index cd65c1175..1d09c2233 100644
--- a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml
+++ b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml
@@ -61,10 +61,10 @@ body_behavior:
# 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
# http://www.ros.org/reps/rep-0103.html#axis-orientation
- kick_x_enter: 0.2
+ kick_x_enter: 0.25
kick_x_leave: 0.25
kick_y_enter: 0.12
- kick_y_leave: 0.14
+ kick_y_leave: 0.12
# defines the radius around the goal (in form of a box)
# in this area, the goalie will react to the ball.
diff --git a/bitbots_misc/bitbots_bringup/launch/demo.launch b/bitbots_misc/bitbots_bringup/launch/demo.launch
index 6076d5490..e3a627dba 100644
--- a/bitbots_misc/bitbots_bringup/launch/demo.launch
+++ b/bitbots_misc/bitbots_bringup/launch/demo.launch
@@ -10,13 +10,6 @@
-
-
-
-
-
-
-
+
diff --git a/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml b/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml
index 15f4637e7..21519c932 100644
--- a/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml
+++ b/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml
@@ -20,8 +20,8 @@ soccer_ipm:
robots:
footpoint_out_of_image_threshold: 0.8
object_default_dimensions:
- x: 0.2
- y: 0.2
+ x: 0.5
+ y: 0.5
z: 1.0
output_frame: 'base_footprint'
diff --git a/bitbots_misc/bitbots_ipm/launch/ipm.launch b/bitbots_misc/bitbots_ipm/launch/ipm.launch
index 81b264066..efdf028c9 100644
--- a/bitbots_misc/bitbots_ipm/launch/ipm.launch
+++ b/bitbots_misc/bitbots_ipm/launch/ipm.launch
@@ -2,7 +2,7 @@
-
+
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py
index 1ff8568e9..7f7dadb25 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py
@@ -66,7 +66,7 @@ def step(self, path: Path) -> tuple[Twist, PointStamped]:
# Calculate the heading angle from our current position to the final position of the global plan
final_walk_angle = math.atan2(
- end_pose.position.y - current_pose.position.y, end_pose.position.x - current_pose.position.x
+ goal_pose.position.y - current_pose.position.y, goal_pose.position.x - current_pose.position.x
)
if len(path.poses) < 3:
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py
deleted file mode 100644
index 9754a7659..000000000
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py
+++ /dev/null
@@ -1,179 +0,0 @@
-import cv2
-import numpy as np
-import soccer_vision_3d_msgs.msg as sv3dm
-import tf2_ros as tf2
-from bitbots_utils.utils import get_parameters_from_other_node
-from geometry_msgs.msg import Point
-from nav_msgs.msg import OccupancyGrid
-from ros2_numpy import msgify, numpify
-from tf2_geometry_msgs import PointStamped, PoseWithCovarianceStamped
-
-from bitbots_path_planning import NodeWithConfig
-
-
-class Map:
- """
- Costmap that keeps track of obstacles like the ball or other robots.
- """
-
- def __init__(self, node: NodeWithConfig, buffer: tf2.BufferInterface) -> None:
- self.node = node
- self.buffer = buffer
- self.resolution: int = self.node.config.map.resolution
- parameters = get_parameters_from_other_node(
- self.node, "/parameter_blackboard", ["field.size.x", "field.size.y", "field.size.padding"]
- )
- self.size: tuple[float, float] = (
- parameters["field.size.x"] + 2 * parameters["field.size.padding"],
- parameters["field.size.y"] + 2 * parameters["field.size.padding"],
- )
- self.map: np.ndarray = np.ones(
- (np.array(self.size) * self.resolution).astype(int),
- dtype=np.int8,
- )
-
- self.frame: str = self.node.config.map.planning_frame
- self.ball_buffer: list[Point] = []
- self.robot_buffer: list[sv3dm.Robot] = []
- self.config_ball_diameter: float = self.node.config.map.ball_diameter
- 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:
- """
- Adds a given ball to the ball buffer
- """
- point = PointStamped()
- point.header.frame_id = ball.header.frame_id
- 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:
- self.node.get_logger().warn(str(e))
-
- def _render_balls(self) -> None:
- """
- Draws the ball buffer onto the costmap
- """
- ball: sv3dm.Ball
- for ball in self.ball_buffer:
- cv2.circle(
- self.map,
- self.to_map_space(ball.x, ball.y)[::-1],
- round(self.config_ball_diameter / 2 * self.resolution),
- self.config_obstacle_value,
- -1,
- ) # type: ignore
-
- def set_robots(self, robots: sv3dm.RobotArray):
- """
- Adds a given robot array to the robot buffer
- """
- new_buffer: list[sv3dm.Robot] = []
- robot: sv3dm.Robot
- for robot in robots.robots:
- point = PointStamped()
- point.header.frame_id = robots.header.frame_id
- point.point = robot.bb.center.position
- try:
- robot.bb.center.position = self.buffer.transform(point, self.frame).point
- new_buffer.append(robot)
- except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
- self.node.get_logger().warn(str(e))
- self.robot_buffer = new_buffer
-
- def _render_robots(self) -> None:
- """
- Draws the robot buffer onto the costmap
- """
- robot: sv3dm.Robot
- for robot in self.robot_buffer:
- cv2.circle(
- self.map,
- self.to_map_space(robot.bb.center.position.x, robot.bb.center.position.y)[::-1],
- round(max(numpify(robot.bb.size)[:2]) * self.resolution),
- self.config_obstacle_value,
- -1,
- ) # type: ignore
-
- def to_map_space(self, x: float, y: float) -> tuple[int, int]:
- """
- Maps a point (x, y in meters) to corresponding pixel on the costmap
- """
- return (
- max(0, min(round((x - self.get_origin()[0]) * self.resolution), self.map.shape[0] - 1)),
- max(0, min(round((y - self.get_origin()[1]) * self.resolution), self.map.shape[1] - 1)),
- )
-
- def from_map_space_np(self, points: np.ndarray) -> np.ndarray:
- """
- Maps an array of pixel coordinates from the costmap to world coordinates (meters)
- """
- return points / self.resolution + self.get_origin()
-
- def get_origin(self) -> np.ndarray:
- """
- Origin of the costmap in meters
- """
- return np.array([-self.map.shape[0] / self.resolution / 2, -self.map.shape[1] / self.resolution / 2])
-
- def clear(self) -> None:
- """
- Clears the complete cost map
- """
- self.map[...] = 1
-
- def inflate(self) -> None:
- """
- Applies inflation to all occupied areas of the costmap
- """
- idx = self.map == 1
- map = cv2.dilate(
- self.map.astype(np.uint8),
- np.ones((self.config_inflation_dialation, self.config_inflation_dialation), np.uint8),
- iterations=2,
- )
- self.map[idx] = cv2.blur(map, (self.config_inflation_blur, self.config_inflation_blur)).astype(np.int8)[idx]
-
- def update(self) -> None:
- """
- Regenerates the costmap based on the ball and robot buffer
- """
- self.clear()
- 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
- """
- return self.map
-
- def get_frame(self) -> str:
- """
- Returns the frame of reference of the map
- """
- return self.frame
-
- def to_msg(self) -> OccupancyGrid:
- """
- Returns the costmap as an OccupancyGrid message
- """
- msg: OccupancyGrid = msgify(OccupancyGrid, self.get_map().T)
- msg.header.frame_id = self.frame
- msg.info.width = self.map.shape[0]
- msg.info.height = self.map.shape[1]
- msg.info.resolution = 1 / self.resolution
- msg.info.origin.position.x = self.get_origin()[0]
- msg.info.origin.position.y = self.get_origin()[1]
- return msg
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 a9374c8ad..0a17d86e8 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
@@ -2,16 +2,14 @@
import soccer_vision_3d_msgs.msg as sv3dm
from bitbots_tf_buffer import Buffer
from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Twist
-from nav_msgs.msg import OccupancyGrid, Path
-from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+from nav_msgs.msg import Path
from rclpy.duration import Duration
-from rclpy.executors import MultiThreadedExecutor
from std_msgs.msg import Bool, Empty
+from visualization_msgs.msg import MarkerArray
from bitbots_path_planning import NodeWithConfig
from bitbots_path_planning.controller import Controller
-from bitbots_path_planning.map import Map
-from bitbots_path_planning.planner import planner_factory
+from bitbots_path_planning.planner import VisibilityPlanner
class PathPlanning(NodeWithConfig):
@@ -25,57 +23,29 @@ def __init__(self) -> None:
# We need to create a tf buffer
self.tf_buffer = Buffer(self, Duration(seconds=self.config.tf_buffer_duration))
- # Create submodules
- self.map = Map(node=self, buffer=self.tf_buffer)
- self.planner = planner_factory(self)(node=self, buffer=self.tf_buffer, map=self.map)
+ self.planner = VisibilityPlanner(node=self, buffer=self.tf_buffer)
self.controller = Controller(node=self, buffer=self.tf_buffer)
# Subscriber
- self.create_subscription(
- PoseWithCovarianceStamped,
- self.config.map.ball_update_topic,
- self.map.set_ball,
- 5,
- callback_group=MutuallyExclusiveCallbackGroup(),
- )
- self.create_subscription(
- sv3dm.RobotArray,
- self.config.map.robot_update_topic,
- self.map.set_robots,
- 5,
- callback_group=MutuallyExclusiveCallbackGroup(),
- )
- self.goal_sub = self.create_subscription(
- PoseStamped, "goal_pose", self.planner.set_goal, 5, callback_group=MutuallyExclusiveCallbackGroup()
- )
- self.create_subscription(
- Empty,
- "pathfinding/cancel",
- lambda _: self.planner.cancel(),
- 5,
- callback_group=MutuallyExclusiveCallbackGroup(),
- )
+ self.create_subscription(PoseWithCovarianceStamped, self.config.map.ball_update_topic, self.planner.set_ball, 5)
+ self.create_subscription(sv3dm.RobotArray, self.config.map.robot_update_topic, self.planner.set_robots, 5)
+ self.goal_sub = self.create_subscription(PoseStamped, "goal_pose", self.planner.set_goal, 5)
+ self.create_subscription(Empty, "pathfinding/cancel", lambda _: self.planner.cancel_goal(), 5)
self.create_subscription(
Bool,
"ball_obstacle_active",
- lambda msg: self.map.avoid_ball(msg.data), # type: ignore
+ lambda msg: self.planner.avoid_ball(msg.data), # type: ignore
5,
- callback_group=MutuallyExclusiveCallbackGroup(),
)
# Publisher
self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 1)
- self.costmap_pub = self.create_publisher(OccupancyGrid, "costmap", 1)
self.path_pub = self.create_publisher(Path, "path", 1)
self.carrot_pub = self.create_publisher(PointStamped, "carrot", 1)
+ self.graph_pub = self.create_publisher(MarkerArray, "visibility_graph", 1)
# Timer that updates the path and command velocity at a given rate
- self.create_timer(
- 1 / self.config.rate,
- self.step,
- clock=self.get_clock(),
- callback_group=MutuallyExclusiveCallbackGroup(),
- )
+ self.create_timer(1 / self.config.rate, self.step, clock=self.get_clock())
def step(self) -> None:
"""
@@ -85,11 +55,6 @@ def step(self) -> None:
self.param_listener.refresh_dynamic_parameters()
self.config = self.param_listener.get_params()
try:
- # Update the map with the latest ball and robot positions
- self.map.update()
- # Publish the costmap for visualization
- self.costmap_pub.publish(self.map.to_msg())
-
if self.planner.active():
# Calculate the path to the goal pose considering the current map
path = self.planner.step()
@@ -109,10 +74,7 @@ def main(args=None):
rclpy.init(args=args)
node = PathPlanning()
- # choose number of threads by number of callback_groups + 1 for simulation time
- ex = MultiThreadedExecutor(num_threads=8)
- ex.add_node(node)
- ex.spin()
+ rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
index 9b2c34e69..0691e87a0 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
@@ -1,28 +1,90 @@
-import numpy as np
-import pyastar2d
+from abc import ABC, abstractmethod
+from typing import Optional
+
+import soccer_vision_3d_msgs.msg as sv3dm
import tf2_ros as tf2
-from geometry_msgs.msg import PoseStamped, Vector3
+from bitbots_rust_nav import ObstacleMap, ObstacleMapConfig, RoundObstacle
+from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path
from rclpy.duration import Duration
from rclpy.time import Time
+from ros2_numpy import numpify
from std_msgs.msg import Header
+from tf2_geometry_msgs import PointStamped, PoseWithCovarianceStamped
from bitbots_path_planning import NodeWithConfig
-from bitbots_path_planning.map import Map
-class Planner:
- """
- A simple grid based A* interface
- """
+class Planner(ABC):
+ @abstractmethod
+ def set_goal(self, pose: PoseStamped) -> None:
+ pass
+
+ @abstractmethod
+ def cancel_goal(self) -> None:
+ pass
+
+ @abstractmethod
+ def set_robots(self, robots: sv3dm.RobotArray) -> None:
+ pass
+
+ @abstractmethod
+ def set_ball(self, ball: PoseWithCovarianceStamped) -> None:
+ pass
+
+ @abstractmethod
+ def avoid_ball(self, state: bool) -> None:
+ pass
+
+ @abstractmethod
+ def active(self) -> bool:
+ pass
+
+ @abstractmethod
+ def step(self) -> Path:
+ pass
+
- def __init__(self, node: NodeWithConfig, buffer: tf2.BufferInterface, map: Map) -> None:
+class VisibilityPlanner(Planner):
+ def __init__(self, node: NodeWithConfig, buffer: tf2.BufferInterface) -> None:
self.node = node
self.buffer = buffer
- self.map = map
- self.goal: PoseStamped | None = None
- self.path: Path | None = None
+ self.robots: list[RoundObstacle] = []
+ self.ball: Optional[RoundObstacle] = None
+ self.goal: Optional[PoseStamped] = None
self.base_footprint_frame: str = self.node.config.base_footprint_frame
+ self.ball_obstacle_active: bool = True
+ self.frame: str = self.node.config.map.planning_frame
+
+ def set_robots(self, robots: sv3dm.RobotArray):
+ new_buffer: list[RoundObstacle] = []
+ for robot in robots.robots:
+ point = PointStamped()
+ point.header.frame_id = robots.header.frame_id
+ point.point = robot.bb.center.position
+ # Use the maximum dimension of the bounding box as the radius
+ radius = max(numpify(robot.bb.size)[:2]) / 2
+ try:
+ # Transform the point to the planning frame
+ position = self.buffer.transform(point, self.frame).point
+ # Add the robot to the buffer if the transformation was successful
+ new_buffer.append(RoundObstacle(center=(position.x, position.y), radius=radius))
+ except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
+ self.node.get_logger().warn(str(e))
+ self.robots = new_buffer
+
+ def set_ball(self, ball: PoseWithCovarianceStamped) -> None:
+ point = PointStamped()
+ point.header.frame_id = ball.header.frame_id
+ point.point = ball.pose.pose.position
+ try:
+ # Transform the point to the planning frame
+ tf_point = self.buffer.transform(point, self.frame).point
+ # Create a new ball obstacle
+ 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))
def set_goal(self, pose: PoseStamped) -> None:
"""
@@ -31,7 +93,13 @@ def set_goal(self, pose: PoseStamped) -> None:
pose.header.stamp = Time(clock_type=self.node.get_clock().clock_type).to_msg()
self.goal = pose
- def cancel(self) -> None:
+ def avoid_ball(self, state: bool) -> None:
+ """
+ Activates or deactivates the ball obstacle
+ """
+ self.ball_obstacle_active = state
+
+ def cancel_goal(self) -> None:
"""
Removes the current goal
"""
@@ -44,88 +112,44 @@ def active(self) -> bool:
"""
return self.goal is not None
- def get_my_position(self) -> Vector3:
- """
- Returns the current position of the robot
- """
- return self.buffer.lookup_transform(
- self.map.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2)
- ).transform.translation
-
def step(self) -> Path:
"""
- Generates a new A* path to the goal pose with respect to the costmap
+ Computes the next path to the goal
"""
- goal: PoseStamped = self.goal
- assert goal is not None, "No goal set, cannot plan path"
-
- # Get current costmap
- navigation_grid = self.map.get_map()
-
- # Get my pose and position on the map
- my_position = self.get_my_position()
-
- # Transform goal pose to map frame if needed
- if goal.header.frame_id != self.map.frame:
- goal = self.buffer.transform(goal, self.map.frame, timeout=Duration(seconds=0.2))
+ assert self.goal is not None, "No goal set"
+ # Define goal
+ goal = (self.goal.pose.position.x, self.goal.pose.position.y)
+ # Get our current position
+ my_position = self.buffer.lookup_transform(
+ self.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2)
+ ).transform.translation
+ start = (my_position.x, my_position.y)
- # Run A* from our current position to the goal position
- path = pyastar2d.astar_path(
- navigation_grid.astype(np.float32),
- self.map.to_map_space(my_position.x, my_position.y),
- self.map.to_map_space(goal.pose.position.x, goal.pose.position.y),
- allow_diagonal=False,
+ # Configure how obstacles are represented
+ config = ObstacleMapConfig(
+ robot_radius=self.node.config.map.inflation.robot_radius,
+ margin=self.node.config.map.inflation.obstacle_margin,
+ num_vertices=12,
)
-
- # Convert the pixel coordinates to world coordinates
- path = self.map.from_map_space_np(path)
-
- # Build path message
- def to_pose_msg(element):
+ # Add robots to obstacles
+ obstacles = self.robots.copy()
+ # Add ball to obstacles if active
+ if self.ball is not None:
+ obstacles.append(self.ball)
+ obstacle_map = ObstacleMap(config, obstacles)
+
+ # Calculate the shortest path
+ path = obstacle_map.shortest_path(start, goal)
+
+ # Convert the path to a ROS messages
+ def map_to_pose(position):
pose = PoseStamped()
- pose.pose.position.x = element[0]
- pose.pose.position.y = element[1]
+ pose.pose.position.x = position[0]
+ pose.pose.position.y = position[1]
return pose
- poses = list(map(to_pose_msg, path))
-
- poses.append(goal)
- self.path = Path(
- header=Header(frame_id=self.map.get_frame(), stamp=self.node.get_clock().now().to_msg()), poses=poses
+ # Generate the path message
+ return Path(
+ header=Header(frame_id=self.frame, stamp=self.node.get_clock().now().to_msg()),
+ poses=list(map(map_to_pose, path)) + [self.goal],
)
-
- return self.path
-
- def get_path(self) -> Path | None:
- """
- Returns the most recent path
- """
- return self.path
-
-
-class DummyPlanner(Planner):
- def __init__(self, node: NodeWithConfig, buffer: tf2.BufferInterface, map: Map) -> None:
- super().__init__(node, buffer, map)
-
- def step(self) -> Path:
- return self.get_path()
-
- def get_path(self) -> Path:
- pose = PoseStamped()
- my_position = self.get_my_position()
- pose.pose.position.x = my_position.x
- pose.pose.position.y = my_position.y
-
- self.path = Path(
- header=Header(frame_id=self.map.get_frame(), stamp=self.node.get_clock().now().to_msg()),
- poses=[pose, self.goal],
- )
-
- return self.path
-
-
-def planner_factory(node: NodeWithConfig) -> type:
- if node.config.planner.dummy:
- return DummyPlanner
- else:
- return Planner
diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml
index 498c04350..c821b3bbe 100644
--- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml
+++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml
@@ -16,34 +16,12 @@ bitbots_path_planning:
description: 'The rate at which the path planning is executed'
validation:
bounds<>: [0.0, 100.0]
-
- planner:
- dummy:
- type: bool
- default_value: false
- description: 'If true, the dummy planner is used, which just returns a straight line to the goal. This ignores all obstacles, but also has no limitations on the map size.'
-
+
map:
planning_frame:
type: string
default_value: map
description: 'The frame in which the path planning is done'
- resolution:
- type: int
- default_value: 20
- description: 'Pixel per meter'
- read_only: true
- size:
- x:
- type: double
- default_value: 11.0
- description: 'The size of the map in x direction'
- read_only: true
- y:
- type: double
- default_value: 8.0
- description: 'The size of the map in y direction'
- read_only: true
ball_update_topic:
type: string
default_value: ball_position_relative_filtered
@@ -59,33 +37,26 @@ bitbots_path_planning:
default_value: 0.13
description: 'The diameter of the ball'
read_only: true
- obstacle_value:
- type: int
- default_value: 50
- description: 'The value of the obstacles in the map'
- read_only: true
inflation:
- dialate:
- type: int
- default_value: 3
- description: 'The dialte value for the inflation'
- read_only: true
- blur:
- type: int
- default_value: 11
- description: 'The blur value for the inflation'
- read_only: true
+ robot_radius:
+ type: double
+ default_value: 0.3
+ description: 'Radius of a circle on the ground that represents the space occupied by our robot. Instead of planning with both a robot polygon/circle and an obstacle polygon, we just inflate the obstacles and assume the robot is a point. This is faster and simpler.'
+ obstacle_margin:
+ type: double
+ default_value: 0.1
+ description: "Distance we want to keep to obstacles when planning a path around them. No immediate action is required if the robot is closer than this distance to an obstacle, but we don't consider paths this close during the visibility graph generation."
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]
max_rotation_vel:
type: double
- default_value: 0.3
+ default_value: 0.6
description: 'The maximum rotation velocity of the robot in rad/s around the z-axis'
validation:
bounds<>: [0.0, 1.0]
@@ -109,7 +80,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]
@@ -121,7 +92,7 @@ bitbots_path_planning:
bounds<>: [0.0, 1.0]
rotation_slow_down_factor:
type: double
- default_value: 0.3
+ default_value: 0.6
description: 'Clamped p gain of the rotation controller'
validation:
bounds<>: [0.0, 1.0]
@@ -133,7 +104,7 @@ bitbots_path_planning:
bounds<>: [0.0, 1.0]
orient_to_goal_distance:
type: double
- default_value: 1.0
+ default_value: 0.4
description: 'Distance at which we switch from orienting towards the path to orienting towards the goal poses orientation (in meters)'
validation:
bounds<>: [0.0, 10.0]
diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz b/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz
new file mode 100644
index 000000000..892b7e6f0
--- /dev/null
+++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_viz.rviz
@@ -0,0 +1,535 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /Grid1
+ - /Map1
+ - /Map1/Topic1
+ - /Path1
+ - /Path1/Offset1
+ - /Pose1
+ Splitter Ratio: 0.5
+ Tree Height: 1586
+ - Class: rviz_common/Selection
+ Name: Selection
+ - Class: rviz_common/Tool Properties
+ Expanded:
+ - /2D Goal Pose1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz_common/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 20
+ Reference Frame:
+ Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/RobotModel
+ Collision Enabled: false
+ Description File: ""
+ Description Source: Topic
+ Description Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /robot_description
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_optical_frame_uncalibrated:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ imu_frame_2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ imu_frame_uncalibrated:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ l_ankle:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_cleat_l_back:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ l_cleat_l_front:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ l_cleat_r_back:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ l_cleat_r_front:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ l_foot:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_hip_1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_hip_2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_lower_arm:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_lower_leg:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_shoulder:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_sole:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ l_toe:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ l_upper_arm:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_upper_leg:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_wrist:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ llb:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ llf:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ lrb:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ lrf:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ neck:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_ankle:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_cleat_l_back:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ r_cleat_l_front:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ r_cleat_r_back:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ r_cleat_r_front:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ r_foot:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_hip_1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_hip_2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_lower_arm:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_lower_leg:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_shoulder:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_sole:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ r_toe:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ r_upper_arm:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_upper_leg:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_wrist:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ rlb:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rlf:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rrb:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rrf:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ torso:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Mass Properties:
+ Inertia: false
+ Mass: false
+ Name: RobotModel
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Alpha: 0.699999988079071
+ Binary representation: true
+ Binary threshold: 100
+ Class: rviz_default_plugins/Map
+ Color Scheme: map
+ Draw Behind: false
+ Enabled: true
+ Name: Map
+ Topic:
+ Depth: 5
+ Durability Policy: Transient Local
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /field/map
+ Update Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /field/map_updates
+ Use Timestamp: false
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz_default_plugins/Path
+ Color: 129; 61; 156
+ Enabled: true
+ Head Diameter: 1
+ Head Length: 0.20000000298023224
+ Length: 0.30000001192092896
+ Line Style: Billboards
+ Line Width: 0.029999999329447746
+ Name: Path
+ Offset:
+ X: 0
+ Y: 0
+ Z: 1
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.10000000149011612
+ Shaft Length: 0.10000000149011612
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /path
+ Value: true
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz_default_plugins/Pose
+ Color: 51; 209; 122
+ Enabled: true
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: Pose
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /goal_pose
+ Value: true
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz_default_plugins/PoseWithCovariance
+ Color: 255; 25; 0
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: PoseWithCovariance
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /pose_with_covariance
+ Value: true
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz_default_plugins/PoseWithCovariance
+ Color: 255; 25; 0
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: PoseWithCovariance
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /ball_position_relative_filtered
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: MarkerArray
+ Namespaces:
+ visibility_graph_edges: true
+ visibility_graph_nodes: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /visibility_graph
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/Interact
+ Hide Inactive Objects: true
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /initialpose
+ - Class: rviz_default_plugins/SetGoal
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /goal_pose
+ - Class: rviz_default_plugins/PublishPoint
+ Single click: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Angle: -1.429999589920044
+ Class: rviz_default_plugins/TopDownOrtho
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Scale: 77.23892974853516
+ Target Frame:
+ Value: TopDownOrtho (rviz_default_plugins)
+ X: 0
+ Y: 0
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1883
+ Hide Left Dock: false
+ Hide Right Dock: true
+ QMainWindow State: 000000ff00000000fd0000000400000000000001b0000006bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000002d000000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000006bd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000006bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000006bd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002fa000006bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1200
+ X: 1920
+ Y: 0
diff --git a/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch b/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch
index d87afb55c..0774f196a 100755
--- a/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch
+++ b/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch
@@ -1,9 +1,7 @@
-
-
diff --git a/bitbots_navigation/bitbots_path_planning/package.xml b/bitbots_navigation/bitbots_path_planning/package.xml
index 6634a76ee..b16edc786 100644
--- a/bitbots_navigation/bitbots_path_planning/package.xml
+++ b/bitbots_navigation/bitbots_path_planning/package.xml
@@ -8,8 +8,9 @@
MIT
bitbots_tf_buffer
- geometry_msgs
+ cargo
generate_parameter_library
+ geometry_msgs
nav_msgs
python3-numpy
python3-opencv
diff --git a/bitbots_navigation/bitbots_path_planning/test/__snapshots__/test_controller.ambr b/bitbots_navigation/bitbots_path_planning/test/__snapshots__/test_controller.ambr
index 20d6a1728..61ff899d2 100644
--- a/bitbots_navigation/bitbots_path_planning/test/__snapshots__/test_controller.ambr
+++ b/bitbots_navigation/bitbots_path_planning/test/__snapshots__/test_controller.ambr
@@ -1,18 +1,18 @@
# serializer version: 1
# name: test_default_setup
dict({
- 'carrot_distance': 4,
- 'max_rotation_vel': 0.3,
+ 'carrot_distance': 1,
+ 'max_rotation_vel': 0.6,
'max_vel_x': 0.12,
'max_vel_y': 0.07,
'min_vel_x': -0.06,
- 'orient_to_goal_distance': 1.0,
+ 'orient_to_goal_distance': 0.4,
'rotation_i_factor': 0.0,
- 'rotation_slow_down_factor': 0.3,
- 'smoothing_tau': 1.0,
+ 'rotation_slow_down_factor': 0.6,
+ 'smoothing_tau': 0.1,
'translation_slow_down_factor': 1.0,
})
# ---
# name: test_step_cmd_vel_smoothing
- 'geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.06466145812714323, y=0.04175428591906269, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.19312811636676133))'
+ 'geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.06466145812714323, y=0.04175428591906269, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.38625623273352266))'
# ---
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 e29757d35..705ac3d23 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
@@ -44,7 +44,7 @@ bitbots_ball_filter:
covariance:
process_noise:
type: double
- default_value: 0.005
+ default_value: 0.002
description: 'Noise which is added to the estimated position without new measurements'
validation:
bounds<>: [0.0, 1.0]
diff --git a/requirements/common.txt b/requirements/common.txt
index 046b3efa4..4ea870841 100644
--- a/requirements/common.txt
+++ b/requirements/common.txt
@@ -1,8 +1,9 @@
# This file includes common dependencies for all environments
pip
transforms3d==0.4.1
-git+https://github.com/Flova/pyastar2d
git+https://github.com/bit-bots/YOEO
+git+https://github.com/bit-bots/bitbots_rust_nav
+maturin
simpleeval
beartype
jaxtyping