From 3ae92c1031faedeab435915e3e7b9d08649fa9b2 Mon Sep 17 00:00:00 2001 From: N8BWert Date: Tue, 28 Jan 2025 14:50:08 -0500 Subject: [PATCH 1/4] create straight line testing executable --- launch/framework.bash | 7 +- launch/soccer.launch.py | 12 +- makefile | 10 ++ soccer/CMakeLists.txt | 9 + soccer/src/soccer/CMakeLists.txt | 6 + .../strategy/agent/straight_line_test.cpp | 168 ++++++++++++++++++ .../strategy/agent/straight_line_test.hpp | 82 +++++++++ .../agent/straight_line_test_main.cpp | 21 +++ 8 files changed, 313 insertions(+), 2 deletions(-) create mode 100644 soccer/src/soccer/strategy/agent/straight_line_test.cpp create mode 100644 soccer/src/soccer/strategy/agent/straight_line_test.hpp create mode 100644 soccer/src/soccer/strategy/agent/straight_line_test_main.cpp diff --git a/launch/framework.bash b/launch/framework.bash index b5dcc9e9f77..2bf07a401f1 100755 --- a/launch/framework.bash +++ b/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" == TRUE ]]; then + make run-line-test-stack +else + make run-our-stack +fi # Wait for the background process to complete wait $binary_pid diff --git a/launch/soccer.launch.py b/launch/soccer.launch.py index d364b823474..e9980094e5a 100644 --- a/launch/soccer.launch.py +++ b/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", diff --git a/makefile b/makefile index 837c56b2949..3379b9b35f5 100644 --- a/makefile +++ b/makefile @@ -69,11 +69,18 @@ again: run-sim: ./launch/framework.bash +# Run the line test in simulation +run-sim-line-test: + ./launch/framework.bash TRUE + # 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 diff --git a/soccer/CMakeLists.txt b/soccer/CMakeLists.txt index df564030637..83081ca81dc 100644 --- a/soccer/CMakeLists.txt +++ 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( diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index 2e776364e2a..afae3b97c5a 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/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}) diff --git a/soccer/src/soccer/strategy/agent/straight_line_test.cpp b/soccer/src/soccer/strategy/agent/straight_line_test.cpp new file mode 100644 index 00000000000..0f57e33b297 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/straight_line_test.cpp @@ -0,0 +1,168 @@ +#include "straight_line_test.hpp" + +namespace strategy { +using RobotMove = rj_msgs::action::RobotMove; +using GoalHandleRobotMove = rclcpp_action::ClientGoalHandle; + +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(r_id)}, + robot_id_{r_id} { + client_ptr_ = rclcpp_action::create_client(this, "robot_move"); + + current_state_publisher_ = create_publisher( + fmt::format("strategy/position/robot_state/robot_{}", r_id), 1); + + world_state_sub_ = create_subscription( + ::vision_filter::topics::kWorldStateTopic, 1, + [this](const rj_msgs::msg::WorldState::SharedPtr msg) { world_state_callback(msg); } + ); + + play_state_sub_ = create_subscription( + ::referee::topics::kPlayStateTopic, 1, + [this](const rj_msgs::msg::PlayState::SharedPtr msg) { play_state_callback(msg); } + ); + + field_dimensions_sub_ = create_subscription( + "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( + ::radio::topics::kAliveRobotsTopic, 1, + [this](const rj_msgs::msg::AliveRobots::SharedPtr msg) { alive_robots_callback(msg); } + ); + + game_settings_sub_ = create_subscription( + "config/game_settings", 1, + [this](const rj_msgs::msg::GameSettings::SharedPtr msg) { game_settings_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; +} + +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().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::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 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; + } +} + +} \ No newline at end of file diff --git a/soccer/src/soccer/strategy/agent/straight_line_test.hpp b/soccer/src/soccer/strategy/agent/straight_line_test.hpp new file mode 100644 index 00000000000..cdc6b312bd1 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/straight_line_test.hpp @@ -0,0 +1,82 @@ +#pragma once + +#include "rj_constants/topic_names.hpp" + +#include "rj_msgs/msg/world_state.hpp" +#include "rj_msgs/msg/field_dimensions.hpp" +#include "rj_msgs/msg/game_settings.hpp" +#include "rj_msgs/msg/agent_state.hpp" +#include "rj_msgs/action/robot_move.hpp" +#include "rj_msgs/msg/play_state.hpp" +#include "rj_msgs/msg/alive_robots.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "game_state.hpp" +#include "position/position.hpp" +#include "position/line.hpp" +#include "robot_intent.hpp" + +namespace strategy { + +class StraightLineTest : public rclcpp::Node { + +public: + using AgentStateMsg = rj_msgs::msg::AgentState; + using RobotMove = rj_msgs::action::RobotMove; + using GoalHandleRobotMove = rclcpp_action::ClientGoalHandle; + + StraightLineTest(); + StraightLineTest(int r_id); + ~StraightLineTest() = default; + +private: + // ros subscriptions + rclcpp::Subscription::SharedPtr world_state_sub_; + rclcpp::Subscription::SharedPtr field_dimensions_sub_; + rclcpp::Subscription::SharedPtr game_settings_sub_; + rclcpp::Subscription::SharedPtr play_state_sub_; + rclcpp::Subscription::SharedPtr alive_robots_sub_; + + // subscription callbacks + void world_state_callback(const rj_msgs::msg::WorldState::SharedPtr& msg); + void play_state_callback(const rj_msgs::msg::PlayState::SharedPtr& msg); + void alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg); + void field_dimensions_callback(const rj_msgs::msg::FieldDimensions::SharedPtr& msg); + void game_settings_callback(const rj_msgs::msg::GameSettings::SharedPtr& msg); + + rclcpp::Publisher::SharedPtr current_state_publisher_; + + // Ros ActionClient spec, for calls to planning ActionServer + rclcpp_action::Client::SharedPtr client_ptr_; + void goal_response_callback(GoalHandleRobotMove::SharedPtr future); + void feedback_callback(GoalHandleRobotMove::SharedPtr, + const std::shared_ptr feedback); + void result_callback(const GoalHandleRobotMove::WrappedResult& result); + + void send_new_goal(); + + std::unique_ptr current_position_; + + void get_task(); + rclcpp::TimerBase::SharedPtr get_task_timer_; + + RobotIntent last_task_; + + FieldDimensions field_dimensions_; + PlayState play_state_ = PlayState::halt(); + std::array alive_robots_{}; + bool is_simulated_ = false; + static constexpr double field_padding_ = 0.3; + + bool check_robot_alive(uint8_t robot_id); + + const int robot_id_; + + [[nodiscard]] WorldState* world_state(); + WorldState last_world_state_; + mutable std::mutex world_state_mutex_; +}; // class StraightLineTest + +} // namespace strategy \ No newline at end of file diff --git a/soccer/src/soccer/strategy/agent/straight_line_test_main.cpp b/soccer/src/soccer/strategy/agent/straight_line_test_main.cpp new file mode 100644 index 00000000000..dc3386d2181 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/straight_line_test_main.cpp @@ -0,0 +1,21 @@ +#include "straight_line_test.hpp" +#include "global_params.hpp" +#include "rj_utils/logging.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rj_utils::set_spdlog_default_ros2("processor"); + + rclcpp::executors::MultiThreadedExecutor executor; + + std::vector agents; + for (int i = 0; i < 6; i++) { + auto agent = std::make_shared(i); + start_global_param_provider(agent.get(), kGlobalParamServerNode); + agents.push_back(agent); + } + for (const auto& agent : agents) { + executor.add_node(agent); + } + executor.spin(); +} \ No newline at end of file From 0b9a3f93162a5e213b878b0fc4f3ee859ad14ea2 Mon Sep 17 00:00:00 2001 From: Field Computer Date: Tue, 28 Jan 2025 18:47:47 -0500 Subject: [PATCH 2/4] make pretty-lines --- .../strategy/agent/straight_line_test.cpp | 55 ++++++++----------- .../strategy/agent/straight_line_test.hpp | 30 +++++----- .../agent/straight_line_test_main.cpp | 2 +- 3 files changed, 38 insertions(+), 49 deletions(-) diff --git a/soccer/src/soccer/strategy/agent/straight_line_test.cpp b/soccer/src/soccer/strategy/agent/straight_line_test.cpp index 0f57e33b297..31087c00ba1 100644 --- a/soccer/src/soccer/strategy/agent/straight_line_test.cpp +++ b/soccer/src/soccer/strategy/agent/straight_line_test.cpp @@ -4,15 +4,13 @@ namespace strategy { using RobotMove = rj_msgs::action::RobotMove; using GoalHandleRobotMove = rclcpp_action::ClientGoalHandle; -StraightLineTest::StraightLineTest() : StraightLineTest(0) { - -} +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)), + rclcpp::NodeOptions{} + .automatically_declare_parameters_from_overrides(true) + .allow_undeclared_parameters(true)), current_position_{std::make_unique(r_id)}, robot_id_{r_id} { client_ptr_ = rclcpp_action::create_client(this, "robot_move"); @@ -22,31 +20,29 @@ StraightLineTest::StraightLineTest(int r_id) world_state_sub_ = create_subscription( ::vision_filter::topics::kWorldStateTopic, 1, - [this](const rj_msgs::msg::WorldState::SharedPtr msg) { world_state_callback(msg); } - ); + [this](const rj_msgs::msg::WorldState::SharedPtr msg) { world_state_callback(msg); }); play_state_sub_ = create_subscription( ::referee::topics::kPlayStateTopic, 1, - [this](const rj_msgs::msg::PlayState::SharedPtr msg) { play_state_callback(msg); } - ); + [this](const rj_msgs::msg::PlayState::SharedPtr msg) { play_state_callback(msg); }); field_dimensions_sub_ = create_subscription( "config/field_dimensions", rclcpp::QoS(1).transient_local(), - [this](const rj_msgs::msg::FieldDimensions::SharedPtr msg) { field_dimensions_callback(msg); } - ); + [this](const rj_msgs::msg::FieldDimensions::SharedPtr msg) { + field_dimensions_callback(msg); + }); alive_robots_sub_ = create_subscription( ::radio::topics::kAliveRobotsTopic, 1, - [this](const rj_msgs::msg::AliveRobots::SharedPtr msg) { alive_robots_callback(msg); } - ); + [this](const rj_msgs::msg::AliveRobots::SharedPtr msg) { alive_robots_callback(msg); }); game_settings_sub_ = create_subscription( "config/game_settings", 1, - [this](const rj_msgs::msg::GameSettings::SharedPtr msg) { game_settings_callback(msg); } - ); + [this](const rj_msgs::msg::GameSettings::SharedPtr msg) { game_settings_callback(msg); }); int hz = 10; - get_task_timer_ = create_wall_timer(std::chrono::milliseconds(1000 / hz), std::bind(&StraightLineTest::get_task, this)); + 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) { @@ -61,7 +57,8 @@ void StraightLineTest::play_state_callback(const rj_msgs::msg::PlayState::Shared current_position_->update_play_state(play_state); } -void StraightLineTest::field_dimensions_callback(const rj_msgs::msg::FieldDimensions::SharedPtr& msg) { +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); @@ -81,7 +78,8 @@ bool StraightLineTest::check_robot_alive(uint8_t robot_id) { 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::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); @@ -93,7 +91,8 @@ bool StraightLineTest::check_robot_alive(uint8_t robot_id) { 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_); + auto optional_task = + current_position_->get_task(last_world_state_, field_dimensions_, play_state_); if (optional_task.has_value()) { RobotIntent task = optional_task.value(); @@ -105,8 +104,7 @@ void StraightLineTest::get_task() { } current_state_publisher_->publish(rj_msgs::build().state( - rj_convert::convert_to_ros(current_position_->get_current_state()) - )); + rj_convert::convert_to_ros(current_position_->get_current_state()))); } void StraightLineTest::send_new_goal() { @@ -121,15 +119,11 @@ void StraightLineTest::send_new_goal() { goal_msg.robot_intent = rj_convert::convert_to_ros(last_task_); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = [this](auto arg) { - goal_response_callback(arg); - }; + 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); - }; + send_goal_options.result_callback = [this](auto arg) { result_callback(arg); }; client_ptr_->async_send_goal(goal_msg, send_goal_options); } @@ -145,8 +139,7 @@ void StraightLineTest::goal_response_callback(GoalHandleRobotMove::SharedPtr goa } void StraightLineTest::feedback_callback( - GoalHandleRobotMove::SharedPtr, const std::shared_ptr feedback -) { + GoalHandleRobotMove::SharedPtr, const std::shared_ptr feedback) { double time_left = rj_convert::convert_from_ros(feedback->time_left).count(); current_position_->set_time_left(time_left); } @@ -165,4 +158,4 @@ void StraightLineTest::result_callback(const GoalHandleRobotMove::WrappedResult& } } -} \ No newline at end of file +} // namespace strategy \ No newline at end of file diff --git a/soccer/src/soccer/strategy/agent/straight_line_test.hpp b/soccer/src/soccer/strategy/agent/straight_line_test.hpp index cdc6b312bd1..89f1733a8f6 100644 --- a/soccer/src/soccer/strategy/agent/straight_line_test.hpp +++ b/soccer/src/soccer/strategy/agent/straight_line_test.hpp @@ -1,27 +1,23 @@ #pragma once +#include "game_state.hpp" +#include "position/line.hpp" +#include "position/position.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "rj_constants/topic_names.hpp" - -#include "rj_msgs/msg/world_state.hpp" +#include "rj_msgs/action/robot_move.hpp" +#include "rj_msgs/msg/agent_state.hpp" +#include "rj_msgs/msg/alive_robots.hpp" #include "rj_msgs/msg/field_dimensions.hpp" #include "rj_msgs/msg/game_settings.hpp" -#include "rj_msgs/msg/agent_state.hpp" -#include "rj_msgs/action/robot_move.hpp" #include "rj_msgs/msg/play_state.hpp" -#include "rj_msgs/msg/alive_robots.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" - -#include "game_state.hpp" -#include "position/position.hpp" -#include "position/line.hpp" +#include "rj_msgs/msg/world_state.hpp" #include "robot_intent.hpp" namespace strategy { class StraightLineTest : public rclcpp::Node { - public: using AgentStateMsg = rj_msgs::msg::AgentState; using RobotMove = rj_msgs::action::RobotMove; @@ -52,11 +48,11 @@ class StraightLineTest : public rclcpp::Node { rclcpp_action::Client::SharedPtr client_ptr_; void goal_response_callback(GoalHandleRobotMove::SharedPtr future); void feedback_callback(GoalHandleRobotMove::SharedPtr, - const std::shared_ptr feedback); + const std::shared_ptr feedback); void result_callback(const GoalHandleRobotMove::WrappedResult& result); void send_new_goal(); - + std::unique_ptr current_position_; void get_task(); @@ -77,6 +73,6 @@ class StraightLineTest : public rclcpp::Node { [[nodiscard]] WorldState* world_state(); WorldState last_world_state_; mutable std::mutex world_state_mutex_; -}; // class StraightLineTest +}; // class StraightLineTest -} // namespace strategy \ No newline at end of file +} // namespace strategy \ No newline at end of file diff --git a/soccer/src/soccer/strategy/agent/straight_line_test_main.cpp b/soccer/src/soccer/strategy/agent/straight_line_test_main.cpp index dc3386d2181..154e1ca17e4 100644 --- a/soccer/src/soccer/strategy/agent/straight_line_test_main.cpp +++ b/soccer/src/soccer/strategy/agent/straight_line_test_main.cpp @@ -1,6 +1,6 @@ -#include "straight_line_test.hpp" #include "global_params.hpp" #include "rj_utils/logging.hpp" +#include "straight_line_test.hpp" int main(int argc, char** argv) { rclcpp::init(argc, argv); From 15edb5e4ea370d1ccb8a98028d47380d7165dc2c Mon Sep 17 00:00:00 2001 From: Field Computer Date: Tue, 28 Jan 2025 19:21:07 -0500 Subject: [PATCH 3/4] make line direction based on topic --- .../soccer/strategy/agent/position/line.cpp | 2 ++ .../soccer/strategy/agent/position/line.hpp | 1 + .../strategy/agent/straight_line_test.cpp | 11 ++++++++ .../strategy/agent/straight_line_test.hpp | 25 +++++++++++++++++++ 4 files changed, 39 insertions(+) diff --git a/soccer/src/soccer/strategy/agent/position/line.cpp b/soccer/src/soccer/strategy/agent/position/line.cpp index daf99ef1112..185ab6ac325 100644 --- a/soccer/src/soccer/strategy/agent/position/line.cpp +++ b/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 Line::derived_get_task(RobotIntent intent) { if (check_is_done()) { forward_ = !forward_; diff --git a/soccer/src/soccer/strategy/agent/position/line.hpp b/soccer/src/soccer/strategy/agent/position/line.hpp index a47b31701a6..f26b2a911fb 100644 --- a/soccer/src/soccer/strategy/agent/position/line.hpp +++ b/soccer/src/soccer/strategy/agent/position/line.hpp @@ -10,6 +10,7 @@ class Line : public Position { 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; diff --git a/soccer/src/soccer/strategy/agent/straight_line_test.cpp b/soccer/src/soccer/strategy/agent/straight_line_test.cpp index 31087c00ba1..7814b8747cf 100644 --- a/soccer/src/soccer/strategy/agent/straight_line_test.cpp +++ b/soccer/src/soccer/strategy/agent/straight_line_test.cpp @@ -40,6 +40,10 @@ StraightLineTest::StraightLineTest(int r_id) "config/game_settings", 1, [this](const rj_msgs::msg::GameSettings::SharedPtr msg) { game_settings_callback(msg); }); + line_direction_sub_ = create_subscription( + "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)); @@ -73,6 +77,13 @@ void StraightLineTest::game_settings_callback(const rj_msgs::msg::GameSettings:: 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(robot_id_, vertical_); + } +} + bool StraightLineTest::check_robot_alive(uint8_t robot_id) { if (!is_simulated_) { return alive_robots_.at(robot_id); diff --git a/soccer/src/soccer/strategy/agent/straight_line_test.hpp b/soccer/src/soccer/strategy/agent/straight_line_test.hpp index 89f1733a8f6..74bc8269363 100644 --- a/soccer/src/soccer/strategy/agent/straight_line_test.hpp +++ b/soccer/src/soccer/strategy/agent/straight_line_test.hpp @@ -14,6 +14,10 @@ #include "rj_msgs/msg/play_state.hpp" #include "rj_msgs/msg/world_state.hpp" #include "robot_intent.hpp" +#include "std_msgs/msg/bool.hpp" + +// Note: The direction of the line can be changed by running: +// `ros2 topic pub -1 line_direction std_msgs/msg/Bool "{data: VERTICAL}"` namespace strategy { @@ -34,6 +38,7 @@ class StraightLineTest : public rclcpp::Node { rclcpp::Subscription::SharedPtr game_settings_sub_; rclcpp::Subscription::SharedPtr play_state_sub_; rclcpp::Subscription::SharedPtr alive_robots_sub_; + rclcpp::Subscription::SharedPtr line_direction_sub_; // subscription callbacks void world_state_callback(const rj_msgs::msg::WorldState::SharedPtr& msg); @@ -41,20 +46,36 @@ class StraightLineTest : public rclcpp::Node { void alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg); void field_dimensions_callback(const rj_msgs::msg::FieldDimensions::SharedPtr& msg); void game_settings_callback(const rj_msgs::msg::GameSettings::SharedPtr& msg); + void line_direction_callback(const std_msgs::msg::Bool::SharedPtr& msg); rclcpp::Publisher::SharedPtr current_state_publisher_; // Ros ActionClient spec, for calls to planning ActionServer rclcpp_action::Client::SharedPtr client_ptr_; + /** + * Send a new long term goal to the robot movement path planner + */ void goal_response_callback(GoalHandleRobotMove::SharedPtr future); + /** + * Retrieve the amount of time left for the current trajectory goal + */ void feedback_callback(GoalHandleRobotMove::SharedPtr, const std::shared_ptr feedback); + /** + * Tell the current line position whether it has finished moving + */ void result_callback(const GoalHandleRobotMove::WrappedResult& result); + /** + * Send a new goal to the path planner + */ void send_new_goal(); std::unique_ptr current_position_; + /** + * Get the task the agent should be running (based on the current line position) + */ void get_task(); rclcpp::TimerBase::SharedPtr get_task_timer_; @@ -66,6 +87,9 @@ class StraightLineTest : public rclcpp::Node { bool is_simulated_ = false; static constexpr double field_padding_ = 0.3; + /** + * Check whether a given robot is alive + */ bool check_robot_alive(uint8_t robot_id); const int robot_id_; @@ -73,6 +97,7 @@ class StraightLineTest : public rclcpp::Node { [[nodiscard]] WorldState* world_state(); WorldState last_world_state_; mutable std::mutex world_state_mutex_; + bool vertical_ = false; }; // class StraightLineTest } // namespace strategy \ No newline at end of file From 6f13faefe9c7e1edda57043ef0fa76563ca2cfda Mon Sep 17 00:00:00 2001 From: Field Computer Date: Tue, 28 Jan 2025 19:33:47 -0500 Subject: [PATCH 4/4] change parameter to framework.bash --- launch/framework.bash | 2 +- makefile | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/framework.bash b/launch/framework.bash index 2bf07a401f1..f085a2436bb 100755 --- a/launch/framework.bash +++ b/launch/framework.bash @@ -18,7 +18,7 @@ trap 'kill $binary_pid; exit' INT # Run "make run-our-stack" in the foreground echo "LINE_TEST: $1" -if [[ "$1" == TRUE ]]; then +if [[ "$1" == "line" ]]; then make run-line-test-stack else make run-our-stack diff --git a/makefile b/makefile index 3379b9b35f5..d2854414ffa 100644 --- a/makefile +++ b/makefile @@ -71,7 +71,7 @@ run-sim: # Run the line test in simulation run-sim-line-test: - ./launch/framework.bash TRUE + ./launch/framework.bash line # run our stack with default flags # TODO: actually name our software stack something