fixed merge conflict

.github/workflows/ci.yml

@@ -7,7 +7,7 @@ jobs:
   build-and-test:
     runs-on: ubuntu-latest
     container:
-      image: robojackets/robocup-software:foxy
+      image: robojackets/robocup-software:humble
     defaults:
       run:
         shell: bash
@@ -34,23 +34,23 @@ jobs:
       - name: Build
         run: |
           echo "::add-matcher::ci/clang.json"
-          source /opt/ros/foxy/setup.bash
+          source /opt/ros/humble/setup.bash
           source install/setup.bash
           make all
           echo "::remove-matcher owner=clang::"
 
       - name: Test
         run: |
           echo "::add-matcher::ci/gtest.json"
-          source /opt/ros/foxy/setup.bash
+          source /opt/ros/humble/setup.bash
           source install/setup.bash
           ./install/lib/rj_robocup/test-soccer
           echo "::remove-matcher owner=gtest::"
 
       - name: Run clang-tidy
         run: |
           echo "::add-matcher::ci/clang-tidy.json"
-          source /opt/ros/foxy/setup.bash
+          source /opt/ros/humble/setup.bash
           source install/setup.bash
           DIFFBASE=${{ github.base_ref }} make checktidy-lines
           echo "::remove-matcher owner=clang-tidy::"
@@ -61,7 +61,7 @@ jobs:
     if: startsWith(github.head_ref, 'fix-code-style') == false
     runs-on: ubuntu-latest
     container:
-      image: ros:foxy
+      image: ros:humble
     defaults:
       run:
         shell: bash
@@ -94,7 +94,7 @@ jobs:
         id: style-check
         run: |
             # check formatting style (C++)
-            git diff -U0 --no-color ${{ github.base_ref }} ${{ github.head_ref }} | python3 util/clang-format-diff.py -binary clang-format-10 -i -p1
+            git diff -U0 --no-color ${{ github.base_ref }} ${{ github.head_ref }} | python3 util/clang-format-diff.py -binary clang-format-14 -i -p1
 
             if ! git diff-index --quiet HEAD; then
               echo "::set-output name=changed::true"

.gitignore

@@ -6,7 +6,6 @@
 
 # build directories
 /build
-/install
 /gmon.out
 *.swp
 /.settings

CMakeLists.txt

@@ -78,7 +78,7 @@ endif()
 set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O2 -march=native")
 set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -Og")
 set(CMAKE_CXX_FLAGS_DEBUG
-    "${CMAKE_CXX_FLAGS_DEBUG} -Werror=return-type -Werror=delete-non-virtual-dtor -Werror=switch")
+    "${CMAKE_CXX_FLAGS_DEBUG} -Werror=return-type -Werror=delete-non-virtual-dtor")
 
 # ======================================================================
 # Testing

Dockerfile

@@ -1,5 +1,5 @@
 # Use phusion/baseimage if problems arise
-FROM ros:foxy-ros-base-focal
+FROM ros:humble-ros-base-jammy
 LABEL maintainer="[email protected]"
 
 # Setup apt to be happy with no console input

docs/source/tutorial.rst

@@ -351,6 +351,7 @@ but here are some more hints.
 * The motion command for driving in a straight line is :cpp:`"path_target"`.
 * You will probably need to override some methods relating to passing, but you can leave their implementations empty. They don't need to do anything in your position, as your robot will not pass the ball
 * The simulator tells you the coordinates of your cursor—these are the same coordinates you can use in your motion commands.
+* You will need to add the new file name you create to ``soccer/src/soccer/CMakeLists.txt``. See how this is done for other positions.
 
 Testing
 ~~~~~~~
@@ -668,4 +669,4 @@ Here are all the external links from this document, copied again for your easy r
 
 * `C++ Inheritance`_ 
 
-.. _C++ Inheritance: https://www.learncpp.com/cpp-tutorial/basic-inheritance-in-c/
+.. _C++ Inheritance: https://www.learncpp.com/cpp-tutorial/basic-inheritance-in-c/

install/env.sh

@@ -0,0 +1,5 @@
+# Here are some useful environment variables for setting up a consistent ROS environment 
+# some information on this can be found here: https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Configuring-ROS2-Environment.html 
+export ROS_DOMAIN_ID=19 # this is a random number, it's just important to keep it consistent across shells 
+export ROS_LOCALHOST_ONLY=1 # for our stack specifically, this is quite helpful. 
+

install/setup.bash

@@ -34,7 +34,7 @@ _pythonpath_add() {
 # No direct replacement for the command exists, and in Ubuntu 22.04,
 # this script will produce a warning about this. We may need to find a
 # replacement library in the future.
-_PYTHON_LIB_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(prefix='${_INSTALL_PATH}'))")
+_PYTHON_LIB_PATH=$(python3 -c "import sysconfig; print(sysconfig.get_path('purelib'))")
 _pythonpath_add "${_PYTHON_LIB_PATH}"
 unset _PYTHON_LIB_PATH
 

