diff --git a/.gitignore b/.gitignore index 86622ba02..df3f74bd9 100644 --- a/.gitignore +++ b/.gitignore @@ -226,3 +226,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 0fc7265f9..2c91eed21 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -223,10 +223,12 @@ "ros.distro": "jazzy", "search.useIgnoreFiles": false, "python.autoComplete.extraPaths": [ + "/opt/ros/jazzy/lib/python3.10/site-packages" ], "python.analysis.extraPaths": [ "/opt/ros/jazzy/lib/python3.10/site-packages" + ], "cmake.configureOnOpen": false, "editor.formatOnSave": true, diff --git a/Makefile b/Makefile index 73902c40f..8c97526a7 100644 --- a/Makefile +++ b/Makefile @@ -17,8 +17,10 @@ install-no-root: pull-init update-no-root pip: # Install and upgrade pip dependencies + pip install --upgrade -r requirements/dev.txt --user --break-system-packages + pre-commit: # Install pre-commit hooks for all submodules that have a .pre-commit-config.yaml file pre-commit install diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py index 793e30743..33f236f36 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py +++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py @@ -14,6 +14,8 @@ def __init__(self, node, blackboard): self.active = False # Config + self.grab_ball_animation: str = self._node.get_parameter("Animations.Throw_in.grabBall").value + self.throw_animation: str = self._node.get_parameter("Animations.Throw_in.throw").value self.goalie_arms_animation: str = self._node.get_parameter("Animations.Goalie.goalieArms").value self.goalie_falling_right_animation: str = self._node.get_parameter("Animations.Goalie.fallRight").value self.goalie_falling_left_animation: str = self._node.get_parameter("Animations.Goalie.fallLeft").value 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/get_walkready.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/dynup.py similarity index 77% rename from bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/get_walkready.py rename to bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/dynup.py index 0dfd33caf..a910093dd 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/get_walkready.py +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/dynup.py @@ -5,11 +5,23 @@ from bitbots_msgs.action import Dynup -class GetWalkready(AbstractActionElement): +class PlayAnimationDynup(AbstractActionElement): blackboard: BodyBlackboard def __init__(self, blackboard, dsd, parameters): super().__init__(blackboard, dsd, parameters) + self.direction = parameters.get("direction") + assert self.direction in [ + Dynup.Goal.DIRECTION_FRONT, + Dynup.Goal.DIRECTION_FRONT_ONLY, + Dynup.Goal.DIRECTION_BACK, + Dynup.Goal.DIRECTION_BACK_ONLY, + Dynup.Goal.DIRECTION_RISE, + Dynup.Goal.DIRECTION_DESCEND, + Dynup.Goal.DIRECTION_WALKREADY, + Dynup.Goal.DIRECTION_RISE_NO_ARMS, + Dynup.Goal.DIRECTION_DESCEND_NO_ARMS, + ], f"Direction '{self.direction}' not supported" self.first_perform = True self.active = False @@ -58,7 +70,7 @@ def start_animation(self) -> bool: # Dynup action server is running, we can start the walkready action goal = Dynup.Goal() - goal.direction = Dynup.Goal.DIRECTION_WALKREADY + goal.direction = self.direction self.active = True self.dynup_action_current_goal = self.blackboard.animation.dynup_action_client.send_goal_async(goal) self.dynup_action_current_goal.add_done_callback( @@ -78,3 +90,11 @@ def animation_finished(self) -> bool: :return: True if the animation is finished, False if not """ return not self.active + + +class GetWalkready(PlayAnimationDynup): + blackboard: BodyBlackboard + + def __init__(self, blackboard, dsd, parameters): + parameters["direction"] = Dynup.Goal.DIRECTION_WALKREADY + super().__init__(blackboard, dsd, parameters) 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..728a56811 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 @@ -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/play_animation.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/play_animation.py index 34c93138a..53840f312 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/play_animation.py +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/play_animation.py @@ -75,6 +75,18 @@ def get_animation_name(self): return self.blackboard.animation.cheering_animation +class PlayAnimationGrabBall(AbstractPlayAnimation): + def get_animation_name(self): + self.blackboard.node.get_logger().info("PLAYING GRAB-BALL ANIMATION") + return self.blackboard.animation.grab_ball_animation + + +class PlayAnimationThrow(AbstractPlayAnimation): + def get_animation_name(self): + self.blackboard.node.get_logger().info("PLAYING THROW-IN ANIMATION") + return self.blackboard.animation.throw_animation + + class PlayAnimationInit(AbstractPlayAnimation): def get_animation_name(self): return self.blackboard.animation.init_animation 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 f120e145d..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 @@ -117,6 +117,19 @@ $DoOnce SECOND --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToCornerKickPosition + mode:supporter THIRD --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition OTHER --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToCornerKickPosition + mode:others + THROW_IN --> $SecondaryStateTeamDecider + OUR --> $RankToBallNoGoalie + FIRST --> $DoOnce + NOT_DONE --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @AvoidBallActive, @GoToBall + target:gradient + distance:0.3 + blocking:false + DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.03 + latch:true + NO --> @GoToBall + target:gradient + distance:0.3 + YES --> @LookForward, @ChangeAction + action:kicking, @Stand + duration:1.5 + r:false, @PlayAnimationDynup + direction:descend_no_arms, @PlayAnimationGrabBall + r:false, @PlayAnimationDynup + direction:rise_no_arms, @PlayAnimationThrow + r:false, @GetWalkready + r:false, @Stand + SECOND --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToPassPreparePosition + THIRD --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition + OTHER --> $RankToBallNoGoalie + FIRST --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition + mode:freekick_first + SECOND --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition + mode:freekick_second + THIRD --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition ELSE --> $SecondaryStateTeamDecider OUR --> $RankToBallNoGoalie FIRST --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToBall + target:gradient + distance:0.5 diff --git a/bitbots_behavior/bitbots_body_behavior/config/animations.yaml b/bitbots_behavior/bitbots_body_behavior/config/animations.yaml index 62833efc0..fdfa1bf08 100644 --- a/bitbots_behavior/bitbots_body_behavior/config/animations.yaml +++ b/bitbots_behavior/bitbots_body_behavior/config/animations.yaml @@ -6,6 +6,9 @@ fallLeft: "goalie_falling_left" fallRight: "goalie_falling_right" fallCenter: "goalie_falling_center" + Throw_in: + grabBall: "grab_ball" + throw: "throw" Misc: cheering: "cheering" init: "init_sim" diff --git a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml index cd65c1175..d19474dbc 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.23 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. @@ -146,7 +146,7 @@ body_behavior: dribble_ball_distance_threshold: 0.5 dribble_kick_angle: 0.6 - kick_decision_smoothing: 1.0 + kick_decision_smoothing: 10.0 ################## # costmap params # diff --git a/bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch b/bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch index c219072fa..f76b243c3 100644 --- a/bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch +++ b/bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch @@ -4,6 +4,7 @@ + @@ -25,9 +26,11 @@ - + + + diff --git a/bitbots_lowlevel/bitbots_ros_control/scripts/pose_check.py b/bitbots_lowlevel/bitbots_ros_control/scripts/pose_check.py index 01b3f34bb..d9a037a22 100755 --- a/bitbots_lowlevel/bitbots_ros_control/scripts/pose_check.py +++ b/bitbots_lowlevel/bitbots_ros_control/scripts/pose_check.py @@ -126,37 +126,6 @@ def main(): f"{Style.BRIGHT}Press enter to continue.{Style.RESET_ALL}" ) - ################ - # Arms-Up-Pose # - ################ - - step += 1 - input( - f"\n{Fore.YELLOW}{num_to_emoji(step)}: The robot will now move {Style.BRIGHT}its arms up and its head to the right.{Style.RESET_ALL}\n" - f"{Style.BRIGHT}Press enter to move.{Style.RESET_ALL}" - ) - - move_to_joint_position( - pub, - { - "LElbow": -math.pi / 2, - "LShoulderRoll": -math.pi, - "RElbow": math.pi / 2, - "RShoulderRoll": math.pi, - "HeadPan": -math.pi / 2, - }, - ) - - input( - f"{Style.BRIGHT}\nChecks:{Style.RESET_ALL}\n" - "- Check that all limbs are straight.\n" - "- Check that both arms are symmetrical.\n" - "- Check that the head looks 90 degrees to the right.\n" - "- Check that the head is horizontal.\n" - "- Check for backlash and loose parts.\n" - f"{Style.BRIGHT}Press enter to continue.{Style.RESET_ALL}" - ) - ########################## # Arms-To-The-Front-Pose # ########################## 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_bringup/launch/monitoring_pc.launch b/bitbots_misc/bitbots_bringup/launch/monitoring_pc.launch index d30f64fa6..4af3b2f07 100644 --- a/bitbots_misc/bitbots_bringup/launch/monitoring_pc.launch +++ b/bitbots_misc/bitbots_bringup/launch/monitoring_pc.launch @@ -7,7 +7,7 @@ - + diff --git a/bitbots_misc/bitbots_bringup/launch/motion.launch b/bitbots_misc/bitbots_bringup/launch/motion.launch index a3329d7e4..d766d9375 100644 --- a/bitbots_misc/bitbots_bringup/launch/motion.launch +++ b/bitbots_misc/bitbots_bringup/launch/motion.launch @@ -4,16 +4,17 @@ + - - + + - + @@ -21,6 +22,7 @@ + diff --git a/bitbots_misc/bitbots_bringup/launch/teamplayer.launch b/bitbots_misc/bitbots_bringup/launch/teamplayer.launch index 184429c64..38627eea5 100644 --- a/bitbots_misc/bitbots_bringup/launch/teamplayer.launch +++ b/bitbots_misc/bitbots_bringup/launch/teamplayer.launch @@ -14,6 +14,8 @@ + + @@ -32,6 +34,7 @@ + diff --git a/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml b/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml index ba85a2b93..165d51dbf 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml +++ b/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml @@ -1,11 +1,11 @@ /bitbots_extrinsic_camera_calibration: ros__parameters: - offset_x: -0.055 + offset_x: -0.04 offset_y: 0.0 offset_z: 0.0 /bitbots_extrinsic_imu_calibration: ros__parameters: - offset_x: -0.02 - offset_y: -0.09 + offset_x: 0.28 + offset_y: 0.06 offset_z: 0.0 diff --git a/bitbots_misc/bitbots_extrinsic_calibration/config/donna.yaml b/bitbots_misc/bitbots_extrinsic_calibration/config/donna.yaml index 541e4a11c..a3df520f9 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/config/donna.yaml +++ b/bitbots_misc/bitbots_extrinsic_calibration/config/donna.yaml @@ -1,11 +1,11 @@ /bitbots_extrinsic_camera_calibration: ros__parameters: - offset_x: -0.05 - offset_y: 0.0 - offset_z: 0.0 + offset_x: -0.08 + offset_y: 0.08 + offset_z: 0.05 /bitbots_extrinsic_imu_calibration: ros__parameters: - offset_x: 0.075 + offset_x: 0.05 offset_y: 0.13 offset_z: 0.0 diff --git a/bitbots_misc/bitbots_extrinsic_calibration/config/jack.yaml b/bitbots_misc/bitbots_extrinsic_calibration/config/jack.yaml index 2fa51de3d..e8073c240 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/config/jack.yaml +++ b/bitbots_misc/bitbots_extrinsic_calibration/config/jack.yaml @@ -6,6 +6,6 @@ /bitbots_extrinsic_imu_calibration: ros__parameters: - offset_x: 0.06 - offset_y: -0.085 + offset_x: 0.04 + offset_y: 0.01 offset_z: 0.0 diff --git a/bitbots_misc/bitbots_extrinsic_calibration/config/rory.yaml b/bitbots_misc/bitbots_extrinsic_calibration/config/rory.yaml index bca74efee..5fdb160a2 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/config/rory.yaml +++ b/bitbots_misc/bitbots_extrinsic_calibration/config/rory.yaml @@ -6,6 +6,6 @@ /bitbots_extrinsic_imu_calibration: ros__parameters: - offset_x: 0.0 - offset_y: 0.017 + offset_x: -0.03 + offset_y: -0.06 offset_z: 0.0 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_motion/bitbots_dynup/config/dynup_config.yaml b/bitbots_motion/bitbots_dynup/config/dynup_config.yaml index bff4d114f..aeef4f8a2 100644 --- a/bitbots_motion/bitbots_dynup/config/dynup_config.yaml +++ b/bitbots_motion/bitbots_dynup/config/dynup_config.yaml @@ -239,7 +239,7 @@ bitbots_dynup: descend_time: type: double description: "Time to descend in seconds" - default_value: 0.25 + default_value: 1.5 validation: bounds<>: [0.0, 5.0] diff --git a/bitbots_motion/bitbots_dynup/config/dynup_sim.yaml b/bitbots_motion/bitbots_dynup/config/dynup_sim.yaml index f7e1ef994..aef9cb53e 100644 --- a/bitbots_motion/bitbots_dynup/config/dynup_sim.yaml +++ b/bitbots_motion/bitbots_dynup/config/dynup_sim.yaml @@ -29,7 +29,7 @@ dynup: wait_in_squat_front: 0.032 rise: - rise_time: 0.4 + rise_time: 2.0 stabilizer: end_pause: diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_utils.hpp b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_utils.hpp index 3bc1d00df..abbe41380 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_utils.hpp +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_utils.hpp @@ -17,7 +17,17 @@ struct DynupResponse { bool is_head_zero; }; -enum DynupDirection { FRONT = 1, BACK = 0, FRONT_ONLY = 4, BACK_ONLY = 5, RISE = 2, DESCEND = 3, WALKREADY = 6 }; +enum DynupDirection { + FRONT = 1, + BACK = 0, + FRONT_ONLY = 4, + BACK_ONLY = 5, + RISE = 2, + DESCEND = 3, + WALKREADY = 6, + RISE_NO_ARMS = 8, + DESCEND_NO_ARMS = 7 +}; DynupDirection getDynupDirection(const std::string& direction); diff --git a/bitbots_motion/bitbots_dynup/scripts/dummy_client.py b/bitbots_motion/bitbots_dynup/scripts/dummy_client.py index 51cf903e0..12bad18f2 100755 --- a/bitbots_motion/bitbots_dynup/scripts/dummy_client.py +++ b/bitbots_motion/bitbots_dynup/scripts/dummy_client.py @@ -20,6 +20,8 @@ Dynup.Goal.DIRECTION_RISE, Dynup.Goal.DIRECTION_DESCEND, Dynup.Goal.DIRECTION_WALKREADY, + Dynup.Goal.DIRECTION_RISE_NO_ARMS, + Dynup.Goal.DIRECTION_DESCEND_NO_ARMS, ] if len(sys.argv) != 2 or sys.argv[1] not in directions: print("Use " + str(directions) + " as parameters!") diff --git a/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp b/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp index f4805c1e4..157c32631 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp @@ -664,7 +664,7 @@ void DynupEngine::setGoals(const DynupRequest &goals) { // Get params and wait for walking to be ready walking_params = walking_param_client_->get_parameters( {"engine.trunk_pitch", "engine.trunk_height", "engine.foot_distance", "engine.trunk_x_offset"}, - std::chrono::seconds(1)); + std::chrono::seconds(5)); // when the walking was killed, service_is_ready is still true but the parameters come back empty if (walking_params.size() != 4) { @@ -714,9 +714,11 @@ void DynupEngine::setGoals(const DynupRequest &goals) { duration_ = calcBackSplines(); break; case DynupDirection::RISE: + case DynupDirection::RISE_NO_ARMS: duration_ = calcWalkreadySplines(0, params_.rise.rise_time); break; case DynupDirection::DESCEND: + case DynupDirection::DESCEND_NO_ARMS: duration_ = calcDescendSplines(); break; case DynupDirection::WALKREADY: diff --git a/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp b/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp index 1e625bd4b..0511f7f8e 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp @@ -69,15 +69,15 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { success &= goal_state_->setFromIK(r_leg_joints_group_, right_foot_goal_msg, 0.005, moveit::core::GroupStateValidityCallbackFn(), leg_ik_options); - goal_state_->updateLinkTransforms(); - - success &= goal_state_->setFromIK(l_arm_joints_group_, left_hand_goal_msg, 0.005, - moveit::core::GroupStateValidityCallbackFn(), ik_options); - - goal_state_->updateLinkTransforms(); - - success &= goal_state_->setFromIK(r_arm_joints_group_, right_hand_goal_msg, 0.005, - moveit::core::GroupStateValidityCallbackFn(), ik_options); + if (direction_ != DynupDirection::RISE_NO_ARMS and direction_ != DynupDirection::DESCEND_NO_ARMS) { + goal_state_->updateLinkTransforms(); + success &= goal_state_->setFromIK(l_arm_joints_group_, left_hand_goal_msg, 0.005, + moveit::core::GroupStateValidityCallbackFn(), ik_options); + + goal_state_->updateLinkTransforms(); + success &= goal_state_->setFromIK(r_arm_joints_group_, right_hand_goal_msg, 0.005, + moveit::core::GroupStateValidityCallbackFn(), ik_options); + } if (success) { /* retrieve joint names and associated positions from */ auto joint_names = all_joints_group_->getActiveJointModelNames(); @@ -86,6 +86,11 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { /* construct result object */ bitbots_splines::JointGoals result = {joint_names, joint_goals}; + + // Store the name of the arm joins so we can remove them if they are not needed + const auto r_arm_motors = r_arm_joints_group_->getActiveJointModelNames(); + const auto l_arm_motors = l_arm_joints_group_->getActiveJointModelNames(); + /* sets head motors to correct positions, as the IK will return random values for those unconstrained motors. */ for (size_t i = result.first.size(); i-- > 0;) { if (result.first[i] == "HeadPan") { @@ -111,6 +116,13 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { result.second[i] = 0; } } + // Remove the arm motors from the goals if we have a goal without arms + else if ((std::find(r_arm_motors.begin(), r_arm_motors.end(), result.first[i]) != r_arm_motors.end() or + std::find(l_arm_motors.begin(), l_arm_motors.end(), result.first[i]) != l_arm_motors.end()) and + (direction_ == DynupDirection::RISE_NO_ARMS or direction_ == DynupDirection::DESCEND_NO_ARMS)) { + result.first.erase(result.first.begin() + i); + result.second.erase(result.second.begin() + i); + } } return result; } else { diff --git a/bitbots_motion/bitbots_dynup/src/dynup_utils.cpp b/bitbots_motion/bitbots_dynup/src/dynup_utils.cpp index 82a85050f..b7a54e703 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_utils.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_utils.cpp @@ -3,10 +3,15 @@ namespace bitbots_dynup { DynupDirection getDynupDirection(const std::string& direction) { - std::map mapping = { - {"front", FRONT}, {"back", BACK}, {"front_only", FRONT_ONLY}, {"back_only", BACK_ONLY}, - {"rise", RISE}, {"descend", DESCEND}, {"walkready", WALKREADY}, - }; + std::map mapping = {{"front", FRONT}, + {"back", BACK}, + {"front_only", FRONT_ONLY}, + {"back_only", BACK_ONLY}, + {"rise", RISE}, + {"descend", DESCEND}, + {"walkready", WALKREADY}, + {"rise_no_arms", RISE_NO_ARMS}, + {"descend_no_arms", DESCEND_NO_ARMS}}; try { return mapping.at(direction); } catch (const std::out_of_range& e) { diff --git a/bitbots_motion/bitbots_quintic_walk/config/robots/donna.yaml b/bitbots_motion/bitbots_quintic_walk/config/robots/donna.yaml index 2941783d7..139a4431c 100644 --- a/bitbots_motion/bitbots_quintic_walk/config/robots/donna.yaml +++ b/bitbots_motion/bitbots_quintic_walk/config/robots/donna.yaml @@ -3,4 +3,4 @@ walking: ros__parameters: engine: - trunk_x_offset: -0.02 + trunk_x_offset: 0.01 diff --git a/bitbots_motion/bitbots_quintic_walk/config/robots/jack.yaml b/bitbots_motion/bitbots_quintic_walk/config/robots/jack.yaml index 8df89c7ac..cda67ccde 100644 --- a/bitbots_motion/bitbots_quintic_walk/config/robots/jack.yaml +++ b/bitbots_motion/bitbots_quintic_walk/config/robots/jack.yaml @@ -3,13 +3,4 @@ walking: ros__parameters: engine: - trunk_x_offset: 0.01 - trunk_y_offset: 0.0 - node: - x_bias: -0.005 - y_bias: -0.008 - trunk_pid: - pitch: - p: 0.0 - i: 0.0 - d: 0.0 + trunk_x_offset: 0.0 diff --git a/bitbots_msgs/action/Dynup.action b/bitbots_msgs/action/Dynup.action index 939261314..1d36345b9 100644 --- a/bitbots_msgs/action/Dynup.action +++ b/bitbots_msgs/action/Dynup.action @@ -8,6 +8,8 @@ string DIRECTION_BACK_ONLY = "back_only" string DIRECTION_RISE = "rise" string DIRECTION_DESCEND = "descend" string DIRECTION_WALKREADY = "walkready" +string DIRECTION_RISE_NO_ARMS = "rise_no_arms" +string DIRECTION_DESCEND_NO_ARMS = "descend_no_arms" # Requests from the HCM have higher prio and are published to a different topic bool from_hcm 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..777c2c682 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.4 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,19 +92,19 @@ 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] translation_slow_down_factor: type: double - default_value: 1.0 + default_value: 0.6 description: 'Clamped p gain of the translation controller' validation: bounds<>: [0.0, 1.0] orient_to_goal_distance: type: double - default_value: 1.0 + default_value: 0.5 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..27a855381 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.4, '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.5, 'rotation_i_factor': 0.0, - 'rotation_slow_down_factor': 0.3, - 'smoothing_tau': 1.0, - 'translation_slow_down_factor': 1.0, + 'rotation_slow_down_factor': 0.6, + 'smoothing_tau': 0.1, + 'translation_slow_down_factor': 0.6, }) # --- # 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.3591891760862001))' # --- diff --git a/bitbots_robot/wolfgang_animations/animations/throw_in/grab_ball.json b/bitbots_robot/wolfgang_animations/animations/throw_in/grab_ball.json new file mode 100644 index 000000000..af1a995d2 --- /dev/null +++ b/bitbots_robot/wolfgang_animations/animations/throw_in/grab_ball.json @@ -0,0 +1,113 @@ +{ + "author": "Maxim, David, Okan", + "description": "Play grab ball animation", + "keyframes": [ + { + "duration": 3.0, + "goals": { + "LAnklePitch": -55.99, + "LAnkleRoll": 2.99, + "LElbow": -86.22, + "LHipPitch": 71.75, + "LHipRoll": 6.86, + "LHipYaw": -2.72, + "LKnee": 139.37, + "LShoulderPitch": -173.58, + "LShoulderRoll": -56.43, + "RAnklePitch": 55.99, + "RAnkleRoll": -2.99, + "RElbow": 86.22, + "RHipPitch": -71.75, + "RHipRoll": -6.86, + "RHipYaw": 2.72, + "RKnee": -139.37, + "RShoulderPitch": 173.58, + "RShoulderRoll": 56.43 + }, + "name": "praise_the_sun", + "pause": 0.0, + "torque": {} + }, + { + "duration": 3.0, + "goals": { + "LElbow": -86.22, + "LShoulderPitch": -58.89, + "LShoulderRoll": -56.43, + "RElbow": 86.22, + "RShoulderPitch": 58.89, + "RShoulderRoll": 56.43, + "LAnklePitch": -57.48, + "LAnkleRoll": 11.96, + "LHipPitch": 121.61, + "LHipRoll": 14.250000000000002, + "LHipYaw": -3.08, + "LKnee": 160.21, + "RAnklePitch": 57.48, + "RAnkleRoll": -11.96, + "RHipPitch": -121.61, + "RHipRoll": -14.250000000000002, + "RHipYaw": 3.08, + "RKnee": -160.21 + }, + "name": "osu", + "pause": 0.0, + "torque": {} + }, + { + "duration": 2.2, + "goals": { + "LAnklePitch": -57.48, + "LAnkleRoll": 11.96, + "LElbow": -86.22, + "LHipPitch": 121.61, + "LHipRoll": 14.250000000000002, + "LHipYaw": -3.08, + "LKnee": 160.21, + "LShoulderPitch": -64.89, + "LShoulderRoll": 19.35, + "RAnklePitch": 57.48, + "RAnkleRoll": -11.96, + "RElbow": 86.22, + "RHipPitch": -121.61, + "RHipRoll": -14.250000000000002, + "RHipYaw": 3.08, + "RKnee": -160.21, + "RShoulderPitch": 64.89, + "RShoulderRoll": -19.35 + }, + "name": "scoop", + "pause": 0.0, + "torque": {} + }, + { + "duration": 1.8, + "goals": { + "LAnklePitch": -62.4, + "LAnkleRoll": 10.11, + "LElbow": -86.22, + "LHipPitch": 71.05, + "LHipRoll": 13.37, + "LHipYaw": 1.23, + "LKnee": 142.19, + "LShoulderPitch": -164.79, + "LShoulderRoll": 19.35, + "RAnklePitch": 62.4, + "RAnkleRoll": -10.11, + "RElbow": 86.22, + "RHipPitch": -71.05, + "RHipRoll": -13.37, + "RHipYaw": -1.23, + "RKnee": -142.19, + "RShoulderPitch": 164.79, + "RShoulderRoll": -19.35 + }, + "name": "pick_up_ball", + "pause": 0.0, + "torque": {} + } + ], + "last_edited": "2024-12-18 16:01:10.522911", + "name": "grab_ball", + "version": "" +} diff --git a/bitbots_robot/wolfgang_animations/animations/throw_in/throw.json b/bitbots_robot/wolfgang_animations/animations/throw_in/throw.json new file mode 100644 index 000000000..c471e24e4 --- /dev/null +++ b/bitbots_robot/wolfgang_animations/animations/throw_in/throw.json @@ -0,0 +1,73 @@ +{ + "author": "", + "description": "", + "keyframes": [ + { + "duration": 1.0, + "goals": { + "HeadPan": 0.0, + "HeadTilt": 0.0, + "LElbow": -73.74, + "LShoulderPitch": -179.38, + "LShoulderRoll": 17.15, + "RElbow": 73.74, + "RShoulderPitch": 179.38, + "RShoulderRoll": -17.15 + }, + "name": "unus", + "pause": 0.0, + "torque": {} + }, + { + "duration": 1.0, + "goals": { + "HeadPan": 0.0, + "HeadTilt": 0.0, + "LElbow": -62.49000000000001, + "LShoulderPitch": -179.38, + "LShoulderRoll": 17.41, + "RElbow": 62.49000000000001, + "RShoulderPitch": 179.38, + "RShoulderRoll": -17.41 + }, + "name": "duo", + "pause": 0.0, + "torque": {} + }, + { + "duration": 0.3, + "goals": { + "HeadPan": 0.0, + "HeadTilt": 0.0, + "LElbow": -94.66, + "LShoulderPitch": -147.13, + "LShoulderRoll": 11.17, + "RElbow": 94.66, + "RShoulderPitch": 147.13, + "RShoulderRoll": -11.17 + }, + "name": "tres", + "pause": 0.5, + "torque": {} + }, + { + "duration": 1.0, + "goals": { + "HeadPan": 0.0, + "HeadTilt": 0.0, + "LElbow": 8.35, + "LShoulderPitch": 37.81, + "LShoulderRoll": -1.93, + "RElbow": -8.35, + "RShoulderPitch": -37.81, + "RShoulderRoll": 1.93 + }, + "name": "quattuor", + "pause": 0.0, + "torque": {} + } + ], + "last_edited": "2024-12-18 16:10:48.875357", + "name": "throw", + "version": "" +} diff --git a/bitbots_robot/wolfgang_description/urdf/robot.urdf b/bitbots_robot/wolfgang_description/urdf/robot.urdf index 93eece425..b964ae9a6 100644 --- a/bitbots_robot/wolfgang_description/urdf/robot.urdf +++ b/bitbots_robot/wolfgang_description/urdf/robot.urdf @@ -732,7 +732,7 @@ - + @@ -1087,7 +1087,7 @@ - + diff --git a/bitbots_simulation/bitbots_webots_sim/protos/robots/Wolfgang/Wolfgang.proto b/bitbots_simulation/bitbots_webots_sim/protos/robots/Wolfgang/Wolfgang.proto index e2d546987..6d41cc9b0 100644 --- a/bitbots_simulation/bitbots_webots_sim/protos/robots/Wolfgang/Wolfgang.proto +++ b/bitbots_simulation/bitbots_webots_sim/protos/robots/Wolfgang/Wolfgang.proto @@ -71,7 +71,7 @@ PROTO Wolfgang [ field SFBool enableBoundingObject TRUE # Is `Robot.enableBoundingObject`. field SFBool enablePhysics TRUE # Is `Robot.enablePhysics`. field SFBool enableFootSensors TRUE # Is `Robot.enableFootSensors`. - field SFFloat cameraFOV 1.04 + field SFFloat cameraFOV 1.517 field SFInt32 cameraWidth 800 field SFInt32 cameraHeight 600 field SFFloat MX64-torque 7.3 diff --git a/bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml b/bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml index ba995f402..8521e1d75 100644 --- a/bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml +++ b/bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml @@ -2,7 +2,7 @@ team_comm: ros__parameters: # UDP broadcast address is the highest IP in the subnet e.g. 172.20.255.255 # Sets local mode if set to loopback (127.0.0.1) - target_ip: 192.168.255.255 + target_ip: 10.142.255.255 # Only used in non local mode with specific target_ip target_port: 3737 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 cca5f64b2..e1996f071 100644 --- a/requirements/common.txt +++ b/requirements/common.txt @@ -1,11 +1,13 @@ # 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/timonegk/colcon-core.git@colors git+https://github.com/timonegk/colcon-notification.git@colors git+https://github.com/timonegk/colcon-output.git@colors +git+https://github.com/bit-bots/bitbots_rust_nav + simpleeval beartype jaxtyping diff --git a/scripts/deploy/tasks/__init__.py b/scripts/deploy/tasks/__init__.py index ab39c822f..f67a23545 100644 --- a/scripts/deploy/tasks/__init__.py +++ b/scripts/deploy/tasks/__init__.py @@ -1,12 +1,16 @@ -from deploy.tasks.abstract_task import AbstractTask, AbstractTaskWhichRequiresSudo -from deploy.tasks.build import Build -from deploy.tasks.check_repos import CheckReposTask -from deploy.tasks.configure import Configure -from deploy.tasks.install import Install -from deploy.tasks.launch import Launch -from deploy.tasks.sync import Sync +INTERNET_TIMEOUT: float = 2.0 + + +from deploy.tasks.abstract_task import AbstractTask, AbstractTaskWhichRequiresSudo # noqa: E402 +from deploy.tasks.build import Build # noqa: E402 +from deploy.tasks.check_repos import CheckReposTask # noqa: E402 +from deploy.tasks.configure import Configure # noqa: E402 +from deploy.tasks.install import Install # noqa: E402 +from deploy.tasks.launch import Launch # noqa: E402 +from deploy.tasks.sync import Sync # noqa: E402 __all__ = [ + "INTERNET_TIMEOUT", "AbstractTask", "AbstractTaskWhichRequiresSudo", "Build", diff --git a/scripts/deploy/tasks/check_repos.py b/scripts/deploy/tasks/check_repos.py index 8c2bbc694..bf25322a1 100644 --- a/scripts/deploy/tasks/check_repos.py +++ b/scripts/deploy/tasks/check_repos.py @@ -4,6 +4,7 @@ from hashlib import md5 from deploy.misc import be_quiet, hide_output, print_debug, print_info, print_success, print_warning +from deploy.tasks import INTERNET_TIMEOUT from deploy.tasks.abstract_task import AbstractTask from fabric import Connection, Group, GroupResult, Result from git import Repo @@ -203,7 +204,7 @@ def _github_available(self, connection: Connection) -> bool: github: str = "https://github.com" print_debug(f"Checking for internet connection to {github}") - cmd = f"timeout --foreground 0.5 curl -sSLI {github}" + cmd = f"timeout --foreground {INTERNET_TIMEOUT:.2f} curl -sSLI {github}" print_debug(f"Calling {cmd}") available = False result: Result | None = None @@ -424,6 +425,7 @@ def _get_friendly_name(self, hash: str) -> str: friendly_name: str = f"{friendly_adjectives[int(hash[: len(hash) // 2], 16) % len(friendly_adjectives)]} {friendly_animals[int(hash[len(hash) // 2 :], 16) % len(friendly_animals)]}" print_debug(f"Generated friendly commit name: '{friendly_name}'.") + return friendly_name def _write_commits(self) -> None: diff --git a/scripts/deploy/tasks/install.py b/scripts/deploy/tasks/install.py index 3f448cfd8..f636901a8 100644 --- a/scripts/deploy/tasks/install.py +++ b/scripts/deploy/tasks/install.py @@ -3,6 +3,7 @@ from typing import Optional from deploy.misc import Connection, get_connections_from_succeeded, hide_output, print_debug, print_error, print_warning +from deploy.tasks import INTERNET_TIMEOUT from deploy.tasks.abstract_task import AbstractTaskWhichRequiresSudo from fabric import Group, GroupResult, Result from fabric.exceptions import GroupException @@ -56,7 +57,7 @@ def _internet_available_on_target(self, connections: Group) -> GroupResult: apt_mirror = "de.archive.ubuntu.com" print_debug("Checking internet connections") - cmd = f"timeout --foreground 0.5 curl -sSLI {apt_mirror}" + cmd = f"timeout --foreground {INTERNET_TIMEOUT} curl -sSLI {apt_mirror}" print_debug(f"Calling {cmd}") try: results = connections.run(cmd, hide=hide_output()) diff --git a/scripts/deploy/tasks/launch.py b/scripts/deploy/tasks/launch.py index 904399682..bc270253e 100644 --- a/scripts/deploy/tasks/launch.py +++ b/scripts/deploy/tasks/launch.py @@ -150,7 +150,7 @@ def _check_tmux_session_already_running(self, connections: Group) -> GroupResult def _launch_teamplayer(self, connections: Group) -> GroupResult: print_debug("Launching teamplayer") # Create tmux session - cmd = f"tmux new-session -d -s {self._tmux_session_name} && tmux send-keys -t {self._tmux_session_name} 'ros2 launch bitbots_bringup teamplayer.launch record:=true' Enter" + cmd = f"tmux new-session -d -s {self._tmux_session_name} && tmux send-keys -t {self._tmux_session_name} 'ros2 launch bitbots_bringup teamplayer.launch record:=true tts:=false' Enter" print_debug(f"Calling {cmd}") try: