Ball placement obstacle with Stadium Shape on their ball placement

.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="oswinso@gmail.com"
 
 # 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:

rj_common/include/rj_common/field_dimensions.hpp

@@ -13,6 +13,7 @@
 #include <rj_geometry/polygon.hpp>
 #include <rj_geometry/rect.hpp>
 #include <rj_geometry/shape_set.hpp>
+#include <rj_geometry/stadium_shape.hpp>
 #include <rj_msgs/msg/field_dimensions.hpp>
 
 using namespace std;

rj_geometry/include/rj_geometry/stadium_shape.hpp

@@ -0,0 +1,93 @@
+#pragma once
+
+#include <memory>
+#include <set>
+#include <vector>
+
+#include "point.hpp"
+#include "polygon.hpp"
+#include "segment.hpp"
+#include "shape.hpp"
+#include "shape_set.hpp"
+
+namespace rj_geometry {
+
+/**
+ * A rj_geometry::StadiumShape is a Shape that is made up of 2 circles and a polygon. It represents
+ * the shape of a track from track and field.
+ */
+class StadiumShape : public Shape {
+public:
+    ~StadiumShape() = default;
+
+    StadiumShape() = default;
+
+    StadiumShape(Point c1, Point c2, float r) { init(c1, c2, r); }
+
+    StadiumShape(const StadiumShape& other) {
+        for (const auto& shape : other.subshapes_) {
+            std::shared_ptr<Shape> itr_shape = std::shared_ptr<Shape>(shape->clone());
+            subshapes_.push_back(itr_shape);
+        }
+        drawshapes_ = other.drawshapes_;
+    }
+
+    [[nodiscard]] Shape* clone() const override;
+
+    [[nodiscard]] bool contains_point(Point pt) const override;
+    [[nodiscard]] bool near_point(Point pt, float threshold) const override;
+
+    using const_iterator = std::vector<std::shared_ptr<Shape>>::const_iterator;
+    using iterator = std::vector<std::shared_ptr<Shape>>::iterator;
+
+    [[nodiscard]] const_iterator begin() const { return subshapes_.begin(); }
+    [[nodiscard]] const_iterator end() const { return subshapes_.end(); }
+
+    iterator begin() { return subshapes_.begin(); }
+    iterator end() { return subshapes_.end(); }
+
+    [[nodiscard]] const std::vector<std::shared_ptr<Shape>>& subshapes() const {
+        return subshapes_;
+    }
+
+    [[nodiscard]] const rj_geometry::ShapeSet drawshapes() const { return drawshapes_; }
+
+    std::shared_ptr<Shape> operator[](unsigned int index) { return subshapes_[index]; }
+
+    std::shared_ptr<const Shape> operator[](unsigned int index) const { return subshapes_[index]; }
+
+    template <typename T>
+    [[nodiscard]] bool hit(const T& obj) const {
+        for (const auto& it : *this) {
+            if (it->hit(obj)) {
+                return true;
+            }
+        }
+
+        return false;
+    }
+
+    [[nodiscard]] bool hit(Point pt) const override { return hit<Point>(pt); }
+
+    [[nodiscard]] bool hit(const Segment& seg) const override { return hit<Segment>(seg); }
+
+    std::string to_string() override {
+        std::stringstream str;
+        str << "StadiumShape<";
+        for (auto& subshape : subshapes_) {
+            str << subshape->to_string() << ", ";
+        }
+        str << ">";
+
+        return str.str();
+    }
+
+protected:
+    void init(Point c1, Point c2, float r);
+
+private:
+    std::vector<std::shared_ptr<Shape>> subshapes_;
+    rj_geometry::ShapeSet drawshapes_;
+};
+
+}  // namespace rj_geometry

rj_geometry/src/CMakeLists.txt

@@ -5,6 +5,7 @@ set(GEOMETRY2D_SRCS
     arc.cpp
     circle.cpp
     composite_shape.cpp
+    stadium_shape.cpp
     line.cpp
     point.cpp
     polygon.cpp

rj_geometry/src/stadium_shape.cpp

@@ -0,0 +1,55 @@
+#include <rj_geometry/stadium_shape.hpp>
+
+namespace rj_geometry {
+
+Shape* StadiumShape::clone() const { return new StadiumShape(*this); }
+
+void StadiumShape::init(Point c1, Point c2, float r) {
+    rj_geometry::Circle first_circle = rj_geometry::Circle{c1, static_cast<float>(r)};
+    rj_geometry::Circle second_circle = rj_geometry::Circle{c2, static_cast<float>(r)};
+
+    rj_geometry::Segment vect{c1, c2};
+
+    rj_geometry::Point end1{c1.x() + r * (c2.x() - c1.x()) / vect.length(),
+                            c1.y() + r * (c2.y() - c1.y()) / vect.length()};
+    rj_geometry::Point end2{c2.x() - r * (c2.x() - c1.x()) / vect.length(),
+                            c2.y() - r * (c2.y() - c1.y()) / vect.length()};
+
+    rj_geometry::Segment vect_updated{end1, end2};
+    rj_geometry::Polygon rect_obs{vect_updated, r};
+
+    std::shared_ptr<rj_geometry::Circle> c1_obs_ptr =
+        std::make_shared<rj_geometry::Circle>(first_circle);
+    std::shared_ptr<rj_geometry::Polygon> rect_obs_ptr =
+        std::make_shared<rj_geometry::Polygon>(rect_obs);
+    std::shared_ptr<rj_geometry::Circle> c2_obs_ptr =
+        std::make_shared<rj_geometry::Circle>(second_circle);
+
+    subshapes_.push_back(c1_obs_ptr);
+    subshapes_.push_back(rect_obs_ptr);
+    subshapes_.push_back(c2_obs_ptr);
+
+    drawshapes_.add(c1_obs_ptr);
+    drawshapes_.add(rect_obs_ptr);
+    drawshapes_.add(c2_obs_ptr);
+}
+
+bool StadiumShape::contains_point(Point pt) const {
+    for (const auto& subshape : subshapes_) {
+        if (subshape->contains_point(pt)) {
+            return true;
+        }
+    }
+    return false;
+}
+
+bool StadiumShape::near_point(Point pt, float threshold) const {
+    for (const auto& subshape : subshapes_) {
+        if (subshape->near_point(pt, threshold)) {
+            return true;
+        }
+    }
+    return false;
+}
+
+}  // namespace rj_geometry

soccer/src/soccer/CMakeLists.txt

@@ -79,6 +79,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/plan_request.cpp

@@ -78,6 +78,23 @@
         }
 
         out_static->add(std::make_shared<rj_geometry::Circle>(std::move(ball_obs)));
+
+        auto maybe_bp_point = in.play_state.ball_placement_point();
+        if (maybe_bp_point.has_value() && in.play_state.is_their_restart()) {
+            rj_geometry::Point bp_point = maybe_bp_point.value();
+            rj_geometry::StadiumShape stadium = rj_geometry::StadiumShape{
+                in.world_state->ball.position, bp_point, ball_obs.radius()};
+
+            std::shared_ptr<rj_geometry::StadiumShape> track_obs_ptr =
+                std::make_shared<rj_geometry::StadiumShape>(stadium);
+
+            out_static->add(track_obs_ptr);
+
+            if (in.debug_drawer != nullptr) {
+                QColor draw_color = Qt::red;
+                in.debug_drawer->draw_stadium(stadium, draw_color);
+            }
+        }
     }
 }
 

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),