Make Straight Line Testing Executable via Makefile

launch/framework.bash

@@ -17,7 +17,12 @@ binary_pid=$!
 trap 'kill $binary_pid; exit' INT
 
 # Run "make run-our-stack" in the foreground
-make run-our-stack
+echo "LINE_TEST: $1"
+if [[ "$1" == "line" ]]; then
+    make run-line-test-stack
+else
+    make run-our-stack
+fi
 
 # Wait for the background process to complete
 wait $binary_pid

launch/soccer.launch.py

@@ -48,6 +48,7 @@ def generate_launch_description():
 
     run_sim = LaunchConfiguration("run_sim")
     sim_flag = LaunchConfiguration("sim_flag")
+    run_line_test = LaunchConfiguration("run_line_test")
 
     use_internal_ref = LaunchConfiguration("use_internal_ref")
     ref_flag = LaunchConfiguration("ref_flag")
@@ -107,6 +108,7 @@ def generate_launch_description():
                     param_config,
                 ],
             ),
+            DeclareLaunchArgument("run_line_test", default_value="False"),
             stdout_linebuf_envvar,
             # Node spawns all of the ROS nodes, defined in main() of various
             # cpp files, e.g. vision_receiver.cpp, planner_node_main.cpp
@@ -184,13 +186,21 @@ def generate_launch_description():
             # spawn internal_ref/external_ref based on internal_ref
             # LaunchArgument
             Node(
-                condition=IfCondition(PythonExpression(["not ", use_manual_control])),
+                condition=IfCondition(PythonExpression(["not ", use_manual_control, " and not ", run_line_test])),
                 package="rj_robocup",
                 executable="agent_action_client_node",
                 output="screen",
                 parameters=[param_config_filepath],
                 on_exit=Shutdown(),
             ),