install/setup.zsh

@@ -30,7 +30,7 @@ _pythonpath_add() {
     fi
 }
 
-_PYTHON_LIB_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(prefix='${_INSTALL_PATH}'))")
+_PYTHON_LIB_PATH=$(python3 -c "import sysconfig; print(sysconfig.get_path('purelib'))")
 _pythonpath_add "${_PYTHON_LIB_PATH}"
 unset _PYTHON_LIB_PATH
 

launch/framework.bash

@@ -10,7 +10,7 @@ if [ -z "$binary" ]; then
 fi
 
 # Run the binary in the background
-"$binary" &
+"$binary" -g 2020B --realism RC2021 &
 binary_pid=$!
 
 # Ensure that pressing Ctrl+C kills all subprocesses

makefile

@@ -72,11 +72,11 @@ run-sim:
 # run our stack with default flags
 # TODO: actually name our software stack something
 run-our-stack:
-	ROS_LOCALHOST_ONLY=1 ros2 launch rj_robocup soccer.launch.py run_sim:=True
+	ros2 launch rj_robocup soccer.launch.py run_sim:=True
 
 # run sim with external referee (SSL Game Controller)
 run-sim-external:
-	ROS_LOCALHOST_ONLY=1 ros2 launch rj_robocup soccer.launch.py run_sim:=True use_internal_ref:=False
+	ros2 launch rj_robocup soccer.launch.py run_sim:=True use_internal_ref:=False
 
 run-sim-ex: run-sim-external
 
@@ -95,7 +95,7 @@ run-manual:
 
 # same as run-real, with different server port
 run-alt-real:
-	ROS_DOMAIN_ID=2 ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False server_port:=25564 use_internal_ref:=False team_name:=AltRoboJackets team_flag:=-b
+	ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False server_port:=25564 use_internal_ref:=False team_name:=AltRoboJackets team_flag:=-b
 
 # run sim2play (requires external referee)
 run-sim2play:

soccer/src/soccer/CMakeLists.txt

@@ -80,6 +80,7 @@ set(ROBOCUP_LIB_SRC
     strategy/agent/position/position.cpp
     strategy/agent/position/robot_factory_position.cpp
     strategy/agent/position/goalie.cpp
+    strategy/agent/position/line.cpp
     strategy/agent/position/offense.cpp
     strategy/agent/position/idle.cpp
     strategy/agent/position/defense.cpp

soccer/src/soccer/planning/planner/line_kick_path_planner.cpp

@@ -88,6 +88,8 @@ Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) {
 
     MotionCommand modified_command{"path_target", target,
                                    FacePoint{plan_request.motion_command.target.position}};
+
+    modified_command.ignore_ball = true;
     modified_request.motion_command = modified_command;
 
     return path_target_.plan(modified_request);

soccer/src/soccer/planning/planner/settle_path_planner.cpp

@@ -221,9 +221,9 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn
 
         // Plan a path from our partial path start location to the intercept
         // test location
-        Trajectory path = CreatePath::rrt(start_instant.linear_motion(), target_robot_intersection,
-                                          plan_request.constraints.mot, start_instant.stamp,
-                                          static_obstacles, dynamic_obstacles);
+        Trajectory path = CreatePath::intermediate(
+            start_instant.linear_motion(), target_robot_intersection, plan_request.constraints.mot,
+            start_instant.stamp, static_obstacles);
 
         // Calculate the
         RJ::Seconds buffer_duration = ball_time - path.duration();
@@ -332,9 +332,9 @@ Trajectory SettlePathPlanner::intercept(const PlanRequest& plan_request, RobotIn
         LinearMotionInstant target{closest_pt,
                                    settle::PARAM_ball_speed_percent_for_dampen * average_ball_vel_};
 
-        Trajectory shortcut =
-            CreatePath::rrt(start_instant.linear_motion(), target, plan_request.constraints.mot,
-                            start_instant.stamp, static_obstacles, dynamic_obstacles);
+        Trajectory shortcut = CreatePath::intermediate(start_instant.linear_motion(), target,
+                                                       plan_request.constraints.mot,
+                                                       start_instant.stamp, static_obstacles);
 
         if (!shortcut.empty()) {
             plan_angles(&shortcut, start_instant, AngleFns::face_point(face_pos),

soccer/src/soccer/planning/planning_params.cpp

@@ -41,6 +41,19 @@ DEFINE_NS_INT64(kPlanningParamModule, rrt, min_iterations, 50,
 DEFINE_NS_INT64(kPlanningParamModule, rrt, max_iterations, 500,
                 "Maximum number of RRT iterations to run before giving up");
 
+DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, min_scale, 0.5,
+                  "Minimum length for intermediate point (m)");
+DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, max_scale, 1.5,
+                  "Maximum length for intermediate point (m)");
+DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, min_angle, 20,
+                  "Minimum angle for intermediate point (deg)");
+DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, max_angle, 140,
+                  "Maximum angle for intermediate point (deg)");
+DEFINE_NS_INT64(kPlanningParamModule, intermediate, num_intermediates, 5,
+                "Number of intermediate points used (unitless)");
+DEFINE_NS_FLOAT64(kPlanningParamModule, intermediate, step_size, 0.1,
+                  "Step size for testing intermediates (m)");
+
 DEFINE_NS_FLOAT64(
     kPlanningParamModule, escape, step_size, 0.1,
     "Step size for the RRT used to find an unblocked point in find_non_blocked_goal()");

soccer/src/soccer/planning/planning_params.hpp

@@ -25,6 +25,13 @@ DECLARE_NS_FLOAT64(kPlanningParamModule, rrt, waypoint_bias);
 DECLARE_NS_INT64(kPlanningParamModule, rrt, min_iterations);
 DECLARE_NS_INT64(kPlanningParamModule, rrt, max_iterations);
 
+DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, min_scale);
+DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, max_scale);
+DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, min_angle);
+DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, max_angle);
+DECLARE_NS_INT64(kPlanningParamModule, intermediate, num_intermediates);
+DECLARE_NS_FLOAT64(kPlanningParamModule, intermediate, step_size);
+
 DECLARE_NS_FLOAT64(kPlanningParamModule, escape, step_size);
 DECLARE_NS_FLOAT64(kPlanningParamModule, escape, goal_change_threshold);
 

