From 7d690f78baca9af6b733465290dffeff3c3e9373 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Sun, 16 Aug 2026 23:03:17 +0200 Subject: [PATCH 1/8] feat: add sampling based active vision head mode Adds an ACTIVE_VISION head mode that samples candidate head trajectories every cycle and scores them against the ball, team ball, robot detections and a decaying field coverage map, instead of replaying a fixed pattern. Extracts the existing head mover into a tested library first; behaviour of the other head modes is unchanged. Missing inputs are reported and the update is dropped rather than substituted for. Also fixes a division by zero producing NaN keyframes for a single scan line, and an off centre coverage grid when the field extent is not a multiple of the cell size. --- .../bitbots_head_mover/CMakeLists.txt | 113 +- .../bitbots_head_mover/config/head_config.yml | 235 +++++ .../bitbots_head_mover/active_vision.hpp | 190 ++++ .../active_vision_debug.hpp | 39 + .../active_vision_scorer.hpp | 178 ++++ .../bitbots_head_mover/camera_model.hpp | 100 ++ .../bitbots_head_mover/field_coverage_map.hpp | 87 ++ .../bitbots_head_mover/head_kinematics.hpp | 94 ++ .../bitbots_head_mover/head_trajectory.hpp | 101 ++ .../include/bitbots_head_mover/look_at.hpp | 26 + .../bitbots_head_mover/search_pattern.hpp | 40 + .../bitbots_head_mover/trajectory_sampler.hpp | 105 ++ .../include/bitbots_head_mover/types.hpp | 64 ++ .../bitbots_head_mover/world_model.hpp | 101 ++ .../bitbots_head_mover/package.xml | 12 + .../bitbots_head_mover/src/active_vision.cpp | 213 ++++ .../src/active_vision_debug.cpp | 218 ++++ .../src/active_vision_scorer.cpp | 190 ++++ .../bitbots_head_mover/src/camera_model.cpp | 103 ++ .../src/field_coverage_map.cpp | 92 ++ .../src/head_kinematics.cpp | 111 ++ .../src/head_trajectory.cpp | 147 +++ .../bitbots_head_mover/src/look_at.cpp | 20 + .../bitbots_head_mover/src/move_head.cpp | 987 ++++++++++++------ .../bitbots_head_mover/src/search_pattern.cpp | 111 ++ .../src/trajectory_sampler.cpp | 140 +++ .../bitbots_head_mover/src/world_model.cpp | 86 ++ .../test/test_active_vision.cpp | 393 +++++++ .../test/test_active_vision_scorer.cpp | 437 ++++++++ .../test/test_camera_model.cpp | 200 ++++ .../test/test_field_coverage_map.cpp | 266 +++++ .../test/test_head_kinematics.cpp | 242 +++++ .../test/test_head_trajectory.cpp | 281 +++++ .../bitbots_head_mover/test/test_look_at.cpp | 83 ++ .../test/test_search_pattern.cpp | 180 ++++ .../test/test_trajectory_sampler.cpp | 260 +++++ .../bitbots_head_mover/test/test_types.cpp | 67 ++ .../test/test_world_model.cpp | 193 ++++ src/bitbots_msgs/msg/HeadMode.msg | 2 + 39 files changed, 6158 insertions(+), 349 deletions(-) create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision_debug.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision_scorer.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/camera_model.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/field_coverage_map.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/head_kinematics.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/head_trajectory.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/look_at.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/search_pattern.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/trajectory_sampler.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/types.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/world_model.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/src/active_vision_debug.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/src/active_vision_scorer.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/src/camera_model.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/src/field_coverage_map.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/src/head_kinematics.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/src/head_trajectory.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/src/look_at.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/src/search_pattern.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/src/trajectory_sampler.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/src/world_model.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_active_vision.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_active_vision_scorer.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_camera_model.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_field_coverage_map.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_head_kinematics.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_head_trajectory.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_look_at.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_search_pattern.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_trajectory_sampler.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_types.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_world_model.cpp diff --git a/src/bitbots_motion/bitbots_head_mover/CMakeLists.txt b/src/bitbots_motion/bitbots_head_mover/CMakeLists.txt index bcd1fcae37..9c10a910df 100644 --- a/src/bitbots_motion/bitbots_head_mover/CMakeLists.txt +++ b/src/bitbots_motion/bitbots_head_mover/CMakeLists.txt @@ -10,13 +10,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) @@ -26,23 +42,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/trajectory_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_trajectory_sampler test/test_trajectory_sampler.cpp) + target_link_libraries(test_trajectory_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 5d2b262dc7..88b01d4199 100644 --- a/src/bitbots_motion/bitbots_head_mover/config/head_config.yml +++ b/src/bitbots_motion/bitbots_head_mover/config/head_config.yml @@ -196,3 +196,238 @@ 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" + + command_lookahead: + type: double + default_value: 0.1 + description: "How far along the selected trajectory the commanded setpoint is taken. Must be greater than zero, as the trajectory starts at the current head position." + validation: + gt<>: [0.0] + + max_velocity_yaw: + type: double + default_value: 4.0 + description: "Maximum yaw speed a sampled trajectory may reach. Deliberately below the motor's mechanical limit." + validation: + gt<>: [0.0] + + max_velocity_pitch: + type: double + default_value: 4.0 + description: "Maximum pitch speed a sampled trajectory may reach" + validation: + gt<>: [0.0] + + sampling: + sample_count: + type: int + default_value: 64 + description: "Number of candidate trajectories drawn per planning cycle" + validation: + gt_eq<>: [1] + + horizon: + type: double + default_value: 1.0 + description: "How far into the future a candidate trajectory reaches" + validation: + gt<>: [0.0] + + midpoint_time: + type: double + default_value: 0.5 + description: "When the intermediate waypoint of a candidate sits. Must be strictly inside the horizon." + validation: + gt<>: [0.0] + + evaluation_points: + type: int + default_value: 2 + description: "Number of points along a candidate that are scored. They are spread evenly over the horizon and end at the endpoint, never including the shared start, so two points score the midpoint and the goal point. This is the main cost driver together with the sample count and the coverage cell size." + validation: + gt_eq<>: [1] + + feasibility_points: + type: int + default_value: 21 + description: "Number of points at which a candidate is checked against the joint and dynamic limits" + validation: + gt_eq<>: [2] + + max_attempts_per_sample: + type: int + default_value: 8 + description: "How often a single candidate is redrawn before it is given up on" + validation: + gt_eq<>: [1] + + midpoint_deviation: + type: double + default_value: 0.3 + description: "How far (in radians) the intermediate waypoint may deviate from the straight path to the endpoint. Larger values allow more curved sweeps but get rejected by the acceleration limit more often." + validation: + gt_eq<>: [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.5 + 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: 8.0 + 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: 3.0 + 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: 3.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: 4.0 + description: "Importance of keeping the filtered ball estimate in view" + validation: + gt_eq<>: [0.0] + + raw_balls: + type: double + default_value: 2.0 + description: "Importance of keeping the raw ball detections in view" + validation: + gt_eq<>: [0.0] + + team_ball: + type: double + default_value: 1.0 + description: "Importance of keeping a ball reported by a teammate in view" + validation: + gt_eq<>: [0.0] + + field_coverage: + type: double + default_value: 1.5 + 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.5 + description: "Importance of keeping detected robots in view" + validation: + gt_eq<>: [0.0] + + + commitment: + type: double + default_value: 1.5 + description: "Importance of agreeing with the previously selected trajectory. This is the knob that trades reactivity against steady head motion." + validation: + gt_eq<>: [0.0] + + debug: + enabled: + type: bool + default_value: false + 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..bddff3e500 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision.hpp @@ -0,0 +1,190 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/// Sampling based head control. +namespace bitbots_head_mover { + +/// Everything the planner needs that is not part of its own state. +struct ActiveVisionInput { + /// Where the head currently is and how it is moving. + HeadPosition head_position; + HeadVelocity head_velocity; + /// 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 trajectory. +/// +/// 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 horizon. +enum class ActiveVisionFailure { + None, + /// An input the planner needs has not arrived yet. + NotReady, + /// The sampler produced no candidate that respects the limits. + NoFeasibleCandidate, + /// The head chain could not resolve a camera pose, so nothing could be scored. + KinematicsFailed, + /// The sampling timings are configured such that no trajectory can be built. + 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 trajectory could be planned at all. + bool valid = false; + /// Why no trajectory 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 head position and velocity to command right now. + HeadPosition position; + 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 trajectories 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. +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 setDynamicLimits(const DynamicLimits& limits) { dynamics_ = limits; } + void setHeadLimits(const HeadLimits& limits) { limits_ = limits; } + 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); } + + /// How far along the selected trajectory the commanded setpoint is taken. + /// + /// Must be greater than zero: the trajectory starts at the measured head + /// position, so a zero lookahead would command the head to stay put. Around + /// one or two control periods gives the motors a target to chase without + /// running ahead of what the next cycle can correct. + void setCommandLookahead(double lookahead) { command_lookahead_ = lookahead; } + + /// 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 candidates. 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: + /// The times along a candidate at which it is scored. + std::vector evaluationTimes() const; + + std::unique_ptr kinematics_; + CameraModel camera_; + WorldModel world_; + std::unique_ptr coverage_; + TrajectorySampler sampler_; + + HeadLimits limits_{{-1.23, 1.23}, {-1.23, 1.01}}; + DynamicLimits dynamics_; + ScoringWeights weights_; + VisibilityWeighting visibility_; + double coverage_distance_half_weight_ = 3.0; + double command_lookahead_ = 0.1; + + /// 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 trajectory selected in the previous cycle and when it was planned, + /// which is what the commitment term is measured against. + HeadTrajectory previous_trajectory_; + double previous_plan_time_ = 0.0; + bool has_previous_ = false; + 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..949a3c6a13 --- /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 candidates and the selected one as markers. +/// +/// Each candidate is drawn as the ground track its optical axis sweeps over, +/// 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 in a distinct colour. +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 trajectories 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 polyline 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, double horizon, 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..713eb8d5c2 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/active_vision_scorer.hpp @@ -0,0 +1,178 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +/// Scoring of candidate head trajectories. +namespace bitbots_head_mover { + +/// Relative importance of the individual scoring terms. +/// +/// Every term is normalized to [0, 1] on its own, so these weights are directly +/// comparable and the total is a plain weighted sum. Raising a weight makes the +/// head care more about that aspect, and setting it to zero disables the term. +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; + /// Agreeing with the previously selected trajectory. This is what keeps the + /// head from flicking between equally good options every cycle. + double commitment = 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; + double commitment = 0.0; + /// The weighted sum of the terms above. + 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 times along a candidate at which it is evaluated. + std::vector evaluation_times; + /// The previously selected trajectory, evaluated at the same times. Empty if + /// there is no previous selection, which disables the commitment term. + std::vector previous_positions; +}; + +/// Scores candidate head trajectories against the current world state. +/// +/// A candidate is evaluated at several points in time rather than as a single +/// pose, so that a trajectory which sweeps across something interesting is +/// preferred over one that merely ends up pointing at it. +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. + /// + /// 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 HeadTrajectory& candidate, 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/trajectory_sampler.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/trajectory_sampler.hpp new file mode 100644 index 0000000000..a7484cb4a3 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/trajectory_sampler.hpp @@ -0,0 +1,105 @@ +#pragma once + +#include +#include +#include +#include + +/// Random generation of candidate head trajectories. +namespace bitbots_head_mover { + +/// The dynamic limits a candidate trajectory has to respect. +struct DynamicLimits { + HeadVelocity max_velocity{4.0, 4.0}; + HeadAcceleration max_acceleration{14.0, 14.0}; +}; + +/// Shape and number of the sampled candidates. +struct SamplerConfig { + /// How many candidates are drawn per planning cycle. + int sample_count = 64; + /// How far into the future a candidate reaches. + double horizon = 1.0; + /// When the intermediate waypoint sits. Splitting the horizon lets a candidate + /// curve instead of only moving straight towards its endpoint. + double midpoint_time = 0.5; + /// How many points along a candidate are evaluated by the scoring. + /// + /// The points are spread evenly over the horizon and always end at the + /// endpoint, never including the start: all candidates share the same start, + /// so scoring it cannot tell them apart. Two points therefore evaluate the + /// midpoint and the goal point. + int evaluation_points = 2; + /// How many points are checked when verifying the dynamic limits. This is + /// finer than the scoring, because a limit violation between two scoring + /// points would go unnoticed otherwise. + int feasibility_points = 21; + /// How many times a single candidate is redrawn before giving up on it. + int max_attempts_per_sample = 8; + /// How far the intermediate waypoint may deviate from the straight path to the + /// endpoint, in radians. This is what lets a candidate curve; drawing the + /// midpoint independently instead would mostly produce detours that violate + /// the acceleration limit and get rejected. + double midpoint_deviation = 0.3; +}; + +/// A candidate trajectory together with the state it ends in. +struct Candidate { + HeadTrajectory trajectory; + /// The head position the candidate ends at, kept for debug output. + HeadPosition endpoint; + /// The intermediate waypoint the candidate was drawn with. + HeadPosition midpoint; +}; + +/// Build a candidate trajectory through a midpoint to a resting endpoint. +/// +/// The trajectory starts in the given state, so a candidate always continues the +/// motion the head is already performing instead of assuming it stands still. +/// The midpoint velocity is derived rather than sampled: it follows the overall +/// direction of travel, which keeps the trajectory from having to stop and +/// restart in the middle. The endpoint is reached at rest, so that a candidate +/// that is never replaced still leaves the head in a defined state. +HeadTrajectory buildCandidateTrajectory(const HeadPosition& start, const HeadVelocity& start_velocity, + const HeadPosition& midpoint, const HeadPosition& endpoint, double midpoint_time, + double horizon, const DynamicLimits& dynamics); + +/// Whether a trajectory stays inside the joint and dynamic limits. +/// +/// Checks position, velocity and acceleration at evenly spaced points, which is +/// an approximation, but a quintic between two waypoints has no room to hide a +/// violation between sufficiently dense samples. +bool isFeasible(const HeadTrajectory& trajectory, const HeadLimits& limits, const DynamicLimits& dynamics, + double horizon, int feasibility_points); + +/// Draws candidate head trajectories. +/// +/// Sampling is done by rejection: candidates are drawn from a range that is +/// roughly reachable within the horizon and then verified against the actual +/// limits, so no candidate that the head cannot physically follow is ever +/// scored. The candidate set always contains the trajectory that holds the +/// current position, which guarantees that there is something to choose from +/// even when every random draw is rejected. +class TrajectorySampler { + public: + explicit TrajectorySampler(const SamplerConfig& config = {}, uint32_t seed = 42); + + void setConfig(const SamplerConfig& config) { config_ = config; } + const SamplerConfig& config() const { return config_; } + + /// Draw candidates starting from the given head state. + std::vector sample(const HeadPosition& start, const HeadVelocity& start_velocity, const HeadLimits& limits, + const DynamicLimits& dynamics); + + private: + /// Draw a single joint value within the reachable part of its limits. + double sampleJoint(double start, double max_velocity, double duration, const JointLimit& limit); + + /// Draw a single joint value near a given center, staying inside the limits. + double sampleAround(double center, double deviation, 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..964578e1a6 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp @@ -0,0 +1,213 @@ +#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::NoFeasibleCandidate: + return "no sampled head trajectory respects the joint and dynamic limits"; + case ActiveVisionFailure::KinematicsFailed: + return "the head chain could not resolve a camera pose for any candidate"; + case ActiveVisionFailure::InvalidSamplerConfig: + return "the sampling horizon and midpoint time cannot describe a trajectory"; + } + 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; +} + +std::vector ActiveVision::evaluationTimes() const { + const SamplerConfig& config = sampler_.config(); + std::vector times; + const int count = std::max(config.evaluation_points, 1); + times.reserve(static_cast(count)); + for (int i = 1; i <= count; i++) { + // Spread over the horizon, ending at the endpoint. The start is deliberately + // not evaluated: every candidate begins at the measured head position, so + // that point scores identically for all of them and can only waste time. + // With two points this evaluates the midpoint and the goal point. + times.push_back(config.horizon * static_cast(i) / static_cast(count)); + } + return times; +} + +ActiveVisionResult ActiveVision::plan(const ActiveVisionInput& input) { + ActiveVisionResult result; + if (!ready()) { + result.failure = ActiveVisionFailure::NotReady; + return result; + } + + // Catch a sampling configuration that cannot describe a trajectory here, so it + // is reported as such instead of surfacing as "every candidate was rejected" + const SamplerConfig& sampler_config = sampler_.config(); + if (!(sampler_config.horizon > 0.0) || !(sampler_config.midpoint_time > 0.0) || + sampler_config.midpoint_time >= sampler_config.horizon) { + 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.evaluation_times = evaluationTimes(); + + // Measure commitment against where the previous selection would be at the very + // same moments in time, not against its raw parameterization, because that + // trajectory started one cycle earlier + if (has_previous_) { + const double offset = input.now - previous_plan_time_; + context.previous_positions.reserve(context.evaluation_times.size()); + for (double time : context.evaluation_times) { + context.previous_positions.push_back( + previous_trajectory_.position(std::min(offset + time, previous_trajectory_.duration()))); + } + } + + result.candidates = sampler_.sample(input.head_position, input.head_velocity, limits_, dynamics_); + if (result.candidates.empty()) { + result.failure = ActiveVisionFailure::NoFeasibleCandidate; + 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].trajectory, 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; + } + + const HeadTrajectory& selected = result.candidates[result.selected].trajectory; + + // Command a point a little way along the selected trajectory rather than its + // start. The trajectory starts at the measured head position, so commanding + // its start would ask the head to stay exactly where it already is and it + // would never move. Only this leading segment is ever executed before the next + // cycle replans, which is what lets the head react immediately while the + // commitment term keeps it from flickering. + const double lookahead = std::clamp(command_lookahead_, 0.0, selected.duration()); + result.position = limits_.clamp(selected.position(lookahead)); + result.velocity = selected.velocity(lookahead); + result.valid = true; + + previous_trajectory_ = selected; + previous_plan_time_ = input.now; + has_previous_ = true; + last_plan_time_ = input.now; + has_planned_ = true; + + return result; +} + +void ActiveVision::reset() { + world_.clear(); + if (coverage_) { + coverage_->reset(); + } + has_previous_ = false; + 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..8d0e27675c --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/active_vision_debug.cpp @@ -0,0 +1,218 @@ +#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); + + for (size_t index = 0; index < result.candidates.size(); index++) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = stamp; + marker.ns = "active_vision_candidates"; + marker.id = static_cast(index); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.scale.x = index == result.selected ? 0.04 : 0.015; + + const cv::Scalar color = scoreColor(normalized[index]); + marker.color.b = static_cast(color[0] / 255.0); + marker.color.g = static_cast(color[1] / 255.0); + marker.color.r = static_cast(color[2] / 255.0); + marker.color.a = index == result.selected ? 1.0f : 0.35f; + + const HeadTrajectory& trajectory = result.candidates[index].trajectory; + const int steps = 12; + for (int step = 0; step <= steps; step++) { + const double t = trajectory.duration() * static_cast(step) / static_cast(steps); + const auto camera_pose = scorer.cameraPoseInMap(trajectory.position(t), robot_pose); + Eigen::Vector3d point; + // A pose the kinematics could not resolve simply has no ground track 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(); + marker.points.push_back(message_point); + } + + // A line strip needs at least two points to be a valid marker + if (marker.points.size() >= 2) { + markers.markers.push_back(marker); + } + } + + return markers; +} + +cv::Mat jointSpaceDebugImage(const ActiveVisionResult& result, const HeadLimits& limits, double horizon, 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); + const int steps = 16; + + // 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 HeadTrajectory& trajectory = result.candidates[index].trajectory; + const bool selected = index == result.selected; + const cv::Scalar color = selected ? cv::Scalar(255, 255, 255) : scoreColor(normalized[index]); + + cv::Point previous = toPixel(trajectory.position(0.0)); + for (int step = 1; step <= steps; step++) { + const double t = horizon * static_cast(step) / static_cast(steps); + const cv::Point current = toPixel(trajectory.position(t)); + cv::line(image, previous, current, color, selected ? 2 : 1, cv::LINE_AA); + previous = current; + } + // Mark where the candidate comes to rest + cv::circle(image, previous, selected ? 4 : 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..963597e6f4 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/active_vision_scorer.cpp @@ -0,0 +1,190 @@ +#include +#include +#include + +namespace bitbots_head_mover { + +namespace { + +/// The joint space distance at which two trajectories count as unrelated. +/// +/// Used to normalize the commitment term. Roughly the width of the reachable +/// head range, so that agreeing to within a few degrees scores close to one. +constexpr double kCommitmentScale = 2.0; + +} // namespace + +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 HeadTrajectory& candidate, const ScoringContext& context) const { + ScoreBreakdown breakdown; + if (!candidate.valid() || context.evaluation_times.empty() || !camera_.valid()) { + return breakdown; + } + + const auto& centers = coverage_.cellCenters(); + + const double point_count = static_cast(context.evaluation_times.size()); + + for (size_t step = 0; step < context.evaluation_times.size(); step++) { + const HeadPosition position = candidate.position(context.evaluation_times[step]); + const auto camera_pose = cameraPoseInMap(position, context.robot_pose); + if (!camera_pose) { + // Partially accumulated terms would understate the candidate rather than + // reject it, so the whole candidate is reported as unscorable + 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. + 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); + } + + // Agreement with the previous selection, evaluated at the same times + if (step < context.previous_positions.size()) { + const HeadPosition& previous = context.previous_positions[step]; + const double distance = std::hypot(position.yaw - previous.yaw, position.pitch - previous.pitch); + breakdown.commitment += std::max(0.0, 1.0 - distance / kCommitmentScale); + } + } + + // Every term is an average over the evaluated points, which keeps it in [0, 1] + // and makes a trajectory that stays on target beat one that only passes over it + breakdown.filtered_ball /= point_count; + breakdown.raw_balls /= point_count; + breakdown.team_ball /= point_count; + breakdown.robots /= point_count; + breakdown.field_coverage /= point_count; + breakdown.commitment /= point_count; + + 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_.commitment * breakdown.commitment; + 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..2c9417d0fe --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/head_kinematics.cpp @@ -0,0 +1,111 @@ +#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..e0b6a8059e --- /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..168e2172a6 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,57 @@ #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 +82,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 +94,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,6 +104,24 @@ 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_; + + // 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")) { // Initialize publisher for head motor goals @@ -113,6 +148,7 @@ class HeadMover { [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { // cppcheck-suppress useInitializationList ball_position_ = *msg; + handle_filtered_ball(*msg); }); // Initialize with a valid frame @@ -132,10 +168,196 @@ 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( + "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"); + } + }); + + balls_subscriber_ = node_->create_subscription( + "balls_relative", 1, + [this](const soccer_vision_3d_msgs::msg::BallArray::SharedPtr msg) { handle_balls(*msg); }); + + robots_subscriber_ = node_->create_subscription( + "robots_relative", 1, + [this](const soccer_vision_3d_msgs::msg::RobotArray::SharedPtr msg) { handle_robots(*msg); }); + + team_data_subscriber_ = node_->create_subscription( + "team_data", 10, [this](const bitbots_msgs::msg::TeamData::SharedPtr msg) { handle_team_data(*msg); }); + + 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.horizon = config.sampling.horizon; + sampler.midpoint_time = config.sampling.midpoint_time; + sampler.evaluation_points = static_cast(config.sampling.evaluation_points); + sampler.feasibility_points = static_cast(config.sampling.feasibility_points); + sampler.max_attempts_per_sample = static_cast(config.sampling.max_attempts_per_sample); + sampler.midpoint_deviation = config.sampling.midpoint_deviation; + active_vision_.setSamplerConfig(sampler); + + bitbots_head_mover::DynamicLimits dynamics; + dynamics.max_velocity = {config.max_velocity_yaw, config.max_velocity_pitch}; + dynamics.max_acceleration = {params_.max_acceleration_yaw, params_.max_acceleration_pitch}; + active_vision_.setDynamicLimits(dynamics); + + active_vision_.setHeadLimits(get_head_limits()); + active_vision_.setCommandLookahead(config.command_lookahead); + 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; + 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.commitment = config.weights.commitment; + 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 +388,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); - - // Check whether the goal is in range yaw and pitch wise - bool goal_not_in_range = check_head_collision(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 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 +483,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 +509,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}; @@ -410,152 +577,93 @@ class HeadMover { } /** - * @brief Returns the current position of the head motors + * @brief Returns the current velocity of the head motors + * + * Sampling starts from the measured velocity, so a replanned trajectory + * continues the motion the head is already performing instead of assuming it + * stands still. Joint states without a velocity field report rest. */ - void get_head_position(double& head_yaw, double& head_pitch) { - head_yaw = 0.0; - head_pitch = 0.0; - // Iterate over all joints and find the head yaw and pitch joints + std::optional get_head_velocity() const { + HeadVelocity velocity; + bool found_yaw = false; + bool found_pitch = false; 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; + } + // A joint state that names the joint but carries no velocity for it must + // not be read as the head standing still: sampling would then plan from a + // standstill while the head is actually moving + if (i >= current_joint_state_->velocity.size()) { + return std::nullopt; + } + if (is_yaw) { + velocity.yaw = current_joint_state_->velocity[i]; + found_yaw = true; + } else { + velocity.pitch = current_joint_state_->velocity[i]; + found_pitch = true; } } - } - - /** - * @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); + if (!found_yaw || !found_pitch) { + return std::nullopt; } - return output_points; + return velocity; } /** - * @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; + * @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. + */ + 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++) { + 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; } - - // 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; - + // 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; + } + 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 +678,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 +707,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 +735,246 @@ 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 { + 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]); + 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}); + } + 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}); + } + 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]); + 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(), active_vision_.samplerConfig().horizon, + 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; + } + + const auto head_velocity = get_head_velocity(); + if (!head_velocity) { + hold_active_vision_position( + "the joint states carry no head joint velocities, which sampling needs to continue the current motion"); + return; + } + + bitbots_head_mover::ActiveVisionInput input; + input.head_position = *head_position; + input.head_velocity = *head_velocity; + input.robot_pose = robot_pose; + input.now = node_->now().seconds(); + + const bitbots_head_mover::ActiveVisionResult 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 trajectory candidates because they could not be scored", + result.unscorable_candidates, result.candidates.size()); + } + + active_vision_hold_position_ = result.position; + // 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) { + 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 +985,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 +1002,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 +1041,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 +1049,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 +1065,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(); } } @@ -793,6 +1083,9 @@ class HeadMover { look_at_point.point = ball_position_.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(); 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/trajectory_sampler.cpp b/src/bitbots_motion/bitbots_head_mover/src/trajectory_sampler.cpp new file mode 100644 index 0000000000..f6f28a345b --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/src/trajectory_sampler.cpp @@ -0,0 +1,140 @@ +#include +#include +#include + +namespace bitbots_head_mover { + +HeadTrajectory buildCandidateTrajectory(const HeadPosition& start, const HeadVelocity& start_velocity, + const HeadPosition& midpoint, const HeadPosition& endpoint, + double midpoint_time, double horizon, const DynamicLimits& dynamics) { + HeadTrajectory trajectory; + if (!(horizon > 0.0) || !(midpoint_time > 0.0) || midpoint_time >= horizon) { + return trajectory; + } + + // The midpoint velocity follows the overall direction of travel, scaled by the + // total duration. This is the usual finite difference tangent: it keeps the + // trajectory moving through the midpoint instead of stopping there, without + // adding another sampled dimension. Clamping it to the maximum keeps the + // waypoint itself from being the reason a candidate is infeasible. + HeadVelocity midpoint_velocity{ + std::clamp((endpoint.yaw - start.yaw) / horizon, -dynamics.max_velocity.yaw, dynamics.max_velocity.yaw), + std::clamp((endpoint.pitch - start.pitch) / horizon, -dynamics.max_velocity.pitch, dynamics.max_velocity.pitch)}; + + trajectory.addPoint(0.0, start, start_velocity); + trajectory.addPoint(midpoint_time, midpoint, midpoint_velocity); + // The endpoint is reached at rest, so a candidate that is never replaced + // leaves the head in a defined state rather than drifting on + trajectory.addPoint(horizon, endpoint); + trajectory.finalize(); + return trajectory; +} + +bool isFeasible(const HeadTrajectory& trajectory, const HeadLimits& limits, const DynamicLimits& dynamics, + double horizon, int feasibility_points) { + if (!trajectory.valid() || feasibility_points < 2 || !(horizon > 0.0)) { + return false; + } + + for (int i = 0; i < feasibility_points; i++) { + const double t = horizon * static_cast(i) / static_cast(feasibility_points - 1); + + const HeadPosition position = trajectory.position(t); + if (!limits.contains(position)) { + return false; + } + + const HeadVelocity velocity = trajectory.velocity(t); + if (std::abs(velocity.yaw) > dynamics.max_velocity.yaw || + std::abs(velocity.pitch) > dynamics.max_velocity.pitch) { + return false; + } + + const HeadAcceleration acceleration = trajectory.acceleration(t); + if (std::abs(acceleration.yaw) > dynamics.max_acceleration.yaw || + std::abs(acceleration.pitch) > dynamics.max_acceleration.pitch) { + return false; + } + } + + return true; +} + +TrajectorySampler::TrajectorySampler(const SamplerConfig& config, uint32_t seed) : config_(config), rng_(seed) {} + +double TrajectorySampler::sampleJoint(double start, double max_velocity, double duration, const JointLimit& limit) { + // Only draw from the part of the joint range that can plausibly be reached in + // the available time. Drawing from the full range would make almost every + // candidate fail the feasibility check when the horizon is short. + const double reach = std::abs(max_velocity) * duration; + const double lower = std::max(limit.lower, start - reach); + const double upper = std::min(limit.upper, start + reach); + if (!(upper > lower)) { + return std::clamp(start, limit.lower, limit.upper); + } + return std::uniform_real_distribution(lower, upper)(rng_); +} + +double TrajectorySampler::sampleAround(double center, double deviation, const JointLimit& limit) { + const double lower = std::max(limit.lower, center - std::abs(deviation)); + const double upper = std::min(limit.upper, center + std::abs(deviation)); + if (!(upper > lower)) { + return std::clamp(center, limit.lower, limit.upper); + } + return std::uniform_real_distribution(lower, upper)(rng_); +} + +std::vector TrajectorySampler::sample(const HeadPosition& start, const HeadVelocity& start_velocity, + const HeadLimits& limits, const DynamicLimits& dynamics) { + std::vector candidates; + if (config_.sample_count <= 0) { + return candidates; + } + candidates.reserve(static_cast(config_.sample_count) + 1); + + // Always offer the candidate that comes to rest where the head already is. It + // gives the scoring something to fall back on when every random draw is + // rejected, and it is the trajectory that suppresses motion when nothing on + // the field is worth looking at. + { + const HeadPosition hold = limits.clamp(start); + HeadTrajectory trajectory = + buildCandidateTrajectory(start, start_velocity, hold, hold, config_.midpoint_time, config_.horizon, dynamics); + if (isFeasible(trajectory, limits, dynamics, config_.horizon, config_.feasibility_points)) { + candidates.push_back({std::move(trajectory), hold, hold}); + } + } + + // Where the midpoint would sit if the head travelled straight to the endpoint + const double straight_fraction = config_.midpoint_time / config_.horizon; + + for (int i = 0; i < config_.sample_count; i++) { + for (int attempt = 0; attempt < config_.max_attempts_per_sample; attempt++) { + const HeadPosition endpoint{ + sampleJoint(start.yaw, dynamics.max_velocity.yaw, config_.horizon, limits.yaw), + sampleJoint(start.pitch, dynamics.max_velocity.pitch, config_.horizon, limits.pitch)}; + + // Draw the midpoint around the straight path towards the endpoint rather + // than independently of it. An independent midpoint usually demands a + // detour that the acceleration limit cannot serve, which is why doing it + // that way rejected roughly a third of all draws. The deviation still + // allows curved sweeps that cover more ground than a straight move. + const HeadPosition midpoint{ + sampleAround(start.yaw + (endpoint.yaw - start.yaw) * straight_fraction, config_.midpoint_deviation, + limits.yaw), + sampleAround(start.pitch + (endpoint.pitch - start.pitch) * straight_fraction, config_.midpoint_deviation, + limits.pitch)}; + + HeadTrajectory trajectory = buildCandidateTrajectory(start, start_velocity, midpoint, endpoint, + config_.midpoint_time, config_.horizon, dynamics); + if (isFeasible(trajectory, limits, dynamics, config_.horizon, config_.feasibility_points)) { + candidates.push_back({std::move(trajectory), endpoint, midpoint}); + break; + } + } + } + + 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..35ed0e37a1 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_active_vision.cpp @@ -0,0 +1,393 @@ +#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; + // A smaller sample count keeps the tests quick without changing the behavior + config.sample_count = 24; + config.horizon = 1.0; + config.midpoint_time = 0.5; + config.evaluation_points = 5; + config.feasibility_points = 11; + 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.head_velocity = {0.0, 0.0}; + input.robot_pose = Eigen::Isometry3d::Identity(); + input.now = now; + return input; +} + +} // 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, PlansAFeasibleTrajectory) { + 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(planner.headLimits().contains(result.position)); +} + +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, CommandsAPositionAheadOfTheCurrentOne) { + ActiveVision planner = makeReadyPlanner(); + planner.setCommandLookahead(0.1); + // 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); + + // Commanding the trajectory's start would mean commanding the position the + // head is already in, and the head would never move + const double distance = std::hypot(result.position.yaw, result.position.pitch); + EXPECT_GT(distance, 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.commitment = 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.position.yaw, 0.0); +} + +TEST(ActiveVision, TurnsTheOtherWayForABallOnTheOtherSide) { + ActiveVision planner = makeReadyPlanner(); + ScoringWeights weights; + weights.field_coverage = 0.0; + weights.commitment = 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.position.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, CommitmentKeepsConsecutivePlansTogether) { + ActiveVision committed = makeReadyPlanner(); + ScoringWeights strong; + strong.commitment = 50.0; + committed.setScoringWeights(strong); + + ActiveVision flighty = makeReadyPlanner(); + ScoringWeights none; + none.commitment = 0.0; + flighty.setScoringWeights(none); + + double committed_travel = 0.0; + double flighty_travel = 0.0; + HeadPosition committed_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 = committed_previous; + const auto a = committed.plan(input); + ASSERT_TRUE(a.valid); + committed_travel += std::hypot(a.position.yaw - committed_previous.yaw, a.position.pitch - committed_previous.pitch); + committed_previous = a.position; + + input.head_position = flighty_previous; + const auto b = flighty.plan(input); + ASSERT_TRUE(b.valid); + flighty_travel += std::hypot(b.position.yaw - flighty_previous.yaw, b.position.pitch - flighty_previous.pitch); + flighty_previous = b.position; + } + + // This is the whole point of the commitment term: without it the head chases + // whichever candidate happens to win this cycle and jitters + EXPECT_LT(committed_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(), makeSamplerConfig().horizon, 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(), makeSamplerConfig().horizon, 400); + const cv::Mat without_candidates = + bitbots_head_mover::jointSpaceDebugImage(ActiveVisionResult(), planner.headLimits(), 1.0, 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(), 1.0, 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..47a61b7e77 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_active_vision_scorer.cpp @@ -0,0 +1,437 @@ +#include + +#include +#include +#include + +using bitbots_head_mover::ActiveVisionScorer; +using bitbots_head_mover::buildCandidateTrajectory; +using bitbots_head_mover::CameraModel; +using bitbots_head_mover::DynamicLimits; +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::HeadTrajectory; +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; +} + +/// A trajectory that holds a single head position for the whole horizon. +HeadTrajectory holdAt(const HeadPosition& position) { + return buildCandidateTrajectory(position, {}, position, position, 0.5, 1.0, DynamicLimits{}); +} + +ScoringContext makeContext() { + ScoringContext context; + // The robot stands in the center of the field looking down the long axis + context.robot_pose = Eigen::Isometry3d::Identity(); + context.evaluation_times = {0.0, 0.25, 0.5, 0.75, 1.0}; + 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()); + EXPECT_DOUBLE_EQ(scorer.score(holdAt({0.0, 0.0}), makeContext()).total, 0.0); +} + +TEST(ActiveVisionScorer, ScoresZeroForAnInvalidCandidate) { + Fixture fixture; + EXPECT_DOUBLE_EQ(fixture.scorer().score(HeadTrajectory(), makeContext()).total, 0.0); +} + +TEST(ActiveVisionScorer, ScoresZeroWithoutEvaluationTimes) { + Fixture fixture; + ScoringContext context = makeContext(); + context.evaluation_times.clear(); + EXPECT_DOUBLE_EQ(fixture.scorer().score(holdAt({0.0, 0.0}), context).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(holdAt({0.0, 0.45}), makeContext()).filtered_ball; + const double away = scorer.score(holdAt({-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(holdAt({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(holdAt({0.0, 0.45}), makeContext()).filtered_ball; + + EXPECT_GT(certain, uncertain); +} + +TEST(ActiveVisionScorer, NoBallMeansNoBallScore) { + Fixture fixture; + EXPECT_DOUBLE_EQ(fixture.scorer().score(holdAt({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(holdAt({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(holdAt({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(holdAt({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(holdAt({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(holdAt({0.0, 0.4}), makeContext()).field_coverage, 0.0); +} + +TEST(ActiveVisionScorer, AlreadyObservedFieldIsWorthLess) { + Fixture fixture; + const auto candidate = holdAt({0.0, 0.4}); + const double before = fixture.scorer().score(candidate, 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(candidate, makeContext()).field_coverage; + + EXPECT_GT(before, 0.0); + EXPECT_LT(after, before); +} + +TEST(ActiveVisionScorer, ObservationsDecayBackIntoInterest) { + Fixture fixture; + const auto candidate = holdAt({0.0, 0.4}); + const double fresh = fixture.scorer().score(candidate, makeContext()).field_coverage; + + fixture.scorer().recordObservation(fixture.coverage, {0.0, 0.4}, makeContext().robot_pose); + const double observed = fixture.scorer().score(candidate, 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(candidate, 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(holdAt({-1.2, 0.4}), context).field_coverage; + const double away_from_field = scorer.score(holdAt({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(holdAt({0.0, -1.2}), makeContext()).field_coverage; + const double field = scorer.score(holdAt({0.0, 0.4}), makeContext()).field_coverage; + + EXPECT_DOUBLE_EQ(sky, 0.0); + EXPECT_GT(field, sky); +} + +TEST(ActiveVisionScorer, DistantFieldCountsForLessThanNearField) { + Fixture fixture; + + // Two cells of field, one close to the robot and one far away, both unseen. + // A view of the far one sweeps up more ground 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(holdAt({0.0, 0.8}), context).field_coverage; + const double far_view = scorer.score(holdAt({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 auto far_view = holdAt({0.0, 0.1}); + const auto near_view = holdAt({0.0, 0.8}); + + const double sharp_ratio = sharp.score(far_view, context).field_coverage / + std::max(sharp.score(near_view, context).field_coverage, 1e-12); + const double flat_ratio = flat.score(far_view, context).field_coverage / + std::max(flat.score(near_view, context).field_coverage, 1e-12); + + // Discounting distance more sharply has to move the balance towards the near view + EXPECT_LT(sharp_ratio, flat_ratio); +} + +// --------------------------------------------------------------------------- +// Commitment +// --------------------------------------------------------------------------- + +TEST(ActiveVisionScorer, AgreeingWithThePreviousSelectionScoresHigher) { + Fixture fixture; + auto scorer = fixture.scorer(); + + ScoringContext context = makeContext(); + context.previous_positions.assign(context.evaluation_times.size(), HeadPosition{0.5, 0.2}); + + const double same = scorer.score(holdAt({0.5, 0.2}), context).commitment; + const double different = scorer.score(holdAt({-0.5, -0.2}), context).commitment; + EXPECT_DOUBLE_EQ(same, 1.0); + EXPECT_LT(different, same); +} + +TEST(ActiveVisionScorer, NoPreviousSelectionMeansNoCommitment) { + Fixture fixture; + // Without a previous trajectory the term must not favour any candidate + EXPECT_DOUBLE_EQ(fixture.scorer().score(holdAt({0.5, 0.2}), makeContext()).commitment, 0.0); +} + +TEST(ActiveVisionScorer, CommitmentStaysWithinTheUnitInterval) { + Fixture fixture; + auto scorer = fixture.scorer(); + ScoringContext context = makeContext(); + context.previous_positions.assign(context.evaluation_times.size(), HeadPosition{1.2, 1.0}); + + for (double yaw = -1.2; yaw <= 1.2; yaw += 0.2) { + const double commitment = scorer.score(holdAt({yaw, 0.0}), context).commitment; + EXPECT_GE(commitment, 0.0); + EXPECT_LE(commitment, 1.0); + } +} + +// --------------------------------------------------------------------------- +// Aggregation +// --------------------------------------------------------------------------- + +TEST(ActiveVisionScorer, EveryTermStaysWithinTheUnitInterval) { + 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_positions.assign(context.evaluation_times.size(), 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(holdAt({yaw, pitch}), context); + for (double term : {breakdown.filtered_ball, breakdown.raw_balls, breakdown.team_ball, breakdown.field_coverage, + breakdown.robots, breakdown.commitment}) { + EXPECT_GE(term, 0.0); + EXPECT_LE(term, 1.0); + } + } + } +} + +TEST(ActiveVisionScorer, TheTotalIsTheWeightedSumOfTheTerms) { + Fixture fixture; + fixture.world.setFilteredBall({2.0, 0.0, 0.0}, 0.1, 0.0); + auto scorer = fixture.scorer(); + const ScoringWeights& weights = scorer.weights(); + + const auto breakdown = scorer.score(holdAt({0.0, 0.4}), makeContext()); + 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.commitment * breakdown.commitment; + EXPECT_NEAR(breakdown.total, expected, 1e-12); +} + +TEST(ActiveVisionScorer, ZeroWeightsDisableATerm) { + 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.commitment = 0.0; + scorer.setWeights(weights); + + EXPECT_DOUBLE_EQ(scorer.score(holdAt({0.0, 0.4}), makeContext()).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(holdAt({0.0, 0.5}), facing).filtered_ball; + const double away = scorer.score(holdAt({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..d34b819c24 --- /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..6e68740baa --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_search_pattern.cpp @@ -0,0 +1,180 @@ +#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_trajectory_sampler.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_trajectory_sampler.cpp new file mode 100644 index 0000000000..a0ac9ab145 --- /dev/null +++ b/src/bitbots_motion/bitbots_head_mover/test/test_trajectory_sampler.cpp @@ -0,0 +1,260 @@ +#include + +#include +#include + +using bitbots_head_mover::buildCandidateTrajectory; +using bitbots_head_mover::DynamicLimits; +using bitbots_head_mover::HeadLimits; +using bitbots_head_mover::HeadPosition; +using bitbots_head_mover::HeadTrajectory; +using bitbots_head_mover::HeadVelocity; +using bitbots_head_mover::isFeasible; +using bitbots_head_mover::SamplerConfig; +using bitbots_head_mover::TrajectorySampler; + +namespace { +constexpr HeadLimits kLimits{{-1.23, 1.23}, {-1.23, 1.01}}; + +DynamicLimits makeDynamics() { + DynamicLimits dynamics; + dynamics.max_velocity = {4.0, 4.0}; + dynamics.max_acceleration = {14.0, 14.0}; + return dynamics; +} + +SamplerConfig makeConfig() { + SamplerConfig config; + config.sample_count = 64; + config.horizon = 1.0; + config.midpoint_time = 0.5; + config.evaluation_points = 5; + config.feasibility_points = 21; + return config; +} +} // namespace + +// --------------------------------------------------------------------------- +// buildCandidateTrajectory +// --------------------------------------------------------------------------- + +TEST(BuildCandidateTrajectory, StartsInTheGivenState) { + const HeadPosition start{0.1, -0.2}; + const HeadVelocity start_velocity{0.5, -0.3}; + HeadTrajectory trajectory = + buildCandidateTrajectory(start, start_velocity, {0.3, 0.0}, {0.5, 0.2}, 0.5, 1.0, makeDynamics()); + + ASSERT_TRUE(trajectory.valid()); + // Continuing the motion the head is already performing is what keeps + // replanning every cycle from producing a jerk + EXPECT_NEAR(trajectory.position(0.0).yaw, start.yaw, 1e-9); + EXPECT_NEAR(trajectory.position(0.0).pitch, start.pitch, 1e-9); + EXPECT_NEAR(trajectory.velocity(0.0).yaw, start_velocity.yaw, 1e-9); + EXPECT_NEAR(trajectory.velocity(0.0).pitch, start_velocity.pitch, 1e-9); +} + +TEST(BuildCandidateTrajectory, PassesThroughTheMidpoint) { + const HeadPosition midpoint{0.3, 0.1}; + HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, midpoint, {0.5, 0.2}, 0.5, 1.0, makeDynamics()); + + ASSERT_TRUE(trajectory.valid()); + EXPECT_NEAR(trajectory.position(0.5).yaw, midpoint.yaw, 1e-9); + EXPECT_NEAR(trajectory.position(0.5).pitch, midpoint.pitch, 1e-9); +} + +TEST(BuildCandidateTrajectory, EndsAtTheEndpointAtRest) { + const HeadPosition endpoint{0.5, 0.2}; + HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, {0.3, 0.1}, endpoint, 0.5, 1.0, makeDynamics()); + + ASSERT_TRUE(trajectory.valid()); + EXPECT_NEAR(trajectory.position(1.0).yaw, endpoint.yaw, 1e-9); + EXPECT_NEAR(trajectory.position(1.0).pitch, endpoint.pitch, 1e-9); + EXPECT_NEAR(trajectory.velocity(1.0).yaw, 0.0, 1e-9); + EXPECT_NEAR(trajectory.velocity(1.0).pitch, 0.0, 1e-9); +} + +TEST(BuildCandidateTrajectory, MidpointVelocityFollowsTheDirectionOfTravel) { + // Moving to a larger yaw means the head is still moving that way at the midpoint + HeadTrajectory forward = buildCandidateTrajectory({0.0, 0.0}, {}, {0.3, 0.0}, {0.6, 0.0}, 0.5, 1.0, makeDynamics()); + ASSERT_TRUE(forward.valid()); + EXPECT_GT(forward.velocity(0.5).yaw, 0.0); + + HeadTrajectory backward = buildCandidateTrajectory({0.0, 0.0}, {}, {-0.3, 0.0}, {-0.6, 0.0}, 0.5, 1.0, makeDynamics()); + ASSERT_TRUE(backward.valid()); + EXPECT_LT(backward.velocity(0.5).yaw, 0.0); +} + +TEST(BuildCandidateTrajectory, MidpointVelocityIsClampedToTheLimit) { + DynamicLimits dynamics = makeDynamics(); + dynamics.max_velocity = {0.1, 0.1}; + // A distant endpoint would imply a midpoint velocity far above the limit + HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, {0.5, 0.0}, {1.0, 0.0}, 0.5, 1.0, dynamics); + ASSERT_TRUE(trajectory.valid()); + EXPECT_LE(std::abs(trajectory.velocity(0.5).yaw), dynamics.max_velocity.yaw + 1e-9); +} + +TEST(BuildCandidateTrajectory, RejectsDegenerateTimings) { + EXPECT_FALSE(buildCandidateTrajectory({}, {}, {}, {}, 0.5, 0.0, makeDynamics()).valid()); + EXPECT_FALSE(buildCandidateTrajectory({}, {}, {}, {}, 0.0, 1.0, makeDynamics()).valid()); + // The midpoint has to sit strictly inside the horizon + EXPECT_FALSE(buildCandidateTrajectory({}, {}, {}, {}, 1.0, 1.0, makeDynamics()).valid()); + EXPECT_FALSE(buildCandidateTrajectory({}, {}, {}, {}, 1.5, 1.0, makeDynamics()).valid()); +} + +// --------------------------------------------------------------------------- +// isFeasible +// --------------------------------------------------------------------------- + +TEST(IsFeasible, AcceptsAGentleTrajectory) { + HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, {0.1, 0.05}, {0.2, 0.1}, 0.5, 1.0, makeDynamics()); + EXPECT_TRUE(isFeasible(trajectory, kLimits, makeDynamics(), 1.0, 21)); +} + +TEST(IsFeasible, RejectsTrajectoriesLeavingTheJointLimits) { + // The endpoint is far outside the head's reachable range + HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, {1.0, 0.0}, {2.0, 0.0}, 0.5, 1.0, makeDynamics()); + EXPECT_FALSE(isFeasible(trajectory, kLimits, makeDynamics(), 1.0, 21)); +} + +TEST(IsFeasible, RejectsTrajectoriesExceedingTheVelocityLimit) { + DynamicLimits dynamics = makeDynamics(); + dynamics.max_velocity = {0.05, 0.05}; + // Crossing the whole range in a second needs far more than the allowed speed + HeadTrajectory trajectory = buildCandidateTrajectory({-1.0, 0.0}, {}, {0.0, 0.0}, {1.0, 0.0}, 0.5, 1.0, dynamics); + EXPECT_FALSE(isFeasible(trajectory, kLimits, dynamics, 1.0, 21)); +} + +TEST(IsFeasible, RejectsTrajectoriesExceedingTheAccelerationLimit) { + DynamicLimits dynamics = makeDynamics(); + dynamics.max_acceleration = {0.01, 0.01}; + HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, {0.5, 0.0}, {1.0, 0.0}, 0.5, 1.0, dynamics); + EXPECT_FALSE(isFeasible(trajectory, kLimits, dynamics, 1.0, 21)); +} + +TEST(IsFeasible, RejectsInvalidTrajectories) { + EXPECT_FALSE(isFeasible(HeadTrajectory(), kLimits, makeDynamics(), 1.0, 21)); +} + +TEST(IsFeasible, NeedsAtLeastTwoCheckPoints) { + HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, {0.1, 0.0}, {0.2, 0.0}, 0.5, 1.0, makeDynamics()); + EXPECT_FALSE(isFeasible(trajectory, kLimits, makeDynamics(), 1.0, 1)); +} + +// --------------------------------------------------------------------------- +// TrajectorySampler +// --------------------------------------------------------------------------- + +TEST(TrajectorySampler, ProducesCandidates) { + TrajectorySampler sampler(makeConfig()); + auto candidates = sampler.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); + EXPECT_FALSE(candidates.empty()); +} + +TEST(TrajectorySampler, AlwaysOffersHoldingTheCurrentPosition) { + TrajectorySampler sampler(makeConfig()); + const HeadPosition start{0.2, -0.1}; + auto candidates = sampler.sample(start, {}, kLimits, makeDynamics()); + + ASSERT_FALSE(candidates.empty()); + // The first candidate comes to rest exactly where the head already is, which + // guarantees the scoring always has something valid to pick + EXPECT_NEAR(candidates.front().endpoint.yaw, start.yaw, 1e-9); + EXPECT_NEAR(candidates.front().endpoint.pitch, start.pitch, 1e-9); +} + +TEST(TrajectorySampler, EveryCandidateIsFeasible) { + TrajectorySampler sampler(makeConfig()); + const DynamicLimits dynamics = makeDynamics(); + auto candidates = sampler.sample({0.3, -0.2}, {1.0, 0.5}, kLimits, dynamics); + + ASSERT_FALSE(candidates.empty()); + for (const auto& candidate : candidates) { + EXPECT_TRUE(isFeasible(candidate.trajectory, kLimits, dynamics, makeConfig().horizon, makeConfig().feasibility_points)); + } +} + +TEST(TrajectorySampler, EveryCandidateStartsInTheCurrentState) { + TrajectorySampler sampler(makeConfig()); + const HeadPosition start{0.3, -0.2}; + const HeadVelocity velocity{1.0, 0.5}; + auto candidates = sampler.sample(start, velocity, kLimits, makeDynamics()); + + ASSERT_FALSE(candidates.empty()); + for (const auto& candidate : candidates) { + EXPECT_NEAR(candidate.trajectory.position(0.0).yaw, start.yaw, 1e-9); + EXPECT_NEAR(candidate.trajectory.velocity(0.0).yaw, velocity.yaw, 1e-9); + } +} + +TEST(TrajectorySampler, CandidatesStayWithinTheJointLimits) { + TrajectorySampler sampler(makeConfig()); + auto candidates = sampler.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); + + ASSERT_FALSE(candidates.empty()); + for (const auto& candidate : candidates) { + EXPECT_TRUE(kLimits.contains(candidate.endpoint)); + EXPECT_TRUE(kLimits.contains(candidate.midpoint)); + } +} + +TEST(TrajectorySampler, CandidatesAreDiverse) { + TrajectorySampler sampler(makeConfig()); + auto candidates = sampler.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); + + ASSERT_GT(candidates.size(), 2u); + double min_yaw = 1e9, max_yaw = -1e9; + for (const auto& candidate : candidates) { + min_yaw = std::min(min_yaw, candidate.endpoint.yaw); + max_yaw = std::max(max_yaw, candidate.endpoint.yaw); + } + // A sampler that always returned the same candidate would make the whole + // search pointless + EXPECT_GT(max_yaw - min_yaw, 0.1); +} + +TEST(TrajectorySampler, IsDeterministicForAGivenSeed) { + TrajectorySampler first(makeConfig(), 1234); + TrajectorySampler second(makeConfig(), 1234); + auto a = first.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); + auto b = second.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); + + ASSERT_EQ(a.size(), b.size()); + for (size_t i = 0; i < a.size(); i++) { + EXPECT_DOUBLE_EQ(a[i].endpoint.yaw, b[i].endpoint.yaw); + EXPECT_DOUBLE_EQ(a[i].endpoint.pitch, b[i].endpoint.pitch); + } +} + +TEST(TrajectorySampler, DifferentSeedsExploreDifferently) { + TrajectorySampler first(makeConfig(), 1); + TrajectorySampler second(makeConfig(), 2); + auto a = first.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); + auto b = second.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); + + ASSERT_FALSE(a.empty()); + ASSERT_FALSE(b.empty()); + // The hold candidate is identical by construction, so compare a random one + ASSERT_GT(a.size(), 1u); + ASSERT_GT(b.size(), 1u); + EXPECT_NE(a[1].endpoint.yaw, b[1].endpoint.yaw); +} + +TEST(TrajectorySampler, SlowJointsRestrictHowFarCandidatesReach) { + DynamicLimits dynamics = makeDynamics(); + dynamics.max_velocity = {0.2, 0.2}; + TrajectorySampler sampler(makeConfig()); + auto candidates = sampler.sample({0.0, 0.0}, {}, kLimits, dynamics); + + ASSERT_FALSE(candidates.empty()); + for (const auto& candidate : candidates) { + // Nothing beyond what the joint could travel within the horizon + EXPECT_LE(std::abs(candidate.endpoint.yaw), 0.2 * makeConfig().horizon + 1e-9); + } +} + +TEST(TrajectorySampler, ZeroSampleCountYieldsNothing) { + SamplerConfig config = makeConfig(); + config.sample_count = 0; + TrajectorySampler sampler(config); + EXPECT_TRUE(sampler.sample({0.0, 0.0}, {}, kLimits, makeDynamics()).empty()); +} 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 From 91857a378342e83465c93aa54066c0bf46a578fe Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Sun, 16 Aug 2026 23:03:17 +0200 Subject: [PATCH 2/8] docs: add failure handling policy to agent instructions No invented defaults for missing input, throttled logging that names what is missing, drop the update rather than the process, and keep retrying inputs that legitimately arrive late. --- AGENTS.md | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/AGENTS.md b/AGENTS.md index 651270ae2a..0f9d951b16 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -43,6 +43,42 @@ 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 From 2e73a4749fe6fb06f5c209744ec8bef5478a192e Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Sun, 16 Aug 2026 23:32:43 +0200 Subject: [PATCH 3/8] docs: require the pixi tasks for building and cleaning Removing build/, install/ or log/ by hand breaks every package that is not rebuilt with it, including ones outside the selection being worked on, and recovering needs a full workspace rebuild. --- AGENTS.md | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 0f9d951b16..2abe45420b 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -82,8 +82,10 @@ nothing. Never invent a value to keep going. ## 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. @@ -112,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. From d59dbd7cae9856a43e930370f82dfcf82f0c1b33 Mon Sep 17 00:00:00 2001 From: Lea Wedmann <116641586+ayin21@users.noreply.github.com> Date: Mon, 24 Aug 2026 23:54:32 +0200 Subject: [PATCH 4/8] Tune params --- .../bitbots_teleop/scripts/teleop_keyboard.py | 5 ++++ .../bitbots_head_mover/config/head_config.yml | 24 +++++++++---------- .../bitbots_head_mover/src/active_vision.cpp | 19 ++++++++++++++- .../bitbots_head_mover/src/move_head.cpp | 2 +- 4 files changed, 36 insertions(+), 14 deletions(-) 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/config/head_config.yml b/src/bitbots_motion/bitbots_head_mover/config/head_config.yml index 88b01d4199..a0e0e5a827 100644 --- a/src/bitbots_motion/bitbots_head_mover/config/head_config.yml +++ b/src/bitbots_motion/bitbots_head_mover/config/head_config.yml @@ -221,21 +221,21 @@ move_head: command_lookahead: type: double - default_value: 0.1 + default_value: 0.5 description: "How far along the selected trajectory the commanded setpoint is taken. Must be greater than zero, as the trajectory starts at the current head position." validation: gt<>: [0.0] max_velocity_yaw: type: double - default_value: 4.0 + default_value: 7.0 description: "Maximum yaw speed a sampled trajectory may reach. Deliberately below the motor's mechanical limit." validation: gt<>: [0.0] max_velocity_pitch: type: double - default_value: 4.0 + default_value: 7.0 description: "Maximum pitch speed a sampled trajectory may reach" validation: gt<>: [0.0] @@ -298,7 +298,7 @@ move_head: visibility: center_fraction: type: double - default_value: 0.5 + default_value: 0.3 description: "Fraction of the image around its center that counts as perfectly framed" validation: bounds<>: [0.0, 1.0] @@ -327,14 +327,14 @@ move_head: half_life: type: double - default_value: 8.0 + default_value: 4.0 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: 3.0 + 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] @@ -356,7 +356,7 @@ move_head: team_ball: type: double - default_value: 3.0 + default_value: 1.0 description: "How long a ball reported by a teammate stays relevant" validation: gt<>: [0.0] @@ -385,21 +385,21 @@ move_head: raw_balls: type: double - default_value: 2.0 + 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: 1.0 + 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: 1.5 + default_value: 1.0 description: "Importance of looking at parts of the field that were not observed recently" validation: gt_eq<>: [0.0] @@ -414,7 +414,7 @@ move_head: commitment: type: double - default_value: 1.5 + default_value: 0.1 description: "Importance of agreeing with the previously selected trajectory. This is the knob that trades reactivity against steady head motion." validation: gt_eq<>: [0.0] @@ -422,7 +422,7 @@ move_head: debug: enabled: type: bool - default_value: false + default_value: true description: "Publish the coverage grid, candidate markers and the joint space score image" image_size: diff --git a/src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp b/src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp index 964578e1a6..ceeb4becc0 100644 --- a/src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp +++ b/src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp @@ -149,7 +149,24 @@ ActiveVisionResult ActiveVision::plan(const ActiveVisionInput& input) { } } - result.candidates = sampler_.sample(input.head_position, input.head_velocity, limits_, dynamics_); + // Continue the motion the previous cycle committed to rather than trusting + // whatever the actuators report right now: the measured velocity can be + // noisy, lagged by a control period, or simply wrong for a moment after a + // new position command lands, and the sampler builds its candidates as a + // tangent from this value. A bad start velocity makes the near-term part of + // every candidate bend away from where the debug view shows it heading, + // which is exactly the part that gets commanded, before the spline + // corrects itself further along, where it is never actually executed. + // What we ourselves commanded a moment ago is known exactly, so it is used + // once a previous plan exists; only the very first cycle has nothing to + // fall back on but the measurement. + HeadVelocity start_velocity = input.head_velocity; + if (has_previous_) { + const double offset = std::clamp(input.now - previous_plan_time_, 0.0, previous_trajectory_.duration()); + start_velocity = previous_trajectory_.velocity(offset); + } + + result.candidates = sampler_.sample(input.head_position, start_velocity, limits_, dynamics_); if (result.candidates.empty()) { result.failure = ActiveVisionFailure::NoFeasibleCandidate; return result; 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 168e2172a6..84b2713ca1 100644 --- a/src/bitbots_motion/bitbots_head_mover/src/move_head.cpp +++ b/src/bitbots_motion/bitbots_head_mover/src/move_head.cpp @@ -185,7 +185,7 @@ class HeadMover { [this](const std_msgs::msg::String::SharedPtr msg) { handle_robot_description(msg->data); }); camera_info_subscriber_ = node_->create_subscription( - "camera_info", 1, [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { + "/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"); From 9bcf71fa525c06d4dc6a0d016229fa705604d102 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sun, 6 Sep 2026 11:33:17 +0200 Subject: [PATCH 5/8] Working state of active vision on the robot Signed-off-by: Florian Vahl --- .../bitbots_head_mover/CMakeLists.txt | 6 +- .../bitbots_head_mover/config/head_config.yml | 95 +++---- .../bitbots_head_mover/active_vision.hpp | 83 +++--- .../active_vision_debug.hpp | 20 +- .../active_vision_scorer.hpp | 48 ++-- .../bitbots_head_mover/target_sampler.hpp | 79 ++++++ .../bitbots_head_mover/trajectory_sampler.hpp | 105 ------- .../bitbots_head_mover/src/active_vision.cpp | 126 ++++----- .../src/active_vision_debug.cpp | 104 +++---- .../src/active_vision_scorer.cpp | 107 +++---- .../bitbots_head_mover/src/move_head.cpp | 185 ++++++++----- .../bitbots_head_mover/src/target_sampler.cpp | 81 ++++++ .../src/trajectory_sampler.cpp | 140 ---------- .../test/test_active_vision.cpp | 100 ++++--- .../test/test_active_vision_scorer.cpp | 176 ++++++------ .../test/test_target_sampler.cpp | 174 ++++++++++++ .../test/test_trajectory_sampler.cpp | 260 ------------------ 17 files changed, 880 insertions(+), 1009 deletions(-) create mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/target_sampler.hpp delete mode 100644 src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/trajectory_sampler.hpp create mode 100644 src/bitbots_motion/bitbots_head_mover/src/target_sampler.cpp delete mode 100644 src/bitbots_motion/bitbots_head_mover/src/trajectory_sampler.cpp create mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_target_sampler.cpp delete mode 100644 src/bitbots_motion/bitbots_head_mover/test/test_trajectory_sampler.cpp diff --git a/src/bitbots_motion/bitbots_head_mover/CMakeLists.txt b/src/bitbots_motion/bitbots_head_mover/CMakeLists.txt index e0c4f51f47..b2bcc0910a 100644 --- a/src/bitbots_motion/bitbots_head_mover/CMakeLists.txt +++ b/src/bitbots_motion/bitbots_head_mover/CMakeLists.txt @@ -57,7 +57,7 @@ add_library( src/head_trajectory.cpp src/look_at.cpp src/search_pattern.cpp - src/trajectory_sampler.cpp + src/target_sampler.cpp src/world_model.cpp) target_include_directories( @@ -137,8 +137,8 @@ if(BUILD_TESTING) 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_trajectory_sampler test/test_trajectory_sampler.cpp) - target_link_libraries(test_trajectory_sampler 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) 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 a0e0e5a827..0bce3db6d9 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 @@ -219,77 +219,78 @@ move_head: default_value: "map" description: "Frame the field, the coverage map and all stored detections live in" - command_lookahead: - type: double - default_value: 0.5 - description: "How far along the selected trajectory the commanded setpoint is taken. Must be greater than zero, as the trajectory starts at the current head position." - validation: - gt<>: [0.0] - max_velocity_yaw: type: double - default_value: 7.0 - description: "Maximum yaw speed a sampled trajectory may reach. Deliberately below the motor's mechanical limit." + 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: 7.0 - description: "Maximum pitch speed a sampled trajectory may reach" + default_value: 4.0 + description: "Maximum pitch speed the controller commands while steering the head towards the target" validation: gt<>: [0.0] - sampling: - sample_count: - type: int - default_value: 64 - description: "Number of candidate trajectories drawn per planning cycle" - validation: - gt_eq<>: [1] - - horizon: + controller: + approach_distance: type: double - default_value: 1.0 - description: "How far into the future a candidate trajectory reaches" + 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] - midpoint_time: + control_period: type: double - default_value: 0.5 - description: "When the intermediate waypoint of a candidate sits. Must be strictly inside the horizon." + 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] - evaluation_points: + sampling: + sample_count: type: int - default_value: 2 - description: "Number of points along a candidate that are scored. They are spread evenly over the horizon and end at the endpoint, never including the shared start, so two points score the midpoint and the goal point. This is the main cost driver together with the sample count and the coverage cell size." + default_value: 64 + description: "Number of candidate targets drawn per planning cycle" validation: gt_eq<>: [1] - feasibility_points: - type: int - default_value: 21 - description: "Number of points at which a candidate is checked against the joint and dynamic limits" + 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<>: [2] + gt_eq<>: [0.0] - max_attempts_per_sample: - type: int - default_value: 8 - description: "How often a single candidate is redrawn before it is given up on" + 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<>: [1] + gt_eq<>: [0.0] - midpoint_deviation: + uniform_weight: type: double default_value: 0.3 - description: "How far (in radians) the intermediate waypoint may deviate from the straight path to the endpoint. Larger values allow more curved sweeps but get rejected by the acceleration limit more often." + 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 @@ -327,7 +328,7 @@ move_head: half_life: type: double - default_value: 4.0 + 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] @@ -378,7 +379,7 @@ move_head: weights: filtered_ball: type: double - default_value: 4.0 + default_value: 1.5 description: "Importance of keeping the filtered ball estimate in view" validation: gt_eq<>: [0.0] @@ -399,23 +400,23 @@ move_head: field_coverage: type: double - default_value: 1.0 + 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.5 + default_value: 0.1 description: "Importance of keeping detected robots in view" validation: gt_eq<>: [0.0] - commitment: + smoothness: type: double default_value: 0.1 - description: "Importance of agreeing with the previously selected trajectory. This is the knob that trades reactivity against steady head motion." + 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] 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 index bddff3e500..cd57257bb4 100644 --- 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 @@ -5,20 +5,42 @@ #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 and how it is moving. + /// Where the head currently is. HeadPosition head_position; - HeadVelocity head_velocity; /// 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. @@ -33,20 +55,18 @@ enum class ActiveVisionReadiness { MissingFieldDimensions, }; -/// Why a planning cycle produced no trajectory. +/// 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 horizon. +/// missing camera, a broken chain and a misconfigured sampler. enum class ActiveVisionFailure { None, /// An input the planner needs has not arrived yet. NotReady, - /// The sampler produced no candidate that respects the limits. - NoFeasibleCandidate, /// The head chain could not resolve a camera pose, so nothing could be scored. KinematicsFailed, - /// The sampling timings are configured such that no trajectory can be built. + /// The sampler is configured such that it cannot draw any candidate. InvalidSamplerConfig, }; @@ -58,16 +78,19 @@ const char* describe(ActiveVisionReadiness readiness); /// What the planner decided, plus what it considered while deciding. struct ActiveVisionResult { - /// Whether a trajectory could be planned at all. + /// Whether a command could be produced at all. bool valid = false; - /// Why no trajectory was produced. None when valid. + /// 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 head position and velocity to command right now. + /// 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; @@ -77,13 +100,14 @@ struct ActiveVisionResult { size_t selected = 0; }; - -/// Plans head motion by sampling candidate trajectories and scoring them. +/// 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. +/// 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(); @@ -104,21 +128,13 @@ class ActiveVision { void setFieldCoverageConfig(const FieldCoverageConfig& config); void setSamplerConfig(const SamplerConfig& config) { sampler_.setConfig(config); } - void setDynamicLimits(const DynamicLimits& limits) { dynamics_ = limits; } 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); } - /// How far along the selected trajectory the commanded setpoint is taken. - /// - /// Must be greater than zero: the trajectory starts at the measured head - /// position, so a zero lookahead would command the head to stay put. Around - /// one or two control periods gives the motors a target to chase without - /// running ahead of what the next cycle can correct. - void setCommandLookahead(double lookahead) { command_lookahead_ = lookahead; } - /// Whether the planner has everything it needs. ActiveVisionReadiness readiness() const; bool ready() const { return readiness() == ActiveVisionReadiness::Ready; } @@ -149,40 +165,35 @@ class ActiveVision { /// Run one planning cycle. /// /// Ages out stale detections, decays the coverage record, folds in what the - /// head is currently seeing, then samples and scores candidates. Returns an - /// invalid result if the planner is not ready or no candidate survived. + /// 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: - /// The times along a candidate at which it is scored. - std::vector evaluationTimes() const; - std::unique_ptr kinematics_; CameraModel camera_; WorldModel world_; std::unique_ptr coverage_; - TrajectorySampler sampler_; + TargetSampler sampler_; HeadLimits limits_{{-1.23, 1.23}, {-1.23, 1.01}}; - DynamicLimits dynamics_; + HeadController controller_; ScoringWeights weights_; VisibilityWeighting visibility_; double coverage_distance_half_weight_ = 3.0; - double command_lookahead_ = 0.1; /// 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 trajectory selected in the previous cycle and when it was planned, - /// which is what the commitment term is measured against. - HeadTrajectory previous_trajectory_; - double previous_plan_time_ = 0.0; - bool has_previous_ = 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; }; 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 index 949a3c6a13..464f55fecf 100644 --- 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 @@ -17,23 +17,23 @@ namespace bitbots_head_mover { nav_msgs::msg::OccupancyGrid coverageGrid(const FieldCoverageMap& coverage, const std::string& frame_id, const builtin_interfaces::msg::Time& stamp); -/// Render the sampled candidates and the selected one as markers. +/// Render the sampled candidate targets and the selected one as markers. /// -/// Each candidate is drawn as the ground track its optical axis sweeps over, -/// 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 in a distinct colour. +/// 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 trajectories in the yaw and pitch joint space. +/// 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 polyline 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, double horizon, int size); +/// 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 index 713eb8d5c2..41e5f3decd 100644 --- 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 @@ -4,18 +4,20 @@ #include #include #include -#include +#include #include +#include #include -/// Scoring of candidate head trajectories. +/// Scoring of candidate head targets. namespace bitbots_head_mover { /// Relative importance of the individual scoring terms. /// -/// Every term is normalized to [0, 1] on its own, so these weights are directly -/// comparable and the total is a plain weighted sum. Raising a weight makes the -/// head care more about that aspect, and setting it to zero disables the term. +/// 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. @@ -35,9 +37,11 @@ struct ScoringWeights { double field_coverage = 1.5; /// Keeping detected robots in view, so the obstacle information stays fresh. double robots = 0.5; - /// Agreeing with the previously selected trajectory. This is what keeps the - /// head from flicking between equally good options every cycle. - double commitment = 1.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. @@ -50,8 +54,11 @@ struct ScoreBreakdown { double team_ball = 0.0; double field_coverage = 0.0; double robots = 0.0; - double commitment = 0.0; - /// The weighted sum of the terms above. + /// 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. /// @@ -69,18 +76,17 @@ 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 times along a candidate at which it is evaluated. - std::vector evaluation_times; - /// The previously selected trajectory, evaluated at the same times. Empty if - /// there is no previous selection, which disables the commitment term. - std::vector previous_positions; + /// The previously selected target. Empty if there is no previous selection, + /// which disables the smoothness cost. + std::optional previous_target; }; -/// Scores candidate head trajectories against the current world state. +/// Scores candidate head targets against the current world state. /// -/// A candidate is evaluated at several points in time rather than as a single -/// pose, so that a trajectory which sweeps across something interesting is -/// preferred over one that merely ends up pointing at it. +/// 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 @@ -119,11 +125,11 @@ class ActiveVisionScorer { /// for every candidate, which is what makes their scores comparable. void prepare(const Eigen::Isometry3d& robot_pose); - /// Score a single candidate. + /// 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 HeadTrajectory& candidate, const ScoringContext& context) const; + ScoreBreakdown score(const HeadPosition& target, const ScoringContext& context) const; /// The camera pose in the map frame for a given head configuration. /// 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/trajectory_sampler.hpp b/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/trajectory_sampler.hpp deleted file mode 100644 index a7484cb4a3..0000000000 --- a/src/bitbots_motion/bitbots_head_mover/include/bitbots_head_mover/trajectory_sampler.hpp +++ /dev/null @@ -1,105 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -/// Random generation of candidate head trajectories. -namespace bitbots_head_mover { - -/// The dynamic limits a candidate trajectory has to respect. -struct DynamicLimits { - HeadVelocity max_velocity{4.0, 4.0}; - HeadAcceleration max_acceleration{14.0, 14.0}; -}; - -/// Shape and number of the sampled candidates. -struct SamplerConfig { - /// How many candidates are drawn per planning cycle. - int sample_count = 64; - /// How far into the future a candidate reaches. - double horizon = 1.0; - /// When the intermediate waypoint sits. Splitting the horizon lets a candidate - /// curve instead of only moving straight towards its endpoint. - double midpoint_time = 0.5; - /// How many points along a candidate are evaluated by the scoring. - /// - /// The points are spread evenly over the horizon and always end at the - /// endpoint, never including the start: all candidates share the same start, - /// so scoring it cannot tell them apart. Two points therefore evaluate the - /// midpoint and the goal point. - int evaluation_points = 2; - /// How many points are checked when verifying the dynamic limits. This is - /// finer than the scoring, because a limit violation between two scoring - /// points would go unnoticed otherwise. - int feasibility_points = 21; - /// How many times a single candidate is redrawn before giving up on it. - int max_attempts_per_sample = 8; - /// How far the intermediate waypoint may deviate from the straight path to the - /// endpoint, in radians. This is what lets a candidate curve; drawing the - /// midpoint independently instead would mostly produce detours that violate - /// the acceleration limit and get rejected. - double midpoint_deviation = 0.3; -}; - -/// A candidate trajectory together with the state it ends in. -struct Candidate { - HeadTrajectory trajectory; - /// The head position the candidate ends at, kept for debug output. - HeadPosition endpoint; - /// The intermediate waypoint the candidate was drawn with. - HeadPosition midpoint; -}; - -/// Build a candidate trajectory through a midpoint to a resting endpoint. -/// -/// The trajectory starts in the given state, so a candidate always continues the -/// motion the head is already performing instead of assuming it stands still. -/// The midpoint velocity is derived rather than sampled: it follows the overall -/// direction of travel, which keeps the trajectory from having to stop and -/// restart in the middle. The endpoint is reached at rest, so that a candidate -/// that is never replaced still leaves the head in a defined state. -HeadTrajectory buildCandidateTrajectory(const HeadPosition& start, const HeadVelocity& start_velocity, - const HeadPosition& midpoint, const HeadPosition& endpoint, double midpoint_time, - double horizon, const DynamicLimits& dynamics); - -/// Whether a trajectory stays inside the joint and dynamic limits. -/// -/// Checks position, velocity and acceleration at evenly spaced points, which is -/// an approximation, but a quintic between two waypoints has no room to hide a -/// violation between sufficiently dense samples. -bool isFeasible(const HeadTrajectory& trajectory, const HeadLimits& limits, const DynamicLimits& dynamics, - double horizon, int feasibility_points); - -/// Draws candidate head trajectories. -/// -/// Sampling is done by rejection: candidates are drawn from a range that is -/// roughly reachable within the horizon and then verified against the actual -/// limits, so no candidate that the head cannot physically follow is ever -/// scored. The candidate set always contains the trajectory that holds the -/// current position, which guarantees that there is something to choose from -/// even when every random draw is rejected. -class TrajectorySampler { - public: - explicit TrajectorySampler(const SamplerConfig& config = {}, uint32_t seed = 42); - - void setConfig(const SamplerConfig& config) { config_ = config; } - const SamplerConfig& config() const { return config_; } - - /// Draw candidates starting from the given head state. - std::vector sample(const HeadPosition& start, const HeadVelocity& start_velocity, const HeadLimits& limits, - const DynamicLimits& dynamics); - - private: - /// Draw a single joint value within the reachable part of its limits. - double sampleJoint(double start, double max_velocity, double duration, const JointLimit& limit); - - /// Draw a single joint value near a given center, staying inside the limits. - double sampleAround(double center, double deviation, const JointLimit& limit); - - SamplerConfig config_; - std::mt19937 rng_; -}; - -} // namespace bitbots_head_mover diff --git a/src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp b/src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp index ceeb4becc0..a24b51abbf 100644 --- a/src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp +++ b/src/bitbots_motion/bitbots_head_mover/src/active_vision.cpp @@ -1,6 +1,7 @@ #include #include #include +#include namespace bitbots_head_mover { @@ -12,12 +13,10 @@ const char* describe(ActiveVisionFailure failure) { return "no failure"; case ActiveVisionFailure::NotReady: return "an input the planner needs has not arrived yet"; - case ActiveVisionFailure::NoFeasibleCandidate: - return "no sampled head trajectory respects the joint and dynamic limits"; case ActiveVisionFailure::KinematicsFailed: return "the head chain could not resolve a camera pose for any candidate"; case ActiveVisionFailure::InvalidSamplerConfig: - return "the sampling horizon and midpoint time cannot describe a trajectory"; + return "the sampler cannot draw any candidate with its current configuration"; } return "unknown failure"; } @@ -77,21 +76,6 @@ ActiveVisionReadiness ActiveVision::readiness() const { return ActiveVisionReadiness::Ready; } -std::vector ActiveVision::evaluationTimes() const { - const SamplerConfig& config = sampler_.config(); - std::vector times; - const int count = std::max(config.evaluation_points, 1); - times.reserve(static_cast(count)); - for (int i = 1; i <= count; i++) { - // Spread over the horizon, ending at the endpoint. The start is deliberately - // not evaluated: every candidate begins at the measured head position, so - // that point scores identically for all of them and can only waste time. - // With two points this evaluates the midpoint and the goal point. - times.push_back(config.horizon * static_cast(i) / static_cast(count)); - } - return times; -} - ActiveVisionResult ActiveVision::plan(const ActiveVisionInput& input) { ActiveVisionResult result; if (!ready()) { @@ -99,11 +83,13 @@ ActiveVisionResult ActiveVision::plan(const ActiveVisionInput& input) { return result; } - // Catch a sampling configuration that cannot describe a trajectory here, so it - // is reported as such instead of surfacing as "every candidate was rejected" + // A sampler that cannot draw is reported as such instead of surfacing as + // "every candidate was rejected" const SamplerConfig& sampler_config = sampler_.config(); - if (!(sampler_config.horizon > 0.0) || !(sampler_config.midpoint_time > 0.0) || - sampler_config.midpoint_time >= sampler_config.horizon) { + 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; } @@ -135,40 +121,15 @@ ActiveVisionResult ActiveVision::plan(const ActiveVisionInput& input) { ScoringContext context; context.robot_pose = input.robot_pose; - context.evaluation_times = evaluationTimes(); - - // Measure commitment against where the previous selection would be at the very - // same moments in time, not against its raw parameterization, because that - // trajectory started one cycle earlier - if (has_previous_) { - const double offset = input.now - previous_plan_time_; - context.previous_positions.reserve(context.evaluation_times.size()); - for (double time : context.evaluation_times) { - context.previous_positions.push_back( - previous_trajectory_.position(std::min(offset + time, previous_trajectory_.duration()))); - } - } + context.previous_target = previous_target_; - // Continue the motion the previous cycle committed to rather than trusting - // whatever the actuators report right now: the measured velocity can be - // noisy, lagged by a control period, or simply wrong for a moment after a - // new position command lands, and the sampler builds its candidates as a - // tangent from this value. A bad start velocity makes the near-term part of - // every candidate bend away from where the debug view shows it heading, - // which is exactly the part that gets commanded, before the spline - // corrects itself further along, where it is never actually executed. - // What we ourselves commanded a moment ago is known exactly, so it is used - // once a previous plan exists; only the very first cycle has nothing to - // fall back on but the measurement. - HeadVelocity start_velocity = input.head_velocity; - if (has_previous_) { - const double offset = std::clamp(input.now - previous_plan_time_, 0.0, previous_trajectory_.duration()); - start_velocity = previous_trajectory_.velocity(offset); - } - - result.candidates = sampler_.sample(input.head_position, start_velocity, limits_, dynamics_); + // 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()) { - result.failure = ActiveVisionFailure::NoFeasibleCandidate; + // 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; } @@ -176,7 +137,7 @@ ActiveVisionResult ActiveVision::plan(const ActiveVisionInput& input) { 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].trajectory, context); + 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 @@ -196,22 +157,47 @@ ActiveVisionResult ActiveVision::plan(const ActiveVisionInput& input) { return result; } - const HeadTrajectory& selected = result.candidates[result.selected].trajectory; - - // Command a point a little way along the selected trajectory rather than its - // start. The trajectory starts at the measured head position, so commanding - // its start would ask the head to stay exactly where it already is and it - // would never move. Only this leading segment is ever executed before the next - // cycle replans, which is what lets the head react immediately while the - // commitment term keeps it from flickering. - const double lookahead = std::clamp(command_lookahead_, 0.0, selected.duration()); - result.position = limits_.clamp(selected.position(lookahead)); - result.velocity = selected.velocity(lookahead); + 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_trajectory_ = selected; - previous_plan_time_ = input.now; - has_previous_ = true; + previous_target_ = result.target; last_plan_time_ = input.now; has_planned_ = true; @@ -223,7 +209,7 @@ void ActiveVision::reset() { if (coverage_) { coverage_->reset(); } - has_previous_ = false; + previous_target_.reset(); has_planned_ = false; } 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 index 8d0e27675c..2a0f7bb4c7 100644 --- a/src/bitbots_motion/bitbots_head_mover/src/active_vision_debug.cpp +++ b/src/bitbots_motion/bitbots_head_mover/src/active_vision_debug.cpp @@ -112,51 +112,72 @@ visualization_msgs::msg::MarkerArray candidateMarkers(const ActiveVisionResult& 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++) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frame_id; - marker.header.stamp = stamp; - marker.ns = "active_vision_candidates"; - marker.id = static_cast(index); - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.scale.x = index == result.selected ? 0.04 : 0.015; + 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]); - marker.color.b = static_cast(color[0] / 255.0); - marker.color.g = static_cast(color[1] / 255.0); - marker.color.r = static_cast(color[2] / 255.0); - marker.color.a = index == result.selected ? 1.0f : 0.35f; - - const HeadTrajectory& trajectory = result.candidates[index].trajectory; - const int steps = 12; - for (int step = 0; step <= steps; step++) { - const double t = trajectory.duration() * static_cast(step) / static_cast(steps); - const auto camera_pose = scorer.cameraPoseInMap(trajectory.position(t), robot_pose); - Eigen::Vector3d point; - // A pose the kinematics could not resolve simply has no ground track 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(); - marker.points.push_back(message_point); - } + 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); + } - // A line strip needs at least two points to be a valid marker - if (marker.points.size() >= 2) { - markers.markers.push_back(marker); + 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, double horizon, int size) { +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; @@ -182,7 +203,6 @@ cv::Mat jointSpaceDebugImage(const ActiveVisionResult& result, const HeadLimits& } const std::vector normalized = normalizedScores(result); - const int steps = 16; // Draw the selected candidate last so it ends up on top of the others std::vector order; @@ -197,19 +217,9 @@ cv::Mat jointSpaceDebugImage(const ActiveVisionResult& result, const HeadLimits& } for (size_t index : order) { - const HeadTrajectory& trajectory = result.candidates[index].trajectory; const bool selected = index == result.selected; const cv::Scalar color = selected ? cv::Scalar(255, 255, 255) : scoreColor(normalized[index]); - - cv::Point previous = toPixel(trajectory.position(0.0)); - for (int step = 1; step <= steps; step++) { - const double t = horizon * static_cast(step) / static_cast(steps); - const cv::Point current = toPixel(trajectory.position(t)); - cv::line(image, previous, current, color, selected ? 2 : 1, cv::LINE_AA); - previous = current; - } - // Mark where the candidate comes to rest - cv::circle(image, previous, selected ? 4 : 2, color, cv::FILLED, cv::LINE_AA); + cv::circle(image, toPixel(result.candidates[index].target), selected ? 5 : 2, color, cv::FILLED, cv::LINE_AA); } return image; 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 index 963597e6f4..4da5082b63 100644 --- a/src/bitbots_motion/bitbots_head_mover/src/active_vision_scorer.cpp +++ b/src/bitbots_motion/bitbots_head_mover/src/active_vision_scorer.cpp @@ -4,16 +4,6 @@ namespace bitbots_head_mover { -namespace { - -/// The joint space distance at which two trajectories count as unrelated. -/// -/// Used to normalize the commitment term. Roughly the width of the reachable -/// head range, so that agreeing to within a few degrees scores close to one. -constexpr double kCommitmentScale = 2.0; - -} // namespace - double targetVisibility(const std::vector& targets, const Eigen::Isometry3d& map_to_camera, const CameraModel& camera, const VisibilityWeighting& weighting) { if (targets.empty()) { @@ -112,76 +102,61 @@ void ActiveVisionScorer::prepare(const Eigen::Isometry3d& robot_pose) { } } -ScoreBreakdown ActiveVisionScorer::score(const HeadTrajectory& candidate, const ScoringContext& context) const { +ScoreBreakdown ActiveVisionScorer::score(const HeadPosition& target, const ScoringContext& context) const { ScoreBreakdown breakdown; - if (!candidate.valid() || context.evaluation_times.empty() || !camera_.valid()) { + if (!camera_.valid()) { return breakdown; } - const auto& centers = coverage_.cellCenters(); + 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(); - const double point_count = static_cast(context.evaluation_times.size()); + 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_); - for (size_t step = 0; step < context.evaluation_times.size(); step++) { - const HeadPosition position = candidate.position(context.evaluation_times[step]); - const auto camera_pose = cameraPoseInMap(position, context.robot_pose); - if (!camera_pose) { - // Partially accumulated terms would understate the candidate rather than - // reject it, so the whole candidate is reported as unscorable - 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. - 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]; + // 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; } - - 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); + const double quality = camera_.visibility(map_to_camera * centers[index], visibility_); + if (quality <= 0.0) { + continue; } + covered_interest += quality * interest * cell_distance_weights_[index]; + } - // Agreement with the previous selection, evaluated at the same times - if (step < context.previous_positions.size()) { - const HeadPosition& previous = context.previous_positions[step]; - const double distance = std::hypot(position.yaw - previous.yaw, position.pitch - previous.pitch); - breakdown.commitment += std::max(0.0, 1.0 - distance / kCommitmentScale); - } + 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); } - // Every term is an average over the evaluated points, which keeps it in [0, 1] - // and makes a trajectory that stays on target beat one that only passes over it - breakdown.filtered_ball /= point_count; - breakdown.raw_balls /= point_count; - breakdown.team_ball /= point_count; - breakdown.robots /= point_count; - breakdown.field_coverage /= point_count; - breakdown.commitment /= point_count; + // 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_.commitment * breakdown.commitment; + weights_.robots * breakdown.robots - weights_.smoothness * breakdown.smoothness_cost; breakdown.valid = true; return breakdown; 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 84b2713ca1..7ab17f91bb 100644 --- a/src/bitbots_motion/bitbots_head_mover/src/move_head.cpp +++ b/src/bitbots_motion/bitbots_head_mover/src/move_head.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -36,6 +37,7 @@ #include #include #include +#include #include #include @@ -112,6 +114,16 @@ class HeadMover { 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_; @@ -124,6 +136,13 @@ class HeadMover { 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); @@ -142,14 +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"; @@ -192,16 +216,23 @@ class HeadMover { } }); + // 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); }); + "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); }); + [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); }); + "team_data", 10, [this](const bitbots_msgs::msg::TeamData::SharedPtr msg) { handle_team_data(*msg); }, + detection_options); apply_active_vision_parameters(); @@ -315,21 +346,20 @@ class HeadMover { bitbots_head_mover::SamplerConfig sampler; sampler.sample_count = static_cast(config.sampling.sample_count); - sampler.horizon = config.sampling.horizon; - sampler.midpoint_time = config.sampling.midpoint_time; - sampler.evaluation_points = static_cast(config.sampling.evaluation_points); - sampler.feasibility_points = static_cast(config.sampling.feasibility_points); - sampler.max_attempts_per_sample = static_cast(config.sampling.max_attempts_per_sample); - sampler.midpoint_deviation = config.sampling.midpoint_deviation; + 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::DynamicLimits dynamics; - dynamics.max_velocity = {config.max_velocity_yaw, config.max_velocity_pitch}; - dynamics.max_acceleration = {params_.max_acceleration_yaw, params_.max_acceleration_pitch}; - active_vision_.setDynamicLimits(dynamics); + 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_.setCommandLookahead(config.command_lookahead); active_vision_.setVisibilityWeighting({config.visibility.center_fraction, config.visibility.border_score}); active_vision_.setCoverageDistanceHalfWeight(config.coverage.distance_half_weight); @@ -339,7 +369,12 @@ class HeadMover { world.team_ball_timeout = config.timeouts.team_ball; world.robot_timeout = config.timeouts.robot; world.covariance_half_weight = config.covariance_half_weight; - active_vision_.setWorldModelConfig(world); + { + // 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; @@ -347,7 +382,7 @@ class HeadMover { weights.team_ball = config.weights.team_ball; weights.field_coverage = config.weights.field_coverage; weights.robots = config.weights.robots; - weights.commitment = config.weights.commitment; + weights.smoothness = config.weights.smoothness; active_vision_.setScoringWeights(weights); } @@ -576,43 +611,6 @@ class HeadMover { position_publisher_->publish(pos_msg); } - /** - * @brief Returns the current velocity of the head motors - * - * Sampling starts from the measured velocity, so a replanned trajectory - * continues the motion the head is already performing instead of assuming it - * stands still. Joint states without a velocity field report rest. - */ - std::optional get_head_velocity() const { - HeadVelocity velocity; - bool found_yaw = false; - bool found_pitch = false; - for (size_t i = 0; i < current_joint_state_->name.size(); 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; - } - // A joint state that names the joint but carries no velocity for it must - // not be read as the head standing still: sampling would then plan from a - // standstill while the head is actually moving - if (i >= current_joint_state_->velocity.size()) { - return std::nullopt; - } - if (is_yaw) { - velocity.yaw = current_joint_state_->velocity[i]; - found_yaw = true; - } else { - velocity.pitch = current_joint_state_->velocity[i]; - found_pitch = true; - } - } - if (!found_yaw || !found_pitch) { - return std::nullopt; - } - return velocity; - } - /** * @brief Returns the current position of the head motors * @@ -759,6 +757,11 @@ class HeadMover { */ 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; @@ -790,6 +793,9 @@ class HeadMover { // 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()); } @@ -810,6 +816,7 @@ class HeadMover { // 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)); } @@ -829,6 +836,7 @@ class HeadMover { 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)); } @@ -845,6 +853,7 @@ class HeadMover { // 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()); @@ -880,8 +889,7 @@ class HeadMover { bitbots_head_mover::candidateMarkers(result, active_vision_, robot_pose, map_frame, stamp)); const cv::Mat plot = bitbots_head_mover::jointSpaceDebugImage( - result, active_vision_.headLimits(), active_vision_.samplerConfig().horizon, - static_cast(params_.active_vision.debug.image_size)); + result, active_vision_.headLimits(), static_cast(params_.active_vision.debug.image_size)); std_msgs::msg::Header header; header.stamp = stamp; @@ -929,20 +937,18 @@ class HeadMover { return; } - const auto head_velocity = get_head_velocity(); - if (!head_velocity) { - hold_active_vision_position( - "the joint states carry no head joint velocities, which sampling needs to continue the current motion"); - return; - } - bitbots_head_mover::ActiveVisionInput input; input.head_position = *head_position; - input.head_velocity = *head_velocity; input.robot_pose = robot_pose; input.now = node_->now().seconds(); - const bitbots_head_mover::ActiveVisionResult result = active_vision_.plan(input); + 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; @@ -952,15 +958,20 @@ class HeadMover { // 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 trajectory candidates because they could not be scored", + "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 motor goals carry joint speeds, so the direction of travel is dropped + // 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); } } @@ -1077,10 +1088,17 @@ 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) { @@ -1097,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 @@ -1105,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/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/trajectory_sampler.cpp b/src/bitbots_motion/bitbots_head_mover/src/trajectory_sampler.cpp deleted file mode 100644 index f6f28a345b..0000000000 --- a/src/bitbots_motion/bitbots_head_mover/src/trajectory_sampler.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include -#include -#include - -namespace bitbots_head_mover { - -HeadTrajectory buildCandidateTrajectory(const HeadPosition& start, const HeadVelocity& start_velocity, - const HeadPosition& midpoint, const HeadPosition& endpoint, - double midpoint_time, double horizon, const DynamicLimits& dynamics) { - HeadTrajectory trajectory; - if (!(horizon > 0.0) || !(midpoint_time > 0.0) || midpoint_time >= horizon) { - return trajectory; - } - - // The midpoint velocity follows the overall direction of travel, scaled by the - // total duration. This is the usual finite difference tangent: it keeps the - // trajectory moving through the midpoint instead of stopping there, without - // adding another sampled dimension. Clamping it to the maximum keeps the - // waypoint itself from being the reason a candidate is infeasible. - HeadVelocity midpoint_velocity{ - std::clamp((endpoint.yaw - start.yaw) / horizon, -dynamics.max_velocity.yaw, dynamics.max_velocity.yaw), - std::clamp((endpoint.pitch - start.pitch) / horizon, -dynamics.max_velocity.pitch, dynamics.max_velocity.pitch)}; - - trajectory.addPoint(0.0, start, start_velocity); - trajectory.addPoint(midpoint_time, midpoint, midpoint_velocity); - // The endpoint is reached at rest, so a candidate that is never replaced - // leaves the head in a defined state rather than drifting on - trajectory.addPoint(horizon, endpoint); - trajectory.finalize(); - return trajectory; -} - -bool isFeasible(const HeadTrajectory& trajectory, const HeadLimits& limits, const DynamicLimits& dynamics, - double horizon, int feasibility_points) { - if (!trajectory.valid() || feasibility_points < 2 || !(horizon > 0.0)) { - return false; - } - - for (int i = 0; i < feasibility_points; i++) { - const double t = horizon * static_cast(i) / static_cast(feasibility_points - 1); - - const HeadPosition position = trajectory.position(t); - if (!limits.contains(position)) { - return false; - } - - const HeadVelocity velocity = trajectory.velocity(t); - if (std::abs(velocity.yaw) > dynamics.max_velocity.yaw || - std::abs(velocity.pitch) > dynamics.max_velocity.pitch) { - return false; - } - - const HeadAcceleration acceleration = trajectory.acceleration(t); - if (std::abs(acceleration.yaw) > dynamics.max_acceleration.yaw || - std::abs(acceleration.pitch) > dynamics.max_acceleration.pitch) { - return false; - } - } - - return true; -} - -TrajectorySampler::TrajectorySampler(const SamplerConfig& config, uint32_t seed) : config_(config), rng_(seed) {} - -double TrajectorySampler::sampleJoint(double start, double max_velocity, double duration, const JointLimit& limit) { - // Only draw from the part of the joint range that can plausibly be reached in - // the available time. Drawing from the full range would make almost every - // candidate fail the feasibility check when the horizon is short. - const double reach = std::abs(max_velocity) * duration; - const double lower = std::max(limit.lower, start - reach); - const double upper = std::min(limit.upper, start + reach); - if (!(upper > lower)) { - return std::clamp(start, limit.lower, limit.upper); - } - return std::uniform_real_distribution(lower, upper)(rng_); -} - -double TrajectorySampler::sampleAround(double center, double deviation, const JointLimit& limit) { - const double lower = std::max(limit.lower, center - std::abs(deviation)); - const double upper = std::min(limit.upper, center + std::abs(deviation)); - if (!(upper > lower)) { - return std::clamp(center, limit.lower, limit.upper); - } - return std::uniform_real_distribution(lower, upper)(rng_); -} - -std::vector TrajectorySampler::sample(const HeadPosition& start, const HeadVelocity& start_velocity, - const HeadLimits& limits, const DynamicLimits& dynamics) { - std::vector candidates; - if (config_.sample_count <= 0) { - return candidates; - } - candidates.reserve(static_cast(config_.sample_count) + 1); - - // Always offer the candidate that comes to rest where the head already is. It - // gives the scoring something to fall back on when every random draw is - // rejected, and it is the trajectory that suppresses motion when nothing on - // the field is worth looking at. - { - const HeadPosition hold = limits.clamp(start); - HeadTrajectory trajectory = - buildCandidateTrajectory(start, start_velocity, hold, hold, config_.midpoint_time, config_.horizon, dynamics); - if (isFeasible(trajectory, limits, dynamics, config_.horizon, config_.feasibility_points)) { - candidates.push_back({std::move(trajectory), hold, hold}); - } - } - - // Where the midpoint would sit if the head travelled straight to the endpoint - const double straight_fraction = config_.midpoint_time / config_.horizon; - - for (int i = 0; i < config_.sample_count; i++) { - for (int attempt = 0; attempt < config_.max_attempts_per_sample; attempt++) { - const HeadPosition endpoint{ - sampleJoint(start.yaw, dynamics.max_velocity.yaw, config_.horizon, limits.yaw), - sampleJoint(start.pitch, dynamics.max_velocity.pitch, config_.horizon, limits.pitch)}; - - // Draw the midpoint around the straight path towards the endpoint rather - // than independently of it. An independent midpoint usually demands a - // detour that the acceleration limit cannot serve, which is why doing it - // that way rejected roughly a third of all draws. The deviation still - // allows curved sweeps that cover more ground than a straight move. - const HeadPosition midpoint{ - sampleAround(start.yaw + (endpoint.yaw - start.yaw) * straight_fraction, config_.midpoint_deviation, - limits.yaw), - sampleAround(start.pitch + (endpoint.pitch - start.pitch) * straight_fraction, config_.midpoint_deviation, - limits.pitch)}; - - HeadTrajectory trajectory = buildCandidateTrajectory(start, start_velocity, midpoint, endpoint, - config_.midpoint_time, config_.horizon, dynamics); - if (isFeasible(trajectory, limits, dynamics, config_.horizon, config_.feasibility_points)) { - candidates.push_back({std::move(trajectory), endpoint, midpoint}); - break; - } - } - } - - return candidates; -} - -} // 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 index 35ed0e37a1..d28a0517a4 100644 --- a/src/bitbots_motion/bitbots_head_mover/test/test_active_vision.cpp +++ b/src/bitbots_motion/bitbots_head_mover/test/test_active_vision.cpp @@ -1,9 +1,9 @@ #include -#include #include #include #include +#include using bitbots_head_mover::ActiveVision; using bitbots_head_mover::ActiveVisionInput; @@ -72,12 +72,12 @@ FieldCoverageConfig makeCoverageConfig() { SamplerConfig makeSamplerConfig() { SamplerConfig config; - // A smaller sample count keeps the tests quick without changing the behavior - config.sample_count = 24; - config.horizon = 1.0; - config.midpoint_time = 0.5; - config.evaluation_points = 5; - config.feasibility_points = 11; + 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; } @@ -94,12 +94,17 @@ ActiveVision makeReadyPlanner() { ActiveVisionInput makeInput(double now = 0.0) { ActiveVisionInput input; input.head_position = {0.0, 0.0}; - input.head_velocity = {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 // --------------------------------------------------------------------------- @@ -162,7 +167,7 @@ TEST(ActiveVision, CalibrationSurvivesALateRobotDescription) { // Planning // --------------------------------------------------------------------------- -TEST(ActiveVision, PlansAFeasibleTrajectory) { +TEST(ActiveVision, PlansACommand) { ActiveVision planner = makeReadyPlanner(); const ActiveVisionResult result = planner.plan(makeInput()); @@ -170,7 +175,8 @@ TEST(ActiveVision, PlansAFeasibleTrajectory) { EXPECT_FALSE(result.candidates.empty()); EXPECT_EQ(result.scores.size(), result.candidates.size()); EXPECT_LT(result.selected, result.candidates.size()); - EXPECT_TRUE(planner.headLimits().contains(result.position)); + EXPECT_TRUE(withinLimits(result.position, planner.headLimits())); + EXPECT_TRUE(withinLimits(result.target, planner.headLimits())); } TEST(ActiveVision, SelectsTheHighestScoringCandidate) { @@ -183,19 +189,30 @@ TEST(ActiveVision, SelectsTheHighestScoringCandidate) { } } -TEST(ActiveVision, CommandsAPositionAheadOfTheCurrentOne) { +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(); - planner.setCommandLookahead(0.1); // 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); - // Commanding the trajectory's start would mean commanding the position the - // head is already in, and the head would never move - const double distance = std::hypot(result.position.yaw, result.position.pitch); - EXPECT_GT(distance, 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) { @@ -203,7 +220,7 @@ TEST(ActiveVision, TurnsTowardsTheBall) { ScoringWeights weights; // Isolate the ball term so the coverage sweep cannot outvote it weights.field_coverage = 0.0; - weights.commitment = 0.0; + weights.smoothness = 0.0; planner.setScoringWeights(weights); // A ball clearly off to the robot's left @@ -211,21 +228,21 @@ TEST(ActiveVision, TurnsTowardsTheBall) { const ActiveVisionResult result = planner.plan(makeInput()); ASSERT_TRUE(result.valid); - EXPECT_GT(result.position.yaw, 0.0); + EXPECT_GT(result.target.yaw, 0.0); } TEST(ActiveVision, TurnsTheOtherWayForABallOnTheOtherSide) { ActiveVision planner = makeReadyPlanner(); ScoringWeights weights; weights.field_coverage = 0.0; - weights.commitment = 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.position.yaw, 0.0); + EXPECT_LT(result.target.yaw, 0.0); } TEST(ActiveVision, AgesOutDetectionsWhilePlanning) { @@ -267,41 +284,41 @@ TEST(ActiveVision, CoverageRecoversOverTime) { EXPECT_GT(planner.coverage().totalInterest(), after_looking); } -TEST(ActiveVision, CommitmentKeepsConsecutivePlansTogether) { - ActiveVision committed = makeReadyPlanner(); +TEST(ActiveVision, SmoothnessKeepsConsecutiveTargetsTogether) { + ActiveVision steady = makeReadyPlanner(); ScoringWeights strong; - strong.commitment = 50.0; - committed.setScoringWeights(strong); + strong.smoothness = 50.0; + steady.setScoringWeights(strong); ActiveVision flighty = makeReadyPlanner(); ScoringWeights none; - none.commitment = 0.0; + none.smoothness = 0.0; flighty.setScoringWeights(none); - double committed_travel = 0.0; + double steady_travel = 0.0; double flighty_travel = 0.0; - HeadPosition committed_previous{0.0, 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 = committed_previous; - const auto a = committed.plan(input); + input.head_position = steady_previous; + const auto a = steady.plan(input); ASSERT_TRUE(a.valid); - committed_travel += std::hypot(a.position.yaw - committed_previous.yaw, a.position.pitch - committed_previous.pitch); - committed_previous = a.position; + 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.position.yaw - flighty_previous.yaw, b.position.pitch - flighty_previous.pitch); - flighty_previous = b.position; + 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 commitment term: without it the head chases - // whichever candidate happens to win this cycle and jitters - EXPECT_LT(committed_travel, flighty_travel); + // 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) { @@ -361,8 +378,7 @@ TEST(ActiveVisionDebug, CandidateMarkersClearThePreviousCycle) { TEST(ActiveVisionDebug, JointSpaceImageHasTheRequestedSize) { ActiveVision planner = makeReadyPlanner(); const auto result = planner.plan(makeInput()); - const cv::Mat image = - bitbots_head_mover::jointSpaceDebugImage(result, planner.headLimits(), makeSamplerConfig().horizon, 400); + const cv::Mat image = bitbots_head_mover::jointSpaceDebugImage(result, planner.headLimits(), 400); EXPECT_EQ(image.rows, 400); EXPECT_EQ(image.cols, 400); @@ -372,10 +388,9 @@ TEST(ActiveVisionDebug, JointSpaceImageHasTheRequestedSize) { TEST(ActiveVisionDebug, JointSpaceImageDrawsTheCandidates) { ActiveVision planner = makeReadyPlanner(); const auto result = planner.plan(makeInput()); - const cv::Mat with_candidates = - bitbots_head_mover::jointSpaceDebugImage(result, planner.headLimits(), makeSamplerConfig().horizon, 400); + 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(), 1.0, 400); + bitbots_head_mover::jointSpaceDebugImage(ActiveVisionResult(), planner.headLimits(), 400); // The plot has to actually show something, otherwise it is useless for tuning cv::Mat difference; @@ -387,7 +402,6 @@ TEST(ActiveVisionDebug, JointSpaceImageDrawsTheCandidates) { TEST(ActiveVisionDebug, JointSpaceImageHandlesAnEmptyResult) { ActiveVision planner = makeReadyPlanner(); - const cv::Mat image = - bitbots_head_mover::jointSpaceDebugImage(ActiveVisionResult(), planner.headLimits(), 1.0, 128); + 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 index 47a61b7e77..752b838fd2 100644 --- 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 @@ -1,19 +1,18 @@ #include +#include #include -#include #include +#include +#include using bitbots_head_mover::ActiveVisionScorer; -using bitbots_head_mover::buildCandidateTrajectory; using bitbots_head_mover::CameraModel; -using bitbots_head_mover::DynamicLimits; 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::HeadTrajectory; using bitbots_head_mover::ScoringContext; using bitbots_head_mover::ScoringWeights; using bitbots_head_mover::TimedTarget; @@ -79,16 +78,10 @@ FieldCoverageConfig makeCoverageConfig() { return config; } -/// A trajectory that holds a single head position for the whole horizon. -HeadTrajectory holdAt(const HeadPosition& position) { - return buildCandidateTrajectory(position, {}, position, position, 0.5, 1.0, DynamicLimits{}); -} - ScoringContext makeContext() { ScoringContext context; // The robot stands in the center of the field looking down the long axis context.robot_pose = Eigen::Isometry3d::Identity(); - context.evaluation_times = {0.0, 0.25, 0.5, 0.75, 1.0}; return context; } @@ -116,19 +109,9 @@ TEST(ActiveVisionScorer, ScoresZeroWithoutIntrinsics) { Fixture fixture; CameraModel blind; ActiveVisionScorer scorer(*fixture.kinematics, blind, fixture.world, fixture.coverage, Eigen::Isometry3d::Identity()); - EXPECT_DOUBLE_EQ(scorer.score(holdAt({0.0, 0.0}), makeContext()).total, 0.0); -} - -TEST(ActiveVisionScorer, ScoresZeroForAnInvalidCandidate) { - Fixture fixture; - EXPECT_DOUBLE_EQ(fixture.scorer().score(HeadTrajectory(), makeContext()).total, 0.0); -} - -TEST(ActiveVisionScorer, ScoresZeroWithoutEvaluationTimes) { - Fixture fixture; - ScoringContext context = makeContext(); - context.evaluation_times.clear(); - EXPECT_DOUBLE_EQ(fixture.scorer().score(holdAt({0.0, 0.0}), context).total, 0.0); + const auto breakdown = scorer.score({0.0, 0.0}, makeContext()); + EXPECT_FALSE(breakdown.valid); + EXPECT_DOUBLE_EQ(breakdown.total, 0.0); } // --------------------------------------------------------------------------- @@ -142,8 +125,8 @@ TEST(ActiveVisionScorer, PrefersLookingAtTheFilteredBall) { auto scorer = fixture.scorer(); // Looking down towards the ball beats looking away from it - const double towards = scorer.score(holdAt({0.0, 0.45}), makeContext()).filtered_ball; - const double away = scorer.score(holdAt({-1.2, 0.0}), makeContext()).filtered_ball; + 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); } @@ -151,24 +134,24 @@ TEST(ActiveVisionScorer, PrefersLookingAtTheFilteredBall) { TEST(ActiveVisionScorer, AnUncertainBallContributesLess) { Fixture fixture; fixture.world.setFilteredBall({2.0, 0.0, 0.0}, 0.0, 0.0); - const double certain = fixture.scorer().score(holdAt({0.0, 0.45}), makeContext()).filtered_ball; + 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(holdAt({0.0, 0.45}), makeContext()).filtered_ball; + 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(holdAt({0.0, 0.45}), makeContext()).filtered_ball, 0.0); + 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(holdAt({0.0, 0.45}), makeContext()); + 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); } @@ -176,7 +159,7 @@ TEST(ActiveVisionScorer, RawBallDetectionsAreScoredSeparately) { TEST(ActiveVisionScorer, TeamBallsAreScoredSeparately) { Fixture fixture; fixture.world.setTeamBall(2, {2.0, 0.0, 0.0}, 0.0, 0.0); - auto breakdown = fixture.scorer().score(holdAt({0.0, 0.45}), makeContext()); + 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); } @@ -184,7 +167,7 @@ TEST(ActiveVisionScorer, TeamBallsAreScoredSeparately) { TEST(ActiveVisionScorer, RobotDetectionsAreScoredSeparately) { Fixture fixture; fixture.world.setRobots({TimedTarget{{2.0, 0.0, 0.3}, 1.0, 0.0}}); - auto breakdown = fixture.scorer().score(holdAt({0.0, 0.4}), makeContext()); + auto breakdown = fixture.scorer().score({0.0, 0.4}, makeContext()); EXPECT_GT(breakdown.robots, 0.0); } @@ -196,7 +179,7 @@ TEST(ActiveVisionScorer, CenteringABallBeatsCatchingItAtTheEdge) { 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(holdAt({0.0, pitch}), makeContext()).filtered_ball; + const double score = scorer.score({0.0, pitch}, makeContext()).filtered_ball; if (score > best) { best = score; best_pitch = pitch; @@ -213,17 +196,16 @@ TEST(ActiveVisionScorer, CenteringABallBeatsCatchingItAtTheEdge) { TEST(ActiveVisionScorer, UnobservedFieldIsWorthLookingAt) { Fixture fixture; - EXPECT_GT(fixture.scorer().score(holdAt({0.0, 0.4}), makeContext()).field_coverage, 0.0); + EXPECT_GT(fixture.scorer().score({0.0, 0.4}, makeContext()).field_coverage, 0.0); } TEST(ActiveVisionScorer, AlreadyObservedFieldIsWorthLess) { Fixture fixture; - const auto candidate = holdAt({0.0, 0.4}); - const double before = fixture.scorer().score(candidate, makeContext()).field_coverage; + 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(candidate, makeContext()).field_coverage; + const double after = fixture.scorer().score({0.0, 0.4}, makeContext()).field_coverage; EXPECT_GT(before, 0.0); EXPECT_LT(after, before); @@ -231,15 +213,14 @@ TEST(ActiveVisionScorer, AlreadyObservedFieldIsWorthLess) { TEST(ActiveVisionScorer, ObservationsDecayBackIntoInterest) { Fixture fixture; - const auto candidate = holdAt({0.0, 0.4}); - const double fresh = fixture.scorer().score(candidate, makeContext()).field_coverage; + 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(candidate, makeContext()).field_coverage; + 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(candidate, makeContext()).field_coverage; + const double stale = fixture.scorer().score({0.0, 0.4}, makeContext()).field_coverage; EXPECT_LT(observed, fresh); EXPECT_GT(stale, observed); @@ -254,8 +235,8 @@ TEST(ActiveVisionScorer, LookingOffTheFieldEarnsNoCoverage) { 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(holdAt({-1.2, 0.4}), context).field_coverage; - const double away_from_field = scorer.score(holdAt({1.2, 0.4}), context).field_coverage; + 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. @@ -270,8 +251,8 @@ TEST(ActiveVisionScorer, LookingAtTheSkyEarnsNoCoverage) { // 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(holdAt({0.0, -1.2}), makeContext()).field_coverage; - const double field = scorer.score(holdAt({0.0, 0.4}), makeContext()).field_coverage; + 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); @@ -280,15 +261,14 @@ TEST(ActiveVisionScorer, LookingAtTheSkyEarnsNoCoverage) { TEST(ActiveVisionScorer, DistantFieldCountsForLessThanNearField) { Fixture fixture; - // Two cells of field, one close to the robot and one far away, both unseen. - // A view of the far one sweeps up more ground area simply because distance - // packs more of it into the same image, so without a falloff it would win. + // 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(holdAt({0.0, 0.8}), context).field_coverage; - const double far_view = scorer.score(holdAt({0.0, 0.1}), context).field_coverage; + 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); @@ -308,59 +288,74 @@ TEST(ActiveVisionScorer, TheDistanceFalloffDampensFarViewpoints) { flat.prepare(context.robot_pose); // A view aimed towards the horizon, which is where the far cells are - const auto far_view = holdAt({0.0, 0.1}); - const auto near_view = holdAt({0.0, 0.8}); - - const double sharp_ratio = sharp.score(far_view, context).field_coverage / - std::max(sharp.score(near_view, context).field_coverage, 1e-12); - const double flat_ratio = flat.score(far_view, context).field_coverage / - std::max(flat.score(near_view, context).field_coverage, 1e-12); + 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); } // --------------------------------------------------------------------------- -// Commitment +// Smoothness cost // --------------------------------------------------------------------------- -TEST(ActiveVisionScorer, AgreeingWithThePreviousSelectionScoresHigher) { +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_positions.assign(context.evaluation_times.size(), HeadPosition{0.5, 0.2}); + context.previous_target = HeadPosition{0.0, 0.0}; - const double same = scorer.score(holdAt({0.5, 0.2}), context).commitment; - const double different = scorer.score(holdAt({-0.5, -0.2}), context).commitment; - EXPECT_DOUBLE_EQ(same, 1.0); - EXPECT_LT(different, same); + 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, NoPreviousSelectionMeansNoCommitment) { +TEST(ActiveVisionScorer, NoPreviousTargetMeansNoSmoothnessCost) { Fixture fixture; - // Without a previous trajectory the term must not favour any candidate - EXPECT_DOUBLE_EQ(fixture.scorer().score(holdAt({0.5, 0.2}), makeContext()).commitment, 0.0); + // 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, CommitmentStaysWithinTheUnitInterval) { +TEST(ActiveVisionScorer, TheSmoothnessCostLowersTheTotal) { Fixture fixture; + fixture.world.setFilteredBall({2.0, 0.0, 0.0}, 0.0, 0.0); auto scorer = fixture.scorer(); - ScoringContext context = makeContext(); - context.previous_positions.assign(context.evaluation_times.size(), HeadPosition{1.2, 1.0}); + ScoringWeights weights = scorer.weights(); + weights.smoothness = 2.0; + scorer.setWeights(weights); - for (double yaw = -1.2; yaw <= 1.2; yaw += 0.2) { - const double commitment = scorer.score(holdAt({yaw, 0.0}), context).commitment; - EXPECT_GE(commitment, 0.0); - EXPECT_LE(commitment, 1.0); - } + // 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, EveryTermStaysWithinTheUnitInterval) { +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}}); @@ -369,35 +364,38 @@ TEST(ActiveVisionScorer, EveryTermStaysWithinTheUnitInterval) { auto scorer = fixture.scorer(); ScoringContext context = makeContext(); - context.previous_positions.assign(context.evaluation_times.size(), HeadPosition{0.0, 0.3}); + 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(holdAt({yaw, pitch}), context); + 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, breakdown.commitment}) { + breakdown.robots}) { EXPECT_GE(term, 0.0); EXPECT_LE(term, 1.0); } + EXPECT_GE(breakdown.smoothness_cost, 0.0); } } } -TEST(ActiveVisionScorer, TheTotalIsTheWeightedSumOfTheTerms) { +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(); - const auto breakdown = scorer.score(holdAt({0.0, 0.4}), makeContext()); + 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.commitment * breakdown.commitment; + 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, ZeroWeightsDisableATerm) { +TEST(ActiveVisionScorer, ZeroWeightsDisableEveryTerm) { Fixture fixture; fixture.world.setFilteredBall({2.0, 0.0, 0.0}, 0.0, 0.0); auto scorer = fixture.scorer(); @@ -408,10 +406,12 @@ TEST(ActiveVisionScorer, ZeroWeightsDisableATerm) { weights.team_ball = 0.0; weights.field_coverage = 0.0; weights.robots = 0.0; - weights.commitment = 0.0; + weights.smoothness = 0.0; scorer.setWeights(weights); - EXPECT_DOUBLE_EQ(scorer.score(holdAt({0.0, 0.4}), makeContext()).total, 0.0); + 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) { @@ -430,8 +430,8 @@ TEST(ActiveVisionScorer, TheRobotPoseMovesWhatIsVisible) { // 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(holdAt({0.0, 0.5}), facing).filtered_ball; - const double away = scorer.score(holdAt({0.0, 0.5}), turned_away).filtered_ball; + 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_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_trajectory_sampler.cpp b/src/bitbots_motion/bitbots_head_mover/test/test_trajectory_sampler.cpp deleted file mode 100644 index a0ac9ab145..0000000000 --- a/src/bitbots_motion/bitbots_head_mover/test/test_trajectory_sampler.cpp +++ /dev/null @@ -1,260 +0,0 @@ -#include - -#include -#include - -using bitbots_head_mover::buildCandidateTrajectory; -using bitbots_head_mover::DynamicLimits; -using bitbots_head_mover::HeadLimits; -using bitbots_head_mover::HeadPosition; -using bitbots_head_mover::HeadTrajectory; -using bitbots_head_mover::HeadVelocity; -using bitbots_head_mover::isFeasible; -using bitbots_head_mover::SamplerConfig; -using bitbots_head_mover::TrajectorySampler; - -namespace { -constexpr HeadLimits kLimits{{-1.23, 1.23}, {-1.23, 1.01}}; - -DynamicLimits makeDynamics() { - DynamicLimits dynamics; - dynamics.max_velocity = {4.0, 4.0}; - dynamics.max_acceleration = {14.0, 14.0}; - return dynamics; -} - -SamplerConfig makeConfig() { - SamplerConfig config; - config.sample_count = 64; - config.horizon = 1.0; - config.midpoint_time = 0.5; - config.evaluation_points = 5; - config.feasibility_points = 21; - return config; -} -} // namespace - -// --------------------------------------------------------------------------- -// buildCandidateTrajectory -// --------------------------------------------------------------------------- - -TEST(BuildCandidateTrajectory, StartsInTheGivenState) { - const HeadPosition start{0.1, -0.2}; - const HeadVelocity start_velocity{0.5, -0.3}; - HeadTrajectory trajectory = - buildCandidateTrajectory(start, start_velocity, {0.3, 0.0}, {0.5, 0.2}, 0.5, 1.0, makeDynamics()); - - ASSERT_TRUE(trajectory.valid()); - // Continuing the motion the head is already performing is what keeps - // replanning every cycle from producing a jerk - EXPECT_NEAR(trajectory.position(0.0).yaw, start.yaw, 1e-9); - EXPECT_NEAR(trajectory.position(0.0).pitch, start.pitch, 1e-9); - EXPECT_NEAR(trajectory.velocity(0.0).yaw, start_velocity.yaw, 1e-9); - EXPECT_NEAR(trajectory.velocity(0.0).pitch, start_velocity.pitch, 1e-9); -} - -TEST(BuildCandidateTrajectory, PassesThroughTheMidpoint) { - const HeadPosition midpoint{0.3, 0.1}; - HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, midpoint, {0.5, 0.2}, 0.5, 1.0, makeDynamics()); - - ASSERT_TRUE(trajectory.valid()); - EXPECT_NEAR(trajectory.position(0.5).yaw, midpoint.yaw, 1e-9); - EXPECT_NEAR(trajectory.position(0.5).pitch, midpoint.pitch, 1e-9); -} - -TEST(BuildCandidateTrajectory, EndsAtTheEndpointAtRest) { - const HeadPosition endpoint{0.5, 0.2}; - HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, {0.3, 0.1}, endpoint, 0.5, 1.0, makeDynamics()); - - ASSERT_TRUE(trajectory.valid()); - EXPECT_NEAR(trajectory.position(1.0).yaw, endpoint.yaw, 1e-9); - EXPECT_NEAR(trajectory.position(1.0).pitch, endpoint.pitch, 1e-9); - EXPECT_NEAR(trajectory.velocity(1.0).yaw, 0.0, 1e-9); - EXPECT_NEAR(trajectory.velocity(1.0).pitch, 0.0, 1e-9); -} - -TEST(BuildCandidateTrajectory, MidpointVelocityFollowsTheDirectionOfTravel) { - // Moving to a larger yaw means the head is still moving that way at the midpoint - HeadTrajectory forward = buildCandidateTrajectory({0.0, 0.0}, {}, {0.3, 0.0}, {0.6, 0.0}, 0.5, 1.0, makeDynamics()); - ASSERT_TRUE(forward.valid()); - EXPECT_GT(forward.velocity(0.5).yaw, 0.0); - - HeadTrajectory backward = buildCandidateTrajectory({0.0, 0.0}, {}, {-0.3, 0.0}, {-0.6, 0.0}, 0.5, 1.0, makeDynamics()); - ASSERT_TRUE(backward.valid()); - EXPECT_LT(backward.velocity(0.5).yaw, 0.0); -} - -TEST(BuildCandidateTrajectory, MidpointVelocityIsClampedToTheLimit) { - DynamicLimits dynamics = makeDynamics(); - dynamics.max_velocity = {0.1, 0.1}; - // A distant endpoint would imply a midpoint velocity far above the limit - HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, {0.5, 0.0}, {1.0, 0.0}, 0.5, 1.0, dynamics); - ASSERT_TRUE(trajectory.valid()); - EXPECT_LE(std::abs(trajectory.velocity(0.5).yaw), dynamics.max_velocity.yaw + 1e-9); -} - -TEST(BuildCandidateTrajectory, RejectsDegenerateTimings) { - EXPECT_FALSE(buildCandidateTrajectory({}, {}, {}, {}, 0.5, 0.0, makeDynamics()).valid()); - EXPECT_FALSE(buildCandidateTrajectory({}, {}, {}, {}, 0.0, 1.0, makeDynamics()).valid()); - // The midpoint has to sit strictly inside the horizon - EXPECT_FALSE(buildCandidateTrajectory({}, {}, {}, {}, 1.0, 1.0, makeDynamics()).valid()); - EXPECT_FALSE(buildCandidateTrajectory({}, {}, {}, {}, 1.5, 1.0, makeDynamics()).valid()); -} - -// --------------------------------------------------------------------------- -// isFeasible -// --------------------------------------------------------------------------- - -TEST(IsFeasible, AcceptsAGentleTrajectory) { - HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, {0.1, 0.05}, {0.2, 0.1}, 0.5, 1.0, makeDynamics()); - EXPECT_TRUE(isFeasible(trajectory, kLimits, makeDynamics(), 1.0, 21)); -} - -TEST(IsFeasible, RejectsTrajectoriesLeavingTheJointLimits) { - // The endpoint is far outside the head's reachable range - HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, {1.0, 0.0}, {2.0, 0.0}, 0.5, 1.0, makeDynamics()); - EXPECT_FALSE(isFeasible(trajectory, kLimits, makeDynamics(), 1.0, 21)); -} - -TEST(IsFeasible, RejectsTrajectoriesExceedingTheVelocityLimit) { - DynamicLimits dynamics = makeDynamics(); - dynamics.max_velocity = {0.05, 0.05}; - // Crossing the whole range in a second needs far more than the allowed speed - HeadTrajectory trajectory = buildCandidateTrajectory({-1.0, 0.0}, {}, {0.0, 0.0}, {1.0, 0.0}, 0.5, 1.0, dynamics); - EXPECT_FALSE(isFeasible(trajectory, kLimits, dynamics, 1.0, 21)); -} - -TEST(IsFeasible, RejectsTrajectoriesExceedingTheAccelerationLimit) { - DynamicLimits dynamics = makeDynamics(); - dynamics.max_acceleration = {0.01, 0.01}; - HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, {0.5, 0.0}, {1.0, 0.0}, 0.5, 1.0, dynamics); - EXPECT_FALSE(isFeasible(trajectory, kLimits, dynamics, 1.0, 21)); -} - -TEST(IsFeasible, RejectsInvalidTrajectories) { - EXPECT_FALSE(isFeasible(HeadTrajectory(), kLimits, makeDynamics(), 1.0, 21)); -} - -TEST(IsFeasible, NeedsAtLeastTwoCheckPoints) { - HeadTrajectory trajectory = buildCandidateTrajectory({0.0, 0.0}, {}, {0.1, 0.0}, {0.2, 0.0}, 0.5, 1.0, makeDynamics()); - EXPECT_FALSE(isFeasible(trajectory, kLimits, makeDynamics(), 1.0, 1)); -} - -// --------------------------------------------------------------------------- -// TrajectorySampler -// --------------------------------------------------------------------------- - -TEST(TrajectorySampler, ProducesCandidates) { - TrajectorySampler sampler(makeConfig()); - auto candidates = sampler.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); - EXPECT_FALSE(candidates.empty()); -} - -TEST(TrajectorySampler, AlwaysOffersHoldingTheCurrentPosition) { - TrajectorySampler sampler(makeConfig()); - const HeadPosition start{0.2, -0.1}; - auto candidates = sampler.sample(start, {}, kLimits, makeDynamics()); - - ASSERT_FALSE(candidates.empty()); - // The first candidate comes to rest exactly where the head already is, which - // guarantees the scoring always has something valid to pick - EXPECT_NEAR(candidates.front().endpoint.yaw, start.yaw, 1e-9); - EXPECT_NEAR(candidates.front().endpoint.pitch, start.pitch, 1e-9); -} - -TEST(TrajectorySampler, EveryCandidateIsFeasible) { - TrajectorySampler sampler(makeConfig()); - const DynamicLimits dynamics = makeDynamics(); - auto candidates = sampler.sample({0.3, -0.2}, {1.0, 0.5}, kLimits, dynamics); - - ASSERT_FALSE(candidates.empty()); - for (const auto& candidate : candidates) { - EXPECT_TRUE(isFeasible(candidate.trajectory, kLimits, dynamics, makeConfig().horizon, makeConfig().feasibility_points)); - } -} - -TEST(TrajectorySampler, EveryCandidateStartsInTheCurrentState) { - TrajectorySampler sampler(makeConfig()); - const HeadPosition start{0.3, -0.2}; - const HeadVelocity velocity{1.0, 0.5}; - auto candidates = sampler.sample(start, velocity, kLimits, makeDynamics()); - - ASSERT_FALSE(candidates.empty()); - for (const auto& candidate : candidates) { - EXPECT_NEAR(candidate.trajectory.position(0.0).yaw, start.yaw, 1e-9); - EXPECT_NEAR(candidate.trajectory.velocity(0.0).yaw, velocity.yaw, 1e-9); - } -} - -TEST(TrajectorySampler, CandidatesStayWithinTheJointLimits) { - TrajectorySampler sampler(makeConfig()); - auto candidates = sampler.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); - - ASSERT_FALSE(candidates.empty()); - for (const auto& candidate : candidates) { - EXPECT_TRUE(kLimits.contains(candidate.endpoint)); - EXPECT_TRUE(kLimits.contains(candidate.midpoint)); - } -} - -TEST(TrajectorySampler, CandidatesAreDiverse) { - TrajectorySampler sampler(makeConfig()); - auto candidates = sampler.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); - - ASSERT_GT(candidates.size(), 2u); - double min_yaw = 1e9, max_yaw = -1e9; - for (const auto& candidate : candidates) { - min_yaw = std::min(min_yaw, candidate.endpoint.yaw); - max_yaw = std::max(max_yaw, candidate.endpoint.yaw); - } - // A sampler that always returned the same candidate would make the whole - // search pointless - EXPECT_GT(max_yaw - min_yaw, 0.1); -} - -TEST(TrajectorySampler, IsDeterministicForAGivenSeed) { - TrajectorySampler first(makeConfig(), 1234); - TrajectorySampler second(makeConfig(), 1234); - auto a = first.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); - auto b = second.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); - - ASSERT_EQ(a.size(), b.size()); - for (size_t i = 0; i < a.size(); i++) { - EXPECT_DOUBLE_EQ(a[i].endpoint.yaw, b[i].endpoint.yaw); - EXPECT_DOUBLE_EQ(a[i].endpoint.pitch, b[i].endpoint.pitch); - } -} - -TEST(TrajectorySampler, DifferentSeedsExploreDifferently) { - TrajectorySampler first(makeConfig(), 1); - TrajectorySampler second(makeConfig(), 2); - auto a = first.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); - auto b = second.sample({0.0, 0.0}, {}, kLimits, makeDynamics()); - - ASSERT_FALSE(a.empty()); - ASSERT_FALSE(b.empty()); - // The hold candidate is identical by construction, so compare a random one - ASSERT_GT(a.size(), 1u); - ASSERT_GT(b.size(), 1u); - EXPECT_NE(a[1].endpoint.yaw, b[1].endpoint.yaw); -} - -TEST(TrajectorySampler, SlowJointsRestrictHowFarCandidatesReach) { - DynamicLimits dynamics = makeDynamics(); - dynamics.max_velocity = {0.2, 0.2}; - TrajectorySampler sampler(makeConfig()); - auto candidates = sampler.sample({0.0, 0.0}, {}, kLimits, dynamics); - - ASSERT_FALSE(candidates.empty()); - for (const auto& candidate : candidates) { - // Nothing beyond what the joint could travel within the horizon - EXPECT_LE(std::abs(candidate.endpoint.yaw), 0.2 * makeConfig().horizon + 1e-9); - } -} - -TEST(TrajectorySampler, ZeroSampleCountYieldsNothing) { - SamplerConfig config = makeConfig(); - config.sample_count = 0; - TrajectorySampler sampler(config); - EXPECT_TRUE(sampler.sample({0.0, 0.0}, {}, kLimits, makeDynamics()).empty()); -} From dd587e43bb43fcdd522c050a3fb4dde77b6b3a5a Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sun, 6 Sep 2026 11:38:35 +0200 Subject: [PATCH 6/8] Use active vision in behavior Signed-off-by: Florian Vahl --- .../behavior_dsd/actions/head_modes.py | 7 +++ .../behavior_dsd/main.dsd | 44 +++++++++---------- 2 files changed, 29 insertions(+), 22 deletions(-) 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..9cb41049f6 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,10 @@ 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 3f2baaf6f1..2bf8c713ee 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 STRIKER --> #SearchBall @@ -93,14 +93,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 @@ -131,7 +131,7 @@ $IsPenalized DONE --> $AnyGoalScoreRecently + time:50 YES --> #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 --> #PositioningReady SET --> $WhistleDetected DETECTED --> $SecondaryStateTeamDecider @@ -143,7 +143,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 From 4372811e8c97aef6dd7c0210c0ed4092e3235571 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sun, 6 Sep 2026 11:49:06 +0200 Subject: [PATCH 7/8] Add to typing Signed-off-by: Florian Vahl --- .../bitbots_blackboard/capsules/misc_capsule.py | 1 + 1 file changed, 1 insertion(+) 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, ] From 902c1f2f9b4a74187f60bbdc6c43ca77c684d755 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Mon, 7 Sep 2026 18:38:47 +0200 Subject: [PATCH 8/8] style: apply the repository formatting Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01NcdgjqxnvjxCH8iqfgmTUx --- .../behavior_dsd/actions/head_modes.py | 1 + .../bitbots_head_mover/src/head_kinematics.cpp | 3 ++- src/bitbots_motion/bitbots_head_mover/src/look_at.cpp | 4 ++-- .../bitbots_head_mover/test/test_head_trajectory.cpp | 2 +- .../bitbots_head_mover/test/test_search_pattern.cpp | 7 +++---- 5 files changed, 9 insertions(+), 8 deletions(-) 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 9cb41049f6..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 @@ -108,6 +108,7 @@ 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""" diff --git a/src/bitbots_motion/bitbots_head_mover/src/head_kinematics.cpp b/src/bitbots_motion/bitbots_head_mover/src/head_kinematics.cpp index 2c9417d0fe..bf0618f4d0 100644 --- a/src/bitbots_motion/bitbots_head_mover/src/head_kinematics.cpp +++ b/src/bitbots_motion/bitbots_head_mover/src/head_kinematics.cpp @@ -1,7 +1,8 @@ +#include + #include #include #include -#include #include 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 index e0b6a8059e..0effe3b57a 100644 --- a/src/bitbots_motion/bitbots_head_mover/src/look_at.cpp +++ b/src/bitbots_motion/bitbots_head_mover/src/look_at.cpp @@ -10,8 +10,8 @@ HeadPosition motorGoalsFromPoint(const geometry_msgs::msg::Point& head_yaw_point 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)); + 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)}; 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 index d34b819c24..4937984ec8 100644 --- a/src/bitbots_motion/bitbots_head_mover/test/test_head_trajectory.cpp +++ b/src/bitbots_motion/bitbots_head_mover/test/test_head_trajectory.cpp @@ -19,7 +19,7 @@ const std::vector kPattern = {{-30.0, 35.0}, {30.0, 35.0}, {30.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}; +const HeadPosition kPatternStart{kPattern[0].yaw * kDegToRad, kPattern[0].pitch* kDegToRad}; } // namespace // --------------------------------------------------------------------------- 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 index 6e68740baa..de4a3eb7eb 100644 --- a/src/bitbots_motion/bitbots_head_mover/test/test_search_pattern.cpp +++ b/src/bitbots_motion/bitbots_head_mover/test/test_search_pattern.cpp @@ -124,10 +124,9 @@ TEST(GeneratePattern, VisitsEveryScanLine) { 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"; + 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"; } }