<commit_before>/**
 * @file PathPlanner.cpp
 *
 * @author <a href="mailto:<EMAIL>"><NAME></a>
 * @author <a href="mailto:<EMAIL>"><NAME></a>
 * Implementation of class PathPlanner
 */

#include "PathPlanner.h"
//#include <iostream>

PathPlanner::PathPlanner() 
:
  isWalking(false)
{
  // Stand
  DEBUG_REQUEST_REGISTER("PathPlanner:motion:stand",
                         "Stand still", true);

  // Walk
  DEBUG_REQUEST_REGISTER("PathPlanner:motion:walk_forward",
                         "Walk forward", false);
  // Go to ball (XABSL: go_to_ball)
  DEBUG_REQUEST_REGISTER("PathPlanner:motion:go_to_ball",
                         "Go to the ball", false);
  // Search for the ball
  DEBUG_REQUEST_REGISTER("PathPlanner:head:Search",
                         "Search for the ball", false);
  // Look at the ball
  DEBUG_REQUEST_REGISTER("PathPlanner:head:LookAtMultiBallPercept",
                "Look at the ball", false);
}

void PathPlanner::execute() 
{
  // reset some stuff by default
  getMotionRequest().forced = false;
  getMotionRequest().standHeight = -1; // sit in a stable position

  getMotionRequest().walkRequest.target = Pose2D();
  getMotionRequest().forced = false;

  // Stand still
  DEBUG_REQUEST("PathPlanner:motion:stand",
                getMotionRequest().id = motion::stand;
                );

  // Walk forward
  DEBUG_REQUEST("PathPlanner:motion:walk_forward",
                getMotionRequest().id = motion::walk;
                getMotionRequest().walkRequest.target.translation.x = 500;
                );

  // Walk to the ball (XABSL: go_to_ball)
  DEBUG_REQUEST(
    "PathPlanner:motion:go_to_ball",
    if (getMultiBallPercept().wasSeen())
    {
      Vector2d preview = getBallModel().positionPreview;
      Vector2d pos     = getBallModel().position;
      double ballRad   = getMultiBallPercept().getPercepts().at(0).radiusInImage;
      getMotionRequest().id                               = motion::walk;
      getMotionRequest().walkRequest.coordinate           = WalkRequest::Hip;
      getMotionRequest().walkRequest.character            = 0.5;
      getMotionRequest().walkRequest.target.translation.x = std::max(0.7 * (pos.x - ballRad), 0.0);
      getMotionRequest().walkRequest.target.translation.y = preview.y;
      getMotionRequest().walkRequest.target.rotation      = preview.abs() > 250
                                                          ? preview.angle()
                                                          : 0;
      /*std::cout << "preview.x: "   << preview.x                           << std::endl
                << "pos.x: "       << pos.x                               << std::endl
                << "ballRad: "     << ballRad                             << std::endl
                << "translate x: " << 0.7 * (preview.x - pos.x - ballRad) << std::endl;*/
    }
  );
<commit_msg>More go to ball XABSL stuff implemented<commit_after>/**
 * @file PathPlanner.cpp
 *
 * @author <a href="mailto:<EMAIL>"><NAME></a>
 * @author <a href="mailto:<EMAIL>"><NAME></a>
 * Implementation of class PathPlanner
 */

#include "PathPlanner.h"

PathPlanner::PathPlanner() 
:
  isWalking(false)
{
  // Stand
  DEBUG_REQUEST_REGISTER("PathPlanner:motion:stand",
                         "Stand still", true);

  // Walk
  DEBUG_REQUEST_REGISTER("PathPlanner:motion:walk_forward",
                         "Walk forward", false);
  // Go to ball (XABSL: go_to_ball)
  DEBUG_REQUEST_REGISTER("PathPlanner:motion:go_to_ball",
                         "Go to the ball", false);
  // Go to ball with left foot (XABSL: go_to_ball_with_foot decision left)
  DEBUG_REQUEST_REGISTER("PathPlanner:motion:go_to_ball_left_foot",
                         "Go to the ball with the left foot", false);
  // Go to ball with right foot (XABSL: go_to_ball_with_foot decision right)
  DEBUG_REQUEST_REGISTER("PathPlanner:motion:go_to_ball_right_foot",
                         "Go to the ball with the right foot", false);
  // Go to ball with left foot dynamic (XABSL: go_to_ball_with_foot_dynamic decision left)
  DEBUG_REQUEST_REGISTER("PathPlanner:motion:go_to_ball_left_foot_dynamic",
                         "Go to the ball with the left foot dynamically", false);
  // Go to ball with right foot dynamic (XABSL: go_to_ball_with_foot_dynamic decision right)
  DEBUG_REQUEST_REGISTER("PathPlanner:motion:go_to_ball_right_foot_dynamic",
                         "Go to the ball with the right foot dynamically", false);
  // Search for the ball
  DEBUG_REQUEST_REGISTER("PathPlanner:head:Search",
                         "Search for the ball", false);
  // Look at the ball
  DEBUG_REQUEST_REGISTER("PathPlanner:head:LookAtMultiBallPercept",
                "Look at the ball", false);
}

void PathPlanner::execute() 
{
  // reset some stuff by default
  getMotionRequest().forced = false;
  getMotionRequest().standHeight = -1; // sit in a stable position

  getMotionRequest().walkRequest.target = Pose2D();
  getMotionRequest().forced = false;

  // Stand still
  DEBUG_REQUEST("PathPlanner:motion:stand",
                getMotionRequest().id = motion::stand;
                );

  // Walk forward
  DEBUG_REQUEST("PathPlanner:motion:walk_forward",
                getMotionRequest().id = motion::walk;
                getMotionRequest().walkRequest.target.translation.x = 500;
                );

  // Walk to the ball (XABSL: go_to_ball)
  DEBUG_REQUEST(
    "PathPlanner:motion:go_to_ball",
    if (getMultiBallPercept().wasSeen())
    {
      Vector2d preview = getBallModel().positionPreview;
      double ballRad   = getMultiBallPercept().getPercepts().at(0).radiusInImage;
      getMotionRequest().id                               = motion::walk;
      getMotionRequest().walkRequest.coordinate           = WalkRequest::Hip;
      getMotionRequest().walkRequest.character            = 0.5;
      getMotionRequest().walkRequest.target.translation.x = std::max(0.7 * (preview.x - ballRad), 0.0);
      getMotionRequest().walkRequest.target.translation.y = preview.y;
      getMotionRequest().walkRequest.target.rotation      = preview.abs() > 250
                                                          ? preview.angle()
                                                          : 0;
    }
  );

  // Go to ball with left foot (XABSL: go_to_ball_with_foot decision left)
  DEBUG_REQUEST(
    "PathPlanner:motion:go_to_ball_left_foot",
    if (getMultiBallPercept().wasSeen())
    {
      Vector2d preview = getBallModel().positionPreviewInLFoot;
      double ballRad   = getMultiBallPercept().getPercepts().at(0).radiusInImage;
      getMotionRequest().id                               = motion::walk;
      getMotionRequest().walkRequest.coordinate           = WalkRequest::LFoot;
      getMotionRequest().walkRequest.character            = 1.0;
      getMotionRequest().walkRequest.target.translation.x = std::max(0.7 * (preview.x - ballRad), 0.0);
      getMotionRequest().walkRequest.target.translation.y = preview.y; // + offset.y (what is it?)
      getMotionRequest().walkRequest.target.rotation      = 0;
    }
  );
  // Go to ball with right foot (XABSL: go_to_ball_with_foot decision right)
  DEBUG_REQUEST(
    "PathPlanner:motion:go_to_ball_right_foot",
    if (getMultiBallPercept().wasSeen())
    {
      Vector2d preview = getBallModel().positionPreviewInRFoot;
      double ballRad   = getMultiBallPercept().getPercepts().at(0).radiusInImage;
      getMotionRequest().id                               = motion::walk;
      getMotionRequest().walkRequest.coordinate           = WalkRequest::RFoot;
      getMotionRequest().walkRequest.character            = 1.0;
      getMotionRequest().walkRequest.target.translation.x = std::max(0.7 * (preview.x - ballRad), 0.0);
      getMotionRequest().walkRequest.target.translation.y = preview.y; // + offset.y (what is it?)
      getMotionRequest().walkRequest.target.rotation      = 0;
    }
  );
  // Go to ball with left foot dynamic (XABSL: go_to_ball_with_foot_dynamic decision left)
  DEBUG_REQUEST(
    "PathPlanner:motion:go_to_ball_left_foot_dynamic",
    if (getMultiBallPercept().wasSeen())
    {
      Vector2d preview = getBallModel().positionPreviewInLFoot;
      double ballRad   = getMultiBallPercept().getPercepts().at(0).radiusInImage;
      getMotionRequest().id                               = motion::walk;
      getMotionRequest().walkRequest.coordinate           = WalkRequest::LFoot;
      getMotionRequest().walkRequest.character            = 1.0;
      getMotionRequest().walkRequest.target.translation.x =
          0.7 * (preview.x - std::abs(preview.y/* + offset.y*/) - ballRad);
      getMotionRequest().walkRequest.target.translation.y = 0.7 * (preview.y/* + offset.y*/);
      getMotionRequest().walkRequest.target.rotation      = preview.abs() > 250
                                                          ? preview.angle()
                                                          : 0;
    }
  );
  // Go to ball with right foot dynamic (XABSL: go_to_ball_with_foot_dynamic decision right)
  DEBUG_REQUEST(
    "PathPlanner:motion:go_to_ball_right_foot_dynamic",
    if (getMultiBallPercept().wasSeen())
    {
      Vector2d preview = getBallModel().positionPreviewInRFoot;
      double ballRad   = getMultiBallPercept().getPercepts().at(0).radiusInImage;
      getMotionRequest().id                               = motion::walk;
      getMotionRequest().walkRequest.coordinate           = WalkRequest::RFoot;
      getMotionRequest().walkRequest.character            = 1.0;
      getMotionRequest().walkRequest.target.translation.x =
          0.7 * (preview.x - std::abs(preview.y/* + offset.y*/) - ballRad);
      getMotionRequest().walkRequest.target.translation.y = 0.7 * (preview.y/* + offset.y*/);
      getMotionRequest().walkRequest.target.rotation      = preview.abs() > 250
                                                          ? preview.angle()
                                                          : 0;
    }
  );

{'fraction_non_alphanumeric': 0.11946725415581815, 'fraction_numerical': 0.007911075505707991, 'mean_word_length': 2.2830374753451674, 'pattern_counts': {'":': 0, '<': 44, '<?xml version=': 0, '>': 24, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 23, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4046552', 'n_tokens_mistral': 2976, 'n_tokens_neox': 2799, 'n_words': 522}