+            Node(
+                condition=IfCondition(PythonExpression([run_line_test])),
+                package="rj_robocup",
+                executable="straight_line_test_node",
+                output="screen",
+                parameters=[param_config_filepath],
+                on_exit=Shutdown(),
+            ),
             Node(
                 condition=IfCondition(PythonExpression([use_internal_ref])),
                 package="rj_robocup",

makefile

@@ -69,11 +69,18 @@ again:
 run-sim:
 	./launch/framework.bash
 
+# Run the line test in simulation
+run-sim-line-test:
+	./launch/framework.bash line
+
 # run our stack with default flags
 # TODO: actually name our software stack something
 run-our-stack:
 	ros2 launch rj_robocup soccer.launch.py run_sim:=True
 
+run-line-test-stack:
+	ros2 launch rj_robocup soccer.launch.py run_sim:=True run_line_test:=True
+
 # run sim with external referee (SSL Game Controller)
 run-sim-external:
 	ros2 launch rj_robocup soccer.launch.py run_sim:=True use_internal_ref:=False
@@ -93,6 +100,9 @@ run-real-ex:
 run-manual:
 	ros2 launch rj_robocup soccer.launch.py run_sim:=False use_manual_control:=True use_sim_radio:=False
 
+run-real-line-test:
+	ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False run_line_test:=True
+
 # same as run-real, with different server port
 run-alt-real:
 	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

soccer/CMakeLists.txt

@@ -86,6 +86,7 @@ add_executable(network_radio_node)
 add_executable(manual_control_node)
 add_executable(global_param_server_node)
 add_executable(agent_action_client_node)
+add_executable(straight_line_test_node)
 
 if(BUILD_TESTING)
   add_executable(test-soccer)
@@ -187,6 +188,9 @@ set(GLOBAL_PARAM_SERVER_NODE_DEPS_LIBRARIES robocup)
 set(AGENT_ACTION_CLIENT_NODE_DEPS_SYSTEM_LIBRARIES)
 set(AGENT_ACTION_CLIENT_NODE_DEPS_LIBRARIES robocup)
 
+set(STRAIGHT_LINE_TEST_NODE_DEPS_SYSTEM_LIBRARIES)
+set(STRAIGHT_LINE_TEST_NODE_DEPS_LIBRARIES robocup)
+
 # ======================================================================
 # Include and Linking
 # ======================================================================
@@ -249,6 +253,10 @@ target_link_libraries(global_param_server_node PRIVATE ${GLOBAL_PARAM_SERVER_NOD
 target_link_libraries(agent_action_client_node PRIVATE ${AGENT_ACTION_CLIENT_NODE_DEPS_SYSTEM_LIBRARIES}
     ${AGENT_ACTION_CLIENT_NODE_DEPS_LIBRARIES})
 
+# -- straight_line_testing_node --
+target_link_libraries(straight_line_test_node PRIVATE ${STRAIGHT_LINE_TEST_NODE_DEPS_SYSTEM_LIBRARIES}
+    ${STRAIGHT_LINE_TEST_NODE_DEPS_LIBRARIES})
+
 # ======================================================================
 # Testing
 # ======================================================================
@@ -293,6 +301,7 @@ install(
         manual_control_node
         global_param_server_node
         agent_action_client_node
+        straight_line_test_node
         DESTINATION lib/${CMAKE_PROJECT_NAME})
 
 install(

soccer/src/soccer/CMakeLists.txt

@@ -76,6 +76,7 @@ set(ROBOCUP_LIB_SRC
     ros2_temp/autonomy_interface.cpp
     ros2_temp/debug_draw_interface.cpp
     strategy/agent/agent_action_client.cpp
+    strategy/agent/straight_line_test.cpp
     strategy/agent/communication/communication.cpp
     strategy/agent/position/position.cpp
     strategy/agent/position/robot_factory_position.cpp
@@ -142,6 +143,8 @@ set(GLOBAL_PARAMETER_SERVER_NODE_SRC global_param_server.cpp)
 
 set(AGENT_ACTION_CLIENT_NODE_SRC strategy/agent/agent_action_client_main.cpp)
 
+set(STRAIGHT_LINE_TEST_NODE_SRC strategy/agent/straight_line_test_main.cpp)
+
 # ======================================================================
 # Add sources
 # ======================================================================
@@ -185,6 +188,9 @@ target_sources(global_param_server_node PRIVATE ${GLOBAL_PARAMETER_SERVER_NODE_S
 # ---- agent_action_client_node ----
 target_sources(agent_action_client_node PRIVATE ${AGENT_ACTION_CLIENT_NODE_SRC})
 
+# --- straight_line_test_node ----
+target_sources(straight_line_test_node PRIVATE ${STRAIGHT_LINE_TEST_NODE_SRC})
+
 # Set the directory so AUTOUIC know where to find .ui files
 set_target_properties(log_viewer PROPERTIES AUTOUIC_SEARCH_PATHS ${UI_PATH})
 

soccer/src/soccer/strategy/agent/position/line.cpp

@@ -6,6 +6,8 @@ Line::Line(const Position& other) : Position{other} { position_name_ = "Line"; }
 
 Line::Line(int r_id) : Position{r_id, "Line"} {}
 
+Line::Line(int r_id, bool vertical) : Position{r_id, "Line"}, vertical_{vertical} {}
+
 std::optional<RobotIntent> Line::derived_get_task(RobotIntent intent) {
     if (check_is_done()) {
         forward_ = !forward_;

soccer/src/soccer/strategy/agent/position/line.hpp

@@ -10,6 +10,7 @@
 public:
     Line(const Position& other);
     Line(int r_id);
+    Line(int r_id, bool forward);
     ~Line() override = default;
     Line(const Line& other) = default;
     Line(Line&& other) = default;

soccer/src/soccer/strategy/agent/straight_line_test.cpp

@@ -0,0 +1,172 @@
+#include "straight_line_test.hpp"
+
+namespace strategy {
+using RobotMove = rj_msgs::action::RobotMove;
+using GoalHandleRobotMove = rclcpp_action::ClientGoalHandle<RobotMove>;
+
+StraightLineTest::StraightLineTest() : StraightLineTest(0) {}
+
+StraightLineTest::StraightLineTest(int r_id)
+    : rclcpp::Node(::fmt::format("agent_{}_straight_line_node", r_id),
+                   rclcpp::NodeOptions{}
+                       .automatically_declare_parameters_from_overrides(true)
+                       .allow_undeclared_parameters(true)),
+      current_position_{std::make_unique<Line>(r_id)},
+      robot_id_{r_id} {
+    client_ptr_ = rclcpp_action::create_client<RobotMove>(this, "robot_move");
+
+    current_state_publisher_ = create_publisher<AgentStateMsg>(
+        fmt::format("strategy/position/robot_state/robot_{}", r_id), 1);
+
+    world_state_sub_ = create_subscription<rj_msgs::msg::WorldState>(
+        ::vision_filter::topics::kWorldStateTopic, 1,
+        [this](const rj_msgs::msg::WorldState::SharedPtr msg) { world_state_callback(msg); });
+
+    play_state_sub_ = create_subscription<rj_msgs::msg::PlayState>(
+        ::referee::topics::kPlayStateTopic, 1,
+        [this](const rj_msgs::msg::PlayState::SharedPtr msg) { play_state_callback(msg); });
+
+    field_dimensions_sub_ = create_subscription<rj_msgs::msg::FieldDimensions>(
+        "config/field_dimensions", rclcpp::QoS(1).transient_local(),
+        [this](const rj_msgs::msg::FieldDimensions::SharedPtr msg) {
+            field_dimensions_callback(msg);
+        });
+
+    alive_robots_sub_ = create_subscription<rj_msgs::msg::AliveRobots>(
+        ::radio::topics::kAliveRobotsTopic, 1,
+        [this](const rj_msgs::msg::AliveRobots::SharedPtr msg) { alive_robots_callback(msg); });
+
+    game_settings_sub_ = create_subscription<rj_msgs::msg::GameSettings>(
+        "config/game_settings", 1,
+        [this](const rj_msgs::msg::GameSettings::SharedPtr msg) { game_settings_callback(msg); });
+
+    line_direction_sub_ = create_subscription<std_msgs::msg::Bool>(
+        "line_direction", 1,
+        [this](const std_msgs::msg::Bool::SharedPtr msg) { line_direction_callback(msg); });
+
+    int hz = 10;
+    get_task_timer_ = create_wall_timer(std::chrono::milliseconds(1000 / hz),
+                                        std::bind(&StraightLineTest::get_task, this));
+}
+
+void StraightLineTest::world_state_callback(const rj_msgs::msg::WorldState::SharedPtr& msg) {
+    WorldState world_state = rj_convert::convert_from_ros(*msg);
+    auto lock = std::lock_guard(world_state_mutex_);
+    last_world_state_ = std::move(world_state);
+}
+
+void StraightLineTest::play_state_callback(const rj_msgs::msg::PlayState::SharedPtr& msg) {
+    PlayState play_state = rj_convert::convert_from_ros(*msg);
+    play_state_ = play_state;
+    current_position_->update_play_state(play_state);
+}
+
+void StraightLineTest::field_dimensions_callback(
+    const rj_msgs::msg::FieldDimensions::SharedPtr& msg) {
+    FieldDimensions field_dimensions = rj_convert::convert_from_ros(*msg);
+    field_dimensions_ = field_dimensions;
+    current_position_->update_field_dimensions(field_dimensions);
+}
+
+void StraightLineTest::alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg) {
+    alive_robots_ = msg->alive_robots;
+    current_position_->update_alive_robots(alive_robots_);
+}
+
+void StraightLineTest::game_settings_callback(const rj_msgs::msg::GameSettings::SharedPtr& msg) {
+    is_simulated_ = msg->simulation;
+}
+
+void StraightLineTest::line_direction_callback(const std_msgs::msg::Bool::SharedPtr& msg) {
+    if (msg->data != vertical_) {
+        vertical_ = msg->data;
+        current_position_ = std::make_unique<Line>(robot_id_, vertical_);
+    }
+}
+
+bool StraightLineTest::check_robot_alive(uint8_t robot_id) {
+    if (!is_simulated_) {
+        return alive_robots_.at(robot_id);
+    } else {
+        if (this->world_state()->get_robot(true, robot_id).visible) {
+            rj_geometry::Point robot_position =
+                this->world_state()->get_robot(true, robot_id).pose.position();
+            rj_geometry::Rect padded_field_rect = field_dimensions_.field_coordinates();
+            padded_field_rect.pad(field_padding_);
+            return padded_field_rect.contains_point(robot_position);
+        }
+        return false;
+    }
+}
+
+void StraightLineTest::get_task() {
+    auto lock = std::lock_guard(world_state_mutex_);
+
+    auto optional_task =
+        current_position_->get_task(last_world_state_, field_dimensions_, play_state_);
+
+    if (optional_task.has_value()) {
+        RobotIntent task = optional_task.value();
+
+        if (task != last_task_) {
+            last_task_ = task;
+            send_new_goal();
+        }
+    }
+
+    current_state_publisher_->publish(rj_msgs::build<rj_msgs::msg::AgentState>().state(
+        rj_convert::convert_to_ros(current_position_->get_current_state())));
+}
+
+void StraightLineTest::send_new_goal() {
+    using namespace std::placeholders;
+
+    if (!client_ptr_->wait_for_action_server()) {
+        SPDLOG_ERROR("Action server not available after waiting");
+        rclcpp::shutdown();
+    }
+
+    auto goal_msg = RobotMove::Goal();
+    goal_msg.robot_intent = rj_convert::convert_to_ros(last_task_);
+
+    auto send_goal_options = rclcpp_action::Client<RobotMove>::SendGoalOptions();
+    send_goal_options.goal_response_callback = [this](auto arg) { goal_response_callback(arg); };
+    send_goal_options.feedback_callback = [this](auto arg1, auto arg2) {
+        feedback_callback(arg1, arg2);
+    };
+    send_goal_options.result_callback = [this](auto arg) { result_callback(arg); };
+    client_ptr_->async_send_goal(goal_msg, send_goal_options);
+}
+
+[[nodiscard]] WorldState* StraightLineTest::world_state() {
+    auto lock = std::lock_guard(world_state_mutex_);
+    return &last_world_state_;
+}
+
+void StraightLineTest::goal_response_callback(GoalHandleRobotMove::SharedPtr goal_handle) {
+    if (!goal_handle) {
+        current_position_->set_goal_canceled();
+    }
+}
+
+void StraightLineTest::feedback_callback(
+    GoalHandleRobotMove::SharedPtr, const std::shared_ptr<const RobotMove::Feedback> feedback) {
+    double time_left = rj_convert::convert_from_ros(feedback->time_left).count();
+    current_position_->set_time_left(time_left);
+}
+
+void StraightLineTest::result_callback(const GoalHandleRobotMove::WrappedResult& result) {
+    switch (result.code) {
+        case rclcpp_action::ResultCode::SUCCEEDED:
+            current_position_->set_is_done();
+            break;
+        case rclcpp_action::ResultCode::ABORTED:
+            return;
+        case rclcpp_action::ResultCode::CANCELED:
+            return;
+        default:
+            return;
+    }
+}
+
+}  // namespace strategy