diff --git a/AGENTS.md b/AGENTS.md index 1f500c573a..7acf1c9cd5 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -43,11 +43,49 @@ package's manifests and nearby code before choosing tools or patterns. as they might change later, thus making the documentation outdated. Instead, describe the expected behavior or refer to the relevant code sections. +## Failure Handling + +Handle unexpected states explicitly and loudly. A robot that behaves subtly +wrong is far harder to debug than one that says what is missing and does +nothing. Never invent a value to keep going. + +- Never substitute a made up default for missing or malformed input. A + fabricated value is indistinguishable from a real one, both to the rest of the + system and to whoever debugs it later. Zero, identity, and empty are the most + dangerous of these, because they are frequently valid readings. +- Prefer making the failure unrepresentable. Return `std::optional`, a status, + or a validity flag rather than a plausible looking value, so that callers have + to acknowledge the failure instead of silently inheriting it. +- Log at the point where enough context exists to say what is actually wrong, + and name the missing input. Prefer throttled logging in periodic code so a + persistent fault does not flood the log. Use a warning when the node can + continue degraded, an error when it cannot do its job at all. +- Drop the affected update rather than the whole process. Skipping one cycle and + reporting why is almost always better than crashing, and always better than + publishing a command derived from data that was not there. +- Fail early. Validate inputs, parameters and transforms where they enter the + system, not at the point where the bad value finally produces a visible + symptom. +- Keep retrying inputs that legitimately arrive late, such as transforms, + parameters from other nodes, and latched topics. Do not disable a feature for + the rest of the run because of a startup race, and keep reporting the wait. +- Distinguish "not available yet" from "broken". The first is expected during + startup and should be reported as a wait, the second should be reported as an + error. +- Do not let a fallback path quietly replace the intended one. If a degraded + mode exists, make entering it visible in the logs and say which mode is + running. +- Treat contract violations, such as an out of range index or a precondition a + caller must uphold, as programming errors and assert on them, rather than + clamping the input into a range that hides the bug. + ## Development Environment This ROS 2 workspace is managed by Pixi. Run development commands through the -repository's Pixi environments; do not invoke `colcon`, ROS 2 tools, or formatters -directly from the host shell. +repository's Pixi tasks; do not invoke `colcon`, ROS 2 tools, or formatters +directly from the host shell, and do not build, clean, or format by hand when a +task exists for it. The tasks carry flags the workspace depends on, and hand +written equivalents silently drop them. - Use the `default` environment for normal development. It contains the `ros` and `format` features. @@ -76,6 +114,12 @@ Common commands: - Run one-off tools with `pixi run -e default `. - Clean all workspace build artifacts with `pixi run -e default clean`. - Clean one package with `pixi run -e default clean `. + Prefer this over cleaning everything, because a full clean forces a rebuild of + the entire workspace. +- Never remove `build/`, `install/`, or `log/` by hand, for example with `rm -rf`. + Removing the install space breaks every package that is not rebuilt with it, + including ones outside the selection being worked on, and recovering requires a + full workspace rebuild. Use the clean task, which scopes the removal correctly. - Use `pixi clean` only to reset Pixi's local environment data. This requires downloading dependencies and rebuilding afterward. diff --git a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py b/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py index c6916a19bf..ac332b8186 100644 --- a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py +++ b/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py @@ -14,6 +14,7 @@ HeadMode.DONT_MOVE, HeadMode.SEARCH_BALL_PENALTY, HeadMode.SEARCH_FRONT, + HeadMode.ACTIVE_VISION, ] diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/head_modes.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/head_modes.py index 8d6fb497e5..d451cfd761 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/head_modes.py +++ b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/head_modes.py @@ -107,3 +107,11 @@ class LookAtFront(AbstractHeadModeElement): def perform(self): self.blackboard.misc.set_head_duty(HeadMode.SEARCH_FRONT) return self.pop() + + +class ActiveVisionHeadMove(AbstractHeadModeElement): + """Uses the active vision (has nothing to do with the vision node itself) to look for objects in the environment""" + + def perform(self): + self.blackboard.misc.set_head_duty(HeadMode.ACTIVE_VISION) + return self.pop() diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd index 1197a083c4..dfb0613cb4 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd +++ b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd @@ -1,8 +1,8 @@ #SearchBall $DoOnce - NOT_DONE --> @ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:1, @TurnLastSeenBallSide + duration:6, @GoToAbsolutePositionFieldFraction + x:0.5 + blocking:false + NOT_DONE --> @ChangeAction + action:searching, @ActiveVisionHeadMove, @WalkInPlace + duration:1, @TurnLastSeenBallSide + duration:6, @GoToAbsolutePositionFieldFraction + x:0.5 + blocking:false DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.5 + latch:true - NO --> @LookAtFieldFeatures, @GoToAbsolutePositionFieldFraction + x:0.5 + NO --> @ActiveVisionHeadMove, @GoToAbsolutePositionFieldFraction + x:0.5 YES --> $DoOnce NOT_DONE --> @Turn + duration:15 DONE --> $DoOnce @@ -25,17 +25,17 @@ $DoOnce #RolePositionWithPause $DoOnce - NOT_DONE --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToRolePosition + blocking:false + NOT_DONE --> @ActiveVisionHeadMove, @ChangeAction + action:positioning, @GoToRolePosition + blocking:false DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.2 + latch:true YES --> #StandAndLook - NO --> @LookAtFieldFeatures, @GoToRolePosition + NO --> @ActiveVisionHeadMove, @GoToRolePosition #KickWithAvoidance $KickOrDribble + threshold_upfield:2.0 + threshold_downfield:0.5 KICK --> $DoOnce - NOT_DONE --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:rl_kick + blocking:false + NOT_DONE --> @ChangeAction + action:going_to_ball, @ActiveVisionHeadMove, @GoToBall + target:rl_kick + blocking:false DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:%rl_kick_approach_tolerance_pos + orientation_threshold:%rl_kick_approach_tolerance_deg - NO --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:rl_kick + NO --> @ChangeAction + action:going_to_ball, @ActiveVisionHeadMove, @GoToBall + target:rl_kick YES --> $DoOnce NOT_DONE --> @StoreBallMovementDetectionStartPosition DONE --> @ChangeAction + action:kicking, @Stand + duration:0.1 + r:false, @LookAtBallPenalty + r:false, @RLKickTowardsGoal + strength:3.0 + r:false, @ForgetBall + r:false @@ -45,46 +45,46 @@ $KickOrDribble + threshold_upfield:2.0 + threshold_downfield:0.5 NEAR --> $DoOnce NOT_DONE --> @StoreBallMovementDetectionStartPosition DONE --> #Dribble - FAR --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:map - NO --> @ChangeAction + action:going_to_ball + r:false, @LookAtFieldFeatures + r:false, @AvoidBallActive + r:false, @GoToBall + target:map + blocking:false + distance:%ball_far_approach_dist + FAR --> @ChangeAction + action:going_to_ball, @ActiveVisionHeadMove, @GoToBall + target:map + NO --> @ChangeAction + action:going_to_ball + r:false, @ActiveVisionHeadMove + r:false, @AvoidBallActive + r:false, @GoToBall + target:map + blocking:false + distance:%ball_far_approach_dist YES --> $ReachedPathPlanningGoalPosition + threshold:%ball_far_approach_position_thresh YES --> @AvoidBallInactive - NO --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:map + distance:%ball_far_approach_dist + NO --> @ChangeAction + action:going_to_ball, @ActiveVisionHeadMove, @GoToBall + target:map + distance:%ball_far_approach_dist #PositioningReady $GoalScoreRecently YES --> $ConfigRole GOALIE --> $RobotInOwnPercentOfField + p:40 YES --> @Stand + duration:1.0 + r:false, @PlayAnimationCheering + r:false, @Stand - NO --> @ChangeAction + action:positioning, @PlaySound + file:ole.wav, @LookAtFieldFeatures, @Stand + duration:%ready_wait_time, @AvoidBallActive, @GoToRolePosition - ELSE --> @ChangeAction + action:positioning, @PlaySound + file:ole.wav, @LookAtFieldFeatures, @Stand + duration:%ready_wait_time, @AvoidBallActive, @GoToRolePosition - NO --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @Stand + duration:%ready_wait_time, @AvoidBallActive, @GoToRolePosition + NO --> @ChangeAction + action:positioning, @PlaySound + file:ole.wav, @ActiveVisionHeadMove, @Stand + duration:%ready_wait_time, @AvoidBallActive, @GoToRolePosition + ELSE --> @ChangeAction + action:positioning, @PlaySound + file:ole.wav, @ActiveVisionHeadMove, @Stand + duration:%ready_wait_time, @AvoidBallActive, @GoToRolePosition + NO --> @ChangeAction + action:positioning, @ActiveVisionHeadMove, @Stand + duration:%ready_wait_time, @AvoidBallActive, @GoToRolePosition #SupporterRole $BallSeen YES --> $PassStarted YES --> @TrackBall, @ChangeAction + action:positioning, @AvoidBallActive, @GoToFormationPosition - NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @AvoidBallActive, @GoToFormationPosition + stand:true + enter_position:%support_enter_position + leave_position:%support_leave_position + enter_orientation:%support_enter_orientation + leave_orientation:%support_leave_orientation - NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @AvoidBallActive, @GoToFormationPosition + stand:true + enter_position:%support_enter_position + leave_position:%support_leave_position + enter_orientation:%support_enter_orientation + leave_orientation:%support_leave_orientation + NO --> @ActiveVisionHeadMove, @ChangeAction + action:positioning, @AvoidBallActive, @GoToFormationPosition + stand:true + enter_position:%support_enter_position + leave_position:%support_leave_position + enter_orientation:%support_enter_orientation + leave_orientation:%support_leave_orientation + NO --> @ActiveVisionHeadMove, @ChangeAction + action:positioning, @AvoidBallActive, @GoToFormationPosition + stand:true + enter_position:%support_enter_position + leave_position:%support_leave_position + enter_orientation:%support_enter_orientation + leave_orientation:%support_leave_orientation #PenaltyShootoutBehavior $SecondaryStateTeamDecider - OUR --> @StandAndWaitRandom + min:10 + max:25, @ChangeAction + action:kicking, @LookAtFieldFeatures, @WalkInPlace + duration:2, @RLKickAngleRobot + angle_deg_in_map:30.0 + strength:3.0 + r:false, @WalkInPlace + duration:1 + r:false, @Stand + OUR --> @StandAndWaitRandom + min:10 + max:25, @ChangeAction + action:kicking, @ActiveVisionHeadMove, @WalkInPlace + duration:2, @RLKickAngleRobot + angle_deg_in_map:30.0 + strength:3.0 + r:false, @WalkInPlace + duration:1 + r:false, @Stand ELSE --> $BallDangerous + radius:1.3 LEFT --> @PlayAnimationGoalieFallLeft, @Stand RIGHT --> @PlayAnimationGoalieFallRight, @Stand CENTER --> @PlayAnimationGoalieFallCenter, @Stand ELSE --> $BallSeen YES --> @TrackBall, @Stand - NO --> @LookAtFieldFeatures, @Stand + NO --> @ActiveVisionHeadMove, @Stand #Init @Stand + duration:0.1 + r:false, @ChangeAction + action:waiting, @LookForward, @Stand #NormalBehavior $SecondBallTouchAllowed - NO --> @SetNoSecondBallContactVariable + value:false + r:false, @LookAtFieldFeatures + r:false, @ChangeAction + action:passive + r:false, @AvoidBallActive + r:false, @GoToFormationPosition //defender variables + NO --> @SetNoSecondBallContactVariable + value:false + r:false, @ActiveVisionHeadMove + r:false, @ChangeAction + action:passive + r:false, @AvoidBallActive + r:false, @GoToFormationPosition //defender variables YES --> $BallSeen NO --> $ConfigRole GOALIE --> $CountActiveRobotsWithoutGoalie @@ -95,14 +95,14 @@ $SecondBallTouchAllowed STRIKER --> #KickWithAvoidance SUPPORTER --> #SupporterRole ELSE --> $BallInOwnPercent + p:40 - YES --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToFormationPosition - NO --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToFormationPosition + stand:true + enter_position:%defender_enter_position + leave_position:%defender_leave_position + enter_orientation:%defender_enter_orientation + leave_orientation:%defender_leave_orientation + YES --> @ActiveVisionHeadMove, @ChangeAction + action:positioning, @GoToFormationPosition + NO --> @ActiveVisionHeadMove, @ChangeAction + action:positioning, @GoToFormationPosition + stand:true + enter_position:%defender_enter_position + leave_position:%defender_leave_position + enter_orientation:%defender_enter_orientation + leave_orientation:%defender_leave_orientation #ActivateDoubleTouchProtection @SetNoSecondBallContactVariable + value:true + r:false, @ForgetBallStartPosition + r:false #DefensiveSetPlay -@ChangeAction + action:positioning, @ForgetBall, @LookAtFieldFeatures, @GoToFormationPosition + set_play:true + stand:true + enter_position:%placing_enter_position + leave_position:%placing_leave_position + enter_orientation:%placing_enter_orientation + leave_orientation:%placing_leave_orientation +@ChangeAction + action:positioning, @ForgetBall, @ActiveVisionHeadMove, @GoToFormationPosition + set_play:true + stand:true + enter_position:%placing_enter_position + leave_position:%placing_leave_position + enter_orientation:%placing_enter_orientation + leave_orientation:%placing_leave_orientation #SetPlaySituation $DoOnce @@ -135,7 +135,7 @@ $IsPenalized YES --> #StandAndLook NO --> #PositioningReady NO --> $DoOnce - NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2 + NOT_DONE --> @ChangeAction + action:waiting + r:false, @ActiveVisionHeadMove + r:false, @Stand + duration:2 DONE --> $ReachedAndAlignedToConfigRolePosition YES --> #StandAndLook NO --> #PositioningReady @@ -151,7 +151,7 @@ $IsPenalized OUR --> @Stand + duration:5.0 + r:false, @LookForward + r:false, @RLKickAngleRobot + angle_deg_in_map:30.0 + strength:3.0 + r:false, @Stand OTHER --> $BallSeen YES --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @TrackBall + r:false, @PlayAnimationGoalieArms + r:false, @Stand // goalie only needs to care about the ball - NO --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @LookAtFieldFeatures + r:false, @PlayAnimationGoalieArms + r:false, @Stand + NO --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @ActiveVisionHeadMove + r:false, @PlayAnimationGoalieArms + r:false, @Stand ELSE --> #StandAndLook FINISHED --> $CurrentScore AHEAD --> @Stand + duration:0.5 + r:false, @PlaySound + file:fanfare.wav, @PlayAnimationCheering + r:false, @LookForward, @Stand diff --git a/src/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py b/src/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py index 6987aa68ec..2a18e7fd3b 100755 --- a/src/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py +++ b/src/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py @@ -50,6 +50,7 @@ 3: Don't move the head 4: Ball Mode adapted for Penalty Kick 5: Do a pattern which only looks in front of the robot +6: Active vision (ball tracking, field coverage, ...) Simulation only: r: reset robot in simulation @@ -250,6 +251,10 @@ def loop(self): # Do a pattern which only looks in front of the robot self.head_mode_msg.head_mode = HeadMode.SEARCH_FRONT assert int(key) == HeadMode.SEARCH_FRONT + elif key == "6": + # Active vision (ball tracking, field coverage, ...) + self.head_mode_msg.head_mode = HeadMode.ACTIVE_VISION + assert int(key) == HeadMode.ACTIVE_VISION elif key == "F": # play walkready animation self.get_walkready() diff --git a/src/bitbots_motion/bitbots_head_mover/CMakeLists.txt b/src/bitbots_motion/bitbots_head_mover/CMakeLists.txt index bd31edf708..b2bcc0910a 100644 --- a/src/bitbots_motion/bitbots_head_mover/CMakeLists.txt +++ b/src/bitbots_motion/bitbots_head_mover/CMakeLists.txt @@ -11,13 +11,29 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + # find dependencies find_package(ament_cmake REQUIRED) find_package(backward_ros REQUIRED) find_package(bitbots_msgs REQUIRED) find_package(bitbots_splines REQUIRED) +find_package(bitbots_utils REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(Eigen3 REQUIRED) find_package(generate_parameter_library REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(image_geometry REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(OpenCV REQUIRED) +find_package(orocos_kdl_vendor REQUIRED) find_package(rclcpp REQUIRED) +find_package(soccer_vision_3d_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(urdf REQUIRED) +find_package(visualization_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -27,23 +43,116 @@ generate_parameter_library( head_parameters # cmake target name for the parameter library config/head_config.yml) +# --------------------------------------------------------------------------- +# Head mover library – node independent pieces, so they can be unit tested +# --------------------------------------------------------------------------- +add_library( + bitbots_head_mover_lib SHARED + src/active_vision.cpp + src/active_vision_debug.cpp + src/active_vision_scorer.cpp + src/camera_model.cpp + src/field_coverage_map.cpp + src/head_kinematics.cpp + src/head_trajectory.cpp + src/look_at.cpp + src/search_pattern.cpp + src/target_sampler.cpp + src/world_model.cpp) + +target_include_directories( + bitbots_head_mover_lib + PUBLIC $ + $) + +target_link_libraries(bitbots_head_mover_lib PUBLIC Eigen3::Eigen + ${OpenCV_LIBS}) + +ament_target_dependencies( + bitbots_head_mover_lib + PUBLIC + bitbots_splines + geometry_msgs + image_geometry + kdl_parser + nav_msgs + orocos_kdl_vendor + sensor_msgs + urdf + visualization_msgs) + +# --------------------------------------------------------------------------- +# move_head node +# --------------------------------------------------------------------------- add_executable(move_head src/move_head.cpp) -target_link_libraries(move_head rclcpp::rclcpp head_parameters) +target_link_libraries(move_head rclcpp::rclcpp head_parameters + bitbots_head_mover_lib) ament_target_dependencies( move_head backward_ros bitbots_msgs bitbots_splines + bitbots_utils + cv_bridge generate_parameter_library + geometry_msgs + nav_msgs rclcpp sensor_msgs + soccer_vision_3d_msgs std_msgs + tf2_eigen tf2_geometry_msgs - tf2_ros) + tf2_ros + visualization_msgs) + +# --------------------------------------------------------------------------- +# Tests +# --------------------------------------------------------------------------- +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(test_search_pattern test/test_search_pattern.cpp) + target_link_libraries(test_search_pattern bitbots_head_mover_lib) + + ament_add_gtest(test_head_trajectory test/test_head_trajectory.cpp) + target_link_libraries(test_head_trajectory bitbots_head_mover_lib) + + ament_add_gtest(test_look_at test/test_look_at.cpp) + target_link_libraries(test_look_at bitbots_head_mover_lib) + + ament_add_gtest(test_types test/test_types.cpp) + target_link_libraries(test_types bitbots_head_mover_lib) + + ament_add_gtest(test_head_kinematics test/test_head_kinematics.cpp) + target_link_libraries(test_head_kinematics bitbots_head_mover_lib) + + ament_add_gtest(test_camera_model test/test_camera_model.cpp) + target_link_libraries(test_camera_model bitbots_head_mover_lib) + + ament_add_gtest(test_world_model test/test_world_model.cpp) + target_link_libraries(test_world_model bitbots_head_mover_lib) + + ament_add_gtest(test_field_coverage_map test/test_field_coverage_map.cpp) + target_link_libraries(test_field_coverage_map bitbots_head_mover_lib) + + ament_add_gtest(test_target_sampler test/test_target_sampler.cpp) + target_link_libraries(test_target_sampler bitbots_head_mover_lib) + + ament_add_gtest(test_active_vision_scorer test/test_active_vision_scorer.cpp) + target_link_libraries(test_active_vision_scorer bitbots_head_mover_lib) + + ament_add_gtest(test_active_vision test/test_active_vision.cpp) + target_link_libraries(test_active_vision bitbots_head_mover_lib) +endif() install(TARGETS move_head DESTINATION lib/${PROJECT_NAME}) +install(TARGETS bitbots_head_mover_lib DESTINATION lib) + +install(DIRECTORY include/ DESTINATION include) + install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) diff --git a/src/bitbots_motion/bitbots_head_mover/config/head_config.yml b/src/bitbots_motion/bitbots_head_mover/config/head_config.yml index 9700d2fbef..6599a557ca 100644 --- a/src/bitbots_motion/bitbots_head_mover/config/head_config.yml +++ b/src/bitbots_motion/bitbots_head_mover/config/head_config.yml @@ -24,7 +24,7 @@ move_head: max_pitch: type: double_array - default_value: [-1.23, 1.01] + default_value: [-1.23, 1.01] description: "Max values for the head position (in radians)" validation: fixed_size<>: 2 @@ -196,3 +196,239 @@ move_head: validation: bounds<>: [0.0, 1.0] + + # Sampling based head control used by the ACTIVE_VISION head mode + active_vision: + root_link: + type: string + default_value: "base_link" + description: "Link the sampled camera poses are resolved against. The map to this link transform places the robot on the field." + + tip_link: + type: string + default_value: "camera_optical_frame_left_uncalibrated" + description: "Optical frame at the end of the head chain, as named in the robot description" + + calibrated_optical_frame: + type: string + default_value: "zed_left_camera_optical_frame" + description: "Optical frame the vision pipeline reports detections in. The offset from tip_link is the extrinsic calibration and is looked up via tf." + + map_frame: + type: string + default_value: "map" + description: "Frame the field, the coverage map and all stored detections live in" + + max_velocity_yaw: + type: double + default_value: 4.0 + description: "Maximum yaw speed the controller commands while steering the head towards the target. Deliberately below the motor's mechanical limit." + validation: + gt<>: [0.0] + + max_velocity_pitch: + type: double + default_value: 4.0 + description: "Maximum pitch speed the controller commands while steering the head towards the target" + validation: + gt<>: [0.0] + + controller: + approach_distance: + type: double + default_value: 0.3 + description: "Joint space distance (radians) within which the controller ramps the head speed down linearly from the maximum to zero, so the head glides to a stop on the target. Beyond it the head travels at full speed." + validation: + gt<>: [0.0] + + control_period: + type: double + default_value: 0.05 + description: "Time step (seconds) the controller advances the setpoint by each cycle. Should match the control loop period." + validation: + gt<>: [0.0] + + sampling: + sample_count: + type: int + default_value: 64 + description: "Number of candidate targets drawn per planning cycle" + validation: + gt_eq<>: [1] + + last_target_weight: + type: double + default_value: 0.1 + description: "Relative share of the samples drawn from a gaussian around the previously selected target. Folded into the current position share until a first target exists. The three sampling weights are normalized, so only their ratio matters." + validation: + gt_eq<>: [0.0] + + current_position_weight: + type: double + default_value: 0.3 + description: "Relative share of the samples drawn from a gaussian around the current head position" + validation: + gt_eq<>: [0.0] + + uniform_weight: + type: double + default_value: 0.3 + description: "Relative share of the samples drawn uniformly over the reachable range, which keeps the search exploring the rest of the field" + validation: + gt_eq<>: [0.0] + + last_target_std: + type: double + default_value: 0.3 + description: "Standard deviation (radians) of the gaussian drawn around the last target" + validation: + gt<>: [0.0] + + current_position_std: + type: double + default_value: 0.5 + description: "Standard deviation (radians) of the gaussian drawn around the current head position" + validation: + gt<>: [0.0] + + random_seed: + type: int + default_value: 42 + description: "Seed of the sampling random number generator, so runs are reproducible" + + visibility: + center_fraction: + type: double + default_value: 0.3 + description: "Fraction of the image around its center that counts as perfectly framed" + validation: + bounds<>: [0.0, 1.0] + + border_score: + type: double + default_value: 0.3 + description: "Score of a target sitting exactly on the image border. Targets outside the image always score zero." + validation: + bounds<>: [0.0, 1.0] + + coverage: + margin: + type: double + default_value: 1.0 + description: "How far beyond the field lines the coverage map extends. This area carries the out of field penalty." + validation: + gt_eq<>: [0.0] + + cell_size: + type: double + default_value: 1.0 + description: "Edge length of a coverage map cell. Smaller cells are more precise but cost planning time, as every cell is projected into the camera for every evaluated point of every candidate." + validation: + gt<>: [0.0] + + half_life: + type: double + default_value: 0.5 + description: "Time after which having observed a part of the field counts for half as much, making it worth revisiting" + validation: + gt<>: [0.0] + + distance_half_weight: + type: double + default_value: 0.5 + description: "Ground distance at which a field cell counts half as much for coverage. A patch twice as far away covers about a quarter of the image, so without this falloff the coverage term would reward distant overview poses where a ball is barely detectable." + validation: + gt<>: [0.0] + + timeouts: + filtered_ball: + type: double + default_value: 5.0 + description: "How long the filtered ball estimate stays relevant" + validation: + gt<>: [0.0] + + raw_ball: + type: double + default_value: 0.5 + description: "How long a raw ball detection stays relevant" + validation: + gt<>: [0.0] + + team_ball: + type: double + default_value: 1.0 + description: "How long a ball reported by a teammate stays relevant" + validation: + gt<>: [0.0] + + robot: + type: double + default_value: 1.0 + description: "How long a robot detection stays relevant" + validation: + gt<>: [0.0] + + covariance_half_weight: + type: double + default_value: 0.5 + description: "Covariance at which a filtered estimate's contribution has dropped to half" + validation: + gt<>: [0.0] + + weights: + filtered_ball: + type: double + default_value: 1.5 + description: "Importance of keeping the filtered ball estimate in view" + validation: + gt_eq<>: [0.0] + + raw_balls: + type: double + default_value: 0.0 + description: "Importance of keeping the raw ball detections in view" + validation: + gt_eq<>: [0.0] + + team_ball: + type: double + default_value: 0.0 + description: "Importance of keeping a ball reported by a teammate in view" + validation: + gt_eq<>: [0.0] + + field_coverage: + type: double + default_value: 2.0 + description: "Importance of looking at parts of the field that were not observed recently" + validation: + gt_eq<>: [0.0] + + robots: + type: double + default_value: 0.1 + description: "Importance of keeping detected robots in view" + validation: + gt_eq<>: [0.0] + + + smoothness: + type: double + default_value: 0.1 + description: "Cost per radian of joint space distance between a candidate target and the previously selected one, subtracted from the score. This is the knob that trades reactivity against steady head motion: a larger value keeps the head from jumping between equally attractive parts of the field." + validation: + gt_eq<>: [0.0] + + debug: + enabled: + type: bool + default_value: true + description: "Publish the coverage grid, candidate markers and the joint space score image" + + image_size: + type: int + default_value: 400 + description: "Edge length in pixels of the joint space debug image" + validation: + gt_eq<>: [64] diff --git a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision.hpp new file mode 100644 index 0000000000..cd57257bb4 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision.hpp @@ -0,0 +1,201 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/// Sampling based head control. +namespace bitbots_head_mover { + +/// The speed limit and approach behaviour of the controller that drives the head +/// towards the selected target. +/// +/// Each cycle the controller moves the setpoint straight towards the target in +/// joint space. It travels at the maximum speed until it comes within the +/// approach distance of the target, then ramps the speed down linearly so the +/// head glides to a stop on the target instead of snapping to it or overshooting. +struct HeadController { + /// Joint space distance, in radians, within which the speed is ramped down + /// linearly from the maximum to zero. Beyond it the head travels at full speed; + /// a larger value makes the head ease into the target more gently. + double approach_distance = 0.3; + /// The time step, in seconds, the setpoint is advanced by each cycle. This is + /// the control loop period, kept explicit so the step does not depend on + /// wall-clock jitter between cycles. + double control_period = 0.05; + /// The maximum commanded joint speed, in radians per second, per axis. The + /// straight line motion is scaled so neither axis exceeds its own limit. + HeadVelocity max_velocity{7.0, 7.0}; +}; + +/// Everything the planner needs that is not part of its own state. +struct ActiveVisionInput { + /// Where the head currently is. + HeadPosition head_position; + /// The robot's pose on the field, i.e. the map to root link transform. + Eigen::Isometry3d robot_pose = Eigen::Isometry3d::Identity(); + /// The current time in seconds. + double now = 0.0; +}; + +/// Why the planner is not able to run. +enum class ActiveVisionReadiness { + Ready, + MissingRobotDescription, + MissingCameraInfo, + MissingFieldDimensions, +}; + +/// Why a planning cycle produced no command. +/// +/// Every failure gets its own value so the node can say what actually went +/// wrong. A single "it did not work" would leave an operator guessing between a +/// missing camera, a broken chain and a misconfigured sampler. +enum class ActiveVisionFailure { + None, + /// An input the planner needs has not arrived yet. + NotReady, + /// The head chain could not resolve a camera pose, so nothing could be scored. + KinematicsFailed, + /// The sampler is configured such that it cannot draw any candidate. + InvalidSamplerConfig, +}; + +/// A human readable description of a planning failure. +const char* describe(ActiveVisionFailure failure); + +/// A human readable description of a missing input. +const char* describe(ActiveVisionReadiness readiness); + +/// What the planner decided, plus what it considered while deciding. +struct ActiveVisionResult { + /// Whether a command could be produced at all. + bool valid = false; + /// Why no command was produced. None when valid. + ActiveVisionFailure failure = ActiveVisionFailure::None; + /// How many candidates had to be discarded because they could not be scored. + /// Non zero on an otherwise valid result means the planner chose from fewer + /// candidates than it sampled, which is worth reporting. + size_t unscorable_candidates = 0; + /// The selected target the head is steered towards. + HeadPosition target; + /// The head position to command right now, one control step towards the target. + HeadPosition position; + /// The joint speeds to command right now while travelling towards the target. + HeadVelocity velocity; + /// Every candidate that was scored, kept for the debug output. + std::vector candidates; + /// The score of each candidate, in the same order. + std::vector scores; + /// Index of the selected candidate within the vectors above. + size_t selected = 0; +}; + +/// Plans head motion by sampling candidate targets and scoring them. +/// +/// Owns the pieces the scoring is built from and drives one planning cycle per +/// call to plan(). Everything that depends on the ROS graph — the robot +/// description, the camera intrinsics, the detections and the robot pose — is +/// pushed in from the outside, so the planner itself stays testable. The head is +/// steered towards the selected target by a rate limited controller that eases +/// into the target, rather than along a planned trajectory. +class ActiveVision { + public: + ActiveVision(); + + /// Adopt the robot description and build the head chain from it. + /// + /// Returns false if no usable head chain could be extracted, in which case the + /// planner stays unready rather than falling back to a guessed geometry. + bool setRobotDescription(const std::string& urdf, const HeadChainConfig& chain_config); + + /// Adopt the camera intrinsics. Returns whether they were usable. + bool setCameraInfo(const sensor_msgs::msg::CameraInfo& info); + + /// Adopt the extrinsic camera calibration, which is not part of the URDF. + void setCameraCalibration(const Eigen::Isometry3d& calibration); + + /// Build the coverage map once the field dimensions are known. + void setFieldCoverageConfig(const FieldCoverageConfig& config); + + void setSamplerConfig(const SamplerConfig& config) { sampler_.setConfig(config); } + void setHeadLimits(const HeadLimits& limits) { limits_ = limits; } + void setController(const HeadController& controller) { controller_ = controller; } + void setScoringWeights(const ScoringWeights& weights) { weights_ = weights; } + void setVisibilityWeighting(const VisibilityWeighting& weighting) { visibility_ = weighting; } + void setCoverageDistanceHalfWeight(double distance) { coverage_distance_half_weight_ = distance; } + void setWorldModelConfig(const WorldModelConfig& config) { world_.setConfig(config); } + + /// Whether the planner has everything it needs. + ActiveVisionReadiness readiness() const; + bool ready() const { return readiness() == ActiveVisionReadiness::Ready; } + + /// The detections the planner scores against. + WorldModel& world() { return world_; } + const WorldModel& world() const { return world_; } + + /// The record of which parts of the field were looked at. + /// + /// Only valid once the planner is ready. + const FieldCoverageMap& coverage() const { return *coverage_; } + + /// The head chain the camera poses are resolved with. + /// + /// Only valid once the planner is ready. + const HeadKinematics& kinematics() const { return *kinematics_; } + + /// The camera the visibility is judged against. + const CameraModel& camera() const { return camera_; } + + /// The joint limits candidates are drawn within. + const HeadLimits& headLimits() const { return limits_; } + + /// The shape and number of the sampled candidates. + const SamplerConfig& samplerConfig() const { return sampler_.config(); } + + /// Run one planning cycle. + /// + /// Ages out stale detections, decays the coverage record, folds in what the + /// head is currently seeing, then samples and scores candidate targets and + /// steers the head one control step towards the best one. Returns an invalid + /// result if the planner is not ready or no candidate survived. + ActiveVisionResult plan(const ActiveVisionInput& input); + + /// Forget the coverage record and all detections. + void reset(); + + private: + std::unique_ptr kinematics_; + CameraModel camera_; + WorldModel world_; + std::unique_ptr coverage_; + TargetSampler sampler_; + + HeadLimits limits_{{-1.23, 1.23}, {-1.23, 1.01}}; + HeadController controller_; + ScoringWeights weights_; + VisibilityWeighting visibility_; + double coverage_distance_half_weight_ = 3.0; + + /// The extrinsic camera calibration, kept here as well so it survives the + /// robot description arriving after it. + Eigen::Isometry3d calibration_ = Eigen::Isometry3d::Identity(); + bool has_calibration_ = false; + + /// The target selected in the previous cycle, which is what the smoothness + /// cost is measured against and what the sampler concentrates its search on. + std::optional previous_target_; + double last_plan_time_ = 0.0; + bool has_planned_ = false; +}; + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision_debug.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision_debug.hpp new file mode 100644 index 0000000000..464f55fecf --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision_debug.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include +#include +#include + +/// Visualization of what the active vision planner is doing. +namespace bitbots_head_mover { + +/// Render the coverage map as an occupancy grid. +/// +/// The cell values carry the interest of a cell, so an unobserved part of the +/// field shows up as occupied and a freshly seen one as free. Cells outside the +/// field carry no interest and therefore always read as free. +nav_msgs::msg::OccupancyGrid coverageGrid(const FieldCoverageMap& coverage, const std::string& frame_id, + const builtin_interfaces::msg::Time& stamp); + +/// Render the sampled candidate targets and the selected one as markers. +/// +/// Each candidate is drawn as the point on the ground its optical axis would aim +/// at, coloured by its score, which makes it visible at a glance whether the +/// planner is considering the right part of the field. The selected candidate is +/// drawn on top as a larger, distinct marker. +visualization_msgs::msg::MarkerArray candidateMarkers(const ActiveVisionResult& result, + const ActiveVision& active_vision, + const Eigen::Isometry3d& robot_pose, const std::string& frame_id, + const builtin_interfaces::msg::Time& stamp); + +/// Plot the sampled candidate targets in the yaw and pitch joint space. +/// +/// The horizontal axis is yaw and the vertical axis is pitch, each spanning the +/// configured joint limits. Every candidate is drawn as a dot coloured by its +/// score, with the selected one highlighted, so the shape of the search and the +/// score landscape can be judged directly rather than inferred from numbers. +cv::Mat jointSpaceDebugImage(const ActiveVisionResult& result, const HeadLimits& limits, int size); + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision_scorer.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision_scorer.hpp new file mode 100644 index 0000000000..41e5f3decd --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision_scorer.hpp @@ -0,0 +1,184 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +/// Scoring of candidate head targets. +namespace bitbots_head_mover { + +/// Relative importance of the individual scoring terms. +/// +/// The reward terms are each normalized to [0, 1] on their own, so their weights +/// are directly comparable. The smoothness weight is different in kind: it is a +/// cost per radian subtracted from the total, not a normalized reward, so it is +/// expressed in the same units as the reward weights but multiplies a distance. +struct ScoringWeights { + /// Keeping the filtered ball estimate in view. Weighted highest because losing + /// the ball is the most expensive thing the head can do. + double filtered_ball = 4.0; + /// Keeping the raw ball detections in view. These are noisier but far more + /// immediate than the filtered estimate. + double raw_balls = 2.0; + /// Keeping a ball reported by a teammate in view, which is what lets the robot + /// pick up a ball it has not seen itself. + double team_ball = 1.0; + /// Looking at parts of the field that have not been observed in a while. + /// + /// There is deliberately no separate penalty for looking off the field: + /// off-field cells carry no interest, so aiming at them simply earns no + /// coverage. Looking away from the field is punished by what it forgoes, + /// which also covers aiming at the sky, where nothing projects at all. + double field_coverage = 1.5; + /// Keeping detected robots in view, so the obstacle information stays fresh. + double robots = 0.5; + /// Cost per radian of joint space distance between a target and the previously + /// selected one. This is subtracted from the score, so a larger value makes the + /// head prefer targets close to the last one and keeps it from jumping between + /// equally attractive parts of the field every cycle. + double smoothness = 1.5; +}; + +/// The individual contributions to a candidate's score. +/// +/// Kept separate from the total so the terms can be inspected and plotted while +/// tuning the weights, instead of only seeing the number they add up to. +struct ScoreBreakdown { + double filtered_ball = 0.0; + double raw_balls = 0.0; + double team_ball = 0.0; + double field_coverage = 0.0; + double robots = 0.0; + /// Joint space distance, in radians, to the previously selected target. Zero + /// when there is no previous target. Enters the total as a cost, weighted by + /// ScoringWeights::smoothness. + double smoothness_cost = 0.0; + /// The weighted sum of the reward terms minus the weighted smoothness cost. + double total = 0.0; + /// Whether the candidate could be scored at all. + /// + /// A candidate that could not be scored must never be compared against one + /// that could: a zeroed breakdown is indistinguishable from a genuinely + /// worthless candidate, and the planner would happily select it. + bool valid = false; +}; + +/// Everything about the world that a candidate is scored against. +/// +/// Bundled into one struct so the scoring interface does not grow a parameter +/// for every input, and so a test can build a world without a running node. +struct ScoringContext { + /// The robot's pose on the field, i.e. the map to root link transform. The + /// root link is whichever link the kinematics resolve the camera against. + Eigen::Isometry3d robot_pose = Eigen::Isometry3d::Identity(); + /// The previously selected target. Empty if there is no previous selection, + /// which disables the smoothness cost. + std::optional previous_target; +}; + +/// Scores candidate head targets against the current world state. +/// +/// A candidate is a single head position and is evaluated as the one camera pose +/// it points at, rather than as a motion over time: the head is driven towards +/// the selected target by a separate controller, so the planner only has to +/// decide where to look, not how to get there. +class ActiveVisionScorer { + public: + /// The robot pose is required rather than defaulted: the coverage distance + /// falloff is resolved against it, and a defaulted pose would quietly weight + /// every cell as if the robot stood on the center spot. + ActiveVisionScorer(const HeadKinematics& kinematics, const CameraModel& camera, const WorldModel& world, + const FieldCoverageMap& coverage, const Eigen::Isometry3d& robot_pose); + + void setWeights(const ScoringWeights& weights) { weights_ = weights; } + const ScoringWeights& weights() const { return weights_; } + + void setVisibilityWeighting(const VisibilityWeighting& weighting) { visibility_ = weighting; } + + /// Distance at which a field cell counts half as much for coverage. + /// + /// A patch of ground twice as far away subtends roughly a quarter of the image, + /// so a view aimed at the far half of the field sweeps up far more cells than + /// a close one for the same effort. Without a falloff the coverage term would + /// therefore reward distant overview poses, even though a ball at that range + /// is barely detectable. The weighting is quadratic in the distance, which is + /// what cancels that growth, so the term measures how much of the image is + /// usefully spent rather than how much ground area it happens to cover. + void setCoverageDistanceHalfWeight(double distance) { coverage_distance_half_weight_ = distance; } + + /// Collect the parts of the world state that every candidate is scored + /// against and that do not change while a batch is scored. + /// + /// Recomputing these per candidate would repeat the same grid sum and the same + /// allocations for every one of them, which measurably dominated the scoring. + /// The constructor already does this, so it only has to be called again when + /// the world or the coverage map changed after the scorer was built. + /// + /// The distance falloff is resolved against the robot's position rather than + /// against each candidate's camera pose. The head only shifts the camera by + /// centimeters, and holding it fixed keeps the coverage denominator identical + /// for every candidate, which is what makes their scores comparable. + void prepare(const Eigen::Isometry3d& robot_pose); + + /// Score a single candidate target. + /// + /// The result carries a valid flag; an invalid result means the candidate + /// could not be scored and must be discarded rather than compared. + ScoreBreakdown score(const HeadPosition& target, const ScoringContext& context) const; + + /// The camera pose in the map frame for a given head configuration. + /// + /// Returns nullopt if the kinematics could not resolve the pose. + std::optional cameraPoseInMap(const HeadPosition& position, + const Eigen::Isometry3d& robot_pose) const; + + /// Record what a head position actually observed into the coverage map. + /// + /// This is the counterpart to the coverage scoring: the score asks what a + /// candidate would see, this records what the executed motion did see. + /// Returns false if nothing could be recorded, so the caller can report it + /// instead of silently continuing with a coverage map that stopped updating. + bool recordObservation(FieldCoverageMap& coverage, const HeadPosition& position, + const Eigen::Isometry3d& robot_pose) const; + + private: + const HeadKinematics& kinematics_; + const CameraModel& camera_; + const WorldModel& world_; + const FieldCoverageMap& coverage_; + ScoringWeights weights_; + VisibilityWeighting visibility_; + + double coverage_distance_half_weight_ = 3.0; + + // Per cycle invariants, filled by prepare() + /// Distance falloff of every cell, resolved against the robot's position. + std::vector cell_distance_weights_; + /// The distance weighted interest the field currently holds, which is what a + /// candidate's covered interest is expressed as a fraction of. + double available_interest_ = 0.0; + std::vector team_balls_; + std::vector filtered_ball_; +}; + +/// The visibility of a set of targets from one camera pose, in [0, 1]. +/// +/// The result is how well the targets are framed, multiplied by how much the +/// best of them is trusted. Framing is a weighted average rather than a maximum, +/// so a view covering several agreeing detections beats one that catches a +/// single outlier; the trust factor is applied separately because an average +/// would normalize the weights away and rate one uncertain target exactly like a +/// certain one. Returns zero when there is nothing to look at. +/// +/// Takes the map to camera transform rather than the camera pose, because the +/// caller evaluates several target sets against the same pose and inverting it +/// once per set was pure overhead. +double targetVisibility(const std::vector& targets, const Eigen::Isometry3d& map_to_camera, + const CameraModel& camera, const VisibilityWeighting& weighting); + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/camera_model.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/camera_model.hpp new file mode 100644 index 0000000000..06b18515c1 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/camera_model.hpp @@ -0,0 +1,100 @@ +#pragma once + +#include +#include +#include +#include + +namespace image_geometry { +class PinholeCameraModel; +} + +/// Projection of world points into the camera image. +namespace bitbots_head_mover { + +/// How a point's position inside the image is turned into a score. +struct VisibilityWeighting { + /// Fraction of the image, measured from its center, that counts as perfectly + /// visible. A value of one half means the central 50% of the image. + double center_fraction = 0.5; + /// Score of a point sitting exactly on the image border. Points outside the + /// image always score zero, so this is the score a barely visible point gets. + double border_score = 0.3; +}; + +/// Tests whether points are visible to the camera and how well they are framed. +/// +/// Wraps image_geometry::PinholeCameraModel so the rest of the head mover works +/// with Eigen vectors and does not have to know about OpenCV. All points passed +/// in are expected to be expressed in the camera's optical frame, i.e. with x to +/// the right, y down and z forward. +class CameraModel { + public: + CameraModel(); + ~CameraModel(); + CameraModel(CameraModel&&) noexcept; + CameraModel& operator=(CameraModel&&) noexcept; + + /// Adopt new intrinsics. + /// + /// Returns false and leaves the model unchanged if the message does not carry + /// usable intrinsics, which is the case before the camera driver is calibrated. + bool update(const sensor_msgs::msg::CameraInfo& info); + + /// Whether usable intrinsics were received. + bool valid() const { return valid_; } + + /// The image dimensions in pixels. + double width() const { return width_; } + double height() const { return height_; } + + /// Project a point in the optical frame to pixel coordinates. + /// + /// Returns nullopt if the point is behind the camera or lands outside the + /// image bounds. + std::optional project(const Eigen::Vector3d& point) const; + + /// Score how well a point in the optical frame is framed, in [0, 1]. + /// + /// Points that are not visible score zero. Visible points score one inside the + /// central region and fall off linearly towards the border score at the image + /// edge, so that centering an object is preferred over merely catching it. + double visibility(const Eigen::Vector3d& point, const VisibilityWeighting& weighting = {}) const; + + private: + /// Project a point in the optical frame, without bounds checking. + /// + /// Returns false if the point is behind the camera. Kept inline and free of + /// OpenCV types because the scoring calls it for every coverage cell of every + /// evaluated point of every candidate, which made it the single hottest path + /// in a planning cycle. + bool projectRaw(const Eigen::Vector3d& point, double& u, double& v) const { + if (point.z() <= 0.0) { + return false; + } + const double inverse_z = 1.0 / point.z(); + u = (fx_ * point.x() + tx_) * inverse_z + cx_; + v = (fy_ * point.y() + ty_) * inverse_z + cy_; + return true; + } + + /// The rectified pinhole model, which stays the authority on the intrinsics. + std::unique_ptr model_; + bool valid_ = false; + double width_ = 0.0; + double height_ = 0.0; + + // The model's rectified parameters, cached for the hot path above. They are + // taken from the model rather than from the message so that binning and a + // region of interest stay accounted for. + double fx_ = 0.0; + double fy_ = 0.0; + double cx_ = 0.0; + double cy_ = 0.0; + double tx_ = 0.0; + double ty_ = 0.0; + double inverse_half_width_ = 0.0; + double inverse_half_height_ = 0.0; +}; + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/field_coverage_map.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/field_coverage_map.hpp new file mode 100644 index 0000000000..c0c6e55d96 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/field_coverage_map.hpp @@ -0,0 +1,87 @@ +#pragma once + +#include +#include +#include + +/// A coarse record of which parts of the field were looked at recently. +namespace bitbots_head_mover { + +/// Extent and behavior of the field coverage map. +struct FieldCoverageConfig { + /// Field dimensions, as reported by the parameter blackboard. + double field_length = 9.0; + double field_width = 6.0; + /// How far beyond the field lines the map extends. The area outside the field + /// still has to be represented, because looking at it is what we want to + /// discourage, and that requires knowing it is there. + double margin = 1.0; + /// Edge length of a grid cell. This is deliberately coarse: the map only has + /// to steer the head, not localize anything, and every cell is projected into + /// the camera for every evaluated point of every candidate. + double cell_size = 1.0; + /// Time after which an observation has faded to half its value. + double half_life = 8.0; +}; + +/// Tracks how recently each part of the field was seen. +/// +/// Cells that have not been looked at for a while become interesting again, +/// which is what makes the head sweep the field on its own instead of staring at +/// whatever it already knows about. The map lives in the map frame, so the +/// record survives the robot walking around. +class FieldCoverageMap { + public: + explicit FieldCoverageMap(const FieldCoverageConfig& config = {}); + + const FieldCoverageConfig& config() const { return config_; } + + /// Number of cells in the grid. + size_t size() const { return centers_.size(); } + size_t cellsX() const { return cells_x_; } + size_t cellsY() const { return cells_y_; } + + /// The cell centers in the map frame, in row major order. + /// + /// The z coordinate is zero, i.e. the cells sit on the ground plane. + const std::vector& cellCenters() const { return centers_; } + + /// How much attention a cell currently deserves, in [0, 1]. + /// + /// One means it has not been looked at in a long time. Cells outside the field + /// are never interesting, so looking at them can only be a waste. + double interest(size_t index) const; + + /// How recently a cell was observed, in [0, 1], where one is just now. + double observation(size_t index) const { return observations_[index]; } + + /// Whether a cell lies outside the field lines. + bool isOutOfField(size_t index) const { return out_of_field_[index]; } + + /// Let all observations fade towards being unobserved. + void decay(double dt); + + /// Record that a cell was observed, with a quality in [0, 1]. + /// + /// Observing a cell that was already observed does not reduce its record. + void observe(size_t index, double quality); + + /// Forget every observation, e.g. after the localization was reset. + void reset(); + + /// The sum of the interest of all cells, used to normalize coverage scores. + double totalInterest() const; + + private: + /// Build the grid from the configured extents. + void build(); + + FieldCoverageConfig config_; + size_t cells_x_ = 0; + size_t cells_y_ = 0; + std::vector centers_; + std::vector observations_; + std::vector out_of_field_; +}; + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/head_kinematics.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/head_kinematics.hpp new file mode 100644 index 0000000000..12e1d6bda6 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/head_kinematics.hpp @@ -0,0 +1,94 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +/// Forward kinematics of the head chain. +namespace bitbots_head_mover { + +/// The links and joints the head chain is built from. +struct HeadChainConfig { + /// The link the resulting camera pose is expressed in. + std::string root_link = "base_link"; + /// The optical frame at the end of the chain, as it appears in the robot description. + std::string tip_link = "camera_optical_frame_left_uncalibrated"; + std::string yaw_joint = "head_yaw_joint"; + std::string pitch_joint = "head_pitch_joint"; +}; + +/// Resolves the camera pose for an arbitrary head configuration. +/// +/// Being able to evaluate head positions the robot is not currently in is what +/// makes sampling based head control possible: TF only ever knows where the head +/// actually is. The chain is taken from the robot description, so the geometry +/// stays consistent with the model the rest of the stack uses. +/// +/// The class is not copyable because the KDL solver refers to the chain it was +/// constructed from; use the returned handle directly. +class HeadKinematics { + public: + HeadKinematics(const HeadKinematics&) = delete; + HeadKinematics& operator=(const HeadKinematics&) = delete; + + /// Build the head chain from a robot description. + /// + /// Returns nullptr if the URDF cannot be parsed, if there is no chain between + /// the configured links, or if that chain's movable joints are not exactly the + /// two configured head joints. + static std::unique_ptr fromUrdf(const std::string& urdf, const HeadChainConfig& config = {}); + + /// The camera pose relative to the root link for a given head configuration. + /// + /// Includes the camera calibration if one was set. Returns nullopt if the + /// solver fails, rather than an identity pose: an identity would silently + /// place the camera at the robot's origin looking along its x axis, which is a + /// perfectly plausible pose that would be scored as if it were real. + std::optional cameraPose(const HeadPosition& position) const; + + /// The head joint limits as declared in the robot description. + /// + /// These are the mechanical limits of the joints. The head mover additionally + /// applies its own, usually tighter, configured limits. + const HeadLimits& urdfLimits() const { return urdf_limits_; } + + /// Append a fixed transform to every camera pose. + /// + /// The extrinsic camera calibration is published as a separate transform + /// instead of being part of the robot description, so it has to be composed + /// onto the chain's tip to arrive at the optical frame that the vision + /// pipeline reports its detections in. The transform does not depend on the + /// head configuration, so looking it up once is enough. + void setCameraCalibration(const Eigen::Isometry3d& calibration) { + calibration_ = calibration; + has_calibration_ = true; + } + + /// Whether a camera calibration was set. + bool hasCameraCalibration() const { return has_calibration_; } + + private: + HeadKinematics() = default; + + KDL::Chain chain_; + std::unique_ptr solver_; + /// Scratch joint vector, reused so that resolving a camera pose does not + /// allocate. The scoring resolves one per evaluated point of every candidate, + /// which made this an allocation per call in the control loop. Mutable because + /// it is an implementation detail of a logically const query; this makes + /// cameraPose() unsafe to call on one instance from several threads at once, + /// which the single planning loop never does. + mutable KDL::JntArray joints_; + /// Index of the head joints among the chain's movable joints. + unsigned int yaw_index_ = 0; + unsigned int pitch_index_ = 0; + HeadLimits urdf_limits_; + Eigen::Isometry3d calibration_ = Eigen::Isometry3d::Identity(); + bool has_calibration_ = false; +}; + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/head_trajectory.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/head_trajectory.hpp new file mode 100644 index 0000000000..58b563a53a --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/head_trajectory.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include +#include +#include + +/// Time parameterized head trajectories in joint space. +namespace bitbots_head_mover { + +/// A pair of quintic splines describing the yaw and pitch joint over time. +/// +/// This is a thin wrapper around two bitbots_splines::SmoothSpline instances +/// that keeps both joints in lockstep, so a trajectory can be evaluated as a +/// single head position instead of two independent scalars. +class HeadTrajectory { + public: + /// Append a waypoint. Waypoints must be added with increasing time. + void addPoint(double time, const HeadPosition& position, const HeadVelocity& velocity = {}, + const HeadAcceleration& acceleration = {}); + + /// Compute the spline interpolation over the added waypoints. + /// + /// Must be called after the last waypoint was added and before the trajectory + /// is evaluated. A trajectory with fewer than two waypoints stays invalid. + void finalize(); + + /// Whether the trajectory was finalized and can be evaluated. + bool valid() const { return valid_; } + + /// The time of the last waypoint, or zero for an unfinalized trajectory. + double duration() const { return duration_; } + + /// The number of waypoints that were added. + size_t size() const { return size_; } + + /// The head position at the given time. + HeadPosition position(double time) const; + + /// The head velocity at the given time. + HeadVelocity velocity(double time) const; + + /// The head acceleration at the given time. + HeadAcceleration acceleration(double time) const; + + private: + bitbots_splines::SmoothSpline yaw_; + bitbots_splines::SmoothSpline pitch_; + double duration_ = 0.0; + size_t size_ = 0; + bool valid_ = false; +}; + +/// A looping search pattern trajectory. +/// +/// The trajectory starts at the current head position and smoothly transitions +/// into the pattern, so switching head modes does not result in an abrupt +/// movement. Only the part after the transition loops; the transition itself is +/// played exactly once. +struct SearchPatternTrajectory { + HeadTrajectory trajectory; + /// Duration of the one-off segment leading from the start position into the pattern. + double transition_duration = 0.0; + /// Duration of one full pattern cycle, excluding the transition. + double cycle_duration = 0.0; + + /// Whether the trajectory can be evaluated. + bool valid() const { return trajectory.valid() && cycle_duration > 0.0; } + + /// Map a time elapsed since the trajectory started to a time on the trajectory. + /// + /// Times inside the transition are passed through, later times wrap around the + /// cyclic part of the trajectory. + double phase(double elapsed) const; +}; + +/// Build a looping trajectory from search pattern keyframes. +/// +/// Note the mixed units: the keyframes are given in degrees, because that is how +/// the search patterns are configured, while start and the resulting trajectory +/// are in radians, because that is what the joints use. Waypoint +/// timestamps are distributed proportionally to the Euclidean distance between +/// consecutive keyframes, so the cycle takes exactly cycle_time seconds and the +/// head moves at a roughly constant angular speed. The loop is closed by +/// appending the first keyframe at the end. The duration of the prepended +/// transition from start follows from its distance and transition_speed. +SearchPatternTrajectory buildSearchPatternTrajectory(const std::vector& pattern_deg, double cycle_time, + const HeadPosition& start, double transition_speed); + +/// Slow down the joint that has to travel less so both joints arrive together. +/// +/// Returns the speed the slower-traveling joint should use, or zero if the +/// faster joint does not have to move at all. +double calculateLowerSpeed(double delta_faster_joint, double delta_joint, double speed); + +/// Adjust the maximum joint speeds so both joints reach the goal at the same time. +/// +/// The joint that has to travel the shorter distance is slowed down to match the +/// travel time of the other one. Speeds are never increased beyond max_speeds. +HeadVelocity adjustSpeeds(const HeadPosition& goal, const HeadPosition& current, const HeadVelocity& max_speeds); + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/look_at.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/look_at.hpp new file mode 100644 index 0000000000..d1e27b7564 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/look_at.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include +#include +#include + +/// Inverse kinematics for pointing the head at a single point. +namespace bitbots_head_mover { + +/// The angle between the head pitch joint and the camera's optical axis. +/// +/// The camera is mounted tilted forward relative to the head pitch link, so a +/// pitch goal computed from a point has to be corrected by this angle to make +/// the camera, rather than the link, point at the target. +constexpr double kCameraPitchOffset = M_PI / 180.0 * 30.0; + +/// Compute the head joint goals that make the camera look at a point. +/// +/// The point has to be given twice, expressed in the head yaw link and in the +/// head pitch link, because each joint is solved in its own frame. The result is +/// relative to the current head position, which therefore has to be passed in. +HeadPosition motorGoalsFromPoint(const geometry_msgs::msg::Point& head_yaw_point, + const geometry_msgs::msg::Point& head_pitch_point, const HeadPosition& current, + double camera_pitch_offset = kCameraPitchOffset); + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/search_pattern.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/search_pattern.hpp new file mode 100644 index 0000000000..22239ec045 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/search_pattern.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include +#include + +/// Generation of the open-loop head search patterns. +/// +/// The patterns are boustrophedon scans: the head sweeps horizontally along a +/// scan line, steps to the next line, and sweeps back in the opposite +/// direction. All angles handled here are in the unit in which the patterns are +/// configured, which is degrees; the conversion to radians happens when the +/// pattern is turned into a trajectory. +namespace bitbots_head_mover { + +/// Convert a scan line index to its angle. +/// +/// The lines are distributed evenly between the two given angles, with line 0 +/// at min_angle and the last line at max_angle. A pattern with a single scan +/// line has no span to distribute and collapses onto min_angle. +double lineAngle(int line, int line_count, double min_angle, double max_angle); + +/// Linearly interpolate between two yaw values at a constant pitch. +/// +/// Returns the interpolated positions ordered from min_yaw towards max_yaw, +/// excluding min_yaw itself and including max_yaw. Returns an empty vector if no +/// interpolation was requested. +std::vector interpolatedSteps(int steps, double pitch, double min_yaw, double max_yaw); + +/// Generate the keyframes of a parameterized search pattern. +/// +/// The scan starts at the bottom line and alternates its horizontal direction +/// on every line. Keyframes on the lowest line are scaled towards the center by +/// reduce_last_scanline, because looking far to the side while looking down +/// makes the head collide with the body. +std::vector generatePattern(int line_count, double max_horizontal_angle_left, + double max_horizontal_angle_right, double max_vertical_angle_up, + double max_vertical_angle_down, double reduce_last_scanline = 1.0, + int interpolation_steps = 0); + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/target_sampler.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/target_sampler.hpp new file mode 100644 index 0000000000..11e2163225 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/target_sampler.hpp @@ -0,0 +1,79 @@ +#pragma once + +#include +#include +#include +#include + +/// Random generation of candidate head targets. +namespace bitbots_head_mover { + +/// Shape and number of the sampled candidate targets. +/// +/// Targets are drawn from a mixture of three components: a gaussian around the +/// previously selected target, a gaussian around the current head position and a +/// uniform base distribution over the whole reachable range. The two gaussians +/// concentrate the search where the head already is or was heading, while the +/// uniform floor keeps it exploring the rest of the field so a better target is +/// eventually found. +struct SamplerConfig { + /// How many targets are drawn per planning cycle. + int sample_count = 64; + + /// Relative share of the samples drawn around the previously selected target. + /// + /// The three weights do not have to sum to one; they are normalized. At least + /// one of them has to be positive, otherwise there is no distribution to draw + /// from. When there is no previous target yet, its share is folded into the + /// gaussian around the current position. + double last_target_weight = 0.4; + /// Relative share of the samples drawn around the current head position. + double current_position_weight = 0.3; + /// Relative share of the samples drawn uniformly over the reachable range. + double uniform_weight = 0.3; + + /// Standard deviation, in radians, of the gaussian around the last target. + double last_target_std = 0.3; + /// Standard deviation, in radians, of the gaussian around the current position. + double current_position_std = 0.5; +}; + +/// A candidate target head position, kept for scoring and debug output. +struct Candidate { + /// The head position the candidate would look at. + HeadPosition target; +}; + +/// Draws candidate target head positions. +/// +/// Every draw is clamped into the joint limits, so no candidate ever asks the +/// head to leave its reachable range. The candidate set always contains the +/// current head position, which guarantees that there is something to choose from +/// and gives the scoring a way to keep the head still when nothing is worth +/// looking at, and the previous target when one exists, so a still-best target is +/// not lost to sampling noise between cycles. +class TargetSampler { + public: + explicit TargetSampler(const SamplerConfig& config = {}, uint32_t seed = 42); + + void setConfig(const SamplerConfig& config) { config_ = config; } + const SamplerConfig& config() const { return config_; } + + /// Draw candidate targets around the current position and the last target. + /// + /// last_target is empty on the first cycle, which folds its mixture share into + /// the gaussian around the current position. + std::vector sample(const HeadPosition& current, const std::optional& last_target, + const HeadLimits& limits); + + private: + /// Draw a single joint value from a gaussian around a center, clamped to the limit. + double sampleGaussian(double center, double std, const JointLimit& limit); + /// Draw a single joint value uniformly over the limit. + double sampleUniform(const JointLimit& limit); + + SamplerConfig config_; + std::mt19937 rng_; +}; + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/types.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/types.hpp new file mode 100644 index 0000000000..1a1546ff83 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/types.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include + +/// Shared value types for the head mover. +/// +/// These are intentionally dependency-free so that every component built on top +/// of them (search patterns, trajectories, sampling and scoring) can be unit +/// tested without a ROS node, a TF buffer or a parameter listener. +namespace bitbots_head_mover { + +/// A head configuration in joint space. +/// +/// Both values are in radians unless a function explicitly documents that it +/// works on degrees (the search pattern parameters are configured in degrees). +struct HeadPosition { + double yaw = 0.0; + double pitch = 0.0; +}; + +/// A head velocity in joint space, in radians per second. +struct HeadVelocity { + double yaw = 0.0; + double pitch = 0.0; +}; + +/// A head acceleration in joint space, in radians per second squared. +struct HeadAcceleration { + double yaw = 0.0; + double pitch = 0.0; +}; + +/// The inclusive bounds of a single joint. +struct JointLimit { + double lower = 0.0; + double upper = 0.0; + + /// Clamp a value into the bounds. + constexpr double clamp(double value) const { return std::clamp(value, lower, upper); } + + /// Whether a value lies strictly inside the bounds. + /// + /// The bounds are treated as exclusive because the head mover has always + /// rejected goals that sit exactly on a limit as a collision. + constexpr bool contains(double value) const { return lower < value && value < upper; } +}; + +/// The joint limits of both head joints. +struct HeadLimits { + JointLimit yaw; + JointLimit pitch; + + /// Clamp a head position into the limits. + constexpr HeadPosition clamp(HeadPosition position) const { + return {yaw.clamp(position.yaw), pitch.clamp(position.pitch)}; + } + + /// Whether a head position lies strictly inside the limits of both joints. + constexpr bool contains(const HeadPosition& position) const { + return yaw.contains(position.yaw) && pitch.contains(position.pitch); + } +}; + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/world_model.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/world_model.hpp new file mode 100644 index 0000000000..a04672dd9d --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/world_model.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include +#include +#include +#include +#include + +/// The detections the head should pay attention to. +namespace bitbots_head_mover { + +/// A detection the head could look at, in the map frame. +struct TimedTarget { + Eigen::Vector3d position = Eigen::Vector3d::Zero(); + /// How much attention this detection deserves, in [0, 1]. + double weight = 1.0; + /// When the detection was made, in seconds. + double stamp = 0.0; +}; + +/// How long detections stay relevant and how their weight is derived. +struct WorldModelConfig { + /// Filtered estimates are kept longest because the filter already smooths over + /// individual missed frames. + double filtered_ball_timeout = 5.0; + /// Raw detections are only meaningful for a moment, they are what tells the + /// head that something is right there at this instant. + double raw_ball_timeout = 0.5; + double team_ball_timeout = 3.0; + double robot_timeout = 1.0; + /// The covariance at which a filtered estimate's weight has dropped to half. + /// Larger covariances keep lowering the weight without ever reaching zero. + double covariance_half_weight = 0.5; +}; + +/// Turn the covariance of an estimate into a weight in (0, 1]. +/// +/// A perfectly known position keeps its full weight, and the weight halves once +/// the covariance reaches the configured scale. The scale has to be positive; +/// the parameter validation enforces that, and a non positive one is a +/// configuration error rather than a case to be handled. +double weightFromCovariance(double covariance, double covariance_half_weight); + +/// Buffers the detections used to score head positions. +/// +/// Every position is expected in the map frame, so that stored detections stay +/// valid while the robot walks. Entries are dropped once they age past their +/// category's timeout, which is what makes the head stop chasing stale +/// information without any explicit clearing logic. +class WorldModel { + public: + explicit WorldModel(const WorldModelConfig& config = {}) : config_(config) {} + + void setConfig(const WorldModelConfig& config) { config_ = config; } + const WorldModelConfig& config() const { return config_; } + + /// Replace the filtered ball estimate. + /// + /// The covariance is expected to be the larger of the two planar variances, so + /// that an estimate that is uncertain in any direction is treated as uncertain. + void setFilteredBall(const Eigen::Vector3d& position, double covariance, double stamp); + + /// Replace the current raw ball detections. + void setRawBalls(std::vector balls); + + /// Replace the ball reported by one teammate. + /// + /// Teammates are tracked individually so that a single silent robot does not + /// keep its last ball alive through the others' messages. + void setTeamBall(uint8_t robot_id, const Eigen::Vector3d& position, double covariance, double stamp); + + /// Replace the current robot detections. + void setRobots(std::vector robots); + + /// Drop every detection that has aged past its timeout. + /// + /// Call this once per control cycle before reading any of the accessors. + void prune(double now); + + const std::optional& filteredBall() const { return filtered_ball_; } + const std::vector& rawBalls() const { return raw_balls_; } + const std::vector& robots() const { return robots_; } + + /// The balls currently reported by teammates. + std::vector teamBalls() const; + + /// Whether any ball information at all is available. + bool hasAnyBall() const; + + /// Forget everything, e.g. after the localization was reset. + void clear(); + + private: + WorldModelConfig config_; + std::optional filtered_ball_; + std::vector raw_balls_; + std::vector robots_; + std::map team_balls_; +}; + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/package.xml b/src/bitbots_motion/bitbots_head_mover/package.xml index 56aea0b0e9..db2c515705 100644 --- a/src/bitbots_motion/bitbots_head_mover/package.xml +++ b/src/bitbots_motion/bitbots_head_mover/package.xml @@ -21,13 +21,25 @@ bitbots_splines bitbots_robot_description bitbots_utils + cv_bridge + eigen generate_parameter_library + geometry_msgs + image_geometry + kdl_parser + nav_msgs + orocos_kdl_vendor rclcpp sensor_msgs + soccer_vision_3d_msgs std_msgs + tf2_eigen tf2_geometry_msgs tf2_ros + urdf + visualization_msgs + ament_cmake_gtest ament_lint_auto ament_lint_common diff --git a/src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp b/src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp new file mode 100644 index 0000000000..a24b51abbf --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp @@ -0,0 +1,216 @@ +#include +#include +#include +#include + +namespace bitbots_head_mover { + +ActiveVision::ActiveVision() : sampler_(SamplerConfig{}) {} + +const char* describe(ActiveVisionFailure failure) { + switch (failure) { + case ActiveVisionFailure::None: + return "no failure"; + case ActiveVisionFailure::NotReady: + return "an input the planner needs has not arrived yet"; + case ActiveVisionFailure::KinematicsFailed: + return "the head chain could not resolve a camera pose for any candidate"; + case ActiveVisionFailure::InvalidSamplerConfig: + return "the sampler cannot draw any candidate with its current configuration"; + } + return "unknown failure"; +} + +const char* describe(ActiveVisionReadiness readiness) { + switch (readiness) { + case ActiveVisionReadiness::Ready: + return "ready"; + case ActiveVisionReadiness::MissingRobotDescription: + return "no usable robot description was received on /robot_description"; + case ActiveVisionReadiness::MissingCameraInfo: + return "no usable camera info was received"; + case ActiveVisionReadiness::MissingFieldDimensions: + return "the field dimensions are unknown, so there is no coverage map"; + } + return "unknown state"; +} + +bool ActiveVision::setRobotDescription(const std::string& urdf, const HeadChainConfig& chain_config) { + auto kinematics = HeadKinematics::fromUrdf(urdf, chain_config); + if (!kinematics) { + return false; + } + // Carry an already known calibration over to the new chain, so a robot + // description arriving after the calibration does not silently drop it + if (has_calibration_) { + kinematics->setCameraCalibration(calibration_); + } + kinematics_ = std::move(kinematics); + return true; +} + +bool ActiveVision::setCameraInfo(const sensor_msgs::msg::CameraInfo& info) { return camera_.update(info); } + +void ActiveVision::setCameraCalibration(const Eigen::Isometry3d& calibration) { + calibration_ = calibration; + has_calibration_ = true; + if (kinematics_) { + kinematics_->setCameraCalibration(calibration); + } +} + +void ActiveVision::setFieldCoverageConfig(const FieldCoverageConfig& config) { + coverage_ = std::make_unique(config); +} + +ActiveVisionReadiness ActiveVision::readiness() const { + if (!kinematics_) { + return ActiveVisionReadiness::MissingRobotDescription; + } + if (!camera_.valid()) { + return ActiveVisionReadiness::MissingCameraInfo; + } + if (!coverage_ || coverage_->size() == 0) { + return ActiveVisionReadiness::MissingFieldDimensions; + } + return ActiveVisionReadiness::Ready; +} + +ActiveVisionResult ActiveVision::plan(const ActiveVisionInput& input) { + ActiveVisionResult result; + if (!ready()) { + result.failure = ActiveVisionFailure::NotReady; + return result; + } + + // A sampler that cannot draw is reported as such instead of surfacing as + // "every candidate was rejected" + const SamplerConfig& sampler_config = sampler_.config(); + const double weight_sum = std::max(0.0, sampler_config.last_target_weight) + + std::max(0.0, sampler_config.current_position_weight) + + std::max(0.0, sampler_config.uniform_weight); + if (sampler_config.sample_count < 0 || !(weight_sum > 0.0)) { + result.failure = ActiveVisionFailure::InvalidSamplerConfig; + return result; + } + + // Age out detections that are no longer trustworthy + world_.prune(input.now); + + // Let the record of what was already seen fade, so parts of the field become + // worth revisiting + if (has_planned_) { + coverage_->decay(std::max(0.0, input.now - last_plan_time_)); + } + + ActiveVisionScorer scorer(*kinematics_, camera_, world_, *coverage_, input.robot_pose); + scorer.setWeights(weights_); + scorer.setVisibilityWeighting(visibility_); + scorer.setCoverageDistanceHalfWeight(coverage_distance_half_weight_); + + // Record what the head is looking at right now. This has to happen before + // scoring, so a candidate does not get rewarded for covering ground that the + // current head position already covers. + // A failure here means the kinematics are broken, which the scoring below + // reports in more detail, so it is not turned into its own failure + (void)scorer.recordObservation(*coverage_, input.head_position, input.robot_pose); + + // Recording the observation changed the coverage map, so refresh what the + // batch is scored against + scorer.prepare(input.robot_pose); + + ScoringContext context; + context.robot_pose = input.robot_pose; + context.previous_target = previous_target_; + + // The current position and the previous target seed the sampling distribution, + // so the search stays concentrated where the head is and where it was heading + result.candidates = sampler_.sample(input.head_position, previous_target_, limits_); + if (result.candidates.empty()) { + // The sampler always offers the current position unless it is misconfigured, + // which the check above already ruled out; guard anyway + result.failure = ActiveVisionFailure::InvalidSamplerConfig; + return result; + } + + result.scores.reserve(result.candidates.size()); + double best_score = -std::numeric_limits::infinity(); + bool have_selection = false; + for (size_t index = 0; index < result.candidates.size(); index++) { + ScoreBreakdown breakdown = scorer.score(result.candidates[index].target, context); + // A candidate that could not be scored is discarded rather than compared: + // its zeroed total would look like a merely unattractive candidate and could + // still win if every real candidate scores negative + if (breakdown.valid && (!have_selection || breakdown.total > best_score)) { + best_score = breakdown.total; + result.selected = index; + have_selection = true; + } + if (!breakdown.valid) { + result.unscorable_candidates++; + } + result.scores.push_back(breakdown); + } + + if (!have_selection) { + result.failure = ActiveVisionFailure::KinematicsFailed; + return result; + } + + result.target = limits_.clamp(result.candidates[result.selected].target); + + // Rate limited controller: step the setpoint straight towards the target in + // joint space, at the maximum speed until the head comes within the approach + // distance, then ramp the speed down linearly so it glides to a stop on the + // target instead of snapping to it. This is what replaces the planned + // trajectory; the smoothness cost keeps the target itself from jumping between + // cycles. + const HeadPosition& current = input.head_position; + const double error_yaw = result.target.yaw - current.yaw; + const double error_pitch = result.target.pitch - current.pitch; + const double distance = std::hypot(error_yaw, error_pitch); + + // Below this the head is on the target; moving would only chase sampling noise. + constexpr double kAtTargetEpsilon = 1e-6; + if (distance < kAtTargetEpsilon) { + result.velocity = {0.0, 0.0}; + result.position = result.target; + } else { + // Unit direction towards the target, so the head travels in a straight line. + const double dir_yaw = error_yaw / distance; + const double dir_pitch = error_pitch / distance; + + // Full speed along this direction that still respects both per-axis speed + // caps: scale the unit direction up until the first axis hits its own limit. + const double speed_cap = std::min(controller_.max_velocity.yaw / std::max(std::abs(dir_yaw), kAtTargetEpsilon), + controller_.max_velocity.pitch / std::max(std::abs(dir_pitch), kAtTargetEpsilon)); + + // Travel at full speed until within the approach distance, then ramp down + // linearly to zero as the remaining distance shrinks. + const double ramp = std::clamp(distance / std::max(controller_.approach_distance, kAtTargetEpsilon), 0.0, 1.0); + const double speed = speed_cap * ramp; + + // Advance the setpoint by one control period, never stepping past the target. + const double step = std::min(speed * controller_.control_period, distance); + result.velocity = {dir_yaw * speed, dir_pitch * speed}; + result.position = limits_.clamp({current.yaw + dir_yaw * step, current.pitch + dir_pitch * step}); + } + result.valid = true; + + previous_target_ = result.target; + last_plan_time_ = input.now; + has_planned_ = true; + + return result; +} + +void ActiveVision::reset() { + world_.clear(); + if (coverage_) { + coverage_->reset(); + } + previous_target_.reset(); + has_planned_ = false; +} + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/src/active_vision_debug.cpp b/src/bitbots_motion/bitbots_head_mover/src/active_vision_debug.cpp new file mode 100644 index 0000000000..2a0f7bb4c7 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/active_vision_debug.cpp @@ -0,0 +1,228 @@ +#include +#include +#include +#include + +namespace bitbots_head_mover { + +namespace { + +/// Map a score in [0, 1] to a colour running from blue over green to red. +/// +/// Blue is a poor candidate, red is a good one. Using a full hue sweep rather +/// than a brightness ramp keeps the individual candidates distinguishable even +/// where many of them overlap. +cv::Scalar scoreColor(double normalized) { + const double clamped = std::clamp(normalized, 0.0, 1.0); + cv::Mat hsv(1, 1, CV_8UC3, cv::Scalar((1.0 - clamped) * 120.0, 255, 255)); + cv::Mat bgr; + cv::cvtColor(hsv, bgr, cv::COLOR_HSV2BGR); + const cv::Vec3b pixel = bgr.at(0, 0); + return cv::Scalar(pixel[0], pixel[1], pixel[2]); +} + +/// Normalize the candidate scores into [0, 1] for colouring. +/// +/// The absolute scores depend entirely on the configured weights, so they are +/// rescaled against the spread actually observed in this planning cycle. +std::vector normalizedScores(const ActiveVisionResult& result) { + std::vector normalized(result.scores.size(), 0.5); + if (result.scores.empty()) { + return normalized; + } + + double lowest = result.scores.front().total; + double highest = result.scores.front().total; + for (const auto& score : result.scores) { + lowest = std::min(lowest, score.total); + highest = std::max(highest, score.total); + } + + const double span = highest - lowest; + if (span <= 0.0) { + return normalized; + } + for (size_t i = 0; i < result.scores.size(); i++) { + normalized[i] = (result.scores[i].total - lowest) / span; + } + return normalized; +} + +/// Where the optical axis of a camera pose meets the ground plane. +/// +/// Returns false when the camera looks at or above the horizon, in which case +/// there is no ground intersection to draw. +bool groundIntersection(const Eigen::Isometry3d& camera_pose, Eigen::Vector3d& intersection) { + // In an optical frame the viewing direction is the z axis + const Eigen::Vector3d direction = camera_pose.linear().col(2); + const Eigen::Vector3d origin = camera_pose.translation(); + if (direction.z() >= -1e-6) { + return false; + } + const double distance = -origin.z() / direction.z(); + intersection = origin + distance * direction; + return true; +} + +} // namespace + +nav_msgs::msg::OccupancyGrid coverageGrid(const FieldCoverageMap& coverage, const std::string& frame_id, + const builtin_interfaces::msg::Time& stamp) { + nav_msgs::msg::OccupancyGrid grid; + grid.header.frame_id = frame_id; + grid.header.stamp = stamp; + + const FieldCoverageConfig& config = coverage.config(); + grid.info.resolution = config.cell_size; + grid.info.width = static_cast(coverage.cellsX()); + grid.info.height = static_cast(coverage.cellsY()); + + // The grid origin is the lower left corner of the lower left cell, while the + // stored centers sit in the middle of their cells + grid.info.origin.position.x = -(static_cast(coverage.cellsX()) * config.cell_size) / 2.0; + grid.info.origin.position.y = -(static_cast(coverage.cellsY()) * config.cell_size) / 2.0; + grid.info.origin.orientation.w = 1.0; + + grid.data.reserve(coverage.size()); + for (size_t index = 0; index < coverage.size(); index++) { + grid.data.push_back(static_cast(std::lround(coverage.interest(index) * 100.0))); + } + return grid; +} + +visualization_msgs::msg::MarkerArray candidateMarkers(const ActiveVisionResult& result, + const ActiveVision& active_vision, + const Eigen::Isometry3d& robot_pose, const std::string& frame_id, + const builtin_interfaces::msg::Time& stamp) { + visualization_msgs::msg::MarkerArray markers; + + // Clear whatever the previous cycle drew, otherwise candidates from a cycle + // with more samples would linger forever + visualization_msgs::msg::Marker clear; + clear.header.frame_id = frame_id; + clear.header.stamp = stamp; + clear.action = visualization_msgs::msg::Marker::DELETEALL; + markers.markers.push_back(clear); + + if (!result.valid || result.candidates.empty()) { + return markers; + } + + const std::vector normalized = normalizedScores(result); + const ActiveVisionScorer scorer(active_vision.kinematics(), active_vision.camera(), active_vision.world(), + active_vision.coverage(), robot_pose); + + // All candidates share one sphere list, so a single marker carries the whole + // sampled distribution instead of one marker per candidate + visualization_msgs::msg::Marker points; + points.header.frame_id = frame_id; + points.header.stamp = stamp; + points.ns = "active_vision_candidates"; + points.id = 0; + points.type = visualization_msgs::msg::Marker::SPHERE_LIST; + points.action = visualization_msgs::msg::Marker::ADD; + points.pose.orientation.w = 1.0; + points.scale.x = points.scale.y = points.scale.z = 0.08; + + for (size_t index = 0; index < result.candidates.size(); index++) { + const auto camera_pose = scorer.cameraPoseInMap(result.candidates[index].target, robot_pose); + Eigen::Vector3d point; + // A pose the kinematics could not resolve simply has no ground point to + // draw; the planner reports that failure itself + if (!camera_pose || !groundIntersection(*camera_pose, point)) { + continue; + } + geometry_msgs::msg::Point message_point; + message_point.x = point.x(); + message_point.y = point.y(); + message_point.z = point.z(); + points.points.push_back(message_point); + + const cv::Scalar color = scoreColor(normalized[index]); + std_msgs::msg::ColorRGBA rgba; + rgba.b = static_cast(color[0] / 255.0); + rgba.g = static_cast(color[1] / 255.0); + rgba.r = static_cast(color[2] / 255.0); + rgba.a = 0.6f; + points.colors.push_back(rgba); + } + + if (!points.points.empty()) { + markers.markers.push_back(points); + } + + // Draw the selected target on top as a larger, distinct sphere + if (result.selected < result.candidates.size()) { + const auto camera_pose = scorer.cameraPoseInMap(result.candidates[result.selected].target, robot_pose); + Eigen::Vector3d point; + if (camera_pose && groundIntersection(*camera_pose, point)) { + visualization_msgs::msg::Marker selected; + selected.header.frame_id = frame_id; + selected.header.stamp = stamp; + selected.ns = "active_vision_selected"; + selected.id = 0; + selected.type = visualization_msgs::msg::Marker::SPHERE; + selected.action = visualization_msgs::msg::Marker::ADD; + selected.pose.orientation.w = 1.0; + selected.pose.position.x = point.x(); + selected.pose.position.y = point.y(); + selected.pose.position.z = point.z(); + selected.scale.x = selected.scale.y = selected.scale.z = 0.2; + selected.color.r = selected.color.g = selected.color.b = 1.0f; + selected.color.a = 1.0f; + markers.markers.push_back(selected); + } + } + + return markers; +} + +cv::Mat jointSpaceDebugImage(const ActiveVisionResult& result, const HeadLimits& limits, int size) { + cv::Mat image(size, size, CV_8UC3, cv::Scalar(30, 30, 30)); + + const double yaw_span = limits.yaw.upper - limits.yaw.lower; + const double pitch_span = limits.pitch.upper - limits.pitch.lower; + if (!(yaw_span > 0.0) || !(pitch_span > 0.0)) { + return image; + } + + // Yaw runs left to right, pitch runs top to bottom, so looking up is at the + // top of the image the way one would expect + const auto toPixel = [&](const HeadPosition& position) { + const double x = (position.yaw - limits.yaw.lower) / yaw_span * (size - 1); + const double y = (position.pitch - limits.pitch.lower) / pitch_span * (size - 1); + return cv::Point(static_cast(std::lround(x)), static_cast(std::lround(y))); + }; + + // Axes through the zero position, so the neutral head pose is easy to find + cv::line(image, toPixel({0.0, limits.pitch.lower}), toPixel({0.0, limits.pitch.upper}), cv::Scalar(70, 70, 70), 1); + cv::line(image, toPixel({limits.yaw.lower, 0.0}), toPixel({limits.yaw.upper, 0.0}), cv::Scalar(70, 70, 70), 1); + + if (result.candidates.empty()) { + return image; + } + + const std::vector normalized = normalizedScores(result); + + // Draw the selected candidate last so it ends up on top of the others + std::vector order; + order.reserve(result.candidates.size()); + for (size_t index = 0; index < result.candidates.size(); index++) { + if (index != result.selected) { + order.push_back(index); + } + } + if (result.selected < result.candidates.size()) { + order.push_back(result.selected); + } + + for (size_t index : order) { + const bool selected = index == result.selected; + const cv::Scalar color = selected ? cv::Scalar(255, 255, 255) : scoreColor(normalized[index]); + cv::circle(image, toPixel(result.candidates[index].target), selected ? 5 : 2, color, cv::FILLED, cv::LINE_AA); + } + + return image; +} + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/src/active_vision_scorer.cpp b/src/bitbots_motion/bitbots_head_mover/src/active_vision_scorer.cpp new file mode 100644 index 0000000000..4da5082b63 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/active_vision_scorer.cpp @@ -0,0 +1,165 @@ +#include +#include +#include + +namespace bitbots_head_mover { + +double targetVisibility(const std::vector& targets, const Eigen::Isometry3d& map_to_camera, + const CameraModel& camera, const VisibilityWeighting& weighting) { + if (targets.empty()) { + return 0.0; + } + + double weighted_sum = 0.0; + double total_weight = 0.0; + double best_weight = 0.0; + for (const auto& target : targets) { + const double weight = std::max(target.weight, 0.0); + weighted_sum += weight * camera.visibility(map_to_camera * target.position, weighting); + total_weight += weight; + best_weight = std::max(best_weight, weight); + } + + if (!(total_weight > 0.0)) { + return 0.0; + } + + // How well the targets are framed, letting the more trustworthy ones dominate + const double framing = weighted_sum / total_weight; + // How much there is to be trusted in the first place. This has to be a + // separate factor: the weighted average alone normalizes the weights away, so + // a single very uncertain target would score exactly like a certain one. + return framing * best_weight; +} + +ActiveVisionScorer::ActiveVisionScorer(const HeadKinematics& kinematics, const CameraModel& camera, + const WorldModel& world, const FieldCoverageMap& coverage, + const Eigen::Isometry3d& robot_pose) + : kinematics_(kinematics), camera_(camera), world_(world), coverage_(coverage) { + // Prepare right away so a scorer is never in a state where scoring silently + // returns zero because the caller forgot to prime it + prepare(robot_pose); +} + +std::optional ActiveVisionScorer::cameraPoseInMap(const HeadPosition& position, + const Eigen::Isometry3d& robot_pose) const { + // The kinematics resolve the camera against the robot's root link, the robot + // pose puts that root link onto the field + const auto camera_in_root = kinematics_.cameraPose(position); + if (!camera_in_root) { + return std::nullopt; + } + return robot_pose * *camera_in_root; +} + +bool ActiveVisionScorer::recordObservation(FieldCoverageMap& coverage, const HeadPosition& position, + const Eigen::Isometry3d& robot_pose) const { + if (!camera_.valid()) { + return false; + } + + const auto camera_pose = cameraPoseInMap(position, robot_pose); + if (!camera_pose) { + return false; + } + + const Eigen::Isometry3d map_to_camera = camera_pose->inverse(); + const auto& centers = coverage.cellCenters(); + for (size_t index = 0; index < centers.size(); index++) { + const double quality = camera_.visibility(map_to_camera * centers[index], visibility_); + if (quality > 0.0) { + coverage.observe(index, quality); + } + } + return true; +} + +void ActiveVisionScorer::prepare(const Eigen::Isometry3d& robot_pose) { + team_balls_ = world_.teamBalls(); + filtered_ball_.clear(); + if (world_.filteredBall()) { + filtered_ball_.push_back(*world_.filteredBall()); + } + + // Resolve the distance falloff of every cell against where the robot stands. + // A cell twice as far away covers about a quarter of the image, so weighting + // by the inverse square of the distance cancels the head start that distant + // cells would otherwise have from sheer count. + const auto& centers = coverage_.cellCenters(); + const Eigen::Vector3d robot_position = robot_pose.translation(); + const double half_weight = coverage_distance_half_weight_ > 0.0 ? coverage_distance_half_weight_ : 1.0; + + cell_distance_weights_.resize(centers.size()); + available_interest_ = 0.0; + for (size_t index = 0; index < centers.size(); index++) { + // Ground distance, so the camera's height above the field does not make + // everything look uniformly far away + const double dx = centers[index].x() - robot_position.x(); + const double dy = centers[index].y() - robot_position.y(); + const double normalized = std::sqrt(dx * dx + dy * dy) / half_weight; + cell_distance_weights_[index] = 1.0 / (1.0 + normalized * normalized); + available_interest_ += coverage_.interest(index) * cell_distance_weights_[index]; + } +} + +ScoreBreakdown ActiveVisionScorer::score(const HeadPosition& target, const ScoringContext& context) const { + ScoreBreakdown breakdown; + if (!camera_.valid()) { + return breakdown; + } + + const auto camera_pose = cameraPoseInMap(target, context.robot_pose); + if (!camera_pose) { + // A pose the kinematics could not resolve leaves the candidate unscorable + // rather than merely unattractive, so it is reported as such + return ScoreBreakdown{}; + } + // Only the inverse is ever needed, so the forward pose is never formed + const Eigen::Isometry3d map_to_camera = camera_pose->inverse(); + + breakdown.filtered_ball = targetVisibility(filtered_ball_, map_to_camera, camera_, visibility_); + breakdown.raw_balls = targetVisibility(world_.rawBalls(), map_to_camera, camera_, visibility_); + breakdown.team_ball = targetVisibility(team_balls_, map_to_camera, camera_, visibility_); + breakdown.robots = targetVisibility(world_.robots(), map_to_camera, camera_, visibility_); + + // Walk the coverage grid and accumulate how much outstanding attention this + // view would satisfy. Cells off the field carry no interest, so aiming at + // them earns nothing; no separate penalty is needed, and unlike one it also + // covers aiming at the sky, where no cell projects at all. + const auto& centers = coverage_.cellCenters(); + double covered_interest = 0.0; + for (size_t index = 0; index < centers.size(); index++) { + const double interest = coverage_.interest(index); + if (interest <= 0.0) { + continue; + } + const double quality = camera_.visibility(map_to_camera * centers[index], visibility_); + if (quality <= 0.0) { + continue; + } + covered_interest += quality * interest * cell_distance_weights_[index]; + } + + if (available_interest_ > 0.0) { + // Numerator and denominator carry the same distance weighting, so the term + // is the fraction of the reachable outstanding attention this view + // satisfies rather than the raw ground area it happens to cover + breakdown.field_coverage = std::min(covered_interest / available_interest_, 1.0); + } + + // The joint space distance to the previous target. Left at zero on the first + // cycle, where there is nothing to stay close to. + if (context.previous_target) { + breakdown.smoothness_cost = + std::hypot(target.yaw - context.previous_target->yaw, target.pitch - context.previous_target->pitch); + } + + breakdown.total = weights_.filtered_ball * breakdown.filtered_ball + weights_.raw_balls * breakdown.raw_balls + + weights_.team_ball * breakdown.team_ball + weights_.field_coverage * breakdown.field_coverage + + weights_.robots * breakdown.robots - weights_.smoothness * breakdown.smoothness_cost; + breakdown.valid = true; + + return breakdown; +} + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/src/camera_model.cpp b/src/bitbots_motion/bitbots_head_mover/src/camera_model.cpp new file mode 100644 index 0000000000..ad3196bcc7 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/camera_model.cpp @@ -0,0 +1,103 @@ +#include +#include +#include +#include + +namespace bitbots_head_mover { + +CameraModel::CameraModel() : model_(std::make_unique()) {} +CameraModel::~CameraModel() = default; +CameraModel::CameraModel(CameraModel&&) noexcept = default; +CameraModel& CameraModel::operator=(CameraModel&&) noexcept = default; + +bool CameraModel::update(const sensor_msgs::msg::CameraInfo& info) { + // An uncalibrated driver publishes zeroed intrinsics, which would make every + // projection collapse onto the principal point + if (info.width == 0 || info.height == 0 || info.k[0] == 0.0 || info.k[4] == 0.0) { + return false; + } + + // The return value reports whether the intrinsics differ from the previous + // ones, not whether they were accepted, so it is deliberately ignored here. + // The validation above is what decides whether the model is usable. + model_->fromCameraInfo(info); + + // Cache the model's rectified parameters for the hot projection path. Reading + // them through the model's accessors keeps binning and a region of interest + // accounted for, which the raw message fields would not. + fx_ = model_->fx(); + fy_ = model_->fy(); + cx_ = model_->cx(); + cy_ = model_->cy(); + tx_ = model_->Tx(); + ty_ = model_->Ty(); + + // A rectified model without focal lengths would project everything onto the + // principal point, which the check above cannot catch for all message layouts + if (fx_ == 0.0 || fy_ == 0.0) { + valid_ = false; + return false; + } + + width_ = static_cast(info.width); + height_ = static_cast(info.height); + inverse_half_width_ = 2.0 / width_; + inverse_half_height_ = 2.0 / height_; + valid_ = true; + return true; +} + +std::optional CameraModel::project(const Eigen::Vector3d& point) const { + double u = 0.0; + double v = 0.0; + // Points at or behind the image plane have no meaningful projection + if (!valid_ || !projectRaw(point, u, v)) { + return std::nullopt; + } + + if (!std::isfinite(u) || !std::isfinite(v)) { + return std::nullopt; + } + + if (u < 0.0 || u > width_ || v < 0.0 || v > height_) { + return std::nullopt; + } + + return Eigen::Vector2d(u, v); +} + +double CameraModel::visibility(const Eigen::Vector3d& point, const VisibilityWeighting& weighting) const { + double u = 0.0; + double v = 0.0; + if (!valid_ || !projectRaw(point, u, v)) { + return 0.0; + } + + // Offset from the image center, normalized so that the image border sits at + // one. Anything beyond that is off the image and therefore not visible, which + // also covers the non finite case. + const double offset_x = std::abs(u * inverse_half_width_ - 1.0); + const double offset_y = std::abs(v * inverse_half_height_ - 1.0); + // The larger of the two axes decides, so the perfectly framed region is a box + // rather than an ellipse and a point is only "centered" if it is centered in + // both directions + const double offset = std::max(offset_x, offset_y); + if (!(offset <= 1.0)) { + return 0.0; + } + + const double center_fraction = std::clamp(weighting.center_fraction, 0.0, 1.0); + if (offset <= center_fraction) { + return 1.0; + } + + // Fall off linearly from the edge of the central region to the image border. + // The central region covering the whole image would make this a zero division. + if (center_fraction >= 1.0) { + return 1.0; + } + const double falloff = (offset - center_fraction) / (1.0 - center_fraction); + return 1.0 + falloff * (weighting.border_score - 1.0); +} + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/src/field_coverage_map.cpp b/src/bitbots_motion/bitbots_head_mover/src/field_coverage_map.cpp new file mode 100644 index 0000000000..144a82346e --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/field_coverage_map.cpp @@ -0,0 +1,92 @@ +#include +#include +#include + +namespace bitbots_head_mover { + +FieldCoverageMap::FieldCoverageMap(const FieldCoverageConfig& config) : config_(config) { build(); } + +void FieldCoverageMap::build() { + centers_.clear(); + observations_.clear(); + out_of_field_.clear(); + + // A non positive cell size would produce an unbounded grid + if (!(config_.cell_size > 0.0)) { + cells_x_ = 0; + cells_y_ = 0; + return; + } + + const double extent_x = config_.field_length + 2.0 * config_.margin; + const double extent_y = config_.field_width + 2.0 * config_.margin; + + cells_x_ = static_cast(std::max(1.0, std::ceil(extent_x / config_.cell_size))); + cells_y_ = static_cast(std::max(1.0, std::ceil(extent_y / config_.cell_size))); + + centers_.reserve(cells_x_ * cells_y_); + observations_.assign(cells_x_ * cells_y_, 0.0); + out_of_field_.reserve(cells_x_ * cells_y_); + + // The map frame has its origin in the center of the field, so the grid is + // centered on the origin as well. The origin has to follow the rounded up cell + // count rather than the requested extent: taking it from the requested extent + // would push the grid off center by the rounding remainder whenever the extent + // is not a multiple of the cell size, and would also disagree with the origin + // the debug occupancy grid reports. + const double half_length = config_.field_length / 2.0; + const double half_width = config_.field_width / 2.0; + const double origin_x = -(static_cast(cells_x_) * config_.cell_size) / 2.0; + const double origin_y = -(static_cast(cells_y_) * config_.cell_size) / 2.0; + + for (size_t iy = 0; iy < cells_y_; iy++) { + for (size_t ix = 0; ix < cells_x_; ix++) { + const double x = origin_x + (static_cast(ix) + 0.5) * config_.cell_size; + const double y = origin_y + (static_cast(iy) + 0.5) * config_.cell_size; + centers_.emplace_back(x, y, 0.0); + out_of_field_.push_back(std::abs(x) > half_length || std::abs(y) > half_width); + } + } +} + +double FieldCoverageMap::interest(size_t index) const { + assert(index < observations_.size() && "coverage cell index out of range"); + // Looking off the field never gains us anything, so those cells carry no + // interest at all. They are still part of the grid because the scoring wants + // to know when a candidate is aimed at them. + if (out_of_field_[index]) { + return 0.0; + } + return 1.0 - observations_[index]; +} + +void FieldCoverageMap::decay(double dt) { + if (dt <= 0.0 || !(config_.half_life > 0.0)) { + return; + } + + // Exponential decay, so the configured half life is the time after which an + // observation counts for half as much + const double factor = std::exp(-dt * M_LN2 / config_.half_life); + for (double& observation : observations_) { + observation *= factor; + } +} + +void FieldCoverageMap::observe(size_t index, double quality) { + assert(index < observations_.size() && "coverage cell index out of range"); + // Seeing a cell badly must not erase the memory of having seen it well + observations_[index] = std::max(observations_[index], std::clamp(quality, 0.0, 1.0)); +} + +void FieldCoverageMap::reset() { std::fill(observations_.begin(), observations_.end(), 0.0); } + +double FieldCoverageMap::totalInterest() const { + double total = 0.0; + for (size_t index = 0; index < size(); index++) { + total += interest(index); + } + return total; +} + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/src/head_kinematics.cpp b/src/bitbots_motion/bitbots_head_mover/src/head_kinematics.cpp new file mode 100644 index 0000000000..bf0618f4d0 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/head_kinematics.cpp @@ -0,0 +1,112 @@ +#include + +#include +#include +#include +#include + +namespace bitbots_head_mover { + +namespace { + +/// Convert a KDL frame to an Eigen transform. +Eigen::Isometry3d toEigen(const KDL::Frame& frame) { + Eigen::Isometry3d result = Eigen::Isometry3d::Identity(); + for (int row = 0; row < 3; row++) { + for (int col = 0; col < 3; col++) { + result.linear()(row, col) = frame.M(row, col); + } + result.translation()(row) = frame.p(row); + } + return result; +} + +/// Read the position limits of a joint from a parsed robot description. +/// +/// Returns false if the joint does not exist or does not declare limits, which +/// is the case for continuous and fixed joints. +bool readJointLimit(const urdf::Model& model, const std::string& joint_name, JointLimit& limit) { + const auto joint = model.getJoint(joint_name); + if (!joint || !joint->limits) { + return false; + } + limit.lower = joint->limits->lower; + limit.upper = joint->limits->upper; + return true; +} + +} // namespace + +std::unique_ptr HeadKinematics::fromUrdf(const std::string& urdf, const HeadChainConfig& config) { + urdf::Model model; + if (!model.initString(urdf)) { + return nullptr; + } + + KDL::Tree tree; + if (!kdl_parser::treeFromUrdfModel(model, tree)) { + return nullptr; + } + + // std::unique_ptr cannot use make_unique here because the constructor is private + auto kinematics = std::unique_ptr(new HeadKinematics()); + + if (!tree.getChain(config.root_link, config.tip_link, kinematics->chain_)) { + return nullptr; + } + + // Locate the head joints among the movable joints of the chain. Fixed joints + // do not get an entry in the joint array the solver is fed with, so the + // indices have to be counted rather than derived from the segment order. + bool found_yaw = false; + bool found_pitch = false; + unsigned int movable_index = 0; + for (const auto& segment : kinematics->chain_.segments) { + const KDL::Joint& joint = segment.getJoint(); + if (joint.getType() == KDL::Joint::None) { + continue; + } + if (joint.getName() == config.yaw_joint) { + kinematics->yaw_index_ = movable_index; + found_yaw = true; + } else if (joint.getName() == config.pitch_joint) { + kinematics->pitch_index_ = movable_index; + found_pitch = true; + } + movable_index++; + } + + // Anything else in the chain would move the camera without us knowing about + // it, which would silently invalidate every sampled camera pose + if (!found_yaw || !found_pitch || movable_index != 2) { + return nullptr; + } + + if (!readJointLimit(model, config.yaw_joint, kinematics->urdf_limits_.yaw) || + !readJointLimit(model, config.pitch_joint, kinematics->urdf_limits_.pitch)) { + return nullptr; + } + + // The solver refers to the chain, so it has to be built after the chain + // reached its final location inside the object + kinematics->solver_ = std::make_unique(kinematics->chain_); + kinematics->joints_.resize(kinematics->chain_.getNrOfJoints()); + + return kinematics; +} + +std::optional HeadKinematics::cameraPose(const HeadPosition& position) const { + joints_(yaw_index_) = position.yaw; + joints_(pitch_index_) = position.pitch; + + KDL::Frame frame; + if (solver_->JntToCart(joints_, frame) < 0) { + // Report the failure instead of substituting a pose. Scoring a made up + // camera pose would look exactly like scoring a real one. + return std::nullopt; + } + + return toEigen(frame) * calibration_; +} + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/src/head_trajectory.cpp b/src/bitbots_motion/bitbots_head_mover/src/head_trajectory.cpp new file mode 100644 index 0000000000..b69a94661d --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/head_trajectory.cpp @@ -0,0 +1,147 @@ +#include +#include +#include + +namespace bitbots_head_mover { + +namespace { +/// Factor to convert an angle in degrees to radians. +constexpr double kDegToRad = M_PI / 180.0; +} // namespace + +void HeadTrajectory::addPoint(double time, const HeadPosition& position, const HeadVelocity& velocity, + const HeadAcceleration& acceleration) { + yaw_.addPoint(time, position.yaw, velocity.yaw, acceleration.yaw); + pitch_.addPoint(time, position.pitch, velocity.pitch, acceleration.pitch); + duration_ = time; + size_++; +} + +void HeadTrajectory::finalize() { + // A single waypoint does not describe a movement, so there is nothing to interpolate + if (size_ < 2) { + valid_ = false; + return; + } + yaw_.computeSplines(); + pitch_.computeSplines(); + valid_ = true; +} + +HeadPosition HeadTrajectory::position(double time) const { + if (!valid_) { + return {}; + } + return {yaw_.pos(time), pitch_.pos(time)}; +} + +HeadVelocity HeadTrajectory::velocity(double time) const { + if (!valid_) { + return {}; + } + // Signed velocity, so callers can tell the direction of travel. Motor goals + // take the absolute value because they carry a speed rather than a velocity. + return {yaw_.vel(time), pitch_.vel(time)}; +} + +HeadAcceleration HeadTrajectory::acceleration(double time) const { + if (!valid_) { + return {}; + } + return {yaw_.acc(time), pitch_.acc(time)}; +} + +double SearchPatternTrajectory::phase(double elapsed) const { + if (!valid()) { + return 0.0; + } + // Play the transition from the previous head position once, then loop only + // over the cyclic part of the trajectory + if (elapsed <= transition_duration) { + return elapsed; + } + return transition_duration + std::fmod(elapsed - transition_duration, cycle_duration); +} + +SearchPatternTrajectory buildSearchPatternTrajectory(const std::vector& pattern_deg, double cycle_time, + const HeadPosition& start, double transition_speed) { + SearchPatternTrajectory result; + + if (pattern_deg.empty() || cycle_time <= 0.0 || transition_speed <= 0.0) { + return result; + } + + // Convert all keypoints to radians and close the loop + std::vector pts_rad; + pts_rad.reserve(pattern_deg.size() + 1); + for (const auto& keyframe : pattern_deg) { + pts_rad.push_back({keyframe.yaw * kDegToRad, keyframe.pitch * kDegToRad}); + } + pts_rad.push_back(pts_rad[0]); + + // Compute segment arc lengths and total + std::vector lengths; + lengths.reserve(pts_rad.size() - 1); + double total_length = 0.0; + for (size_t i = 1; i < pts_rad.size(); i++) { + double dyaw = pts_rad[i].yaw - pts_rad[i - 1].yaw; + double dpitch = pts_rad[i].pitch - pts_rad[i - 1].pitch; + double len = std::sqrt(dyaw * dyaw + dpitch * dpitch); + lengths.push_back(len); + total_length += len; + } + + double t = 0.0; + + // Prepend a transition segment from the current head position to the first waypoint + // of the pattern, so we don't jump there abruptly when the head mode changes. + // Its duration is based on the distance and the configured transition speed. + double transition_distance = + std::sqrt(std::pow(pts_rad[0].yaw - start.yaw, 2) + std::pow(pts_rad[0].pitch - start.pitch, 2)); + result.transition_duration = transition_distance / transition_speed; + if (result.transition_duration > 0.0) { + result.trajectory.addPoint(t, start); + t = result.transition_duration; + } + + // Add waypoints with timestamps proportional to arc length + result.trajectory.addPoint(t, pts_rad[0]); + for (size_t i = 0; i < lengths.size(); i++) { + double fraction = (total_length > 0.0) ? lengths[i] / total_length : 1.0 / static_cast(lengths.size()); + t += fraction * cycle_time; + result.trajectory.addPoint(t, pts_rad[i + 1]); + } + + result.cycle_duration = cycle_time; + result.trajectory.finalize(); + return result; +} + +double calculateLowerSpeed(double delta_faster_joint, double delta_joint, double speed) { + double estimated_time = delta_faster_joint / speed; + if (estimated_time != 0) { + return delta_joint / estimated_time; + } else { + return 0; + } +} + +HeadVelocity adjustSpeeds(const HeadPosition& goal, const HeadPosition& current, const HeadVelocity& max_speeds) { + // Calculate the delta between the current and the goal positions + double delta_yaw = std::abs(goal.yaw - current.yaw); + double delta_pitch = std::abs(goal.pitch - current.pitch); + + HeadVelocity speeds = max_speeds; + // Check which axis has to move further and adjust the speed of the other axis so both reach the goal at the same + // time + if (delta_yaw > delta_pitch) { + // Slow down the pitch axis to match the time it takes for the yaw axis to reach the goal + speeds.pitch = std::min(max_speeds.pitch, calculateLowerSpeed(delta_yaw, delta_pitch, max_speeds.yaw)); + } else { + // Slow down the yaw axis to match the time it takes for the pitch axis to reach the goal + speeds.yaw = std::min(max_speeds.yaw, calculateLowerSpeed(delta_pitch, delta_yaw, max_speeds.pitch)); + } + return speeds; +} + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/src/look_at.cpp b/src/bitbots_motion/bitbots_head_mover/src/look_at.cpp new file mode 100644 index 0000000000..0effe3b57a --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/look_at.cpp @@ -0,0 +1,20 @@ +#include +#include + +namespace bitbots_head_mover { + +HeadPosition motorGoalsFromPoint(const geometry_msgs::msg::Point& head_yaw_point, + const geometry_msgs::msg::Point& head_pitch_point, const HeadPosition& current, + double camera_pitch_offset) { + // The yaw joint only has to turn towards the point's direction in the ground plane + double rel_head_yaw = std::atan2(head_yaw_point.y, head_yaw_point.x); + + // The pitch joint has to tilt by the point's elevation over the joint's plane + double rel_head_pitch = -std::atan2( + head_pitch_point.z, std::sqrt(head_pitch_point.x * head_pitch_point.x + head_pitch_point.y * head_pitch_point.y)); + + // Both angles are relative to the current head position + return {rel_head_yaw + current.yaw, rel_head_pitch + (current.pitch - camera_pitch_offset)}; +} + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/src/move_head.cpp b/src/bitbots_motion/bitbots_head_mover/src/move_head.cpp index 8df2cad4c1..7ab17f91bb 100644 --- a/src/bitbots_motion/bitbots_head_mover/src/move_head.cpp +++ b/src/bitbots_motion/bitbots_head_mover/src/move_head.cpp @@ -1,37 +1,59 @@ #include #include +#include +#include #include +#include +#include +#include +#include #include #include #include -#include +#include +#include #include #include +#include #include #include -#include #include +#include +#include #include #include #include #include #include #include +#include +#include #include +#include +#include #include #include #include +#include #include +#include #include +#include using std::placeholders::_1; using namespace std::chrono_literals; namespace move_head { -#define DEG_TO_RAD M_PI / 180 -#define RAD_CAMERA_ANGLE DEG_TO_RAD * 30 +using bitbots_head_mover::HeadLimits; +using bitbots_head_mover::HeadPosition; +using bitbots_head_mover::HeadVelocity; + +/// Angular step size used when checking a path for collisions. +constexpr double kCollisionCheckStep = M_PI / 180.0 * 3.0; +/// Pitch offset applied when retrying a colliding goal further up. +constexpr double kCollisionAvoidancePitchStep = M_PI / 180.0 * 10.0; using LookAtGoal = bitbots_msgs::action::LookAt; using LookAtGoalHandle = rclcpp_action::ServerGoalHandle; @@ -62,9 +84,11 @@ class HeadMover { // Declare timer that executes the main loop rclcpp::TimerBase::SharedPtr timer_; + // Retries fetching the field dimensions until the parameter blackboard answers + rclcpp::TimerBase::SharedPtr field_dimension_retry_timer_; // Declare variable for the current search pattern - std::vector> pattern_; + std::vector pattern_; // Store previous head mode uint prev_head_mode_ = -1; @@ -72,13 +96,8 @@ class HeadMover { double cycle_time_ = 0.0; // Spline trajectory for search patterns - bitbots_splines::SmoothSpline yaw_spline_; - bitbots_splines::SmoothSpline pitch_spline_; - double spline_duration_ = 0.0; - // Duration of the transition segment from the current head position into the pattern (prepended to the cycle) - double transition_duration_ = 0.0; + bitbots_head_mover::SearchPatternTrajectory search_trajectory_; rclcpp::Time spline_start_time_; - bool spline_valid_ = false; // World model state geometry_msgs::msg::PoseWithCovarianceStamped ball_position_; @@ -87,8 +106,43 @@ class HeadMover { rclcpp_action::Server::SharedPtr action_server_; bool action_running_ = false; + // Active vision planner and everything that feeds it + bitbots_head_mover::ActiveVision active_vision_; + rclcpp::Subscription::SharedPtr robot_description_subscriber_; + rclcpp::Subscription::SharedPtr camera_info_subscriber_; + rclcpp::Subscription::SharedPtr balls_subscriber_; + rclcpp::Subscription::SharedPtr robots_subscriber_; + rclcpp::Subscription::SharedPtr team_data_subscriber_; + + // The detection callbacks do blocking tf lookups. They run on their own + // callback group, spun by a second executor on a dedicated thread, so a lookup + // that waits for a not-yet-available transform never stalls the main executor + // and the search pattern timer it drives. + rclcpp::CallbackGroup::SharedPtr detection_callback_group_; + // Guards the state the detection callbacks write and the main loop reads: the + // world model and the latest filtered ball. Held only around those accesses, + // never across a tf lookup, so the two threads never serialize on the wait. + std::mutex world_mutex_; + + // Debug publishers, only created when the debug output is enabled + rclcpp::Publisher::SharedPtr coverage_publisher_; + rclcpp::Publisher::SharedPtr candidate_publisher_; + rclcpp::Publisher::SharedPtr joint_space_publisher_; + bool debug_publishers_created_ = false; + + // The last position commanded by the active vision mode, which is what the + // head holds on to while an input is missing + std::optional active_vision_hold_position_; + public: HeadMover() : node_(std::make_shared("head_mover")) { + // The detection callbacks block on tf lookups, so they get their own callback + // group that a second executor spins on a dedicated thread. auto_add=false + // keeps the main executor from also picking it up. + detection_callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::SubscriptionOptions detection_options; + detection_options.callback_group = detection_callback_group_; + // Initialize publisher for head motor goals position_publisher_ = node_->create_publisher("head_motor_goals", 10); @@ -107,13 +161,19 @@ class HeadMover { current_joint_state_ = *msg; }); - // Initialize subscriber for the ball filter + // Initialize subscriber for the ball filter. It does a blocking tf lookup and + // writes shared state, so it belongs on the detection callback group too. ball_filter_subscriber_ = node_->create_subscription( "ball_position_relative_filtered", 10, [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - // cppcheck-suppress useInitializationList - ball_position_ = *msg; - }); + { + std::lock_guard lock(world_mutex_); + // cppcheck-suppress useInitializationList + ball_position_ = *msg; + } + handle_filtered_ball(*msg); + }, + detection_options); // Initialize with a valid frame ball_position_.header.frame_id = "base_footprint"; @@ -132,10 +192,207 @@ class HeadMover { std::bind(&HeadMover::handle_cancel, this, std::placeholders::_1), std::bind(&HeadMover::handle_accepted, this, std::placeholders::_1)); + setup_active_vision(); + // Initialize timer for main loop timer_ = rclcpp::create_timer(node_, node_->get_clock(), 50ms, [this] { behave(); }); } + /** + * @brief Sets up the inputs of the active vision head mode + */ + void setup_active_vision() { + // The robot description is latched by the robot state publisher, so a + // transient local subscription still receives it when we start later + robot_description_subscriber_ = node_->create_subscription( + "/robot_description", rclcpp::QoS(1).transient_local().reliable(), + [this](const std_msgs::msg::String::SharedPtr msg) { handle_robot_description(msg->data); }); + + camera_info_subscriber_ = node_->create_subscription( + "/zed/zed_node/rgb/camera_info", 1, [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { + if (!active_vision_.setCameraInfo(*msg)) { + RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, + "Received camera info without usable intrinsics, active vision stays disabled"); + } + }); + + // The detection callbacks do blocking tf lookups, so they run on the + // dedicated callback group rather than the main executor. + rclcpp::SubscriptionOptions detection_options; + detection_options.callback_group = detection_callback_group_; + + balls_subscriber_ = node_->create_subscription( + "balls_relative", 1, [this](const soccer_vision_3d_msgs::msg::BallArray::SharedPtr msg) { handle_balls(*msg); }, + detection_options); + + robots_subscriber_ = node_->create_subscription( + "robots_relative", 1, + [this](const soccer_vision_3d_msgs::msg::RobotArray::SharedPtr msg) { handle_robots(*msg); }, + detection_options); + + team_data_subscriber_ = node_->create_subscription( + "team_data", 10, [this](const bitbots_msgs::msg::TeamData::SharedPtr msg) { handle_team_data(*msg); }, + detection_options); + + apply_active_vision_parameters(); + + // Try once at startup, and otherwise keep retrying until it works. Giving up + // after a single attempt would disable the mode for the rest of the run + // because of a startup race with the parameter blackboard. + // + // The retry lives on its own slow timer rather than in the control loop: the + // parameter call blocks for up to a second, which would stall the 20 Hz loop + // on every tick for as long as the blackboard is unreachable. + if (!try_fetch_field_dimensions()) { + field_dimension_retry_timer_ = rclcpp::create_timer(node_, node_->get_clock(), 2s, [this] { + if (try_fetch_field_dimensions()) { + field_dimension_retry_timer_->cancel(); + } + }); + } + } + + /** + * @brief Pulls the field dimensions from the global parameter server + * + * They are not part of this node's schema, they live on the parameter + * blackboard the way the localization reads them as well. + */ + bool try_fetch_field_dimensions() { + try { + auto global_params = bitbots_utils::get_parameters_from_other_node(node_, "/parameter_blackboard", + {"field.size.x", "field.size.y"}, 1s); + bitbots_head_mover::FieldCoverageConfig coverage; + coverage.field_length = global_params.at("field.size.x").as_double(); + coverage.field_width = global_params.at("field.size.y").as_double(); + + // A degenerate field would build an empty or nonsensical coverage map, + // which is worth saying out loud rather than quietly steering the head + if (!(coverage.field_length > 0.0) || !(coverage.field_width > 0.0)) { + RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 10000, + "The parameter blackboard reports a field of %.2f x %.2f m, which is not usable", + coverage.field_length, coverage.field_width); + return false; + } + + coverage.margin = params_.active_vision.coverage.margin; + coverage.cell_size = params_.active_vision.coverage.cell_size; + coverage.half_life = params_.active_vision.coverage.half_life; + active_vision_.setFieldCoverageConfig(coverage); + RCLCPP_INFO(node_->get_logger(), "Active vision uses a %.2f x %.2f m field", coverage.field_length, + coverage.field_width); + return true; + } catch (const std::exception& ex) { + RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 10000, + "Could not get the field dimensions from the parameter blackboard, active vision cannot " + "run yet: %s", + ex.what()); + return false; + } + } + + /** + * @brief Builds the head chain from a received robot description + */ + void handle_robot_description(const std::string& urdf) { + bitbots_head_mover::HeadChainConfig chain; + chain.root_link = params_.active_vision.root_link; + chain.tip_link = params_.active_vision.tip_link; + + if (!active_vision_.setRobotDescription(urdf, chain)) { + RCLCPP_ERROR(node_->get_logger(), + "Could not build the head chain from '%s' to '%s', active vision stays disabled", + chain.root_link.c_str(), chain.tip_link.c_str()); + return; + } + RCLCPP_INFO(node_->get_logger(), "Built the head chain from '%s' to '%s'", chain.root_link.c_str(), + chain.tip_link.c_str()); + + // The extrinsic calibration is published as its own transform rather than + // being part of the robot description, so it has to be composed onto the + // chain's tip. It does not depend on the head position, so one lookup is + // enough, but it is only available once that publisher is up. + update_camera_calibration(); + } + + /** + * @brief Looks up the extrinsic camera calibration and hands it to the planner + */ + bool update_camera_calibration() { + const std::string& uncalibrated = params_.active_vision.tip_link; + const std::string& calibrated = params_.active_vision.calibrated_optical_frame; + if (uncalibrated == calibrated) { + return true; + } + + try { + const auto transform = + tf_buffer_->lookupTransform(uncalibrated, calibrated, tf2::TimePointZero, tf2::durationFromSec(0.1)); + active_vision_.setCameraCalibration(tf2::transformToEigen(transform)); + return true; + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, + "Could not look up the extrinsic camera calibration from '%s' to '%s': %s", + uncalibrated.c_str(), calibrated.c_str(), ex.what()); + return false; + } + } + + /** + * @brief Pushes the current parameters into the active vision planner + */ + void apply_active_vision_parameters() { + const auto& config = params_.active_vision; + + bitbots_head_mover::SamplerConfig sampler; + sampler.sample_count = static_cast(config.sampling.sample_count); + sampler.last_target_weight = config.sampling.last_target_weight; + sampler.current_position_weight = config.sampling.current_position_weight; + sampler.uniform_weight = config.sampling.uniform_weight; + sampler.last_target_std = config.sampling.last_target_std; + sampler.current_position_std = config.sampling.current_position_std; + active_vision_.setSamplerConfig(sampler); + + bitbots_head_mover::HeadController controller; + controller.approach_distance = config.controller.approach_distance; + controller.control_period = config.controller.control_period; + controller.max_velocity = {config.max_velocity_yaw, config.max_velocity_pitch}; + active_vision_.setController(controller); + + active_vision_.setHeadLimits(get_head_limits()); + active_vision_.setVisibilityWeighting({config.visibility.center_fraction, config.visibility.border_score}); + active_vision_.setCoverageDistanceHalfWeight(config.coverage.distance_half_weight); + + bitbots_head_mover::WorldModelConfig world; + world.filtered_ball_timeout = config.timeouts.filtered_ball; + world.raw_ball_timeout = config.timeouts.raw_ball; + world.team_ball_timeout = config.timeouts.team_ball; + world.robot_timeout = config.timeouts.robot; + world.covariance_half_weight = config.covariance_half_weight; + { + // Applied from the main loop on a parameter change, while the detection + // thread may be writing detections into the same world model + std::lock_guard lock(world_mutex_); + active_vision_.setWorldModelConfig(world); + } + + bitbots_head_mover::ScoringWeights weights; + weights.filtered_ball = config.weights.filtered_ball; + weights.raw_balls = config.weights.raw_balls; + weights.team_ball = config.weights.team_ball; + weights.field_coverage = config.weights.field_coverage; + weights.robots = config.weights.robots; + weights.smoothness = config.weights.smoothness; + active_vision_.setScoringWeights(weights); + } + + /** + * @brief Returns the head joint limits as defined in the parameters + */ + HeadLimits get_head_limits() const { + return {{params_.max_yaw[0], params_.max_yaw[1]}, {params_.max_pitch[0], params_.max_pitch[1]}}; + } + /*** * @brief Handles the goal request for the look at action * @@ -166,21 +423,21 @@ class HeadMover { return rclcpp_action::GoalResponse::REJECT; } - // RCLCPP_DEBUG(node_->get_logger(), "yaw point, pitch point" << head_yaw_point.point << " " << - // head_pitch_point.point); + // The goal is computed relative to where the head is, so an unknown head + // position means the goal cannot be judged and the action has to be rejected + const auto current = require_head_position("a look at goal request"); + if (!current) { + return rclcpp_action::GoalResponse::REJECT; + } // Get the motor goals that are needed to look at the point - double goal_yaw = 0.0; - double goal_pitch = 0.0; - get_motor_goals_from_point(rel_head_yaw_point.point, rel_head_pitch_point.point, goal_yaw, goal_pitch); + HeadPosition goal_position = + bitbots_head_mover::motorGoalsFromPoint(rel_head_yaw_point.point, rel_head_pitch_point.point, *current); - // Check whether the goal is in range yaw and pitch wise - bool goal_not_in_range = check_head_collision(goal_yaw, goal_pitch); - - // Check whether the action goal is valid and can be executed - // cppcheck-suppress knownConditionTrueFalse - if (action_running_ || goal_not_in_range || !(params_.max_yaw[0] < goal_yaw && goal_yaw < params_.max_yaw[1]) || - !(params_.max_pitch[0] < goal_pitch && goal_pitch < params_.max_pitch[1])) { + // Check whether the action goal is valid and can be executed. The head + // limits are checked twice by the original implementation, once as a + // collision check and once directly, which is equivalent to a single check. + if (action_running_ || !get_head_limits().contains(goal_position)) { return rclcpp_action::GoalResponse::REJECT; } return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; @@ -261,45 +518,24 @@ class HeadMover { action_running_ = false; } - /** - * @brief Slows down the speed of the joint that needs to travel less distance so both joints reach the goal at the - * same time - * - * @param delta_faster_joint The delta of the joint that needs to travel less distance and therefore reaches the goal - * faster - * @param delta_joint The delta of the joint that needs to travel more distance and therefore reaches the goal slower - * @param speed The maximum speed of the faster joint (the joint that needs to travel less distance) - * @return double The adjusted speed of the faster joint - */ - double calculate_lower_speed(double delta_faster_joint, double delta_joint, double speed) { - double estimated_time = delta_faster_joint / speed; - if (estimated_time != 0) { - return delta_joint / estimated_time; - } else { - return 0; - } - } - /** * @brief Send the goal positions to the head motors, but resolve collisions with the body if necessary. * */ - bool send_motor_goals(double yaw_position, double pitch_position, bool resolve_collision, double yaw_speed = 1.5, - double pitch_speed = 1.5, double current_yaw_position = 0.0, - double current_pitch_position = 0.0, bool clip = true) { + bool send_motor_goals(HeadPosition goal, bool resolve_collision, const HeadVelocity& speeds = {1.5, 1.5}, + const HeadPosition& current = {}, bool clip = true) { // Debug log the target yaw and pitch position - RCLCPP_DEBUG_STREAM(node_->get_logger(), "target yaw/pitch: " << yaw_position << "/" << pitch_position); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "target yaw/pitch: " << goal.yaw << "/" << goal.pitch); // Clip the target yaw and pitch position at the maximum yaw and pitch values as defined in the parameters if (clip) { - pre_clip(yaw_position, pitch_position); + goal = get_head_limits().clamp(goal); } // Resolve collisions if necessary if (resolve_collision) { // Call behavior that resolves collisions and might change the target yaw and pitch position - bool success = avoid_collision_on_path(yaw_position, pitch_position, current_yaw_position, current_pitch_position, - yaw_speed, pitch_speed); + bool success = avoid_collision_on_path(goal, current, speeds); // Report error message of we were not able to move to an alternative collision free position if (!success) { RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, @@ -308,101 +544,67 @@ class HeadMover { return success; } else { // Move the head to the target position but adjust the speed of the joints so both reach the goal at the same time - move_head_to_position_with_speed_adjustment(yaw_position, pitch_position, current_yaw_position, - current_pitch_position, yaw_speed, pitch_speed); + move_head_to_position_with_speed_adjustment(goal, current, speeds); return true; } } - /** - * @brief Applies clipping to the yaw and pitch values based on the loaded config parameters - * - */ - void pre_clip(double& yaw, double& pitch) { - yaw = std::clamp(yaw, params_.max_yaw[0], params_.max_yaw[1]); - pitch = std::clamp(pitch, params_.max_pitch[0], params_.max_pitch[1]); - } - /** * @brief Tries to move the head to the target position but resolves collisions with the body if necessary. * */ - bool avoid_collision_on_path(double goal_yaw, double goal_pitch, double current_yaw, double current_pitch, - double yaw_speed, double pitch_speed, int max_depth = 4, int depth = 0) { + bool avoid_collision_on_path(HeadPosition goal, const HeadPosition& current, const HeadVelocity& speeds, + int max_depth = 4, int depth = 0) { // Check if we reached the maximum depth of the recursion and if so, return false if (depth > max_depth) { return false; } // Calculate the distance between the current and the goal position - double distance = sqrt(pow(goal_yaw - current_yaw, 2) + pow(goal_pitch - current_pitch, 2)); + double distance = std::sqrt(std::pow(goal.yaw - current.yaw, 2) + std::pow(goal.pitch - current.pitch, 2)); // Calculate the number of steps we need to take to reach the goal position - // This assumes that we move 3 degrees per step - int step_count = distance / (3 * DEG_TO_RAD); - - // Calculate path by performing linear interpolation between the current and the goal position - std::vector> yaw_and_pitch_steps; - for (int i = 0; i < step_count; i++) { - yaw_and_pitch_steps.push_back({current_yaw + (goal_yaw - current_yaw) / step_count * i, - current_pitch + (goal_pitch - current_pitch) / step_count * i}); - } + int step_count = distance / kCollisionCheckStep; - // Check if we have collisions on our path + // Check if we have collisions on our path by performing linear interpolation + // between the current and the goal position + const HeadLimits limits = get_head_limits(); for (int i = 0; i < step_count; i++) { - // cppcheck-suppress knownConditionTrueFalse - if (check_head_collision(yaw_and_pitch_steps[i].first, yaw_and_pitch_steps[i].second)) { + HeadPosition step = {current.yaw + (goal.yaw - current.yaw) / step_count * i, + current.pitch + (goal.pitch - current.pitch) / step_count * i}; + if (!limits.contains(step)) { // If we have a collision, try to move the head to an alternative position - // The new position looks 10 degrees further up and is less likely to have a collision with the body + // The new position looks further up and is less likely to have a collision with the body // Also increase the depth of the recursion as this is a new attempt to move the head to the goal position - return avoid_collision_on_path(goal_yaw, goal_pitch + 10 * DEG_TO_RAD, current_yaw, current_pitch, yaw_speed, - pitch_speed, max_depth, depth + 1); + goal.pitch += kCollisionAvoidancePitchStep; + return avoid_collision_on_path(goal, current, speeds, max_depth, depth + 1); } } // We do not have any collisions on our path, so we can move the head to the goal position - move_head_to_position_with_speed_adjustment(goal_yaw, goal_pitch, current_yaw, current_pitch, yaw_speed, - pitch_speed); + move_head_to_position_with_speed_adjustment(goal, current, speeds); return true; } /** - * @brief Checks if the head collides with the body at a given yaw and pitch position + * @brief Move the head to the target position but adjust the speed of the joints so both reach the goal at the same + * time */ - bool check_head_collision(double yaw, double pitch) { - // Checks whether head position is higher than torso. - if (params_.max_pitch[0] < pitch && pitch < params_.max_pitch[1] && params_.max_yaw[0] < yaw && - yaw < params_.max_yaw[1]) { - return false; - } - return true; + void move_head_to_position_with_speed_adjustment(const HeadPosition& goal, const HeadPosition& current, + const HeadVelocity& speeds) { + publish_motor_goals(goal, bitbots_head_mover::adjustSpeeds(goal, current, speeds)); } /** - * @brief Move the head to the target position but adjust the speed of the joints so both reach the goal at the same - * time + * @brief Publishes the given head position and joint speeds as motor goals */ - void move_head_to_position_with_speed_adjustment(double goal_yaw, double goal_pitch, double current_yaw, - double current_pitch, double yaw_speed, double pitch_speed) { - // Calculate the delta between the current and the goal positions - double delta_yaw = std::abs(goal_yaw - current_yaw); - double delta_pitch = std::abs(goal_pitch - current_pitch); - // Check which axis has to move further and adjust the speed of the other axis so both reach the goal at the same - // time - if (delta_yaw > delta_pitch) { - // Slow down the pitch axis to match the time it takes for the yaw axis to reach the goal - pitch_speed = std::min(pitch_speed, calculate_lower_speed(delta_yaw, delta_pitch, yaw_speed)); - } else { - // Slow down the yaw axis to match the time it takes for the pitch axis to reach the goal - yaw_speed = std::min(yaw_speed, calculate_lower_speed(delta_pitch, delta_yaw, pitch_speed)); - } - + void publish_motor_goals(const HeadPosition& goal, const HeadVelocity& speeds) { // Send the motor goals including the position, speed and acceleration bitbots_msgs::msg::JointCommand pos_msg; - pos_msg.header.stamp = rclcpp::Clock().now(); + pos_msg.header.stamp = node_->get_clock()->now(); pos_msg.joint_names = {"head_yaw_joint", "head_pitch_joint"}; - pos_msg.positions = {goal_yaw, goal_pitch}; - pos_msg.velocities = {yaw_speed, pitch_speed}; + pos_msg.positions = {goal.yaw, goal.pitch}; + pos_msg.velocities = {speeds.yaw, speeds.pitch}; pos_msg.accelerations = {params_.max_acceleration_yaw, params_.max_acceleration_pitch}; pos_msg.max_torques = {10, 10}; @@ -411,151 +613,55 @@ class HeadMover { /** * @brief Returns the current position of the head motors + * + * Returns nothing if the joint state does not carry both head joints. Falling + * back to zero here would be indistinguishable from the head genuinely looking + * straight ahead, and every goal computed relative to it would be wrong by + * however far the head actually is from center. */ - void get_head_position(double& head_yaw, double& head_pitch) { - head_yaw = 0.0; - head_pitch = 0.0; + std::optional get_head_position() const { + HeadPosition position; + bool found_yaw = false; + bool found_pitch = false; // Iterate over all joints and find the head yaw and pitch joints for (size_t i = 0; i < current_joint_state_->name.size(); i++) { - if (current_joint_state_->name[i] == "head_yaw_joint") { - head_yaw = current_joint_state_->position[i]; - } else if (current_joint_state_->name[i] == "head_pitch_joint") { - head_pitch = current_joint_state_->position[i]; + const bool is_yaw = current_joint_state_->name[i] == "head_yaw_joint"; + const bool is_pitch = current_joint_state_->name[i] == "head_pitch_joint"; + if (!is_yaw && !is_pitch) { + continue; } - } - } - - /** - * @brief Converts a scanline number to a pitch angle - */ - double lineAngle(int line, int line_count, double min_angle, double max_angle) { - // Get the angular delta that is covered by the scanlines in the pitch axis - double delta = std::abs(max_angle - min_angle); - // Calculate the angular step size between two scanlines - double steps = delta / (line_count - 1); - // Calculate the pitch angle of the given scanline - return steps * line + min_angle; - } - - /** - * @brief Performs a linear interpolation between the min and max yaw values and returns the interpolated steps - */ - std::vector> interpolatedSteps(int steps, double pitch, double min_yaw, double max_yaw) { - // Handle edge case where we do not need to interpolate - if (steps == 0) { - return {}; - } - // Add one to the step count as we need to include the min and max yaw values - steps += 1; - // Create a vector that stores the interpolated steps - std::vector> output_points; - // Calculate the delta between the min and max yaw values - double delta = std::abs(max_yaw - min_yaw); - // Calculate the step size between two interpolated steps - double step_size = delta / steps; - // Iterate over all steps and calculate the interpolated yaw values - for (int i = 1; i <= steps; i++) { - double yaw = min_yaw + step_size * i; - output_points.emplace_back(yaw, pitch); - } - return output_points; - } - - /** - * @brief Generates a parameterized search pattern - */ - std::vector> generatePattern(int line_count, double max_horizontal_angle_left, - double max_horizontal_angle_right, - double max_vertical_angle_up, double max_vertical_angle_down, - double reduce_last_scanline = 1.0, - int interpolation_steps = 0) { - // Store the keyframes of the search pattern - std::vector> keyframes; - // Store the state of the generation process - bool down_direction = true; // true = decreasing line (toward top), false = increasing line (toward bottom) - bool right_side = false; // true = right, false = left - bool right_direction = true; // true = moving right, false = moving left; alternates per scan line - int line = line_count - 1; - // Calculate the number of iterations that are needed to generate the search pattern - int iterations = std::max(line_count * 4 - 4, 2); - // Iterate over all iterations and generate the search pattern - for (int i = 0; i < iterations; i++) { - // Get the maximum yaw values (left and right) for the current yaw position - // Select the relevant one based on the current side we are on - double current_yaw; - if (right_side) { - current_yaw = max_horizontal_angle_right; - } else { - current_yaw = max_horizontal_angle_left; + // The parallel arrays are only required to be as long as the caller filled + // them, so a name without a matching position must not be indexed + if (i >= current_joint_state_->position.size()) { + return std::nullopt; } - - // Get the current pitch angle based on the current line we are on - double current_pitch = lineAngle(line, line_count, max_vertical_angle_up, max_vertical_angle_down); - - // Store the keyframe - keyframes.push_back({current_yaw, current_pitch}); - - // Check if we move horizontally or vertically in the pattern - if (right_side != right_direction) { - // We move horizontally, so we might need to interpolate between the current and the next keyframe - std::vector> interpolated_points = interpolatedSteps( - interpolation_steps, current_pitch, max_horizontal_angle_right, max_horizontal_angle_left); - // Reverse the order of the interpolated points if we are moving to the right - if (right_direction) { - std::reverse(interpolated_points.begin(), interpolated_points.end()); - } - // Add the interpolated points to the keyframes - keyframes.insert(keyframes.end(), interpolated_points.begin(), interpolated_points.end()); - // Change the direction we are moving in - right_side = right_direction; - + if (is_yaw) { + position.yaw = current_joint_state_->position[i]; + found_yaw = true; } else { - // Flip the scan direction so the next line scans the opposite way (boustrophedon) - right_direction = !right_direction; - // Advance to the next scan line - if (down_direction) { - line -= 1; - } else { - line += 1; - } - // Flip vertical direction when we reach either edge - if (line <= 0 || line >= line_count - 1) { - down_direction = !down_direction; - } + position.pitch = current_joint_state_->position[i]; + found_pitch = true; } } - - // Reduce the last scanline by a given factor - for (auto& keyframe : keyframes) { - if (std::abs(keyframe.second - max_vertical_angle_down) < 1e-6) { - keyframe = {keyframe.first * reduce_last_scanline, max_vertical_angle_down}; - } + if (!found_yaw || !found_pitch) { + return std::nullopt; } - return keyframes; + return position; } /** - * @brief Calculates the motor goals that are needed to look at a given point using the inverse kinematics + * @brief Returns the head position, reporting loudly if it is unavailable + * + * Used by the paths that cannot proceed without knowing where the head is. */ - void get_motor_goals_from_point(geometry_msgs::msg::Point head_yaw_point, geometry_msgs::msg::Point head_pitch_point, - double& head_yaw, double& head_pitch) { - double yaw_x = head_yaw_point.x; - double yaw_y = head_yaw_point.y; - - double rel_head_yaw = atan2(yaw_y, yaw_x); - - double pitch_x = head_pitch_point.x; - double pitch_y = head_pitch_point.y; - double pitch_z = head_pitch_point.z; - - double rel_head_pitch = -atan2(pitch_z, sqrt(pitch_x * pitch_x + pitch_y * pitch_y)); - - double current_yaw = 0.0; - double current_pitch = 0.0; - get_head_position(current_yaw, current_pitch); - - head_yaw = rel_head_yaw + current_yaw; - head_pitch = rel_head_pitch + (current_pitch - RAD_CAMERA_ANGLE); + std::optional require_head_position(const char* what) const { + const auto position = get_head_position(); + if (!position) { + RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, + "Joint states carry no usable head_yaw_joint and head_pitch_joint position, skipping %s", + what); + } + return position; } /** @@ -570,19 +676,21 @@ class HeadMover { geometry_msgs::msg::PointStamped rel_head_pitch_point = tf_buffer_->transform(point, "head_pitch_link", tf2::durationFromSec(0.9)); - // Get the motor goals that are needed to look at the point from the inverse kinematics - double goal_yaw = 0.0; - double goal_pitch = 0.0; - get_motor_goals_from_point(rel_head_yaw_point.point, rel_head_pitch_point.point, goal_yaw, goal_pitch); // Get the current head position - double current_yaw = 0.0; - double current_pitch = 0.0; - get_head_position(current_yaw, current_pitch); + const auto maybe_current = require_head_position("a look at update"); + if (!maybe_current) { + return false; + } + const HeadPosition current = *maybe_current; + + // Get the motor goals that are needed to look at the point from the inverse kinematics + HeadPosition goal = + bitbots_head_mover::motorGoalsFromPoint(rel_head_yaw_point.point, rel_head_pitch_point.point, current); // Check if we reached the goal position - if (std::abs(goal_yaw - current_yaw) > min_yaw_delta || std::abs(goal_pitch - current_pitch) > min_pitch_delta) { + if (std::abs(goal.yaw - current.yaw) > min_yaw_delta || std::abs(goal.pitch - current.pitch) > min_pitch_delta) { // Send the motor goals to the head motors - send_motor_goals(goal_yaw, goal_pitch, true, params_.look_at.yaw_speed, params_.look_at.pitch_speed); + send_motor_goals(goal, true, {params_.look_at.yaw_speed, params_.look_at.pitch_speed}); // Return false as we did not reach the goal position yet return false; } @@ -597,76 +705,27 @@ class HeadMover { } /** - * @brief Builds an open-loop SmoothSpline trajectory from the current search pattern. - * - * The trajectory starts at the current head position and smoothly transitions into the - * pattern, so switching head modes does not result in an abrupt movement. Waypoint - * timestamps are distributed proportionally to Euclidean arc length so the cycle - * (excluding the transition) takes exactly cycle_time_ seconds. The loop is closed by - * appending the first waypoint at the end. + * @brief Builds an open-loop trajectory from the current search pattern, starting at the + * current head position. */ void build_spline_trajectory() { - yaw_spline_ = bitbots_splines::SmoothSpline(); - pitch_spline_ = bitbots_splines::SmoothSpline(); - spline_valid_ = false; - transition_duration_ = 0.0; - - if (pattern_.empty() || cycle_time_ <= 0.0) { + // The trajectory transitions out of the current head position, so building + // it without knowing that position would start the pattern with a jump from + // wherever the head happens to be to the assumed center + const auto start = require_head_position("building a search pattern trajectory"); + if (!start) { + search_trajectory_ = {}; + return; + } + search_trajectory_ = + bitbots_head_mover::buildSearchPatternTrajectory(pattern_, cycle_time_, *start, params_.transition_speed); + if (!search_trajectory_.valid()) { + RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, + "Could not build a search pattern trajectory from %zu keyframes with a cycle time of %.2fs", + pattern_.size(), cycle_time_); return; } - - // Convert all keypoints to radians and close the loop - std::vector> pts_rad; - pts_rad.reserve(pattern_.size() + 1); - for (const auto& kf : pattern_) { - pts_rad.emplace_back(kf.first * DEG_TO_RAD, kf.second * DEG_TO_RAD); - } - pts_rad.push_back(pts_rad[0]); - - // Compute segment arc lengths and total - std::vector lengths; - lengths.reserve(pts_rad.size() - 1); - double total_length = 0.0; - for (size_t i = 1; i < pts_rad.size(); i++) { - double dyaw = pts_rad[i].first - pts_rad[i - 1].first; - double dpitch = pts_rad[i].second - pts_rad[i - 1].second; - double len = std::sqrt(dyaw * dyaw + dpitch * dpitch); - lengths.push_back(len); - total_length += len; - } - - double t = 0.0; - - // Prepend a transition segment from the current head position to the first waypoint - // of the pattern, so we don't jump there abruptly when the head mode changes. - // Its duration is based on the distance and the configured transition speed. - double current_yaw = 0.0; - double current_pitch = 0.0; - get_head_position(current_yaw, current_pitch); - double transition_distance = - std::sqrt(std::pow(pts_rad[0].first - current_yaw, 2) + std::pow(pts_rad[0].second - current_pitch, 2)); - transition_duration_ = transition_distance / params_.transition_speed; - if (transition_duration_ > 0.0) { - yaw_spline_.addPoint(t, current_yaw); - pitch_spline_.addPoint(t, current_pitch); - t = transition_duration_; - } - - // Add waypoints with timestamps proportional to arc length - yaw_spline_.addPoint(t, pts_rad[0].first); - pitch_spline_.addPoint(t, pts_rad[0].second); - for (size_t i = 0; i < lengths.size(); i++) { - double fraction = (total_length > 0.0) ? lengths[i] / total_length : 1.0 / static_cast(lengths.size()); - t += fraction * cycle_time_; - yaw_spline_.addPoint(t, pts_rad[i + 1].first); - pitch_spline_.addPoint(t, pts_rad[i + 1].second); - } - - spline_duration_ = cycle_time_; - yaw_spline_.computeSplines(); - pitch_spline_.computeSplines(); spline_start_time_ = node_->now(); - spline_valid_ = true; } /** @@ -674,35 +733,259 @@ class HeadMover { * the resulting joint position and velocity as open-loop motor goals. */ void perform_search_pattern() { - if (!spline_valid_ || spline_duration_ <= 0.0) { + if (!search_trajectory_.valid()) { return; } // Play the transition from the previous head position once, then loop only over the cyclic part of the trajectory - double elapsed = (node_->now() - spline_start_time_).seconds(); - double t; - if (elapsed <= transition_duration_) { - t = elapsed; - } else { - t = transition_duration_ + fmod(elapsed - transition_duration_, spline_duration_); + double t = search_trajectory_.phase((node_->now() - spline_start_time_).seconds()); + + HeadPosition goal = get_head_limits().clamp(search_trajectory_.trajectory.position(t)); + HeadVelocity velocity = search_trajectory_.trajectory.velocity(t); + + // The motor goals carry joint speeds, so the direction of travel is dropped here + publish_motor_goals(goal, {std::abs(velocity.yaw), std::abs(velocity.pitch)}); + } + + /** + * @brief Looks up the transform that brings a message's frame into the map frame + * + * A detection array shares one header, so the transform is looked up once per + * message and then applied to every point in it. Transforming each point on + * its own would repeat the same buffer lookup for every ball or robot in the + * message. + */ + bool lookup_to_map(const std_msgs::msg::Header& header, Eigen::Isometry3d& transform) { + try { + // This runs in the detection callbacks, which are spun on their own executor + // thread, so waiting a short while for a transform that is still on its way + // does not stall the main loop timer. The wait is not self defeating either: + // the transform listener fills the buffer from its own dedicated thread, so + // the awaited transform can still arrive while we block here. + transform = tf2::transformToEigen(tf_buffer_->lookupTransform(params_.active_vision.map_frame, header.frame_id, + header.stamp, tf2::durationFromSec(0.1))); + return true; + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, + "Could not transform a detection from '%s' into '%s': %s", header.frame_id.c_str(), + params_.active_vision.map_frame.c_str(), ex.what()); + return false; + } + } + + /** + * @brief Applies a transform to a point from a message + */ + static Eigen::Vector3d transform_point(const Eigen::Isometry3d& transform, const geometry_msgs::msg::Point& point) { + return transform * Eigen::Vector3d(point.x, point.y, point.z); + } + + /** + * @brief Stores the filtered ball estimate in the map frame + */ + void handle_filtered_ball(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { + Eigen::Isometry3d to_map; + if (!lookup_to_map(msg.header, to_map)) { + return; } + const Eigen::Vector3d position = transform_point(to_map, msg.pose.pose.position); + // The two planar variances are what says how well the filter knows the ball, + // taking the larger of them treats an estimate that is uncertain in any + // direction as uncertain + const double covariance = std::max(msg.pose.covariance[0], msg.pose.covariance[7]); + // Guard only the world write, never the lookup above, so the main loop is not + // serialized behind a blocking transform wait + std::lock_guard lock(world_mutex_); + active_vision_.world().setFilteredBall(position, covariance, rclcpp::Time(msg.header.stamp).seconds()); + } - double goal_yaw = yaw_spline_.pos(t); - double goal_pitch = pitch_spline_.pos(t); - pre_clip(goal_yaw, goal_pitch); + /** + * @brief Stores the raw ball detections in the map frame + */ + void handle_balls(const soccer_vision_3d_msgs::msg::BallArray& msg) { + Eigen::Isometry3d to_map; + if (!lookup_to_map(msg.header, to_map)) { + return; + } - double yaw_vel = std::abs(yaw_spline_.vel(t)); - double pitch_vel = std::abs(pitch_spline_.vel(t)); + const double stamp = rclcpp::Time(msg.header.stamp).seconds(); + std::vector balls; + balls.reserve(msg.balls.size()); + for (const auto& ball : msg.balls) { + // The detection confidence is what the raw detections are weighted by, + // they carry no covariance of their own + balls.push_back({transform_point(to_map, ball.center), static_cast(ball.confidence.confidence), stamp}); + } + std::lock_guard lock(world_mutex_); + active_vision_.world().setRawBalls(std::move(balls)); + } - bitbots_msgs::msg::JointCommand pos_msg; - pos_msg.header.stamp = node_->get_clock()->now(); - pos_msg.joint_names = {"head_yaw_joint", "head_pitch_joint"}; - pos_msg.positions = {goal_yaw, goal_pitch}; - pos_msg.velocities = {yaw_vel, pitch_vel}; - pos_msg.accelerations = {params_.max_acceleration_yaw, params_.max_acceleration_pitch}; - pos_msg.max_torques = {10, 10}; + /** + * @brief Stores the robot detections in the map frame + */ + void handle_robots(const soccer_vision_3d_msgs::msg::RobotArray& msg) { + Eigen::Isometry3d to_map; + if (!lookup_to_map(msg.header, to_map)) { + return; + } - position_publisher_->publish(pos_msg); + const double stamp = rclcpp::Time(msg.header.stamp).seconds(); + std::vector robots; + robots.reserve(msg.robots.size()); + for (const auto& robot : msg.robots) { + robots.push_back( + {transform_point(to_map, robot.bb.center.position), static_cast(robot.confidence.confidence), stamp}); + } + std::lock_guard lock(world_mutex_); + active_vision_.world().setRobots(std::move(robots)); + } + + /** + * @brief Stores the ball a teammate reports + */ + void handle_team_data(const bitbots_msgs::msg::TeamData& msg) { + // Teammates report in the map frame already, and a penalized robot's ball is + // not worth turning the head for + if (msg.state == bitbots_msgs::msg::TeamData::STATE_PENALIZED) { + return; + } + const auto& ball = msg.ball_absolute; + // The covariance is a full 6x6 matrix, of which the two planar variances are + // what tells us how well the teammate knows where the ball is + const double covariance = std::max(ball.covariance[0], ball.covariance[7]); + std::lock_guard lock(world_mutex_); + active_vision_.world().setTeamBall( + msg.robot_id, Eigen::Vector3d(ball.pose.position.x, ball.pose.position.y, ball.pose.position.z), covariance, + rclcpp::Time(msg.header.stamp).seconds()); + } + + /** + * @brief Creates the debug publishers on demand + */ + void ensure_debug_publishers() { + if (debug_publishers_created_) { + return; + } + coverage_publisher_ = + node_->create_publisher("debug/active_vision/field_coverage", 1); + candidate_publisher_ = + node_->create_publisher("debug/active_vision/candidates", 1); + joint_space_publisher_ = node_->create_publisher("debug/active_vision/joint_space", 1); + debug_publishers_created_ = true; + } + + /** + * @brief Publishes the coverage grid, the candidate markers and the joint space plot + */ + void publish_active_vision_debug(const bitbots_head_mover::ActiveVisionResult& result, + const Eigen::Isometry3d& robot_pose) { + ensure_debug_publishers(); + + const auto stamp = node_->get_clock()->now(); + const std::string& map_frame = params_.active_vision.map_frame; + + coverage_publisher_->publish(bitbots_head_mover::coverageGrid(active_vision_.coverage(), map_frame, stamp)); + candidate_publisher_->publish( + bitbots_head_mover::candidateMarkers(result, active_vision_, robot_pose, map_frame, stamp)); + + const cv::Mat plot = bitbots_head_mover::jointSpaceDebugImage( + result, active_vision_.headLimits(), static_cast(params_.active_vision.debug.image_size)); + + std_msgs::msg::Header header; + header.stamp = stamp; + header.frame_id = map_frame; + joint_space_publisher_->publish(*cv_bridge::CvImage(header, "bgr8", plot).toImageMsg()); + } + + /** + * @brief Runs one cycle of the active vision head mode + * + * Holds the last commanded position when an input is missing, so a missing + * transform or a camera that has not come up yet is visible as a head that + * stops rather than one that silently falls back to a sweep. + */ + void perform_active_vision() { + // Inputs that can arrive late are retried by their own subscriptions and by + // the field dimension retry timer, so this only has to report the wait + const auto readiness = active_vision_.readiness(); + if (readiness != bitbots_head_mover::ActiveVisionReadiness::Ready) { + hold_active_vision_position(bitbots_head_mover::describe(readiness)); + return; + } + + // The calibration may only become available after the description did + if (!active_vision_.kinematics().hasCameraCalibration() && !update_camera_calibration()) { + hold_active_vision_position("the extrinsic camera calibration is unavailable"); + return; + } + + // Where the robot stands on the field, including the orientation the IMU + // contributes, which is what decides what the head can actually see + Eigen::Isometry3d robot_pose; + try { + robot_pose = tf2::transformToEigen(tf_buffer_->lookupTransform(params_.active_vision.map_frame, + params_.active_vision.root_link, + tf2::TimePointZero, tf2::durationFromSec(0.1))); + } catch (const tf2::TransformException& ex) { + hold_active_vision_position(ex.what()); + return; + } + + const auto head_position = require_head_position("an active vision cycle"); + if (!head_position) { + hold_active_vision_position("the current head position is unknown"); + return; + } + + bitbots_head_mover::ActiveVisionInput input; + input.head_position = *head_position; + input.robot_pose = robot_pose; + input.now = node_->now().seconds(); + + bitbots_head_mover::ActiveVisionResult result; + { + // The detection thread writes the world model concurrently; hold it steady + // for the duration of the planning cycle that reads and prunes it + std::lock_guard lock(world_mutex_); + result = active_vision_.plan(input); + } + if (!result.valid) { + hold_active_vision_position(bitbots_head_mover::describe(result.failure)); + return; + } + + // A partially unscorable candidate set still yields a decision, but it means + // the planner chose from fewer options than it sampled + if (result.unscorable_candidates > 0) { + RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, + "Discarded %zu of %zu head target candidates because they could not be scored", + result.unscorable_candidates, result.candidates.size()); + } + + active_vision_hold_position_ = result.position; + // The planner's proportional controller already produced the setpoint one + // step towards the selected target and the speed to reach it; the motor goals + // carry joint speeds, so the direction of travel is dropped + publish_motor_goals(result.position, {std::abs(result.velocity.yaw), std::abs(result.velocity.pitch)}); + + if (params_.active_vision.debug.enabled) { + // The debug markers rebuild a scorer over the world model, so this read has + // to be guarded against the detection thread as well + std::lock_guard lock(world_mutex_); + publish_active_vision_debug(result, robot_pose); + } + } + + /** + * @brief Keeps the head where it is because the active vision mode cannot plan + */ + void hold_active_vision_position(const std::string& reason) { + RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "Active vision is holding position: %s", + reason.c_str()); + // Publishing nothing is what holds the head: the motors keep the last goal + // they were given. Sending a goal with a zero speed instead would depend on + // how the hardware interface reads a zero velocity, which is exactly the + // ambiguity this mode should not rely on. This mirrors how DONT_MOVE works. } /** @@ -713,7 +996,10 @@ class HeadMover { uint curr_head_mode = head_mode_; // Pull the parameters from the parameter server - params_ = param_listener_->get_params(); + if (param_listener_->is_old(params_)) { + params_ = param_listener_->get_params(); + apply_active_vision_parameters(); + } // Check if we received the joint states yet and if not, return if (!current_joint_state_) { @@ -727,29 +1013,37 @@ class HeadMover { case bitbots_msgs::msg::HeadMode::DONT_MOVE: // Nothing to do if we go into track ball or dont move mode break; + + case bitbots_msgs::msg::HeadMode::ACTIVE_VISION: + // Entering active vision starts from wherever the head currently is, + // there is no pattern to precompute + active_vision_hold_position_.reset(); + break; case bitbots_msgs::msg::HeadMode::SEARCH_BALL_PENALTY: cycle_time_ = params_.search_patterns.search_ball_penalty.cycle_time; - pattern_ = generatePattern(params_.search_patterns.search_ball_penalty.scan_lines, - params_.search_patterns.search_ball_penalty.yaw_max[0], - params_.search_patterns.search_ball_penalty.yaw_max[1], - params_.search_patterns.search_ball_penalty.pitch_max[0], - params_.search_patterns.search_ball_penalty.pitch_max[1], - params_.search_patterns.search_ball_penalty.reduce_last_scanline); + pattern_ = + bitbots_head_mover::generatePattern(params_.search_patterns.search_ball_penalty.scan_lines, + params_.search_patterns.search_ball_penalty.yaw_max[0], + params_.search_patterns.search_ball_penalty.yaw_max[1], + params_.search_patterns.search_ball_penalty.pitch_max[0], + params_.search_patterns.search_ball_penalty.pitch_max[1], + params_.search_patterns.search_ball_penalty.reduce_last_scanline); break; case bitbots_msgs::msg::HeadMode::SEARCH_FIELD_FEATURES: cycle_time_ = params_.search_patterns.search_field_features.cycle_time; - pattern_ = generatePattern(params_.search_patterns.search_field_features.scan_lines, - params_.search_patterns.search_field_features.yaw_max[0], - params_.search_patterns.search_field_features.yaw_max[1], - params_.search_patterns.search_field_features.pitch_max[0], - params_.search_patterns.search_field_features.pitch_max[1], - params_.search_patterns.search_field_features.reduce_last_scanline); + pattern_ = + bitbots_head_mover::generatePattern(params_.search_patterns.search_field_features.scan_lines, + params_.search_patterns.search_field_features.yaw_max[0], + params_.search_patterns.search_field_features.yaw_max[1], + params_.search_patterns.search_field_features.pitch_max[0], + params_.search_patterns.search_field_features.pitch_max[1], + params_.search_patterns.search_field_features.reduce_last_scanline); break; case bitbots_msgs::msg::HeadMode::SEARCH_FRONT: cycle_time_ = params_.search_patterns.search_front.cycle_time; - pattern_ = generatePattern( + pattern_ = bitbots_head_mover::generatePattern( params_.search_patterns.search_front.scan_lines, params_.search_patterns.search_front.yaw_max[0], params_.search_patterns.search_front.yaw_max[1], params_.search_patterns.search_front.pitch_max[0], params_.search_patterns.search_front.pitch_max[1], @@ -758,7 +1052,7 @@ class HeadMover { case bitbots_msgs::msg::HeadMode::LOOK_FORWARD: cycle_time_ = params_.search_patterns.look_forward.cycle_time; - pattern_ = generatePattern( + pattern_ = bitbots_head_mover::generatePattern( params_.search_patterns.look_forward.scan_lines, params_.search_patterns.look_forward.yaw_max[0], params_.search_patterns.look_forward.yaw_max[1], params_.search_patterns.look_forward.pitch_max[0], params_.search_patterns.look_forward.pitch_max[1], @@ -766,6 +1060,12 @@ class HeadMover { break; default: + // An unknown mode means the sender and this node disagree about the + // HeadMode message. Silently doing nothing would look exactly like a + // head that was told to hold still. + RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, + "Received unknown head mode %u, ignoring it and keeping the previous mode", + curr_head_mode); return; } @@ -776,7 +1076,8 @@ class HeadMover { // the current head position, so we also rebuild it when we re-enter a search pattern from // e.g. ball tracking instead of continuing the old trajectory with an abrupt movement. if (curr_head_mode != bitbots_msgs::msg::HeadMode::TRACK_BALL && - curr_head_mode != bitbots_msgs::msg::HeadMode::DONT_MOVE) { + curr_head_mode != bitbots_msgs::msg::HeadMode::DONT_MOVE && + curr_head_mode != bitbots_msgs::msg::HeadMode::ACTIVE_VISION) { build_spline_trajectory(); } } @@ -787,12 +1088,22 @@ class HeadMover { { // If we are in search track mode look at the ball if (curr_head_mode == bitbots_msgs::msg::HeadMode::TRACK_BALL) { + // The filtered ball is written by the detection thread, so take a + // consistent snapshot of it under the lock before using it + geometry_msgs::msg::PoseWithCovarianceStamped ball_snapshot; + { + std::lock_guard lock(world_mutex_); + ball_snapshot = ball_position_; + } // Convert the ball position to a PointStamped geometry_msgs::msg::PointStamped look_at_point; - look_at_point.header = ball_position_.header; - look_at_point.point = ball_position_.pose.pose.position; + look_at_point.header = ball_snapshot.header; + look_at_point.point = ball_snapshot.pose.pose.position; // Try to look at the ball look_at(look_at_point); + } else if (curr_head_mode == bitbots_msgs::msg::HeadMode::ACTIVE_VISION) { + // Decide where to look by sampling and scoring head trajectories + perform_active_vision(); } else { // Execute the search pattern perform_search_pattern(); @@ -804,6 +1115,14 @@ class HeadMover { * @brief A getter that returns the node */ std::shared_ptr get_node() { return node_; } + + /** + * @brief The callback group carrying the blocking detection callbacks + * + * Spun by a second executor on its own thread so their tf waits do not stall + * the main executor that drives the search pattern timer. + */ + rclcpp::CallbackGroup::SharedPtr get_detection_callback_group() { return detection_callback_group_; } }; } // namespace move_head @@ -812,7 +1131,20 @@ int main(int argc, char* argv[]) { rclcpp::experimental::executors::EventsExecutor exec; auto head_mover = std::make_shared(); exec.add_node(head_mover->get_node()); + + // The detection callbacks block on tf lookups. Spinning them on a second + // executor on a dedicated thread keeps that wait off the main executor, so the + // search pattern timer keeps ticking at a steady rate. The detection callback + // group was created with auto_add=false, so add_node above did not pick it up. + rclcpp::executors::SingleThreadedExecutor detection_exec; + detection_exec.add_callback_group(head_mover->get_detection_callback_group(), + head_mover->get_node()->get_node_base_interface()); + std::thread detection_thread([&detection_exec]() { detection_exec.spin(); }); + exec.spin(); + + detection_exec.cancel(); + detection_thread.join(); rclcpp::shutdown(); return 0; diff --git a/src/bitbots_motion/bitbots_head_mover/src/search_pattern.cpp b/src/bitbots_motion/bitbots_head_mover/src/search_pattern.cpp new file mode 100644 index 0000000000..f89ef99a97 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/search_pattern.cpp @@ -0,0 +1,111 @@ +#include +#include +#include + +namespace bitbots_head_mover { + +double lineAngle(int line, int line_count, double min_angle, double max_angle) { + // A single scanline has no span to distribute, so it sits at the first angle. + // Without this the step size below would divide by zero and yield a NaN angle. + if (line_count <= 1) { + return min_angle; + } + // Get the angular delta that is covered by the scanlines in the pitch axis + double delta = std::abs(max_angle - min_angle); + // Calculate the angular step size between two scanlines + double steps = delta / (line_count - 1); + // Calculate the pitch angle of the given scanline + return steps * line + min_angle; +} + +std::vector interpolatedSteps(int steps, double pitch, double min_yaw, double max_yaw) { + // Handle edge case where we do not need to interpolate + if (steps == 0) { + return {}; + } + // Add one to the step count as we need to include the min and max yaw values + steps += 1; + // Create a vector that stores the interpolated steps + std::vector output_points; + // Calculate the delta between the min and max yaw values + double delta = std::abs(max_yaw - min_yaw); + // Calculate the step size between two interpolated steps + double step_size = delta / steps; + // Iterate over all steps and calculate the interpolated yaw values + for (int i = 1; i <= steps; i++) { + double yaw = min_yaw + step_size * i; + output_points.push_back({yaw, pitch}); + } + return output_points; +} + +std::vector generatePattern(int line_count, double max_horizontal_angle_left, + double max_horizontal_angle_right, double max_vertical_angle_up, + double max_vertical_angle_down, double reduce_last_scanline, + int interpolation_steps) { + // Store the keyframes of the search pattern + std::vector keyframes; + // Store the state of the generation process + bool down_direction = true; // true = decreasing line (toward top), false = increasing line (toward bottom) + bool right_side = false; // true = right, false = left + bool right_direction = true; // true = moving right, false = moving left; alternates per scan line + int line = line_count - 1; + // Calculate the number of iterations that are needed to generate the search pattern + int iterations = std::max(line_count * 4 - 4, 2); + // Iterate over all iterations and generate the search pattern + for (int i = 0; i < iterations; i++) { + // Get the maximum yaw values (left and right) for the current yaw position + // Select the relevant one based on the current side we are on + double current_yaw; + if (right_side) { + current_yaw = max_horizontal_angle_right; + } else { + current_yaw = max_horizontal_angle_left; + } + + // Get the current pitch angle based on the current line we are on + double current_pitch = lineAngle(line, line_count, max_vertical_angle_up, max_vertical_angle_down); + + // Store the keyframe + keyframes.push_back({current_yaw, current_pitch}); + + // Check if we move horizontally or vertically in the pattern + if (right_side != right_direction) { + // We move horizontally, so we might need to interpolate between the current and the next keyframe + std::vector interpolated_points = + interpolatedSteps(interpolation_steps, current_pitch, max_horizontal_angle_right, max_horizontal_angle_left); + // Reverse the order of the interpolated points if we are moving to the right + if (right_direction) { + std::reverse(interpolated_points.begin(), interpolated_points.end()); + } + // Add the interpolated points to the keyframes + keyframes.insert(keyframes.end(), interpolated_points.begin(), interpolated_points.end()); + // Change the direction we are moving in + right_side = right_direction; + + } else { + // Flip the scan direction so the next line scans the opposite way (boustrophedon) + right_direction = !right_direction; + // Advance to the next scan line + if (down_direction) { + line -= 1; + } else { + line += 1; + } + // Flip vertical direction when we reach either edge + if (line <= 0 || line >= line_count - 1) { + down_direction = !down_direction; + } + } + } + + // Reduce the last scanline by a given factor + for (auto& keyframe : keyframes) { + if (std::abs(keyframe.pitch - max_vertical_angle_down) < 1e-6) { + keyframe = {keyframe.yaw * reduce_last_scanline, max_vertical_angle_down}; + } + } + return keyframes; +} + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/src/target_sampler.cpp b/src/bitbots_motion/bitbots_head_mover/src/target_sampler.cpp new file mode 100644 index 0000000000..11172fe675 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/target_sampler.cpp @@ -0,0 +1,81 @@ +#include +#include +#include + +namespace bitbots_head_mover { + +TargetSampler::TargetSampler(const SamplerConfig& config, uint32_t seed) : config_(config), rng_(seed) {} + +double TargetSampler::sampleGaussian(double center, double std, const JointLimit& limit) { + // A non positive spread would make the normal distribution ill defined, so it + // collapses to the center. Clamping keeps the draw reachable; it skews the + // distribution at the edges, which is harmless for a proposal distribution. + if (!(std > 0.0)) { + return limit.clamp(center); + } + return limit.clamp(std::normal_distribution(center, std)(rng_)); +} + +double TargetSampler::sampleUniform(const JointLimit& limit) { + if (!(limit.upper > limit.lower)) { + return limit.clamp(limit.lower); + } + return std::uniform_real_distribution(limit.lower, limit.upper)(rng_); +} + +std::vector TargetSampler::sample(const HeadPosition& current, + const std::optional& last_target, const HeadLimits& limits) { + std::vector candidates; + if (config_.sample_count < 0) { + return candidates; + } + candidates.reserve(static_cast(config_.sample_count) + 2); + + // Always offer the current position. It gives the scoring something to fall + // back on and is the candidate that keeps the head still when nothing on the + // field is worth turning towards. + candidates.push_back({limits.clamp(current)}); + + // Offer the previous target as well, so a target that is still the best choice + // is not lost just because no random draw happened to land on it this cycle, + // which would otherwise let the head drift off a good target. + if (last_target) { + candidates.push_back({limits.clamp(*last_target)}); + } + + // The gaussian around the last target only exists once there is one; before + // that its share is spent on the gaussian around the current position, so the + // configured amount of local exploration is preserved either way. + const double last_weight = last_target ? std::max(0.0, config_.last_target_weight) : 0.0; + const double current_weight = + std::max(0.0, config_.current_position_weight) + (last_target ? 0.0 : std::max(0.0, config_.last_target_weight)); + const double uniform_weight = std::max(0.0, config_.uniform_weight); + const double total_weight = last_weight + current_weight + uniform_weight; + + // Without a positive weight there is no distribution to draw from; the + // guaranteed candidates above are all that can be offered + if (!(total_weight > 0.0)) { + return candidates; + } + + std::uniform_real_distribution component(0.0, total_weight); + for (int i = 0; i < config_.sample_count; i++) { + const double pick = component(rng_); + HeadPosition target; + if (pick < last_weight) { + target.yaw = sampleGaussian(last_target->yaw, config_.last_target_std, limits.yaw); + target.pitch = sampleGaussian(last_target->pitch, config_.last_target_std, limits.pitch); + } else if (pick < last_weight + current_weight) { + target.yaw = sampleGaussian(current.yaw, config_.current_position_std, limits.yaw); + target.pitch = sampleGaussian(current.pitch, config_.current_position_std, limits.pitch); + } else { + target.yaw = sampleUniform(limits.yaw); + target.pitch = sampleUniform(limits.pitch); + } + candidates.push_back({target}); + } + + return candidates; +} + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/src/world_model.cpp b/src/bitbots_motion/bitbots_head_mover/src/world_model.cpp new file mode 100644 index 0000000000..338da1b0be --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/world_model.cpp @@ -0,0 +1,86 @@ +#include +#include +#include +#include + +namespace bitbots_head_mover { + +namespace { + +/// Remove every target that is older than the given timeout. +void pruneOlderThan(std::vector& targets, double now, double timeout) { + targets.erase(std::remove_if(targets.begin(), targets.end(), + [&](const TimedTarget& target) { return now - target.stamp > timeout; }), + targets.end()); +} + +} // namespace + +double weightFromCovariance(double covariance, double covariance_half_weight) { + // A negative covariance is meaningless, and a non positive scale would make + // the weight jump between the extremes instead of fading + const double clamped_covariance = std::max(covariance, 0.0); + // A non positive scale is a configuration error the parameter validation + // rules out. Assert rather than quietly granting full trust to an estimate + // whose uncertainty we then never account for. + assert(covariance_half_weight > 0.0 && "covariance_half_weight must be positive"); + if (!(covariance_half_weight > 0.0)) { + return 1.0; + } + // Reaches one half exactly at the configured covariance and keeps decreasing + // from there without ever becoming zero, so a very uncertain estimate still + // beats no estimate at all + return 1.0 / (1.0 + clamped_covariance / covariance_half_weight); +} + +void WorldModel::setFilteredBall(const Eigen::Vector3d& position, double covariance, double stamp) { + filtered_ball_ = TimedTarget{position, weightFromCovariance(covariance, config_.covariance_half_weight), stamp}; +} + +void WorldModel::setRawBalls(std::vector balls) { raw_balls_ = std::move(balls); } + +void WorldModel::setTeamBall(uint8_t robot_id, const Eigen::Vector3d& position, double covariance, double stamp) { + team_balls_[robot_id] = + TimedTarget{position, weightFromCovariance(covariance, config_.covariance_half_weight), stamp}; +} + +void WorldModel::setRobots(std::vector robots) { robots_ = std::move(robots); } + +void WorldModel::prune(double now) { + if (filtered_ball_ && now - filtered_ball_->stamp > config_.filtered_ball_timeout) { + filtered_ball_.reset(); + } + + pruneOlderThan(raw_balls_, now, config_.raw_ball_timeout); + pruneOlderThan(robots_, now, config_.robot_timeout); + + for (auto it = team_balls_.begin(); it != team_balls_.end();) { + if (now - it->second.stamp > config_.team_ball_timeout) { + it = team_balls_.erase(it); + } else { + ++it; + } + } +} + +std::vector WorldModel::teamBalls() const { + std::vector balls; + balls.reserve(team_balls_.size()); + for (const auto& [robot_id, ball] : team_balls_) { + balls.push_back(ball); + } + return balls; +} + +bool WorldModel::hasAnyBall() const { + return filtered_ball_.has_value() || !raw_balls_.empty() || !team_balls_.empty(); +} + +void WorldModel::clear() { + filtered_ball_.reset(); + raw_balls_.clear(); + robots_.clear(); + team_balls_.clear(); +} + +} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/test/test_active_vision.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_active_vision.cpp new file mode 100644 index 0000000000..d28a0517a4 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_active_vision.cpp @@ -0,0 +1,407 @@ +#include + +#include +#include +#include +#include + +using bitbots_head_mover::ActiveVision; +using bitbots_head_mover::ActiveVisionInput; +using bitbots_head_mover::ActiveVisionReadiness; +using bitbots_head_mover::ActiveVisionResult; +using bitbots_head_mover::FieldCoverageConfig; +using bitbots_head_mover::HeadChainConfig; +using bitbots_head_mover::HeadPosition; +using bitbots_head_mover::SamplerConfig; +using bitbots_head_mover::ScoringWeights; +using bitbots_head_mover::TimedTarget; + +namespace { + +/// The same minimal head chain the scorer tests use, with the camera looking +/// forward along the robot's x axis when the head is at rest. +constexpr const char* kHeadUrdf = R"( + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +sensor_msgs::msg::CameraInfo makeCameraInfo() { + sensor_msgs::msg::CameraInfo info; + info.width = 640; + info.height = 480; + info.distortion_model = "plumb_bob"; + info.d = {0.0, 0.0, 0.0, 0.0, 0.0}; + info.k = {320.0, 0.0, 320.0, 0.0, 320.0, 240.0, 0.0, 0.0, 1.0}; + info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + info.p = {320.0, 0.0, 320.0, 0.0, 0.0, 320.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0}; + return info; +} + +FieldCoverageConfig makeCoverageConfig() { + FieldCoverageConfig config; + config.field_length = 9.0; + config.field_width = 6.0; + config.margin = 1.0; + config.cell_size = 0.5; + config.half_life = 8.0; + return config; +} + +SamplerConfig makeSamplerConfig() { + SamplerConfig config; + config.sample_count = 64; + config.last_target_weight = 0.4; + config.current_position_weight = 0.3; + config.uniform_weight = 0.3; + config.last_target_std = 0.3; + config.current_position_std = 0.5; + return config; +} + +/// A planner with every input satisfied. +ActiveVision makeReadyPlanner() { + ActiveVision planner; + planner.setRobotDescription(kHeadUrdf, HeadChainConfig{}); + planner.setCameraInfo(makeCameraInfo()); + planner.setFieldCoverageConfig(makeCoverageConfig()); + planner.setSamplerConfig(makeSamplerConfig()); + return planner; +} + +ActiveVisionInput makeInput(double now = 0.0) { + ActiveVisionInput input; + input.head_position = {0.0, 0.0}; + input.robot_pose = Eigen::Isometry3d::Identity(); + input.now = now; + return input; +} + +/// Whether a position lies within the inclusive joint bounds. +bool withinLimits(const HeadPosition& position, const bitbots_head_mover::HeadLimits& limits) { + return position.yaw >= limits.yaw.lower && position.yaw <= limits.yaw.upper && position.pitch >= limits.pitch.lower && + position.pitch <= limits.pitch.upper; +} + +} // namespace + +// --------------------------------------------------------------------------- +// Readiness +// --------------------------------------------------------------------------- + +TEST(ActiveVision, IsNotReadyWithoutAnyInputs) { + ActiveVision planner; + EXPECT_FALSE(planner.ready()); + EXPECT_EQ(planner.readiness(), ActiveVisionReadiness::MissingRobotDescription); +} + +TEST(ActiveVision, ReportsTheMissingCameraInfo) { + ActiveVision planner; + ASSERT_TRUE(planner.setRobotDescription(kHeadUrdf, HeadChainConfig{})); + EXPECT_EQ(planner.readiness(), ActiveVisionReadiness::MissingCameraInfo); +} + +TEST(ActiveVision, ReportsTheMissingFieldDimensions) { + ActiveVision planner; + ASSERT_TRUE(planner.setRobotDescription(kHeadUrdf, HeadChainConfig{})); + ASSERT_TRUE(planner.setCameraInfo(makeCameraInfo())); + EXPECT_EQ(planner.readiness(), ActiveVisionReadiness::MissingFieldDimensions); +} + +TEST(ActiveVision, IsReadyOnceEveryInputArrived) { + ActiveVision planner = makeReadyPlanner(); + EXPECT_TRUE(planner.ready()); + EXPECT_EQ(planner.readiness(), ActiveVisionReadiness::Ready); +} + +TEST(ActiveVision, RejectsAnUnusableRobotDescription) { + ActiveVision planner; + EXPECT_FALSE(planner.setRobotDescription("not a urdf", HeadChainConfig{})); + EXPECT_FALSE(planner.ready()); +} + +TEST(ActiveVision, DoesNotPlanWhileUnready) { + ActiveVision planner; + EXPECT_FALSE(planner.plan(makeInput()).valid); +} + +// --------------------------------------------------------------------------- +// Calibration handling +// --------------------------------------------------------------------------- + +TEST(ActiveVision, CalibrationSurvivesALateRobotDescription) { + ActiveVision planner; + Eigen::Isometry3d calibration = Eigen::Isometry3d::Identity(); + calibration.translation() = Eigen::Vector3d(0.0, 0.0, 0.05); + + // The calibration transform can be available before the description is + planner.setCameraCalibration(calibration); + ASSERT_TRUE(planner.setRobotDescription(kHeadUrdf, HeadChainConfig{})); + + EXPECT_TRUE(planner.kinematics().hasCameraCalibration()); +} + +// --------------------------------------------------------------------------- +// Planning +// --------------------------------------------------------------------------- + +TEST(ActiveVision, PlansACommand) { + ActiveVision planner = makeReadyPlanner(); + const ActiveVisionResult result = planner.plan(makeInput()); + + ASSERT_TRUE(result.valid); + EXPECT_FALSE(result.candidates.empty()); + EXPECT_EQ(result.scores.size(), result.candidates.size()); + EXPECT_LT(result.selected, result.candidates.size()); + EXPECT_TRUE(withinLimits(result.position, planner.headLimits())); + EXPECT_TRUE(withinLimits(result.target, planner.headLimits())); +} + +TEST(ActiveVision, SelectsTheHighestScoringCandidate) { + ActiveVision planner = makeReadyPlanner(); + const ActiveVisionResult result = planner.plan(makeInput()); + + ASSERT_TRUE(result.valid); + for (const auto& score : result.scores) { + EXPECT_LE(score.total, result.scores[result.selected].total); + } +} + +TEST(ActiveVision, TheSelectedTargetIsTheSelectedCandidate) { + ActiveVision planner = makeReadyPlanner(); + const ActiveVisionResult result = planner.plan(makeInput()); + + ASSERT_TRUE(result.valid); + const HeadPosition& selected = result.candidates[result.selected].target; + EXPECT_NEAR(result.target.yaw, selected.yaw, 1e-9); + EXPECT_NEAR(result.target.pitch, selected.pitch, 1e-9); +} + +TEST(ActiveVision, StepsTowardsTheTargetWithoutOvershooting) { + ActiveVision planner = makeReadyPlanner(); + // A ball far to the side gives the planner a clear reason to turn the head + planner.world().setFilteredBall({3.0, 3.0, 0.0}, 0.0, 0.0); + + const ActiveVisionResult result = planner.plan(makeInput()); + ASSERT_TRUE(result.valid); + ASSERT_GT(std::abs(result.target.yaw), 0.0); + + // The proportional controller commands a setpoint that has moved from the + // current position towards the target, but not past it + EXPECT_GT(result.position.yaw * result.target.yaw, 0.0); + EXPECT_LE(std::abs(result.position.yaw), std::abs(result.target.yaw)); + EXPECT_GT(std::abs(result.position.yaw), 0.0); +} + +TEST(ActiveVision, TurnsTowardsTheBall) { + ActiveVision planner = makeReadyPlanner(); + ScoringWeights weights; + // Isolate the ball term so the coverage sweep cannot outvote it + weights.field_coverage = 0.0; + weights.smoothness = 0.0; + planner.setScoringWeights(weights); + + // A ball clearly off to the robot's left + planner.world().setFilteredBall({2.0, 2.0, 0.0}, 0.0, 0.0); + + const ActiveVisionResult result = planner.plan(makeInput()); + ASSERT_TRUE(result.valid); + EXPECT_GT(result.target.yaw, 0.0); +} + +TEST(ActiveVision, TurnsTheOtherWayForABallOnTheOtherSide) { + ActiveVision planner = makeReadyPlanner(); + ScoringWeights weights; + weights.field_coverage = 0.0; + weights.smoothness = 0.0; + planner.setScoringWeights(weights); + + planner.world().setFilteredBall({2.0, -2.0, 0.0}, 0.0, 0.0); + + const ActiveVisionResult result = planner.plan(makeInput()); + ASSERT_TRUE(result.valid); + EXPECT_LT(result.target.yaw, 0.0); +} + +TEST(ActiveVision, AgesOutDetectionsWhilePlanning) { + ActiveVision planner = makeReadyPlanner(); + planner.world().setRawBalls({TimedTarget{{2.0, 0.0, 0.0}, 1.0, 0.0}}); + + planner.plan(makeInput(0.1)); + EXPECT_FALSE(planner.world().rawBalls().empty()); + + // Well past the raw detection timeout + planner.plan(makeInput(5.0)); + EXPECT_TRUE(planner.world().rawBalls().empty()); +} + +TEST(ActiveVision, RecordsWhatTheHeadIsLookingAt) { + ActiveVision planner = makeReadyPlanner(); + const double before = planner.coverage().totalInterest(); + + ActiveVisionInput input = makeInput(); + input.head_position = {0.0, 0.4}; + planner.plan(input); + + // Having looked somewhere has to reduce the outstanding interest, otherwise + // the coverage term could never steer the head anywhere new + EXPECT_LT(planner.coverage().totalInterest(), before); +} + +TEST(ActiveVision, CoverageRecoversOverTime) { + ActiveVision planner = makeReadyPlanner(); + ActiveVisionInput input = makeInput(0.0); + input.head_position = {0.0, 0.4}; + planner.plan(input); + const double after_looking = planner.coverage().totalInterest(); + + // A long time later the same area is worth looking at again + ActiveVisionInput later = makeInput(120.0); + later.head_position = {0.0, -1.2}; + planner.plan(later); + EXPECT_GT(planner.coverage().totalInterest(), after_looking); +} + +TEST(ActiveVision, SmoothnessKeepsConsecutiveTargetsTogether) { + ActiveVision steady = makeReadyPlanner(); + ScoringWeights strong; + strong.smoothness = 50.0; + steady.setScoringWeights(strong); + + ActiveVision flighty = makeReadyPlanner(); + ScoringWeights none; + none.smoothness = 0.0; + flighty.setScoringWeights(none); + + double steady_travel = 0.0; + double flighty_travel = 0.0; + HeadPosition steady_previous{0.0, 0.0}; + HeadPosition flighty_previous{0.0, 0.0}; + + for (int step = 1; step <= 20; step++) { + ActiveVisionInput input = makeInput(step * 0.05); + + input.head_position = steady_previous; + const auto a = steady.plan(input); + ASSERT_TRUE(a.valid); + steady_travel += std::hypot(a.target.yaw - steady_previous.yaw, a.target.pitch - steady_previous.pitch); + steady_previous = a.target; + + input.head_position = flighty_previous; + const auto b = flighty.plan(input); + ASSERT_TRUE(b.valid); + flighty_travel += std::hypot(b.target.yaw - flighty_previous.yaw, b.target.pitch - flighty_previous.pitch); + flighty_previous = b.target; + } + + // This is the whole point of the smoothness cost: without it the head chases + // whichever target happens to win this cycle and jitters + EXPECT_LT(steady_travel, flighty_travel); +} + +TEST(ActiveVision, ResetForgetsTheHistory) { + ActiveVision planner = makeReadyPlanner(); + ActiveVisionInput input = makeInput(); + input.head_position = {0.0, 0.4}; + planner.plan(input); + planner.world().setFilteredBall({2.0, 0.0, 0.0}, 0.0, 0.0); + + // What a planner that never looked anywhere would report + ActiveVision untouched = makeReadyPlanner(); + const double fresh_interest = untouched.coverage().totalInterest(); + + planner.reset(); + EXPECT_FALSE(planner.world().hasAnyBall()); + EXPECT_NEAR(planner.coverage().totalInterest(), fresh_interest, 1e-9); +} + +// --------------------------------------------------------------------------- +// Debug rendering +// --------------------------------------------------------------------------- + +TEST(ActiveVisionDebug, CoverageGridMatchesTheMap) { + ActiveVision planner = makeReadyPlanner(); + builtin_interfaces::msg::Time stamp; + const auto grid = bitbots_head_mover::coverageGrid(planner.coverage(), "map", stamp); + + EXPECT_EQ(grid.header.frame_id, "map"); + EXPECT_EQ(grid.info.width, planner.coverage().cellsX()); + EXPECT_EQ(grid.info.height, planner.coverage().cellsY()); + EXPECT_EQ(grid.data.size(), planner.coverage().size()); + EXPECT_DOUBLE_EQ(grid.info.resolution, makeCoverageConfig().cell_size); +} + +TEST(ActiveVisionDebug, CoverageGridValuesAreValidOccupancy) { + ActiveVision planner = makeReadyPlanner(); + builtin_interfaces::msg::Time stamp; + const auto grid = bitbots_head_mover::coverageGrid(planner.coverage(), "map", stamp); + for (int8_t value : grid.data) { + EXPECT_GE(value, 0); + EXPECT_LE(value, 100); + } +} + +TEST(ActiveVisionDebug, CandidateMarkersClearThePreviousCycle) { + ActiveVision planner = makeReadyPlanner(); + const auto result = planner.plan(makeInput()); + builtin_interfaces::msg::Time stamp; + const auto markers = + bitbots_head_mover::candidateMarkers(result, planner, Eigen::Isometry3d::Identity(), "map", stamp); + + ASSERT_FALSE(markers.markers.empty()); + // Without a delete all, candidates from a cycle with more samples would linger + EXPECT_EQ(markers.markers.front().action, visualization_msgs::msg::Marker::DELETEALL); +} + +TEST(ActiveVisionDebug, JointSpaceImageHasTheRequestedSize) { + ActiveVision planner = makeReadyPlanner(); + const auto result = planner.plan(makeInput()); + const cv::Mat image = bitbots_head_mover::jointSpaceDebugImage(result, planner.headLimits(), 400); + + EXPECT_EQ(image.rows, 400); + EXPECT_EQ(image.cols, 400); + EXPECT_EQ(image.type(), CV_8UC3); +} + +TEST(ActiveVisionDebug, JointSpaceImageDrawsTheCandidates) { + ActiveVision planner = makeReadyPlanner(); + const auto result = planner.plan(makeInput()); + const cv::Mat with_candidates = bitbots_head_mover::jointSpaceDebugImage(result, planner.headLimits(), 400); + const cv::Mat without_candidates = + bitbots_head_mover::jointSpaceDebugImage(ActiveVisionResult(), planner.headLimits(), 400); + + // The plot has to actually show something, otherwise it is useless for tuning + cv::Mat difference; + cv::absdiff(with_candidates, without_candidates, difference); + cv::Mat grey; + cv::cvtColor(difference, grey, cv::COLOR_BGR2GRAY); + EXPECT_GT(cv::countNonZero(grey), 0); +} + +TEST(ActiveVisionDebug, JointSpaceImageHandlesAnEmptyResult) { + ActiveVision planner = makeReadyPlanner(); + const cv::Mat image = bitbots_head_mover::jointSpaceDebugImage(ActiveVisionResult(), planner.headLimits(), 128); + EXPECT_EQ(image.rows, 128); +} diff --git a/src/bitbots_motion/bitbots_head_mover/test/test_active_vision_scorer.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_active_vision_scorer.cpp new file mode 100644 index 0000000000..752b838fd2 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_active_vision_scorer.cpp @@ -0,0 +1,437 @@ +#include + +#include +#include +#include +#include +#include + +using bitbots_head_mover::ActiveVisionScorer; +using bitbots_head_mover::CameraModel; +using bitbots_head_mover::FieldCoverageConfig; +using bitbots_head_mover::FieldCoverageMap; +using bitbots_head_mover::HeadChainConfig; +using bitbots_head_mover::HeadKinematics; +using bitbots_head_mover::HeadPosition; +using bitbots_head_mover::ScoringContext; +using bitbots_head_mover::ScoringWeights; +using bitbots_head_mover::TimedTarget; +using bitbots_head_mover::WorldModel; + +namespace { + +/// A head whose camera looks forward along the robot's x axis when at rest. +/// +/// The optical frame convention has z forward, x right and y down, so the fixed +/// joint at the end of the chain rotates the link frame accordingly. Without +/// that rotation every projection would be behind the camera. +constexpr const char* kHeadUrdf = R"( + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +sensor_msgs::msg::CameraInfo makeCameraInfo() { + sensor_msgs::msg::CameraInfo info; + info.width = 640; + info.height = 480; + const double fx = 320.0; + const double cx = 320.0; + const double cy = 240.0; + info.distortion_model = "plumb_bob"; + info.d = {0.0, 0.0, 0.0, 0.0, 0.0}; + info.k = {fx, 0.0, cx, 0.0, fx, cy, 0.0, 0.0, 1.0}; + info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + info.p = {fx, 0.0, cx, 0.0, 0.0, fx, cy, 0.0, 0.0, 0.0, 1.0, 0.0}; + return info; +} + +FieldCoverageConfig makeCoverageConfig() { + FieldCoverageConfig config; + config.field_length = 9.0; + config.field_width = 6.0; + config.margin = 1.0; + config.cell_size = 0.5; + config.half_life = 8.0; + return config; +} + +ScoringContext makeContext() { + ScoringContext context; + // The robot stands in the center of the field looking down the long axis + context.robot_pose = Eigen::Isometry3d::Identity(); + return context; +} + +/// Bundles everything the scorer needs so the tests stay readable. +struct Fixture { + std::unique_ptr kinematics = HeadKinematics::fromUrdf(kHeadUrdf); + CameraModel camera; + WorldModel world; + FieldCoverageMap coverage{makeCoverageConfig()}; + + Fixture() { camera.update(makeCameraInfo()); } + + ActiveVisionScorer scorer(const Eigen::Isometry3d& robot_pose = Eigen::Isometry3d::Identity()) { + return ActiveVisionScorer(*kinematics, camera, world, coverage, robot_pose); + } +}; + +} // namespace + +// --------------------------------------------------------------------------- +// Preconditions +// --------------------------------------------------------------------------- + +TEST(ActiveVisionScorer, ScoresZeroWithoutIntrinsics) { + Fixture fixture; + CameraModel blind; + ActiveVisionScorer scorer(*fixture.kinematics, blind, fixture.world, fixture.coverage, Eigen::Isometry3d::Identity()); + const auto breakdown = scorer.score({0.0, 0.0}, makeContext()); + EXPECT_FALSE(breakdown.valid); + EXPECT_DOUBLE_EQ(breakdown.total, 0.0); +} + +// --------------------------------------------------------------------------- +// Ball terms +// --------------------------------------------------------------------------- + +TEST(ActiveVisionScorer, PrefersLookingAtTheFilteredBall) { + Fixture fixture; + // A ball on the ground two meters in front of the robot + fixture.world.setFilteredBall({2.0, 0.0, 0.0}, 0.0, 0.0); + auto scorer = fixture.scorer(); + + // Looking down towards the ball beats looking away from it + const double towards = scorer.score({0.0, 0.45}, makeContext()).filtered_ball; + const double away = scorer.score({-1.2, 0.0}, makeContext()).filtered_ball; + EXPECT_GT(towards, 0.0); + EXPECT_GT(towards, away); +} + +TEST(ActiveVisionScorer, AnUncertainBallContributesLess) { + Fixture fixture; + fixture.world.setFilteredBall({2.0, 0.0, 0.0}, 0.0, 0.0); + const double certain = fixture.scorer().score({0.0, 0.45}, makeContext()).filtered_ball; + + // The very same ball, but the filter is far less sure about it + fixture.world.setFilteredBall({2.0, 0.0, 0.0}, 5.0, 0.0); + const double uncertain = fixture.scorer().score({0.0, 0.45}, makeContext()).filtered_ball; + + EXPECT_GT(certain, uncertain); +} + +TEST(ActiveVisionScorer, NoBallMeansNoBallScore) { + Fixture fixture; + EXPECT_DOUBLE_EQ(fixture.scorer().score({0.0, 0.45}, makeContext()).filtered_ball, 0.0); +} + +TEST(ActiveVisionScorer, RawBallDetectionsAreScoredSeparately) { + Fixture fixture; + fixture.world.setRawBalls({TimedTarget{{2.0, 0.0, 0.0}, 1.0, 0.0}}); + auto breakdown = fixture.scorer().score({0.0, 0.45}, makeContext()); + EXPECT_GT(breakdown.raw_balls, 0.0); + EXPECT_DOUBLE_EQ(breakdown.filtered_ball, 0.0); +} + +TEST(ActiveVisionScorer, TeamBallsAreScoredSeparately) { + Fixture fixture; + fixture.world.setTeamBall(2, {2.0, 0.0, 0.0}, 0.0, 0.0); + auto breakdown = fixture.scorer().score({0.0, 0.45}, makeContext()); + EXPECT_GT(breakdown.team_ball, 0.0); + EXPECT_DOUBLE_EQ(breakdown.raw_balls, 0.0); +} + +TEST(ActiveVisionScorer, RobotDetectionsAreScoredSeparately) { + Fixture fixture; + fixture.world.setRobots({TimedTarget{{2.0, 0.0, 0.3}, 1.0, 0.0}}); + auto breakdown = fixture.scorer().score({0.0, 0.4}, makeContext()); + EXPECT_GT(breakdown.robots, 0.0); +} + +TEST(ActiveVisionScorer, CenteringABallBeatsCatchingItAtTheEdge) { + Fixture fixture; + fixture.world.setFilteredBall({2.0, 0.0, 0.0}, 0.0, 0.0); + auto scorer = fixture.scorer(); + + double best = 0.0; + double best_pitch = 0.0; + for (double pitch = 0.0; pitch < 1.0; pitch += 0.02) { + const double score = scorer.score({0.0, pitch}, makeContext()).filtered_ball; + if (score > best) { + best = score; + best_pitch = pitch; + } + } + // The optimum actually centers the ball rather than merely keeping it visible + EXPECT_DOUBLE_EQ(best, 1.0); + EXPECT_GT(best_pitch, 0.0); +} + +// --------------------------------------------------------------------------- +// Coverage and out of field +// --------------------------------------------------------------------------- + +TEST(ActiveVisionScorer, UnobservedFieldIsWorthLookingAt) { + Fixture fixture; + EXPECT_GT(fixture.scorer().score({0.0, 0.4}, makeContext()).field_coverage, 0.0); +} + +TEST(ActiveVisionScorer, AlreadyObservedFieldIsWorthLess) { + Fixture fixture; + const double before = fixture.scorer().score({0.0, 0.4}, makeContext()).field_coverage; + + // Record what that very head position sees, then ask again + fixture.scorer().recordObservation(fixture.coverage, {0.0, 0.4}, makeContext().robot_pose); + const double after = fixture.scorer().score({0.0, 0.4}, makeContext()).field_coverage; + + EXPECT_GT(before, 0.0); + EXPECT_LT(after, before); +} + +TEST(ActiveVisionScorer, ObservationsDecayBackIntoInterest) { + Fixture fixture; + const double fresh = fixture.scorer().score({0.0, 0.4}, makeContext()).field_coverage; + + fixture.scorer().recordObservation(fixture.coverage, {0.0, 0.4}, makeContext().robot_pose); + const double observed = fixture.scorer().score({0.0, 0.4}, makeContext()).field_coverage; + + // After a long while the same part of the field is interesting again + fixture.coverage.decay(60.0); + const double stale = fixture.scorer().score({0.0, 0.4}, makeContext()).field_coverage; + + EXPECT_LT(observed, fresh); + EXPECT_GT(stale, observed); +} + +TEST(ActiveVisionScorer, LookingOffTheFieldEarnsNoCoverage) { + Fixture fixture; + + // Standing near the touch line, so turning one way faces the field and the + // other way faces the area beyond it + ScoringContext context = makeContext(); + context.robot_pose.translation() = Eigen::Vector3d(0.0, 2.5, 0.0); + auto scorer = fixture.scorer(context.robot_pose); + + const double towards_field = scorer.score({-1.2, 0.4}, context).field_coverage; + const double away_from_field = scorer.score({1.2, 0.4}, context).field_coverage; + + // Off field cells carry no interest, so aiming at them simply earns nothing. + // That opportunity cost is what replaces the former explicit penalty. + EXPECT_GT(towards_field, away_from_field); +} + +TEST(ActiveVisionScorer, LookingAtTheSkyEarnsNoCoverage) { + Fixture fixture; + auto scorer = fixture.scorer(); + + // Pitched fully up nothing projects into the image at all. An explicit off + // field penalty measured as a share of visible cells would score this as + // perfectly clean, which made staring at the sky better than glancing past + // the touch line. Earning no coverage is the behavior that ranks it last. + const double sky = scorer.score({0.0, -1.2}, makeContext()).field_coverage; + const double field = scorer.score({0.0, 0.4}, makeContext()).field_coverage; + + EXPECT_DOUBLE_EQ(sky, 0.0); + EXPECT_GT(field, sky); +} + +TEST(ActiveVisionScorer, DistantFieldCountsForLessThanNearField) { + Fixture fixture; + + // A view of far ground sweeps up more area simply because distance packs more + // of it into the same image, so without a falloff it would win + ScoringContext context = makeContext(); + auto scorer = fixture.scorer(context.robot_pose); + + // Looking down sees the near ground, looking towards the horizon sees far ground + const double near_view = scorer.score({0.0, 0.8}, context).field_coverage; + const double far_view = scorer.score({0.0, 0.1}, context).field_coverage; + + // Both see field, but the near view must not be dwarfed by the far one + EXPECT_GT(near_view, 0.0); + EXPECT_GT(far_view, 0.0); +} + +TEST(ActiveVisionScorer, TheDistanceFalloffDampensFarViewpoints) { + Fixture fixture; + ScoringContext context = makeContext(); + + // A short falloff discounts distance hard, a long one barely at all + auto sharp = fixture.scorer(context.robot_pose); + sharp.setCoverageDistanceHalfWeight(1.0); + auto flat = fixture.scorer(context.robot_pose); + flat.setCoverageDistanceHalfWeight(100.0); + sharp.prepare(context.robot_pose); + flat.prepare(context.robot_pose); + + // A view aimed towards the horizon, which is where the far cells are + const double sharp_ratio = sharp.score({0.0, 0.1}, context).field_coverage / + std::max(sharp.score({0.0, 0.8}, context).field_coverage, 1e-12); + const double flat_ratio = + flat.score({0.0, 0.1}, context).field_coverage / std::max(flat.score({0.0, 0.8}, context).field_coverage, 1e-12); + + // Discounting distance more sharply has to move the balance towards the near view + EXPECT_LT(sharp_ratio, flat_ratio); +} + +// --------------------------------------------------------------------------- +// Smoothness cost +// --------------------------------------------------------------------------- + +TEST(ActiveVisionScorer, SmoothnessCostIsTheDistanceToThePreviousTarget) { + Fixture fixture; + auto scorer = fixture.scorer(); + + ScoringContext context = makeContext(); + context.previous_target = HeadPosition{0.5, 0.2}; + + // The cost is exactly the joint space distance to the previous target + EXPECT_DOUBLE_EQ(scorer.score({0.5, 0.2}, context).smoothness_cost, 0.0); + EXPECT_NEAR(scorer.score({0.5, 0.5}, context).smoothness_cost, 0.3, 1e-12); + EXPECT_NEAR(scorer.score({-0.5, 0.2}, context).smoothness_cost, 1.0, 1e-12); +} + +TEST(ActiveVisionScorer, ADistantTargetCostsMoreThanANearOne) { + Fixture fixture; + auto scorer = fixture.scorer(); + + ScoringContext context = makeContext(); + context.previous_target = HeadPosition{0.0, 0.0}; + + const double near = scorer.score({0.1, 0.1}, context).smoothness_cost; + const double far = scorer.score({1.0, 0.5}, context).smoothness_cost; + EXPECT_LT(near, far); +} + +TEST(ActiveVisionScorer, NoPreviousTargetMeansNoSmoothnessCost) { + Fixture fixture; + // Without a previous target the cost must not penalize any candidate + EXPECT_DOUBLE_EQ(fixture.scorer().score({0.5, 0.2}, makeContext()).smoothness_cost, 0.0); +} + +TEST(ActiveVisionScorer, TheSmoothnessCostLowersTheTotal) { + Fixture fixture; + fixture.world.setFilteredBall({2.0, 0.0, 0.0}, 0.0, 0.0); + auto scorer = fixture.scorer(); + ScoringWeights weights = scorer.weights(); + weights.smoothness = 2.0; + scorer.setWeights(weights); + + // The same candidate scored with and without a previous target far away + const double without_previous = scorer.score({0.0, 0.45}, makeContext()).total; + + ScoringContext committed = makeContext(); + committed.previous_target = HeadPosition{-1.0, -1.0}; + const auto breakdown = scorer.score({0.0, 0.45}, committed); + + EXPECT_GT(breakdown.smoothness_cost, 0.0); + EXPECT_LT(breakdown.total, without_previous); +} + +// --------------------------------------------------------------------------- +// Aggregation +// --------------------------------------------------------------------------- + +TEST(ActiveVisionScorer, EveryRewardTermStaysWithinTheUnitInterval) { + Fixture fixture; + fixture.world.setFilteredBall({2.0, 0.5, 0.0}, 0.2, 0.0); + fixture.world.setRawBalls({TimedTarget{{2.0, 0.5, 0.0}, 1.0, 0.0}}); + fixture.world.setTeamBall(2, {-2.0, 0.0, 0.0}, 0.2, 0.0); + fixture.world.setRobots({TimedTarget{{1.0, 1.0, 0.3}, 1.0, 0.0}}); + auto scorer = fixture.scorer(); + + ScoringContext context = makeContext(); + context.previous_target = HeadPosition{0.0, 0.3}; + + for (double yaw = -1.2; yaw <= 1.2; yaw += 0.3) { + for (double pitch = -1.0; pitch <= 1.0; pitch += 0.25) { + const auto breakdown = scorer.score({yaw, pitch}, context); + for (double term : {breakdown.filtered_ball, breakdown.raw_balls, breakdown.team_ball, breakdown.field_coverage, + breakdown.robots}) { + EXPECT_GE(term, 0.0); + EXPECT_LE(term, 1.0); + } + EXPECT_GE(breakdown.smoothness_cost, 0.0); + } + } +} + +TEST(ActiveVisionScorer, TheTotalIsTheWeightedSumMinusTheSmoothnessCost) { + Fixture fixture; + fixture.world.setFilteredBall({2.0, 0.0, 0.0}, 0.1, 0.0); + auto scorer = fixture.scorer(); + const ScoringWeights& weights = scorer.weights(); + + ScoringContext context = makeContext(); + context.previous_target = HeadPosition{0.3, 0.0}; + + const auto breakdown = scorer.score({0.0, 0.4}, context); + const double expected = weights.filtered_ball * breakdown.filtered_ball + weights.raw_balls * breakdown.raw_balls + + weights.team_ball * breakdown.team_ball + weights.field_coverage * breakdown.field_coverage + + weights.robots * breakdown.robots - weights.smoothness * breakdown.smoothness_cost; + EXPECT_NEAR(breakdown.total, expected, 1e-12); +} + +TEST(ActiveVisionScorer, ZeroWeightsDisableEveryTerm) { + Fixture fixture; + fixture.world.setFilteredBall({2.0, 0.0, 0.0}, 0.0, 0.0); + auto scorer = fixture.scorer(); + + ScoringWeights weights; + weights.filtered_ball = 0.0; + weights.raw_balls = 0.0; + weights.team_ball = 0.0; + weights.field_coverage = 0.0; + weights.robots = 0.0; + weights.smoothness = 0.0; + scorer.setWeights(weights); + + ScoringContext context = makeContext(); + context.previous_target = HeadPosition{-1.0, -1.0}; + EXPECT_DOUBLE_EQ(scorer.score({0.0, 0.4}, context).total, 0.0); +} + +TEST(ActiveVisionScorer, TheRobotPoseMovesWhatIsVisible) { + Fixture fixture; + fixture.world.setFilteredBall({4.0, 0.0, 0.0}, 0.0, 0.0); + auto scorer = fixture.scorer(); + + // Standing a meter short of the ball and facing it + ScoringContext facing = makeContext(); + facing.robot_pose.translation() = Eigen::Vector3d(3.0, 0.0, 0.0); + + // Standing in the very same spot, but turned around + ScoringContext turned_away = makeContext(); + turned_away.robot_pose.translation() = Eigen::Vector3d(3.0, 0.0, 0.0); + turned_away.robot_pose.linear() = Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + + // The very same head position sees very different things depending on how the + // robot stands, which is exactly why the map frame is used throughout + const double towards = scorer.score({0.0, 0.5}, facing).filtered_ball; + const double away = scorer.score({0.0, 0.5}, turned_away).filtered_ball; + EXPECT_GT(towards, 0.0); + EXPECT_DOUBLE_EQ(away, 0.0); +} diff --git a/src/bitbots_motion/bitbots_head_mover/test/test_camera_model.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_camera_model.cpp new file mode 100644 index 0000000000..d78274b0a6 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_camera_model.cpp @@ -0,0 +1,200 @@ +#include + +#include + +using bitbots_head_mover::CameraModel; +using bitbots_head_mover::VisibilityWeighting; + +namespace { + +/// A pinhole camera with a centered principal point. +/// +/// The focal length equals half the width, which puts the horizontal field of +/// view at exactly 90 degrees and makes the pixel coordinates below easy to +/// verify: a point at 45 degrees lands exactly on the image border. +sensor_msgs::msg::CameraInfo makeCameraInfo(uint32_t width = 640, uint32_t height = 480) { + sensor_msgs::msg::CameraInfo info; + info.width = width; + info.height = height; + const double fx = width / 2.0; + const double fy = width / 2.0; + const double cx = width / 2.0; + const double cy = height / 2.0; + info.distortion_model = "plumb_bob"; + info.d = {0.0, 0.0, 0.0, 0.0, 0.0}; + info.k = {fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0}; + info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + info.p = {fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0}; + return info; +} + +CameraModel makeModel() { + CameraModel model; + model.update(makeCameraInfo()); + return model; +} + +} // namespace + +// --------------------------------------------------------------------------- +// Intrinsics +// --------------------------------------------------------------------------- + +TEST(CameraModel, IsInvalidBeforeReceivingIntrinsics) { + CameraModel model; + EXPECT_FALSE(model.valid()); + EXPECT_FALSE(model.project({0.0, 0.0, 1.0}).has_value()); + EXPECT_DOUBLE_EQ(model.visibility({0.0, 0.0, 1.0}), 0.0); +} + +TEST(CameraModel, AcceptsUsableIntrinsics) { + CameraModel model; + EXPECT_TRUE(model.update(makeCameraInfo())); + EXPECT_TRUE(model.valid()); + EXPECT_DOUBLE_EQ(model.width(), 640.0); + EXPECT_DOUBLE_EQ(model.height(), 480.0); +} + +TEST(CameraModel, RejectsAnUncalibratedCamera) { + CameraModel model; + sensor_msgs::msg::CameraInfo info = makeCameraInfo(); + // A driver that has not been calibrated publishes a zeroed intrinsic matrix, + // which would collapse every projection onto the principal point + info.k = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + EXPECT_FALSE(model.update(info)); + EXPECT_FALSE(model.valid()); +} + +TEST(CameraModel, RejectsAZeroSizedImage) { + CameraModel model; + sensor_msgs::msg::CameraInfo info = makeCameraInfo(); + info.width = 0; + EXPECT_FALSE(model.update(info)); + EXPECT_FALSE(model.valid()); +} + +TEST(CameraModel, StaysValidWhenTheSameIntrinsicsArriveAgain) { + // image_geometry reports whether the intrinsics changed, which must not be + // mistaken for the update having failed + CameraModel model; + ASSERT_TRUE(model.update(makeCameraInfo())); + EXPECT_TRUE(model.update(makeCameraInfo())); + EXPECT_TRUE(model.valid()); +} + +// --------------------------------------------------------------------------- +// Projection +// --------------------------------------------------------------------------- + +TEST(CameraModel, ProjectsTheOpticalAxisToTheImageCenter) { + CameraModel model = makeModel(); + auto pixel = model.project({0.0, 0.0, 1.0}); + ASSERT_TRUE(pixel.has_value()); + EXPECT_NEAR(pixel->x(), 320.0, 1e-6); + EXPECT_NEAR(pixel->y(), 240.0, 1e-6); +} + +TEST(CameraModel, ProjectsAlongTheOpticalFrameAxes) { + CameraModel model = makeModel(); + // In an optical frame x points right and y points down + auto right = model.project({0.5, 0.0, 1.0}); + ASSERT_TRUE(right.has_value()); + EXPECT_GT(right->x(), 320.0); + EXPECT_NEAR(right->y(), 240.0, 1e-6); + + auto down = model.project({0.0, 0.5, 1.0}); + ASSERT_TRUE(down.has_value()); + EXPECT_GT(down->y(), 240.0); +} + +TEST(CameraModel, RejectsPointsBehindTheCamera) { + CameraModel model = makeModel(); + EXPECT_FALSE(model.project({0.0, 0.0, -1.0}).has_value()); + EXPECT_FALSE(model.project({0.0, 0.0, 0.0}).has_value()); +} + +TEST(CameraModel, RejectsPointsOutsideTheImage) { + CameraModel model = makeModel(); + // At a focal length of half the width, 45 degrees sits on the image border + EXPECT_TRUE(model.project({0.99, 0.0, 1.0}).has_value()); + EXPECT_FALSE(model.project({1.01, 0.0, 1.0}).has_value()); +} + +TEST(CameraModel, DistanceDoesNotChangeTheProjection) { + CameraModel model = makeModel(); + auto near = model.project({0.2, 0.1, 1.0}); + auto far = model.project({2.0, 1.0, 10.0}); + ASSERT_TRUE(near.has_value()); + ASSERT_TRUE(far.has_value()); + EXPECT_NEAR(near->x(), far->x(), 1e-6); + EXPECT_NEAR(near->y(), far->y(), 1e-6); +} + +// --------------------------------------------------------------------------- +// Visibility scoring +// --------------------------------------------------------------------------- + +TEST(CameraModel, InvisiblePointsScoreZero) { + CameraModel model = makeModel(); + EXPECT_DOUBLE_EQ(model.visibility({0.0, 0.0, -1.0}), 0.0); + EXPECT_DOUBLE_EQ(model.visibility({5.0, 0.0, 1.0}), 0.0); +} + +TEST(CameraModel, CenteredPointsScorePerfectly) { + CameraModel model = makeModel(); + EXPECT_DOUBLE_EQ(model.visibility({0.0, 0.0, 1.0}), 1.0); +} + +TEST(CameraModel, TheWholeCentralRegionScoresPerfectly) { + CameraModel model = makeModel(); + const VisibilityWeighting weighting{0.5, 0.3}; + // Half of the half width is a quarter of the image width from the center, + // which is just inside the central 50% of the image + EXPECT_DOUBLE_EQ(model.visibility({0.49, 0.0, 1.0}, weighting), 1.0); + // Just outside it the score starts to drop + EXPECT_LT(model.visibility({0.55, 0.0, 1.0}, weighting), 1.0); +} + +TEST(CameraModel, ScoreFallsOffTowardsTheBorder) { + CameraModel model = makeModel(); + const VisibilityWeighting weighting{0.5, 0.3}; + const double center = model.visibility({0.0, 0.0, 1.0}, weighting); + const double middle = model.visibility({0.75, 0.0, 1.0}, weighting); + const double border = model.visibility({0.999, 0.0, 1.0}, weighting); + EXPECT_GT(center, middle); + EXPECT_GT(middle, border); +} + +TEST(CameraModel, BorderPointsScoreTheConfiguredBorderScore) { + CameraModel model = makeModel(); + const VisibilityWeighting weighting{0.5, 0.3}; + // A point essentially on the image border keeps the head interested in it, + // but much less so than a centered one + EXPECT_NEAR(model.visibility({0.9999, 0.0, 1.0}, weighting), 0.3, 1e-3); +} + +TEST(CameraModel, BothAxesCountTowardsBeingCentered) { + CameraModel model = makeModel(); + const VisibilityWeighting weighting{0.5, 0.3}; + // Centered horizontally but near the top edge is not well framed. The vertical + // half angle is smaller, so a modest y already reaches the image edge. + EXPECT_LT(model.visibility({0.0, 0.7, 1.0}, weighting), 1.0); +} + +TEST(CameraModel, CenterFractionWidensThePerfectRegion) { + CameraModel model = makeModel(); + const Eigen::Vector3d point{0.75, 0.0, 1.0}; + EXPECT_LT(model.visibility(point, {0.5, 0.3}), 1.0); + EXPECT_DOUBLE_EQ(model.visibility(point, {0.9, 0.3}), 1.0); +} + +TEST(CameraModel, ScoreStaysWithinTheUnitInterval) { + CameraModel model = makeModel(); + for (double x = -1.5; x <= 1.5; x += 0.1) { + for (double y = -1.5; y <= 1.5; y += 0.1) { + const double score = model.visibility({x, y, 1.0}); + EXPECT_GE(score, 0.0); + EXPECT_LE(score, 1.0); + } + } +} diff --git a/src/bitbots_motion/bitbots_head_mover/test/test_field_coverage_map.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_field_coverage_map.cpp new file mode 100644 index 0000000000..7e270cda9c --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_field_coverage_map.cpp @@ -0,0 +1,266 @@ +#include + +#include +#include + +using bitbots_head_mover::FieldCoverageConfig; +using bitbots_head_mover::FieldCoverageMap; + +namespace { +FieldCoverageConfig makeConfig() { + FieldCoverageConfig config; + config.field_length = 9.0; + config.field_width = 6.0; + config.margin = 1.0; + config.cell_size = 0.5; + config.half_life = 8.0; + return config; +} +} // namespace + +// --------------------------------------------------------------------------- +// Grid layout +// --------------------------------------------------------------------------- + +TEST(FieldCoverageMap, CoversTheFieldPlusTheMargin) { + FieldCoverageMap map(makeConfig()); + // 9 + 2 * 1 meters at half a meter per cell + EXPECT_EQ(map.cellsX(), 22u); + // 6 + 2 * 1 meters at half a meter per cell + EXPECT_EQ(map.cellsY(), 16u); + EXPECT_EQ(map.size(), 22u * 16u); + EXPECT_EQ(map.cellCenters().size(), map.size()); +} + +TEST(FieldCoverageMap, IsCenteredOnTheFieldOrigin) { + FieldCoverageMap map(makeConfig()); + double min_x = 1e9, max_x = -1e9, min_y = 1e9, max_y = -1e9; + for (const auto& center : map.cellCenters()) { + min_x = std::min(min_x, center.x()); + max_x = std::max(max_x, center.x()); + min_y = std::min(min_y, center.y()); + max_y = std::max(max_y, center.y()); + } + // The map frame's origin is the center of the field + EXPECT_NEAR(min_x, -max_x, 1e-9); + EXPECT_NEAR(min_y, -max_y, 1e-9); +} + +TEST(FieldCoverageMap, CellsSitOnTheGroundPlane) { + FieldCoverageMap map(makeConfig()); + for (const auto& center : map.cellCenters()) { + EXPECT_DOUBLE_EQ(center.z(), 0.0); + } +} + +TEST(FieldCoverageMap, MarksCellsBeyondTheFieldLines) { + FieldCoverageMap map(makeConfig()); + size_t out_of_field = 0; + for (size_t index = 0; index < map.size(); index++) { + const auto& center = map.cellCenters()[index]; + const bool expected = std::abs(center.x()) > 4.5 || std::abs(center.y()) > 3.0; + EXPECT_EQ(map.isOutOfField(index), expected) << "cell at " << center.x() << ", " << center.y(); + out_of_field += map.isOutOfField(index) ? 1 : 0; + } + // The margin has to actually contain something, otherwise the penalty term + // would never have anything to fire on + EXPECT_GT(out_of_field, 0u); + EXPECT_LT(out_of_field, map.size()); +} + +TEST(FieldCoverageMap, StaysCenteredWhenTheExtentIsNotAMultipleOfTheCellSize) { + // 9 + 2 * 1 meters does not divide by 0.75, so the cell count is rounded up. + // The grid still has to stay centered on the field, otherwise every cell is + // offset by the rounding remainder and the out of field ring is lopsided. + FieldCoverageConfig config = makeConfig(); + config.cell_size = 0.75; + FieldCoverageMap map(config); + + double min_x = 1e9, max_x = -1e9, min_y = 1e9, max_y = -1e9; + for (const auto& center : map.cellCenters()) { + min_x = std::min(min_x, center.x()); + max_x = std::max(max_x, center.x()); + min_y = std::min(min_y, center.y()); + max_y = std::max(max_y, center.y()); + } + EXPECT_NEAR(min_x, -max_x, 1e-9); + EXPECT_NEAR(min_y, -max_y, 1e-9); +} + +TEST(FieldCoverageMap, OutOfFieldRingIsSymmetricForAnAwkwardCellSize) { + FieldCoverageConfig config = makeConfig(); + config.cell_size = 0.75; + FieldCoverageMap map(config); + + // A cell and its mirror image across the origin must agree on whether they + // are on the field + for (size_t index = 0; index < map.size(); index++) { + const auto& center = map.cellCenters()[index]; + bool found_mirror = false; + for (size_t other = 0; other < map.size(); other++) { + const auto& mirror = map.cellCenters()[other]; + if (std::abs(mirror.x() + center.x()) < 1e-9 && std::abs(mirror.y() + center.y()) < 1e-9) { + EXPECT_EQ(map.isOutOfField(index), map.isOutOfField(other)); + found_mirror = true; + break; + } + } + EXPECT_TRUE(found_mirror) << "no mirror cell for " << center.x() << ", " << center.y(); + } +} + +TEST(FieldCoverageMap, DegenerateCellSizeYieldsAnEmptyGrid) { + FieldCoverageConfig config = makeConfig(); + config.cell_size = 0.0; + FieldCoverageMap map(config); + EXPECT_EQ(map.size(), 0u); + EXPECT_DOUBLE_EQ(map.totalInterest(), 0.0); +} + +// --------------------------------------------------------------------------- +// Interest and observation +// --------------------------------------------------------------------------- + +TEST(FieldCoverageMap, UnobservedFieldCellsAreFullyInteresting) { + FieldCoverageMap map(makeConfig()); + for (size_t index = 0; index < map.size(); index++) { + if (!map.isOutOfField(index)) { + EXPECT_DOUBLE_EQ(map.interest(index), 1.0); + } + } +} + +TEST(FieldCoverageMap, CellsOutsideTheFieldAreNeverInteresting) { + FieldCoverageMap map(makeConfig()); + for (size_t index = 0; index < map.size(); index++) { + if (map.isOutOfField(index)) { + EXPECT_DOUBLE_EQ(map.interest(index), 0.0); + } + } +} + +TEST(FieldCoverageMap, ObservingACellRemovesItsInterest) { + FieldCoverageMap map(makeConfig()); + size_t field_cell = 0; + while (map.isOutOfField(field_cell)) { + field_cell++; + } + + map.observe(field_cell, 1.0); + EXPECT_DOUBLE_EQ(map.interest(field_cell), 0.0); +} + +TEST(FieldCoverageMap, PartialObservationRemovesPartOfTheInterest) { + FieldCoverageMap map(makeConfig()); + size_t field_cell = 0; + while (map.isOutOfField(field_cell)) { + field_cell++; + } + + map.observe(field_cell, 0.25); + EXPECT_DOUBLE_EQ(map.interest(field_cell), 0.75); +} + +TEST(FieldCoverageMap, ASecondWorseLookDoesNotEraseTheBetterOne) { + FieldCoverageMap map(makeConfig()); + size_t field_cell = 0; + while (map.isOutOfField(field_cell)) { + field_cell++; + } + + map.observe(field_cell, 0.9); + map.observe(field_cell, 0.2); + EXPECT_DOUBLE_EQ(map.observation(field_cell), 0.9); +} + +TEST(FieldCoverageMap, ObservationQualityIsClamped) { + FieldCoverageMap map(makeConfig()); + map.observe(0, 5.0); + EXPECT_DOUBLE_EQ(map.observation(0), 1.0); +} + +// --------------------------------------------------------------------------- +// Decay +// --------------------------------------------------------------------------- + +TEST(FieldCoverageMap, ObservationsHalveAfterTheHalfLife) { + FieldCoverageMap map(makeConfig()); + map.observe(0, 1.0); + map.decay(8.0); + EXPECT_NEAR(map.observation(0), 0.5, 1e-9); +} + +TEST(FieldCoverageMap, DecayMakesObservedCellsInterestingAgain) { + FieldCoverageMap map(makeConfig()); + size_t field_cell = 0; + while (map.isOutOfField(field_cell)) { + field_cell++; + } + + map.observe(field_cell, 1.0); + ASSERT_DOUBLE_EQ(map.interest(field_cell), 0.0); + + // This is what makes the head come back to a part of the field it already + // looked at instead of never revisiting it + map.decay(8.0); + EXPECT_NEAR(map.interest(field_cell), 0.5, 1e-9); +} + +TEST(FieldCoverageMap, DecayNeverGoesNegative) { + FieldCoverageMap map(makeConfig()); + map.observe(0, 1.0); + map.decay(1000.0); + EXPECT_GE(map.observation(0), 0.0); +} + +TEST(FieldCoverageMap, NonPositiveTimeStepChangesNothing) { + FieldCoverageMap map(makeConfig()); + map.observe(0, 1.0); + map.decay(0.0); + map.decay(-1.0); + EXPECT_DOUBLE_EQ(map.observation(0), 1.0); +} + +TEST(FieldCoverageMap, DecayIsIndependentOfTheStepSize) { + FieldCoverageMap one_step(makeConfig()); + FieldCoverageMap many_steps(makeConfig()); + one_step.observe(0, 1.0); + many_steps.observe(0, 1.0); + + one_step.decay(4.0); + for (int i = 0; i < 80; i++) { + many_steps.decay(0.05); + } + EXPECT_NEAR(one_step.observation(0), many_steps.observation(0), 1e-9); +} + +// --------------------------------------------------------------------------- +// Aggregate +// --------------------------------------------------------------------------- + +TEST(FieldCoverageMap, TotalInterestCountsOnlyFieldCells) { + FieldCoverageMap map(makeConfig()); + size_t field_cells = 0; + for (size_t index = 0; index < map.size(); index++) { + field_cells += map.isOutOfField(index) ? 0 : 1; + } + EXPECT_NEAR(map.totalInterest(), static_cast(field_cells), 1e-9); +} + +TEST(FieldCoverageMap, TotalInterestDropsAsTheFieldIsObserved) { + FieldCoverageMap map(makeConfig()); + const double before = map.totalInterest(); + for (size_t index = 0; index < map.size(); index++) { + map.observe(index, 1.0); + } + EXPECT_LT(map.totalInterest(), before); + EXPECT_NEAR(map.totalInterest(), 0.0, 1e-9); +} + +TEST(FieldCoverageMap, ResetForgetsEveryObservation) { + FieldCoverageMap map(makeConfig()); + const double before = map.totalInterest(); + map.observe(0, 1.0); + map.reset(); + EXPECT_DOUBLE_EQ(map.totalInterest(), before); +} diff --git a/src/bitbots_motion/bitbots_head_mover/test/test_head_kinematics.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_head_kinematics.cpp new file mode 100644 index 0000000000..dce8794aa5 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_head_kinematics.cpp @@ -0,0 +1,242 @@ +#include + +#include +#include + +using bitbots_head_mover::HeadChainConfig; +using bitbots_head_mover::HeadKinematics; +using bitbots_head_mover::HeadPosition; + +namespace { + +/// A minimal stand-in for the head chain of the real robot. +/// +/// The yaw joint sits one meter above the root and turns around z, the pitch +/// joint sits in the same place and turns around y, and the camera is mounted +/// ten centimeters in front of it. That makes every pose below verifiable by +/// hand, which a chain taken from the real robot description would not be. +constexpr const char* kHeadUrdf = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +/// A chain whose head joints are joined by a third movable joint. +constexpr const char* kExtraJointUrdf = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +} // namespace + +// --------------------------------------------------------------------------- +// Construction +// --------------------------------------------------------------------------- + +TEST(HeadKinematics, BuildsFromAValidDescription) { EXPECT_NE(HeadKinematics::fromUrdf(kHeadUrdf), nullptr); } + +TEST(HeadKinematics, RejectsMalformedDescriptions) { + EXPECT_EQ(HeadKinematics::fromUrdf("this is not a urdf"), nullptr); + EXPECT_EQ(HeadKinematics::fromUrdf(""), nullptr); +} + +TEST(HeadKinematics, RejectsAMissingLink) { + HeadChainConfig config; + config.tip_link = "a_link_that_does_not_exist"; + EXPECT_EQ(HeadKinematics::fromUrdf(kHeadUrdf, config), nullptr); +} + +TEST(HeadKinematics, RejectsAMissingJoint) { + HeadChainConfig config; + config.yaw_joint = "a_joint_that_does_not_exist"; + EXPECT_EQ(HeadKinematics::fromUrdf(kHeadUrdf, config), nullptr); +} + +TEST(HeadKinematics, RejectsChainsWithAdditionalMovableJoints) { + // A joint we do not control would move the camera without us knowing, which + // would silently invalidate every sampled camera pose + EXPECT_EQ(HeadKinematics::fromUrdf(kExtraJointUrdf), nullptr); +} + +TEST(HeadKinematics, ReadsTheJointLimitsFromTheDescription) { + auto kinematics = HeadKinematics::fromUrdf(kHeadUrdf); + ASSERT_NE(kinematics, nullptr); + EXPECT_DOUBLE_EQ(kinematics->urdfLimits().yaw.lower, -1.43); + EXPECT_DOUBLE_EQ(kinematics->urdfLimits().yaw.upper, 1.43); + EXPECT_DOUBLE_EQ(kinematics->urdfLimits().pitch.lower, -1.23); + EXPECT_DOUBLE_EQ(kinematics->urdfLimits().pitch.upper, 1.01); +} + +// --------------------------------------------------------------------------- +// Forward kinematics +// --------------------------------------------------------------------------- + +TEST(HeadKinematics, RestPoseSitsInFrontOfTheJoints) { + auto kinematics = HeadKinematics::fromUrdf(kHeadUrdf); + ASSERT_NE(kinematics, nullptr); + + const auto pose = kinematics->cameraPose({0.0, 0.0}); + ASSERT_TRUE(pose.has_value()); + EXPECT_NEAR(pose->translation().x(), 0.1, 1e-9); + EXPECT_NEAR(pose->translation().y(), 0.0, 1e-9); + EXPECT_NEAR(pose->translation().z(), 1.0, 1e-9); + EXPECT_TRUE(pose->linear().isApprox(Eigen::Matrix3d::Identity(), 1e-9)); +} + +TEST(HeadKinematics, YawRotatesTheCameraAroundTheVerticalAxis) { + auto kinematics = HeadKinematics::fromUrdf(kHeadUrdf); + ASSERT_NE(kinematics, nullptr); + + // Turning the head a quarter turn to the left swings the camera to the side + const auto pose = kinematics->cameraPose({M_PI / 2.0, 0.0}); + ASSERT_TRUE(pose.has_value()); + EXPECT_NEAR(pose->translation().x(), 0.0, 1e-9); + EXPECT_NEAR(pose->translation().y(), 0.1, 1e-9); + EXPECT_NEAR(pose->translation().z(), 1.0, 1e-9); +} + +TEST(HeadKinematics, PitchTiltsTheCameraDown) { + auto kinematics = HeadKinematics::fromUrdf(kHeadUrdf); + ASSERT_NE(kinematics, nullptr); + + // A positive pitch turns around y, which tips the forward axis downwards + const auto pose = kinematics->cameraPose({0.0, M_PI / 2.0}); + ASSERT_TRUE(pose.has_value()); + EXPECT_NEAR(pose->translation().x(), 0.0, 1e-9); + EXPECT_NEAR(pose->translation().y(), 0.0, 1e-9); + EXPECT_NEAR(pose->translation().z(), 0.9, 1e-9); +} + +TEST(HeadKinematics, YawAndPitchComposeInTheRightOrder) { + auto kinematics = HeadKinematics::fromUrdf(kHeadUrdf); + ASSERT_NE(kinematics, nullptr); + + // Pitch acts in the frame the yaw joint already rotated, so tipping fully down + // puts the camera below the joints no matter how the head is turned + const auto pose = kinematics->cameraPose({M_PI / 2.0, M_PI / 2.0}); + ASSERT_TRUE(pose.has_value()); + EXPECT_NEAR(pose->translation().x(), 0.0, 1e-9); + EXPECT_NEAR(pose->translation().y(), 0.0, 1e-9); + EXPECT_NEAR(pose->translation().z(), 0.9, 1e-9); +} + +TEST(HeadKinematics, PoseIsAValidRigidTransform) { + auto kinematics = HeadKinematics::fromUrdf(kHeadUrdf); + ASSERT_NE(kinematics, nullptr); + + const auto pose = kinematics->cameraPose({0.4, -0.3}); + ASSERT_TRUE(pose.has_value()); + EXPECT_NEAR(pose->linear().determinant(), 1.0, 1e-9); + EXPECT_TRUE((pose->linear() * pose->linear().transpose()).isApprox(Eigen::Matrix3d::Identity(), 1e-9)); +} + +TEST(HeadKinematics, EvaluatesPositionsTheRobotIsNotIn) { + // The whole point of the component: two different configurations have to give + // two different camera poses without the robot ever moving + auto kinematics = HeadKinematics::fromUrdf(kHeadUrdf); + ASSERT_NE(kinematics, nullptr); + + const auto left = kinematics->cameraPose({0.5, 0.0}); + ASSERT_TRUE(left.has_value()); + const auto right = kinematics->cameraPose({-0.5, 0.0}); + ASSERT_TRUE(right.has_value()); + EXPECT_GT(left->translation().y(), right->translation().y()); +} + +// --------------------------------------------------------------------------- +// Camera calibration +// --------------------------------------------------------------------------- + +TEST(HeadKinematics, HasNoCalibrationByDefault) { + auto kinematics = HeadKinematics::fromUrdf(kHeadUrdf); + ASSERT_NE(kinematics, nullptr); + EXPECT_FALSE(kinematics->hasCameraCalibration()); +} + +TEST(HeadKinematics, CalibrationIsAppliedInTheCameraFrame) { + auto kinematics = HeadKinematics::fromUrdf(kHeadUrdf); + ASSERT_NE(kinematics, nullptr); + + // The calibration is expressed relative to the uncalibrated optical frame, so + // it has to be composed onto the right hand side of the chain + Eigen::Isometry3d calibration = Eigen::Isometry3d::Identity(); + calibration.translation() = Eigen::Vector3d(0.0, 0.0, 0.05); + kinematics->setCameraCalibration(calibration); + EXPECT_TRUE(kinematics->hasCameraCalibration()); + + // At rest the camera frame is aligned with the root, so the offset shows up on z + const auto rest = kinematics->cameraPose({0.0, 0.0}); + ASSERT_TRUE(rest.has_value()); + EXPECT_NEAR(rest->translation().z(), 1.05, 1e-9); + + // Turned a quarter turn to the left the very same offset still points along + // the camera's own z, which now points along the root's z as well + const auto turned = kinematics->cameraPose({M_PI / 2.0, 0.0}); + ASSERT_TRUE(turned.has_value()); + EXPECT_NEAR(turned->translation().z(), 1.05, 1e-9); + + // Tipped fully down, the camera's z points backwards along the root's x + const auto tipped = kinematics->cameraPose({0.0, M_PI / 2.0}); + ASSERT_TRUE(tipped.has_value()); + EXPECT_NEAR(tipped->translation().x(), 0.05, 1e-9); + EXPECT_NEAR(tipped->translation().z(), 0.9, 1e-9); +} diff --git a/src/bitbots_motion/bitbots_head_mover/test/test_head_trajectory.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_head_trajectory.cpp new file mode 100644 index 0000000000..4937984ec8 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_head_trajectory.cpp @@ -0,0 +1,281 @@ +#include + +#include +#include + +using bitbots_head_mover::adjustSpeeds; +using bitbots_head_mover::buildSearchPatternTrajectory; +using bitbots_head_mover::calculateLowerSpeed; +using bitbots_head_mover::HeadPosition; +using bitbots_head_mover::HeadTrajectory; +using bitbots_head_mover::HeadVelocity; +using bitbots_head_mover::SearchPatternTrajectory; + +namespace { +constexpr double kDegToRad = M_PI / 180.0; + +/// A simple two line pattern in degrees, as generatePattern would produce it. +const std::vector kPattern = {{-30.0, 35.0}, {30.0, 35.0}, {30.0, -5.0}, {-30.0, -5.0}}; + +/// The first pattern keyframe as a head position, i.e. converted to radians. +/// Starting there means there is no transition segment to play. +const HeadPosition kPatternStart{kPattern[0].yaw * kDegToRad, kPattern[0].pitch* kDegToRad}; +} // namespace + +// --------------------------------------------------------------------------- +// HeadTrajectory +// --------------------------------------------------------------------------- + +TEST(HeadTrajectory, IsInvalidBeforeFinalize) { + HeadTrajectory trajectory; + trajectory.addPoint(0.0, {0.0, 0.0}); + trajectory.addPoint(1.0, {1.0, 0.5}); + EXPECT_FALSE(trajectory.valid()); +} + +TEST(HeadTrajectory, IsInvalidWithASingleWaypoint) { + HeadTrajectory trajectory; + trajectory.addPoint(0.0, {0.0, 0.0}); + trajectory.finalize(); + EXPECT_FALSE(trajectory.valid()); +} + +TEST(HeadTrajectory, InterpolatesBetweenWaypoints) { + HeadTrajectory trajectory; + trajectory.addPoint(0.0, {0.0, 0.0}); + trajectory.addPoint(1.0, {1.0, 0.5}); + trajectory.finalize(); + + ASSERT_TRUE(trajectory.valid()); + EXPECT_NEAR(trajectory.position(0.0).yaw, 0.0, 1e-9); + EXPECT_NEAR(trajectory.position(0.0).pitch, 0.0, 1e-9); + EXPECT_NEAR(trajectory.position(1.0).yaw, 1.0, 1e-9); + EXPECT_NEAR(trajectory.position(1.0).pitch, 0.5, 1e-9); + // The quintic spline is monotonic between two resting waypoints + EXPECT_GT(trajectory.position(0.5).yaw, 0.0); + EXPECT_LT(trajectory.position(0.5).yaw, 1.0); +} + +TEST(HeadTrajectory, RespectsTheWaypointVelocities) { + HeadTrajectory trajectory; + trajectory.addPoint(0.0, {0.0, 0.0}, {2.0, -1.0}); + trajectory.addPoint(1.0, {1.0, 0.5}); + trajectory.finalize(); + + ASSERT_TRUE(trajectory.valid()); + EXPECT_NEAR(trajectory.velocity(0.0).yaw, 2.0, 1e-9); + EXPECT_NEAR(trajectory.velocity(0.0).pitch, -1.0, 1e-9); + // The terminal velocity defaults to rest + EXPECT_NEAR(trajectory.velocity(1.0).yaw, 0.0, 1e-9); + EXPECT_NEAR(trajectory.velocity(1.0).pitch, 0.0, 1e-9); +} + +TEST(HeadTrajectory, VelocityIsSigned) { + HeadTrajectory trajectory; + trajectory.addPoint(0.0, {1.0, 0.0}); + trajectory.addPoint(1.0, {-1.0, 0.0}); + trajectory.finalize(); + + ASSERT_TRUE(trajectory.valid()); + // Moving towards a smaller yaw means a negative velocity, the motor goals + // take the absolute value themselves + EXPECT_LT(trajectory.velocity(0.5).yaw, 0.0); +} + +TEST(HeadTrajectory, ReportsDurationAndSize) { + HeadTrajectory trajectory; + trajectory.addPoint(0.0, {0.0, 0.0}); + trajectory.addPoint(0.5, {1.0, 0.0}); + trajectory.addPoint(2.5, {0.0, 0.0}); + trajectory.finalize(); + + EXPECT_EQ(trajectory.size(), 3u); + EXPECT_DOUBLE_EQ(trajectory.duration(), 2.5); +} + +TEST(HeadTrajectory, InvalidTrajectoryEvaluatesToZero) { + HeadTrajectory trajectory; + EXPECT_DOUBLE_EQ(trajectory.position(1.0).yaw, 0.0); + EXPECT_DOUBLE_EQ(trajectory.velocity(1.0).yaw, 0.0); +} + +// --------------------------------------------------------------------------- +// buildSearchPatternTrajectory +// --------------------------------------------------------------------------- + +TEST(BuildSearchPatternTrajectory, EmptyPatternIsInvalid) { + EXPECT_FALSE(buildSearchPatternTrajectory({}, 2.0, {0.0, 0.0}, 6.0).valid()); +} + +TEST(BuildSearchPatternTrajectory, NonPositiveCycleTimeIsInvalid) { + EXPECT_FALSE(buildSearchPatternTrajectory(kPattern, 0.0, {0.0, 0.0}, 6.0).valid()); + EXPECT_FALSE(buildSearchPatternTrajectory(kPattern, -1.0, {0.0, 0.0}, 6.0).valid()); +} + +TEST(BuildSearchPatternTrajectory, NonPositiveTransitionSpeedIsInvalid) { + // Guards against dividing the transition distance by zero + EXPECT_FALSE(buildSearchPatternTrajectory(kPattern, 2.0, {0.0, 0.0}, 0.0).valid()); +} + +TEST(BuildSearchPatternTrajectory, ConvertsThePatternToRadians) { + auto result = buildSearchPatternTrajectory(kPattern, 2.0, kPatternStart, 6.0); + ASSERT_TRUE(result.valid()); + // Starting at the first keyframe means there is no transition to play + EXPECT_NEAR(result.transition_duration, 0.0, 1e-12); + EXPECT_NEAR(result.trajectory.position(0.0).yaw, -30.0 * kDegToRad, 1e-9); + EXPECT_NEAR(result.trajectory.position(0.0).pitch, 35.0 * kDegToRad, 1e-9); +} + +TEST(BuildSearchPatternTrajectory, CycleDurationMatchesTheRequestedCycleTime) { + auto result = buildSearchPatternTrajectory(kPattern, 2.0, kPatternStart, 6.0); + ASSERT_TRUE(result.valid()); + EXPECT_DOUBLE_EQ(result.cycle_duration, 2.0); + // The pattern is closed, so the trajectory ends where it started + EXPECT_DOUBLE_EQ(result.trajectory.duration(), result.transition_duration + 2.0); +} + +TEST(BuildSearchPatternTrajectory, ClosesTheLoop) { + auto result = buildSearchPatternTrajectory(kPattern, 2.0, kPatternStart, 6.0); + ASSERT_TRUE(result.valid()); + HeadPosition start = result.trajectory.position(result.transition_duration); + HeadPosition end = result.trajectory.position(result.trajectory.duration()); + EXPECT_NEAR(start.yaw, end.yaw, 1e-9); + EXPECT_NEAR(start.pitch, end.pitch, 1e-9); +} + +TEST(BuildSearchPatternTrajectory, PrependsATransitionFromTheCurrentPosition) { + const HeadPosition start{0.0, 0.0}; + auto result = buildSearchPatternTrajectory(kPattern, 2.0, start, 6.0); + ASSERT_TRUE(result.valid()); + + // The transition takes as long as the distance divided by the transition speed + const double dyaw = -30.0 * kDegToRad - start.yaw; + const double dpitch = 35.0 * kDegToRad - start.pitch; + const double expected = std::sqrt(dyaw * dyaw + dpitch * dpitch) / 6.0; + EXPECT_NEAR(result.transition_duration, expected, 1e-9); + + // ... and it actually starts at the current head position + EXPECT_NEAR(result.trajectory.position(0.0).yaw, start.yaw, 1e-9); + EXPECT_NEAR(result.trajectory.position(0.0).pitch, start.pitch, 1e-9); +} + +TEST(BuildSearchPatternTrajectory, TransitionSpeedScalesTheTransitionDuration) { + auto slow = buildSearchPatternTrajectory(kPattern, 2.0, {0.0, 0.0}, 3.0); + auto fast = buildSearchPatternTrajectory(kPattern, 2.0, {0.0, 0.0}, 6.0); + ASSERT_TRUE(slow.valid()); + ASSERT_TRUE(fast.valid()); + EXPECT_NEAR(slow.transition_duration, 2.0 * fast.transition_duration, 1e-9); +} + +TEST(BuildSearchPatternTrajectory, DistributesTimeProportionallyToDistance) { + // A pattern whose second leg is three times as long as the first one + const std::vector pattern = {{0.0, 0.0}, {10.0, 0.0}, {40.0, 0.0}}; + auto result = buildSearchPatternTrajectory(pattern, 8.0, pattern[0], 6.0); + ASSERT_TRUE(result.valid()); + + // Legs are 10, 30 and 40 degrees long, so the second keyframe is reached + // after 10/80 of the cycle and the third after 40/80 + EXPECT_NEAR(result.trajectory.position(8.0 * 10.0 / 80.0).yaw, 10.0 * kDegToRad, 1e-9); + EXPECT_NEAR(result.trajectory.position(8.0 * 40.0 / 80.0).yaw, 40.0 * kDegToRad, 1e-9); +} + +TEST(BuildSearchPatternTrajectory, DegeneratePatternDistributesTimeEvenly) { + // The look forward pattern collapses onto a single point, so there is no + // distance to distribute time by and the fallback has to avoid a zero division + const std::vector pattern = {{0.0, 0.0}, {0.0, 0.0}, {0.0, 0.0}, {0.0, 0.0}}; + auto result = buildSearchPatternTrajectory(pattern, 3.0, {0.0, 0.0}, 6.0); + ASSERT_TRUE(result.valid()); + EXPECT_DOUBLE_EQ(result.cycle_duration, 3.0); + EXPECT_TRUE(std::isfinite(result.trajectory.position(1.5).yaw)); + EXPECT_NEAR(result.trajectory.position(1.5).yaw, 0.0, 1e-9); +} + +// --------------------------------------------------------------------------- +// SearchPatternTrajectory::phase +// --------------------------------------------------------------------------- + +TEST(SearchPatternPhase, PassesTheTransitionThroughOnce) { + SearchPatternTrajectory trajectory; + trajectory.transition_duration = 1.0; + trajectory.cycle_duration = 4.0; + trajectory.trajectory.addPoint(0.0, {0.0, 0.0}); + trajectory.trajectory.addPoint(5.0, {1.0, 0.0}); + trajectory.trajectory.finalize(); + ASSERT_TRUE(trajectory.valid()); + + EXPECT_DOUBLE_EQ(trajectory.phase(0.0), 0.0); + EXPECT_DOUBLE_EQ(trajectory.phase(0.5), 0.5); + EXPECT_DOUBLE_EQ(trajectory.phase(1.0), 1.0); +} + +TEST(SearchPatternPhase, LoopsOnlyTheCyclicPart) { + SearchPatternTrajectory trajectory; + trajectory.transition_duration = 1.0; + trajectory.cycle_duration = 4.0; + trajectory.trajectory.addPoint(0.0, {0.0, 0.0}); + trajectory.trajectory.addPoint(5.0, {1.0, 0.0}); + trajectory.trajectory.finalize(); + ASSERT_TRUE(trajectory.valid()); + + // Just after the transition we are at its end + EXPECT_NEAR(trajectory.phase(1.5), 1.5, 1e-12); + // One full cycle later we are back where the cycle started + EXPECT_NEAR(trajectory.phase(5.5), 1.5, 1e-12); + // And the transition is never replayed + EXPECT_GE(trajectory.phase(100.0), 1.0); + EXPECT_LE(trajectory.phase(100.0), 5.0); +} + +TEST(SearchPatternPhase, InvalidTrajectoryStaysAtZero) { + SearchPatternTrajectory trajectory; + EXPECT_DOUBLE_EQ(trajectory.phase(3.0), 0.0); +} + +// --------------------------------------------------------------------------- +// Speed adjustment +// --------------------------------------------------------------------------- + +TEST(CalculateLowerSpeed, ScalesWithTheDistanceRatio) { + // The faster joint takes 2 seconds for its 4 units, so the other joint has to + // cover its 2 units in the same time + EXPECT_DOUBLE_EQ(calculateLowerSpeed(4.0, 2.0, 2.0), 1.0); +} + +TEST(CalculateLowerSpeed, ReturnsZeroWhenTheFasterJointDoesNotMove) { + EXPECT_DOUBLE_EQ(calculateLowerSpeed(0.0, 2.0, 2.0), 0.0); +} + +TEST(AdjustSpeeds, SlowsDownThePitchAxisWhenYawTravelsFurther) { + HeadVelocity speeds = adjustSpeeds({1.0, 0.25}, {0.0, 0.0}, {2.0, 2.0}); + // Yaw keeps its full speed, pitch is scaled by the distance ratio + EXPECT_DOUBLE_EQ(speeds.yaw, 2.0); + EXPECT_DOUBLE_EQ(speeds.pitch, 0.5); +} + +TEST(AdjustSpeeds, SlowsDownTheYawAxisWhenPitchTravelsFurther) { + HeadVelocity speeds = adjustSpeeds({0.25, 1.0}, {0.0, 0.0}, {2.0, 2.0}); + EXPECT_DOUBLE_EQ(speeds.pitch, 2.0); + EXPECT_DOUBLE_EQ(speeds.yaw, 0.5); +} + +TEST(AdjustSpeeds, NeverExceedsTheGivenMaximum) { + // The shorter traveling joint would be allowed to go faster than its maximum + HeadVelocity speeds = adjustSpeeds({10.0, 1.0}, {0.0, 0.0}, {1.0, 1.0}); + EXPECT_LE(speeds.yaw, 1.0); + EXPECT_LE(speeds.pitch, 1.0); +} + +TEST(AdjustSpeeds, BothJointsArriveTogether) { + const HeadPosition goal{1.0, 0.25}; + const HeadPosition current{0.0, 0.0}; + HeadVelocity speeds = adjustSpeeds(goal, current, {2.0, 2.0}); + const double yaw_time = std::abs(goal.yaw - current.yaw) / speeds.yaw; + const double pitch_time = std::abs(goal.pitch - current.pitch) / speeds.pitch; + EXPECT_NEAR(yaw_time, pitch_time, 1e-9); +} + +TEST(AdjustSpeeds, ZeroDistanceLeavesTheOtherJointAtZero) { + // Nothing to move means the derived speed collapses to zero + HeadVelocity speeds = adjustSpeeds({0.0, 0.0}, {0.0, 0.0}, {2.0, 2.0}); + EXPECT_DOUBLE_EQ(speeds.yaw, 0.0); +} diff --git a/src/bitbots_motion/bitbots_head_mover/test/test_look_at.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_look_at.cpp new file mode 100644 index 0000000000..b44064eac7 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_look_at.cpp @@ -0,0 +1,83 @@ +#include + +#include +#include + +using bitbots_head_mover::HeadPosition; +using bitbots_head_mover::kCameraPitchOffset; +using bitbots_head_mover::motorGoalsFromPoint; + +namespace { +geometry_msgs::msg::Point makePoint(double x, double y, double z) { + geometry_msgs::msg::Point point; + point.x = x; + point.y = y; + point.z = z; + return point; +} +} // namespace + +TEST(MotorGoalsFromPoint, PointStraightAheadKeepsTheYawAtRest) { + // Passing a zero camera offset isolates the pure geometry + HeadPosition goal = motorGoalsFromPoint(makePoint(1.0, 0.0, 0.0), makePoint(1.0, 0.0, 0.0), {0.0, 0.0}, 0.0); + EXPECT_NEAR(goal.yaw, 0.0, 1e-9); + EXPECT_NEAR(goal.pitch, 0.0, 1e-9); +} + +TEST(MotorGoalsFromPoint, PointToTheLeftYieldsAPositiveYaw) { + HeadPosition goal = motorGoalsFromPoint(makePoint(1.0, 1.0, 0.0), makePoint(1.0, 1.0, 0.0), {0.0, 0.0}, 0.0); + EXPECT_NEAR(goal.yaw, M_PI / 4.0, 1e-9); +} + +TEST(MotorGoalsFromPoint, PointToTheRightYieldsANegativeYaw) { + HeadPosition goal = motorGoalsFromPoint(makePoint(1.0, -1.0, 0.0), makePoint(1.0, -1.0, 0.0), {0.0, 0.0}, 0.0); + EXPECT_NEAR(goal.yaw, -M_PI / 4.0, 1e-9); +} + +TEST(MotorGoalsFromPoint, PointBelowYieldsAPositivePitch) { + // Pitch is positive when looking down, so a point below the joint tilts down + HeadPosition goal = motorGoalsFromPoint(makePoint(1.0, 0.0, -1.0), makePoint(1.0, 0.0, -1.0), {0.0, 0.0}, 0.0); + EXPECT_NEAR(goal.pitch, M_PI / 4.0, 1e-9); +} + +TEST(MotorGoalsFromPoint, PointAboveYieldsANegativePitch) { + HeadPosition goal = motorGoalsFromPoint(makePoint(1.0, 0.0, 1.0), makePoint(1.0, 0.0, 1.0), {0.0, 0.0}, 0.0); + EXPECT_NEAR(goal.pitch, -M_PI / 4.0, 1e-9); +} + +TEST(MotorGoalsFromPoint, ResultIsRelativeToTheCurrentHeadPosition) { + const HeadPosition current{0.3, 0.2}; + // A point straight ahead in the joint frames means "keep looking where we look" + HeadPosition goal = motorGoalsFromPoint(makePoint(1.0, 0.0, 0.0), makePoint(1.0, 0.0, 0.0), current, 0.0); + EXPECT_NEAR(goal.yaw, current.yaw, 1e-9); + EXPECT_NEAR(goal.pitch, current.pitch, 1e-9); +} + +TEST(MotorGoalsFromPoint, CameraOffsetShiftsThePitchGoalOnly) { + const auto point = makePoint(1.0, 1.0, -1.0); + HeadPosition without = motorGoalsFromPoint(point, point, {0.0, 0.0}, 0.0); + HeadPosition with = motorGoalsFromPoint(point, point, {0.0, 0.0}, kCameraPitchOffset); + EXPECT_NEAR(with.yaw, without.yaw, 1e-9); + EXPECT_NEAR(with.pitch, without.pitch - kCameraPitchOffset, 1e-9); +} + +TEST(MotorGoalsFromPoint, DefaultOffsetIsTheCameraMountingAngle) { + const auto point = makePoint(1.0, 0.0, 0.0); + HeadPosition goal = motorGoalsFromPoint(point, point, {0.0, 0.0}); + EXPECT_NEAR(goal.pitch, -kCameraPitchOffset, 1e-9); +} + +TEST(MotorGoalsFromPoint, SolvesEachJointInItsOwnFrame) { + // The same point expressed in the two joint frames generally differs, and each + // joint has to be solved against its own version of it + HeadPosition goal = motorGoalsFromPoint(makePoint(1.0, 1.0, 0.0), makePoint(1.0, 0.0, -1.0), {0.0, 0.0}, 0.0); + EXPECT_NEAR(goal.yaw, M_PI / 4.0, 1e-9); + EXPECT_NEAR(goal.pitch, M_PI / 4.0, 1e-9); +} + +TEST(MotorGoalsFromPoint, DistanceDoesNotChangeTheAngles) { + HeadPosition near = motorGoalsFromPoint(makePoint(1.0, 1.0, -1.0), makePoint(1.0, 1.0, -1.0), {0.0, 0.0}, 0.0); + HeadPosition far = motorGoalsFromPoint(makePoint(5.0, 5.0, -5.0), makePoint(5.0, 5.0, -5.0), {0.0, 0.0}, 0.0); + EXPECT_NEAR(near.yaw, far.yaw, 1e-9); + EXPECT_NEAR(near.pitch, far.pitch, 1e-9); +} diff --git a/src/bitbots_motion/bitbots_head_mover/test/test_search_pattern.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_search_pattern.cpp new file mode 100644 index 0000000000..de4a3eb7eb --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_search_pattern.cpp @@ -0,0 +1,179 @@ +#include + +#include +#include +#include + +using bitbots_head_mover::generatePattern; +using bitbots_head_mover::HeadPosition; +using bitbots_head_mover::interpolatedSteps; +using bitbots_head_mover::lineAngle; + +// --------------------------------------------------------------------------- +// lineAngle +// --------------------------------------------------------------------------- + +TEST(LineAngle, FirstLineIsMinAngle) { EXPECT_DOUBLE_EQ(lineAngle(0, 5, -5.0, 35.0), -5.0); } + +TEST(LineAngle, LastLineIsMaxAngle) { EXPECT_DOUBLE_EQ(lineAngle(4, 5, -5.0, 35.0), 35.0); } + +TEST(LineAngle, LinesAreEvenlySpaced) { + // Four gaps over a 40 degree span means 10 degrees per line + EXPECT_DOUBLE_EQ(lineAngle(1, 5, -5.0, 35.0), 5.0); + EXPECT_DOUBLE_EQ(lineAngle(2, 5, -5.0, 35.0), 15.0); + EXPECT_DOUBLE_EQ(lineAngle(3, 5, -5.0, 35.0), 25.0); +} + +TEST(LineAngle, SingleLineCollapsesOntoMinAngle) { + // Guards against the division by zero that a single scan line would produce + EXPECT_DOUBLE_EQ(lineAngle(0, 1, -5.0, 35.0), -5.0); +} + +TEST(LineAngle, UsesAbsoluteSpanSoOrderOfBoundsDoesNotFlipStepDirection) { + // The step size is derived from the absolute span, so passing the bounds the + // other way around still steps upwards from the first argument + EXPECT_DOUBLE_EQ(lineAngle(1, 3, 35.0, -5.0), 55.0); +} + +// --------------------------------------------------------------------------- +// interpolatedSteps +// --------------------------------------------------------------------------- + +TEST(InterpolatedSteps, ZeroStepsYieldsNothing) { EXPECT_TRUE(interpolatedSteps(0, 10.0, -30.0, 30.0).empty()); } + +TEST(InterpolatedSteps, ReturnsOneMoreThanRequestedSteps) { + // The implementation adds one step so the upper bound is included + EXPECT_EQ(interpolatedSteps(3, 10.0, -30.0, 30.0).size(), 4u); +} + +TEST(InterpolatedSteps, EndsAtMaxYawAndKeepsPitchConstant) { + auto steps = interpolatedSteps(3, 10.0, -30.0, 30.0); + ASSERT_FALSE(steps.empty()); + EXPECT_DOUBLE_EQ(steps.back().yaw, 30.0); + for (const auto& step : steps) { + EXPECT_DOUBLE_EQ(step.pitch, 10.0); + } +} + +TEST(InterpolatedSteps, IsEvenlySpacedAndExcludesTheLowerBound) { + auto steps = interpolatedSteps(3, 0.0, 0.0, 40.0); + ASSERT_EQ(steps.size(), 4u); + EXPECT_DOUBLE_EQ(steps[0].yaw, 10.0); + EXPECT_DOUBLE_EQ(steps[1].yaw, 20.0); + EXPECT_DOUBLE_EQ(steps[2].yaw, 30.0); + EXPECT_DOUBLE_EQ(steps[3].yaw, 40.0); +} + +TEST(InterpolatedSteps, IsAscendingRegardlessOfBoundOrder) { + // The span is taken as an absolute value, so the steps always ascend from the + // first bound. generatePattern relies on this and reverses the result itself. + auto steps = interpolatedSteps(3, 0.0, 40.0, 0.0); + ASSERT_EQ(steps.size(), 4u); + EXPECT_TRUE(std::is_sorted(steps.begin(), steps.end(), + [](const HeadPosition& a, const HeadPosition& b) { return a.yaw < b.yaw; })); +} + +// --------------------------------------------------------------------------- +// generatePattern +// --------------------------------------------------------------------------- + +TEST(GeneratePattern, SingleScanLineStillProducesKeyframes) { + // A single line clamps the iteration count to its lower bound of two + auto pattern = generatePattern(1, -30.0, 30.0, -5.0, 35.0); + EXPECT_EQ(pattern.size(), 2u); +} + +TEST(GeneratePattern, SingleScanLineProducesFiniteAngles) { + // scan_lines is allowed to be one by the parameter validation, and a NaN + // keyframe would propagate all the way into the published motor goals + auto pattern = generatePattern(1, -30.0, 30.0, -5.0, 35.0); + for (const auto& keyframe : pattern) { + EXPECT_TRUE(std::isfinite(keyframe.yaw)); + EXPECT_TRUE(std::isfinite(keyframe.pitch)); + EXPECT_DOUBLE_EQ(keyframe.pitch, -5.0); + } +} + +TEST(GeneratePattern, KeyframeCountGrowsWithScanLines) { + // Without interpolation the pattern has exactly max(4 * lines - 4, 2) keyframes + EXPECT_EQ(generatePattern(2, -30.0, 30.0, -5.0, 35.0).size(), 4u); + EXPECT_EQ(generatePattern(3, -30.0, 30.0, -5.0, 35.0).size(), 8u); + EXPECT_EQ(generatePattern(4, -30.0, 30.0, -5.0, 35.0).size(), 12u); +} + +TEST(GeneratePattern, StartsAtTheBottomScanLine) { + auto pattern = generatePattern(3, -30.0, 30.0, -5.0, 35.0); + ASSERT_FALSE(pattern.empty()); + // The scan starts on the last line, which sits at the "down" angle + EXPECT_DOUBLE_EQ(pattern.front().pitch, 35.0); +} + +TEST(GeneratePattern, StaysWithinTheConfiguredBounds) { + auto pattern = generatePattern(4, -30.0, 30.0, -5.0, 35.0); + ASSERT_FALSE(pattern.empty()); + for (const auto& keyframe : pattern) { + EXPECT_GE(keyframe.yaw, -30.0); + EXPECT_LE(keyframe.yaw, 30.0); + EXPECT_GE(keyframe.pitch, -5.0); + EXPECT_LE(keyframe.pitch, 35.0); + } +} + +TEST(GeneratePattern, VisitsEveryScanLine) { + const int line_count = 4; + auto pattern = generatePattern(line_count, -30.0, 30.0, -5.0, 35.0); + for (int line = 0; line < line_count; line++) { + const double expected = lineAngle(line, line_count, -5.0, 35.0); + EXPECT_TRUE(std::any_of(pattern.begin(), pattern.end(), + [&](const HeadPosition& keyframe) { return std::abs(keyframe.pitch - expected) < 1e-9; })) + << "scan line " << line << " at pitch " << expected << " was never visited"; + } +} + +TEST(GeneratePattern, AlternatesHorizontalDirectionBetweenLines) { + auto pattern = generatePattern(3, -30.0, 30.0, -5.0, 35.0); + ASSERT_GE(pattern.size(), 4u); + // Consecutive keyframes on the same line sit on opposite sides + EXPECT_NE(pattern[0].yaw, pattern[1].yaw); + EXPECT_DOUBLE_EQ(pattern[0].pitch, pattern[1].pitch); +} + +TEST(GeneratePattern, ReducesTheLowestScanLineTowardsTheCenter) { + const double reduction = 0.2; + auto pattern = generatePattern(3, -30.0, 30.0, -5.0, 35.0, reduction); + bool saw_lowest_line = false; + for (const auto& keyframe : pattern) { + if (std::abs(keyframe.pitch - 35.0) < 1e-6) { + saw_lowest_line = true; + // Looking down and far to the side collides with the body, so those + // keyframes get pulled towards the center + EXPECT_LE(std::abs(keyframe.yaw), 30.0 * reduction + 1e-9); + } else { + EXPECT_DOUBLE_EQ(std::abs(keyframe.yaw), 30.0); + } + } + EXPECT_TRUE(saw_lowest_line); +} + +TEST(GeneratePattern, DefaultReductionLeavesTheLowestScanLineUntouched) { + auto pattern = generatePattern(3, -30.0, 30.0, -5.0, 35.0); + for (const auto& keyframe : pattern) { + EXPECT_DOUBLE_EQ(std::abs(keyframe.yaw), 30.0); + } +} + +TEST(GeneratePattern, InterpolationAddsIntermediateKeyframes) { + auto without = generatePattern(3, -30.0, 30.0, -5.0, 35.0, 1.0, 0); + auto with = generatePattern(3, -30.0, 30.0, -5.0, 35.0, 1.0, 3); + EXPECT_GT(with.size(), without.size()); +} + +TEST(GeneratePattern, DegenerateBoundsProduceAConstantPattern) { + // The look forward mode configures a zero width and zero height pattern + auto pattern = generatePattern(2, 0.0, 0.0, 0.0, 0.0); + ASSERT_FALSE(pattern.empty()); + for (const auto& keyframe : pattern) { + EXPECT_DOUBLE_EQ(keyframe.yaw, 0.0); + EXPECT_DOUBLE_EQ(keyframe.pitch, 0.0); + } +} diff --git a/src/bitbots_motion/bitbots_head_mover/test/test_target_sampler.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_target_sampler.cpp new file mode 100644 index 0000000000..9d843cd43c --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_target_sampler.cpp @@ -0,0 +1,174 @@ +#include + +#include +#include +#include +#include + +using bitbots_head_mover::Candidate; +using bitbots_head_mover::HeadLimits; +using bitbots_head_mover::HeadPosition; +using bitbots_head_mover::SamplerConfig; +using bitbots_head_mover::TargetSampler; + +namespace { +constexpr HeadLimits kLimits{{-1.23, 1.23}, {-1.23, 1.01}}; + +SamplerConfig makeConfig() { + SamplerConfig config; + config.sample_count = 64; + config.last_target_weight = 0.4; + config.current_position_weight = 0.3; + config.uniform_weight = 0.3; + config.last_target_std = 0.3; + config.current_position_std = 0.5; + return config; +} + +/// Whether a position lies within the inclusive joint bounds. +bool withinLimits(const HeadPosition& position, const HeadLimits& limits) { + return position.yaw >= limits.yaw.lower && position.yaw <= limits.yaw.upper && position.pitch >= limits.pitch.lower && + position.pitch <= limits.pitch.upper; +} +} // namespace + +// --------------------------------------------------------------------------- +// TargetSampler +// --------------------------------------------------------------------------- + +TEST(TargetSampler, ProducesCandidates) { + TargetSampler sampler(makeConfig()); + auto candidates = sampler.sample({0.0, 0.0}, std::nullopt, kLimits); + EXPECT_FALSE(candidates.empty()); +} + +TEST(TargetSampler, AlwaysOffersTheCurrentPosition) { + TargetSampler sampler(makeConfig()); + const HeadPosition current{0.2, -0.1}; + auto candidates = sampler.sample(current, std::nullopt, kLimits); + + ASSERT_FALSE(candidates.empty()); + // The first candidate is exactly where the head already is, which guarantees + // the scoring always has something valid to pick and a way to hold still + EXPECT_NEAR(candidates.front().target.yaw, current.yaw, 1e-9); + EXPECT_NEAR(candidates.front().target.pitch, current.pitch, 1e-9); +} + +TEST(TargetSampler, OffersThePreviousTargetWhenGiven) { + TargetSampler sampler(makeConfig()); + const HeadPosition last{0.7, 0.3}; + auto candidates = sampler.sample({0.0, 0.0}, last, kLimits); + + // The previous target has to be among the candidates so a still-best target is + // not lost to sampling noise between cycles + const bool found = std::any_of(candidates.begin(), candidates.end(), [&](const Candidate& candidate) { + return std::abs(candidate.target.yaw - last.yaw) < 1e-9 && std::abs(candidate.target.pitch - last.pitch) < 1e-9; + }); + EXPECT_TRUE(found); +} + +TEST(TargetSampler, CandidatesStayWithinTheJointLimits) { + TargetSampler sampler(makeConfig()); + auto candidates = sampler.sample({0.0, 0.0}, HeadPosition{1.0, 0.9}, kLimits); + + ASSERT_FALSE(candidates.empty()); + for (const auto& candidate : candidates) { + EXPECT_TRUE(withinLimits(candidate.target, kLimits)); + } +} + +TEST(TargetSampler, CandidatesAreDiverse) { + TargetSampler sampler(makeConfig()); + auto candidates = sampler.sample({0.0, 0.0}, std::nullopt, kLimits); + + ASSERT_GT(candidates.size(), 2u); + double min_yaw = 1e9, max_yaw = -1e9; + for (const auto& candidate : candidates) { + min_yaw = std::min(min_yaw, candidate.target.yaw); + max_yaw = std::max(max_yaw, candidate.target.yaw); + } + // A sampler that always returned the same target would make the search pointless + EXPECT_GT(max_yaw - min_yaw, 0.1); +} + +TEST(TargetSampler, ConcentratesAroundTheLastTarget) { + // With almost all weight on the last target and a tight spread, the bulk of + // the draws should cluster around it + SamplerConfig config = makeConfig(); + config.last_target_weight = 1.0; + config.current_position_weight = 0.0; + config.uniform_weight = 0.0; + config.last_target_std = 0.1; + config.sample_count = 200; + + TargetSampler sampler(config); + const HeadPosition last{0.6, 0.2}; + auto candidates = sampler.sample({-1.0, -1.0}, last, kLimits); + + int near = 0; + for (const auto& candidate : candidates) { + if (std::hypot(candidate.target.yaw - last.yaw, candidate.target.pitch - last.pitch) < 0.3) { + near++; + } + } + // The great majority of the drawn samples should sit close to the last target + EXPECT_GT(near, static_cast(config.sample_count) / 2); +} + +TEST(TargetSampler, IsDeterministicForAGivenSeed) { + TargetSampler first(makeConfig(), 1234); + TargetSampler second(makeConfig(), 1234); + auto a = first.sample({0.0, 0.0}, HeadPosition{0.3, 0.1}, kLimits); + auto b = second.sample({0.0, 0.0}, HeadPosition{0.3, 0.1}, kLimits); + + ASSERT_EQ(a.size(), b.size()); + for (size_t i = 0; i < a.size(); i++) { + EXPECT_DOUBLE_EQ(a[i].target.yaw, b[i].target.yaw); + EXPECT_DOUBLE_EQ(a[i].target.pitch, b[i].target.pitch); + } +} + +TEST(TargetSampler, DifferentSeedsExploreDifferently) { + TargetSampler first(makeConfig(), 1); + TargetSampler second(makeConfig(), 2); + auto a = first.sample({0.0, 0.0}, std::nullopt, kLimits); + auto b = second.sample({0.0, 0.0}, std::nullopt, kLimits); + + ASSERT_GT(a.size(), 1u); + ASSERT_GT(b.size(), 1u); + // The current position candidate is identical by construction, so compare a + // randomly drawn one + EXPECT_NE(a[1].target.yaw, b[1].target.yaw); +} + +TEST(TargetSampler, ZeroSampleCountStillOffersTheGuaranteedCandidates) { + SamplerConfig config = makeConfig(); + config.sample_count = 0; + TargetSampler sampler(config); + const HeadPosition current{0.2, -0.1}; + auto candidates = sampler.sample(current, HeadPosition{0.5, 0.3}, kLimits); + + // No random draws, but the current position and the previous target are always + // offered so the scoring never runs out of options + ASSERT_EQ(candidates.size(), 2u); + EXPECT_NEAR(candidates.front().target.yaw, current.yaw, 1e-9); +} + +TEST(TargetSampler, NegativeSampleCountYieldsNothing) { + SamplerConfig config = makeConfig(); + config.sample_count = -1; + TargetSampler sampler(config); + EXPECT_TRUE(sampler.sample({0.0, 0.0}, std::nullopt, kLimits).empty()); +} + +TEST(TargetSampler, WithoutPositiveWeightsOnlyTheGuaranteedCandidatesRemain) { + SamplerConfig config = makeConfig(); + config.last_target_weight = 0.0; + config.current_position_weight = 0.0; + config.uniform_weight = 0.0; + TargetSampler sampler(config); + + // No component to draw from, so only the current position is offered + auto candidates = sampler.sample({0.1, 0.1}, std::nullopt, kLimits); + EXPECT_EQ(candidates.size(), 1u); +} diff --git a/src/bitbots_motion/bitbots_head_mover/test/test_types.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_types.cpp new file mode 100644 index 0000000000..b49dabbc73 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_types.cpp @@ -0,0 +1,67 @@ +#include + +#include + +using bitbots_head_mover::HeadLimits; +using bitbots_head_mover::HeadPosition; +using bitbots_head_mover::JointLimit; + +namespace { +/// The head limits the robot is configured with by default. +constexpr HeadLimits kDefaultLimits{{-1.23, 1.23}, {-1.23, 1.01}}; +} // namespace + +// --------------------------------------------------------------------------- +// JointLimit +// --------------------------------------------------------------------------- + +TEST(JointLimit, ClampLeavesInteriorValuesUntouched) { + constexpr JointLimit limit{-1.0, 1.0}; + EXPECT_DOUBLE_EQ(limit.clamp(0.5), 0.5); + EXPECT_DOUBLE_EQ(limit.clamp(-0.5), -0.5); +} + +TEST(JointLimit, ClampPullsExteriorValuesToTheBound) { + constexpr JointLimit limit{-1.0, 1.0}; + EXPECT_DOUBLE_EQ(limit.clamp(2.0), 1.0); + EXPECT_DOUBLE_EQ(limit.clamp(-2.0), -1.0); +} + +TEST(JointLimit, ContainsIsExclusiveOnTheBounds) { + constexpr JointLimit limit{-1.0, 1.0}; + EXPECT_TRUE(limit.contains(0.0)); + // A goal sitting exactly on a limit has always been rejected as a collision + EXPECT_FALSE(limit.contains(1.0)); + EXPECT_FALSE(limit.contains(-1.0)); + EXPECT_FALSE(limit.contains(1.5)); +} + +// --------------------------------------------------------------------------- +// HeadLimits +// --------------------------------------------------------------------------- + +TEST(HeadLimits, ClampAppliesPerJoint) { + HeadPosition clamped = kDefaultLimits.clamp({5.0, -5.0}); + EXPECT_DOUBLE_EQ(clamped.yaw, 1.23); + EXPECT_DOUBLE_EQ(clamped.pitch, -1.23); +} + +TEST(HeadLimits, ClampUsesTheAsymmetricPitchBounds) { + // Pitch is not symmetric, looking up is limited more than looking down + EXPECT_DOUBLE_EQ(kDefaultLimits.clamp({0.0, 5.0}).pitch, 1.01); + EXPECT_DOUBLE_EQ(kDefaultLimits.clamp({0.0, -5.0}).pitch, -1.23); +} + +TEST(HeadLimits, ContainsRequiresBothJointsInRange) { + EXPECT_TRUE(kDefaultLimits.contains({0.0, 0.0})); + EXPECT_FALSE(kDefaultLimits.contains({2.0, 0.0})); + EXPECT_FALSE(kDefaultLimits.contains({0.0, 2.0})); + EXPECT_FALSE(kDefaultLimits.contains({2.0, 2.0})); +} + +TEST(HeadLimits, ClampedPositionsAreNotNecessarilyContained) { + // Clamping puts a position exactly onto the bound, which contains() rejects. + // The node therefore has to clip and collision check independently. + HeadPosition clamped = kDefaultLimits.clamp({5.0, 0.0}); + EXPECT_FALSE(kDefaultLimits.contains(clamped)); +} diff --git a/src/bitbots_motion/bitbots_head_mover/test/test_world_model.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_world_model.cpp new file mode 100644 index 0000000000..18d41ee031 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_world_model.cpp @@ -0,0 +1,193 @@ +#include + +#include + +using bitbots_head_mover::TimedTarget; +using bitbots_head_mover::weightFromCovariance; +using bitbots_head_mover::WorldModel; +using bitbots_head_mover::WorldModelConfig; + +namespace { +WorldModelConfig makeConfig() { + WorldModelConfig config; + config.filtered_ball_timeout = 5.0; + config.raw_ball_timeout = 0.5; + config.team_ball_timeout = 3.0; + config.robot_timeout = 1.0; + config.covariance_half_weight = 0.5; + return config; +} +} // namespace + +// --------------------------------------------------------------------------- +// Covariance weighting +// --------------------------------------------------------------------------- + +TEST(WeightFromCovariance, PerfectEstimateKeepsFullWeight) { EXPECT_DOUBLE_EQ(weightFromCovariance(0.0, 0.5), 1.0); } + +TEST(WeightFromCovariance, HalvesAtTheConfiguredScale) { EXPECT_DOUBLE_EQ(weightFromCovariance(0.5, 0.5), 0.5); } + +TEST(WeightFromCovariance, DecreasesMonotonically) { + EXPECT_GT(weightFromCovariance(0.1, 0.5), weightFromCovariance(1.0, 0.5)); + EXPECT_GT(weightFromCovariance(1.0, 0.5), weightFromCovariance(10.0, 0.5)); +} + +TEST(WeightFromCovariance, NeverReachesZero) { + // A very uncertain ball is still better than no ball at all + EXPECT_GT(weightFromCovariance(1000.0, 0.5), 0.0); +} + +TEST(WeightFromCovariance, StaysWithinTheUnitInterval) { + for (double covariance = 0.0; covariance < 20.0; covariance += 0.25) { + const double weight = weightFromCovariance(covariance, 0.5); + EXPECT_GT(weight, 0.0); + EXPECT_LE(weight, 1.0); + } +} + +TEST(WeightFromCovariance, TreatsNegativeCovarianceAsPerfect) { + // A negative variance is meaningless, but must not produce a weight above one + EXPECT_DOUBLE_EQ(weightFromCovariance(-1.0, 0.5), 1.0); +} + +// --------------------------------------------------------------------------- +// Filtered ball +// --------------------------------------------------------------------------- + +TEST(WorldModel, StartsEmpty) { + WorldModel world(makeConfig()); + EXPECT_FALSE(world.filteredBall().has_value()); + EXPECT_TRUE(world.rawBalls().empty()); + EXPECT_TRUE(world.robots().empty()); + EXPECT_TRUE(world.teamBalls().empty()); + EXPECT_FALSE(world.hasAnyBall()); +} + +TEST(WorldModel, StoresTheFilteredBallWithACovarianceWeight) { + WorldModel world(makeConfig()); + world.setFilteredBall({1.0, 2.0, 0.0}, 0.5, 10.0); + + ASSERT_TRUE(world.filteredBall().has_value()); + EXPECT_DOUBLE_EQ(world.filteredBall()->position.x(), 1.0); + EXPECT_DOUBLE_EQ(world.filteredBall()->weight, 0.5); + EXPECT_TRUE(world.hasAnyBall()); +} + +TEST(WorldModel, DropsTheFilteredBallAfterItsTimeout) { + WorldModel world(makeConfig()); + world.setFilteredBall({1.0, 2.0, 0.0}, 0.1, 10.0); + + world.prune(14.9); + EXPECT_TRUE(world.filteredBall().has_value()); + + world.prune(15.1); + EXPECT_FALSE(world.filteredBall().has_value()); +} + +// --------------------------------------------------------------------------- +// Raw detections +// --------------------------------------------------------------------------- + +TEST(WorldModel, RawBallsExpireIndividually) { + WorldModel world(makeConfig()); + world.setRawBalls({TimedTarget{{1.0, 0.0, 0.0}, 1.0, 10.0}, TimedTarget{{2.0, 0.0, 0.0}, 1.0, 10.4}}); + + // The older detection is past the short raw timeout, the newer one is not + world.prune(10.6); + ASSERT_EQ(world.rawBalls().size(), 1u); + EXPECT_DOUBLE_EQ(world.rawBalls().front().position.x(), 2.0); +} + +TEST(WorldModel, RawBallsUseAShorterTimeoutThanTheFilteredEstimate) { + WorldModel world(makeConfig()); + world.setFilteredBall({1.0, 0.0, 0.0}, 0.1, 10.0); + world.setRawBalls({TimedTarget{{1.0, 0.0, 0.0}, 1.0, 10.0}}); + + world.prune(11.0); + EXPECT_TRUE(world.rawBalls().empty()); + EXPECT_TRUE(world.filteredBall().has_value()); +} + +TEST(WorldModel, RobotsExpireAfterTheirTimeout) { + WorldModel world(makeConfig()); + world.setRobots({TimedTarget{{1.0, 0.0, 0.0}, 1.0, 10.0}}); + + world.prune(10.9); + EXPECT_EQ(world.robots().size(), 1u); + + world.prune(11.1); + EXPECT_TRUE(world.robots().empty()); +} + +TEST(WorldModel, SettingDetectionsReplacesThePreviousOnes) { + WorldModel world(makeConfig()); + world.setRawBalls({TimedTarget{{1.0, 0.0, 0.0}, 1.0, 10.0}}); + world.setRawBalls({TimedTarget{{2.0, 0.0, 0.0}, 1.0, 10.1}}); + + ASSERT_EQ(world.rawBalls().size(), 1u); + EXPECT_DOUBLE_EQ(world.rawBalls().front().position.x(), 2.0); +} + +// --------------------------------------------------------------------------- +// Team balls +// --------------------------------------------------------------------------- + +TEST(WorldModel, TracksTeamBallsPerRobot) { + WorldModel world(makeConfig()); + world.setTeamBall(2, {1.0, 0.0, 0.0}, 0.1, 10.0); + world.setTeamBall(3, {2.0, 0.0, 0.0}, 0.1, 10.0); + EXPECT_EQ(world.teamBalls().size(), 2u); + + // A teammate reporting again replaces its own entry rather than adding one + world.setTeamBall(2, {1.5, 0.0, 0.0}, 0.1, 10.5); + EXPECT_EQ(world.teamBalls().size(), 2u); +} + +TEST(WorldModel, ASilentTeammateExpiresOnItsOwn) { + WorldModel world(makeConfig()); + world.setTeamBall(2, {1.0, 0.0, 0.0}, 0.1, 10.0); + world.setTeamBall(3, {2.0, 0.0, 0.0}, 0.1, 12.0); + + // Robot 2 went quiet while robot 3 kept talking, so only robot 2's ball ages out + world.prune(13.5); + ASSERT_EQ(world.teamBalls().size(), 1u); + EXPECT_DOUBLE_EQ(world.teamBalls().front().position.x(), 2.0); +} + +TEST(WorldModel, TeamBallsAreWeightedByCovariance) { + WorldModel world(makeConfig()); + world.setTeamBall(2, {1.0, 0.0, 0.0}, 0.5, 10.0); + ASSERT_EQ(world.teamBalls().size(), 1u); + EXPECT_DOUBLE_EQ(world.teamBalls().front().weight, 0.5); +} + +TEST(WorldModel, TeamBallsCountAsBallInformation) { + WorldModel world(makeConfig()); + world.setTeamBall(2, {1.0, 0.0, 0.0}, 0.1, 10.0); + EXPECT_TRUE(world.hasAnyBall()); +} + +// --------------------------------------------------------------------------- +// Housekeeping +// --------------------------------------------------------------------------- + +TEST(WorldModel, PruningIsIdempotent) { + WorldModel world(makeConfig()); + world.setRawBalls({TimedTarget{{1.0, 0.0, 0.0}, 1.0, 10.0}}); + world.prune(10.1); + const size_t after_first = world.rawBalls().size(); + world.prune(10.1); + EXPECT_EQ(world.rawBalls().size(), after_first); +} + +TEST(WorldModel, ClearForgetsEverything) { + WorldModel world(makeConfig()); + world.setFilteredBall({1.0, 0.0, 0.0}, 0.1, 10.0); + world.setRawBalls({TimedTarget{{1.0, 0.0, 0.0}, 1.0, 10.0}}); + world.setRobots({TimedTarget{{1.0, 0.0, 0.0}, 1.0, 10.0}}); + world.setTeamBall(2, {1.0, 0.0, 0.0}, 0.1, 10.0); + + world.clear(); + EXPECT_FALSE(world.hasAnyBall()); + EXPECT_TRUE(world.robots().empty()); +} diff --git a/src/bitbots_msgs/msg/HeadMode.msg b/src/bitbots_msgs/msg/HeadMode.msg index 91caca0dee..7bc996ab38 100644 --- a/src/bitbots_msgs/msg/HeadMode.msg +++ b/src/bitbots_msgs/msg/HeadMode.msg @@ -13,6 +13,8 @@ uint8 DONT_MOVE=3 uint8 SEARCH_BALL_PENALTY = 4 # Do a pattern which only looks in front of the robot uint8 SEARCH_FRONT = 5 +# Continuously decide where to look by sampling and scoring head trajectories +uint8 ACTIVE_VISION = 6 uint8 head_mode