soccer/src/soccer/planning/primitives/create_path.cpp

@@ -75,4 +75,105 @@ Trajectory rrt(const LinearMotionInstant& start, const LinearMotionInstant& goal
     return path;
 }
 
+static std::optional<std::tuple<double, double, double>> cached_intermediate_tuple_{};
+
+Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal,
+                        const MotionConstraints& motion_constraints, RJ::Time start_time,
+                        const rj_geometry::ShapeSet& static_obstacles) {
+    // if already on goal, no need to move
+    if (start.position.dist_to(goal.position) < 1e-6) {
+        return Trajectory{{RobotInstant{Pose(start.position, 0), Twist(), start_time}}};
+    }
+
+    // maybe straight line works
+    Trajectory straight_trajectory =
+        CreatePath::simple(start, goal, motion_constraints, start_time);
+
+    // If we are very close to the goal (i.e. there physically can't be a robot
+    // in our way) or the straight trajectory is feasible, we can use it.
+    if (start.position.dist_to(goal.position) < kRobotRadius ||
+        (!trajectory_hits_static(straight_trajectory, static_obstacles, start_time, nullptr))) {
+        return straight_trajectory;
+    }
+
+    // Generate list of intermediate points
+    std::vector<rj_geometry::Point> intermediates = get_intermediates(start, goal);
+
+    for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) {
+        rj_geometry::Point final_inter = intermediates[i];
+
+        // Step through the path from the robot to the final intermediate point
+        // and test each point on that path as an intermediate point
+        for (double t = intermediate::PARAM_step_size; t < final_inter.dist_to(start.position);
+             t += intermediate::PARAM_step_size) {
+            rj_geometry::Point intermediate =
+                (final_inter - start.position).normalized(t) + start.position;
+            Trajectory trajectory =
+                CreatePath::simple(start, goal, motion_constraints, start_time, {intermediate});
+
+            // If the trajectory does not hit an obstacle, it is valid
+            if ((!trajectory_hits_static(trajectory, static_obstacles, start_time, nullptr))) {
+                auto angle = (final_inter - start.position).angle();
+                cached_intermediate_tuple_ = {abs(angle), signbit(angle) ? -1 : 1,
+                                              (final_inter - start.position).mag()};
+                return trajectory;
+            }
+        }
+    }
+
+    // If all else fails, return the straight-line trajectory
+    return straight_trajectory;
+}
+
+std::vector<rj_geometry::Point> get_intermediates(const LinearMotionInstant& start,
+                                                  const LinearMotionInstant& goal) {
+    std::random_device rd;
+    std::mt19937 gen(rd());
+    // Create a random distribution for the distance between the start
+    // and the intermediate points
+    std::uniform_real_distribution<> scale_dist(intermediate::PARAM_min_scale,
+                                                intermediate::PARAM_max_scale);
+
+    double angle_range = intermediate::PARAM_max_angle - intermediate::PARAM_min_angle;
+    // Create a random distribution for the angle between the start
+    // and the intermediate points
+    std::uniform_real_distribution<> angle_dist(-angle_range, angle_range);
+
+    std::vector<rj_geometry::Point> intermediates;
+    std::vector<std::tuple<double, double, double>> inter_tuples;
+
+    for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) {
+        double angle = angle_dist(gen);
+        angle += std::copysign(intermediate::PARAM_min_angle, angle);
+        angle = degrees_to_radians(angle);
+        double scale = scale_dist(gen);
+
+        // Generate random tuples of distances and angles
+        inter_tuples.emplace_back(abs(angle), signbit(angle) ? -1 : 1, scale);
+    }
+
+    if (cached_intermediate_tuple_) {
+        inter_tuples.push_back(*cached_intermediate_tuple_);
+    }
+
+    // Sort the list of tuples by the magnitude of angle
+    // This ensures that we take paths with
+    // smaller offsets from the simple path
+    sort(inter_tuples.begin(), inter_tuples.end());
+
+    for (int i = 0; i < intermediate::PARAM_num_intermediates; i++) {
+        double angle = std::get<0>(inter_tuples[i]) * std::get<1>(inter_tuples[i]);
+        double scale = std::get<2>(inter_tuples[i]);
+
+        double fin_angle = goal.position.angle_to(start.position) + angle;
+        double fin_length = scale;
+
+        // Convert the distances and angles into a point
+        intermediates.push_back(start.position + rj_geometry::Point{fin_length * cos(fin_angle),
+                                                                    fin_length * sin(fin_angle)});
+    }
+
+    return intermediates;
+}
+
 }  // namespace planning::CreatePath

soccer/src/soccer/planning/primitives/create_path.hpp

@@ -1,8 +1,11 @@
 #pragma once
 
+#include <optional>
+#include <random>
+
 #include "planning/motion_constraints.hpp"
-#include "planning/trajectory.hpp"
 #include "planning/primitives/path_smoothing.hpp"
+#include "planning/trajectory.hpp"
 
 namespace planning::CreatePath {
 
@@ -24,4 +27,10 @@ Trajectory simple(
     const MotionConstraints& motion_constraints, RJ::Time start_time,
     const std::vector<rj_geometry::Point>& intermediate_points = {});
 
+Trajectory intermediate(const LinearMotionInstant& start, const LinearMotionInstant& goal,
+                        const MotionConstraints& motion_constraints, RJ::Time start_time,
+                        const rj_geometry::ShapeSet& static_obstacles);
+
+std::vector<rj_geometry::Point> get_intermediates(const LinearMotionInstant& start,
+                                                  const LinearMotionInstant& goal);
 }  // namespace planning::CreatePath

soccer/src/soccer/planning/primitives/replanner.cpp

@@ -31,10 +31,9 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory&
     }
 
     Trajectory pre_trajectory = partial_path(previous, params.start.stamp);
-    Trajectory post_trajectory =
-        CreatePath::rrt(pre_trajectory.last().linear_motion(), params.goal, params.constraints.mot,
-                        pre_trajectory.end_time(), params.static_obstacles,
-                        params.dynamic_obstacles, bias_waypoints);
+    Trajectory post_trajectory = CreatePath::intermediate(
+        pre_trajectory.last().linear_motion(), params.goal, params.constraints.mot,
+        pre_trajectory.end_time(), params.static_obstacles);
 
     // If we couldn't profile such that velocity at the end of the partial replan period is valid,
     // do a full replan.
@@ -57,8 +56,8 @@ Trajectory Replanner::partial_replan(const PlanParams& params, const Trajectory&
 
 Trajectory Replanner::full_replan(const Replanner::PlanParams& params) {
     Trajectory path =
-        CreatePath::rrt(params.start.linear_motion(), params.goal, params.constraints.mot,
-                        params.start.stamp, params.static_obstacles, params.dynamic_obstacles);
+        CreatePath::intermediate(params.start.linear_motion(), params.goal, params.constraints.mot,
+                                 params.start.stamp, params.static_obstacles);
 
     // if the initial path is empty, the goal must be blocked
     // try to shift the goal_point until it is no longer blocked
@@ -76,9 +75,9 @@ Trajectory Replanner::full_replan(const Replanner::PlanParams& params) {
 
         almost_goal.position += shift_dir * shift_size;
 
-        path =
-            CreatePath::rrt(params.start.linear_motion(), almost_goal, params.constraints.mot,
-                            params.start.stamp, params.static_obstacles, params.dynamic_obstacles);
+        path = CreatePath::intermediate(params.start.linear_motion(), almost_goal,
+                                        params.constraints.mot, params.start.stamp,
+                                        params.static_obstacles);
     }
 
     if (!path.empty()) {