diff --git a/ateam_ai/CMakeLists.txt b/ateam_ai/CMakeLists.txt deleted file mode 100644 index 4e7730a33..000000000 --- a/ateam_ai/CMakeLists.txt +++ /dev/null @@ -1,76 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(ateam_ai) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(ateam_common REQUIRED) -find_package(ateam_geometry REQUIRED) -find_package(ateam_msgs REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(angles REQUIRED) - -add_library(${PROJECT_NAME} SHARED - src/ateam_ai_node.cpp - src/behavior/behavior_evaluator.cpp - src/behavior/behavior_executor.cpp - src/behavior/behavior_follower.cpp - src/behavior/behavior_realization.cpp - src/trajectory_generation/b_spline_wrapper.cpp - src/trajectory_generation/b_spline.cpp - src/trajectory_generation/trajectory_editor.cpp - src/trajectory_generation/trajectory_generation.cpp - src/trajectory_generation/trapezoidal_motion_profile.cpp - src/util/message_conversions.cpp -) -target_include_directories(${PROJECT_NAME} PRIVATE src) -set_target_properties(${PROJECT_NAME} PROPERTIES CXX_STANDARD 20) -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_components - ateam_common - ateam_geometry - ateam_msgs - Eigen3 - tf2 - tf2_geometry_msgs - angles -) -rclcpp_components_register_node( - ${PROJECT_NAME} - PLUGIN "ateam_ai::ATeamAINode" - EXECUTABLE ateam_ai_node -) - -# Export symbols so we can backtrace -set_property(TARGET ${PROJECT_NAME} PROPERTY ENABLE_EXPORTS ON) - -install(TARGETS ${PROJECT_NAME} DESTINATION lib) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(${PROJECT_NAME}_test - test/behavior/behavior_realization_test.cpp - test/trajectory_generation/b_spline_test.cpp - test/trajectory_generation/b_spline_wrapper_test.cpp - test/trajectory_generation/ilqr_problem_test.cpp - test/trajectory_generation/trajectory_editor_test.cpp - test/trajectory_generation/trapezoidal_motion_profile_test.cpp - test/util/pid_test.cpp - ) - target_include_directories(${PROJECT_NAME}_test PRIVATE test src) - target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) - set_target_properties(${PROJECT_NAME}_test PROPERTIES CXX_STANDARD 20) -endif() - -ament_package() diff --git a/ateam_ai/COLCON_IGNORE b/ateam_ai/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/ateam_ai/package.xml b/ateam_ai/package.xml deleted file mode 100644 index 9add084c4..000000000 --- a/ateam_ai/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - ateam_ai - 0.0.0 - A Team AI module. - Joseph Neiger - TODO: License declaration - - ament_cmake - - rclcpp - rclcpp_components - tf2 - tf2_geometry_msgs - ateam_common - ateam_msgs - ateam_geometry - eigen3_cmake_module - eigen - angles - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/ateam_ai/src/ateam_ai_node.cpp b/ateam_ai/src/ateam_ai_node.cpp deleted file mode 100644 index 5bc912240..000000000 --- a/ateam_ai/src/ateam_ai_node.cpp +++ /dev/null @@ -1,291 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "behavior/behavior_evaluator.hpp" -#include "behavior/behavior_executor.hpp" -#include "behavior/behavior_follower.hpp" -#include "behavior/behavior_realization.hpp" -#include "trajectory_generation/trajectory_editor.hpp" -#include "types/world.hpp" -#include "util/directed_graph.hpp" -#include "util/message_conversions.hpp" - -using namespace std::chrono_literals; -using ateam_common::indexed_topic_helpers::create_indexed_subscribers; -using ateam_common::indexed_topic_helpers::create_indexed_publishers; - -namespace ateam_ai -{ - -class ATeamAINode : public rclcpp::Node -{ -public: - explicit ATeamAINode(const rclcpp::NodeOptions & options) - : rclcpp::Node("ateam_ai_node", options), game_controller_listener_(*this), \ - evaluator_(realization_), executor_(realization_) - { - REGISTER_NODE_PARAMS(this); - ateam_common::Overlay::GetOverlay().SetNamespace("ateam_ai"); - - std::lock_guard lock(world_mutex_); - world_.balls.emplace_back(Ball{}); - - create_indexed_subscribers( - blue_robots_subscriptions_, - Topics::kBlueTeamRobotPrefix, - 10, - &ATeamAINode::blue_robot_state_callback, - this); - - create_indexed_subscribers( - yellow_robots_subscriptions_, - Topics::kYellowTeamRobotPrefix, - 10, - &ATeamAINode::yellow_robot_state_callback, - this); - - create_indexed_publishers( - robot_commands_publishers_, Topics::kRobotMotionCommandPrefix, - rclcpp::SystemDefaultsQoS(), this); - - auto ball_callback = [&](const ateam_msgs::msg::BallState::SharedPtr ball_state_msg) { - ball_state_callback(world_.balls.at(0), ball_state_msg); - }; - ball_subscription_ = create_subscription( - std::string(Topics::kBall), - 10, - ball_callback); - - world_publisher_ = create_publisher( - "~/world", - rclcpp::SystemDefaultsQoS()); - - overlay_publisher_ = create_publisher( - "/overlay", - rclcpp::SystemDefaultsQoS()); - ateam_common::Overlay::GetOverlay().SetOverlayPublishCallback( - [&](ateam_msgs::msg::Overlay overlay) { - overlay_publisher_->publish(overlay); - } - ); - - field_subscription_ = create_subscription( - std::string(Topics::kField), - 10, - std::bind(&ATeamAINode::field_callback, this, std::placeholders::_1)); - - timer_ = create_wall_timer(10ms, std::bind(&ATeamAINode::timer_callback, this)); - } - -private: - rclcpp::TimerBase::SharedPtr timer_; - - rclcpp::Subscription::SharedPtr ball_subscription_; - std::array::SharedPtr, - 16> blue_robots_subscriptions_; - std::array::SharedPtr, - 16> yellow_robots_subscriptions_; - rclcpp::Subscription::SharedPtr - field_subscription_; - std::array::SharedPtr, - 16> robot_commands_publishers_; - rclcpp::Publisher::SharedPtr overlay_publisher_; - - rclcpp::Publisher::SharedPtr world_publisher_; - - ateam_common::GameControllerListener game_controller_listener_; - - BehaviorRealization realization_; - BehaviorEvaluator evaluator_; - BehaviorExecutor executor_; - BehaviorFollower follower_; - std::mutex world_mutex_; - World world_; - std::array, 16> previous_frame_trajectories; - - void blue_robot_state_callback( - const ateam_msgs::msg::RobotState::SharedPtr robot_state_msg, - int id) - { - const auto are_we_blue = game_controller_listener_.GetTeamColor() == - ateam_common::TeamColor::Blue; - auto & robot_state_array = are_we_blue ? world_.our_robots : world_.their_robots; - robot_state_callback(robot_state_array, id, robot_state_msg); - } - - void yellow_robot_state_callback( - const ateam_msgs::msg::RobotState::SharedPtr robot_state_msg, - int id) - { - const auto are_we_yellow = game_controller_listener_.GetTeamColor() == - ateam_common::TeamColor::Yellow; - auto & robot_state_array = are_we_yellow ? world_.our_robots : world_.their_robots; - robot_state_callback(robot_state_array, id, robot_state_msg); - } - - void robot_state_callback( - std::array, 16> & robot_states, - std::size_t id, - const ateam_msgs::msg::RobotState::SharedPtr robot_state_msg) - { - std::lock_guard lock(world_mutex_); - robot_states.at(id) = Robot(); - robot_states.at(id).value().pos.x() = robot_state_msg->pose.position.x; - robot_states.at(id).value().pos.y() = robot_state_msg->pose.position.y; - tf2::Quaternion tf2_quat; - tf2::fromMsg(robot_state_msg->pose.orientation, tf2_quat); - robot_states.at(id).value().theta = tf2::getYaw(tf2_quat); - robot_states.at(id).value().vel.x() = robot_state_msg->twist.linear.x; - robot_states.at(id).value().vel.y() = robot_state_msg->twist.linear.y; - robot_states.at(id).value().omega = robot_state_msg->twist.angular.z; - } - - void ball_state_callback( - Ball & ball_state, - const ateam_msgs::msg::BallState::SharedPtr ball_state_msg) - { - std::lock_guard lock(world_mutex_); - ball_state.pos.x() = ball_state_msg->pose.position.x; - ball_state.pos.y() = ball_state_msg->pose.position.y; - ball_state.vel.x() = ball_state_msg->twist.linear.x; - ball_state.vel.y() = ball_state_msg->twist.linear.y; - } - - void field_callback(const ateam_msgs::msg::FieldInfo::SharedPtr field_msg) - { - Field field { - .field_length = field_msg->field_length, - .field_width = field_msg->field_width, - .goal_width = field_msg->goal_width, - .goal_depth = field_msg->goal_depth, - .boundary_width = field_msg->boundary_width - }; - - // Multiple def error - // field.center_circle = ateam_geometry::makeCircle({field_msg->center_circle.x, - // field_msg->center_circle.y}, field_msg->center_circle_radius); - - // I could have just defined conversion operators for all of this but - // Im pretty sure joe wanted ros separate from cpp - auto convert_point_array = [&](auto & starting_array, auto final_array_iter) { - std::transform( - starting_array.begin(), starting_array.end(), final_array_iter, - [&](auto & val)->Eigen::Vector2d { - return {val.x, val.y}; - }); - }; - - convert_point_array(field_msg->field_corners.points, field.field_corners.begin()); - convert_point_array(field_msg->ours.defense_area_corners.points, field.ours.goalie_box.begin()); - convert_point_array(field_msg->ours.goal_corners.points, field.ours.goal.begin()); - convert_point_array( - field_msg->theirs.defense_area_corners.points, - field.theirs.goalie_box.begin()); - convert_point_array(field_msg->theirs.goal_corners.points, field.theirs.goal.begin()); - - std::lock_guard lock(world_mutex_); - world_.field = field; - } - - void timer_callback() - { - std::lock_guard lock(world_mutex_); - - // - // Preproccess world - // - world_.current_time += 0.01; - for (std::size_t robot_id = 0; robot_id < 16; robot_id++) { - // Estimate of how long it will take for the round trip of - // Command -> Radio -> Robot -> Motion -> Vision change - const auto & maybe_trajectory = previous_frame_trajectories.at(robot_id); - if (maybe_trajectory.has_value()) { - world_.plan_from_our_robots.at(robot_id) = trajectory_editor::state_at_immutable_duration( - maybe_trajectory.value(), - world_.immutable_duration, world_.current_time); - } else { - world_.plan_from_our_robots.at(robot_id) = world_.our_robots.at(robot_id); - } - } - - // Get current game state for world - world_.referee_info.running_command = game_controller_listener_.GetGameCommand(); - world_.referee_info.current_game_stage = game_controller_listener_.GetGameStage(); - // Save off the world to the rosbag - world_publisher_->publish(ateam_ai::message_conversions::toMsg(world_)); - - // - // Plan behavior - // - auto current_behaviors = evaluator_.get_best_behaviors(world_); - auto current_trajectories = executor_.execute_behaviors( - current_behaviors, world_, - world_.behavior_executor_state); - auto robot_motion_commands = follower_.follow(current_trajectories, world_); - - send_all_motion_commands(robot_motion_commands); - - // - // Cleanup for next frame - // - previous_frame_trajectories = current_trajectories; - } - - void send_all_motion_commands( - const std::array, - 16> & robot_motion_commands) - { - for (std::size_t id = 0; id < robot_commands_publishers_.size(); id++) { - const auto & maybe_motion_command = robot_motion_commands.at(id); - if (maybe_motion_command.has_value()) { - robot_commands_publishers_.at(id)->publish(maybe_motion_command.value()); - } - } - } -}; - -} // namespace ateam_ai - -RCLCPP_COMPONENTS_REGISTER_NODE(ateam_ai::ATeamAINode) diff --git a/ateam_ai/src/behavior/behavior_evaluator.cpp b/ateam_ai/src/behavior/behavior_evaluator.cpp deleted file mode 100644 index f721f55fe..000000000 --- a/ateam_ai/src/behavior/behavior_evaluator.cpp +++ /dev/null @@ -1,168 +0,0 @@ -// Copyright 2023 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "behavior/behavior_evaluator.hpp" - -#include -#include -#include - -#include - -CREATE_PARAM(double, "behavior_evaluator/", kRotationSpeed, 0.005); - -BehaviorEvaluator::BehaviorEvaluator(BehaviorRealization & behavior_realization) -: behavior_realization(behavior_realization) {} - -DirectedGraph BehaviorEvaluator::get_best_behaviors(const World & world) -{ - // - // Do preprocessing on world state to get important metrics like possession - // - - // - // Setup different behavior options - // - - - // DirectedGraph three_one_touch_shot; - // Behavior initial_pass_start{ - // Behavior::Type::MovingKick, - // Behavior::Priority::Required, - // KickParam({0, 0})}; - // Behavior first_receiver{ - // Behavior::Type::OneTouchReceiveKick, - // Behavior::Priority::Required, - // ReceiveParam({0, 0}, {10, 10})}; - // Behavior second_receiver{ - // Behavior::Type::OneTouchReceiveKick, - // Behavior::Priority::Required, - // ReceiveParam({10, 10}, {-10, -10})}; - // Behavior final_receiver_shot{ - // Behavior::Type::OneTouchShot, - // Behavior::Priority::Required, - // ReceiveShotParam({-10, -10})}; - - // std::size_t parent = three_one_touch_shot.add_node(initial_pass_start); - // parent = three_one_touch_shot.add_node(first_receiver, parent); - // parent = three_one_touch_shot.add_node(second_receiver, parent); - // parent = three_one_touch_shot.add_node(final_receiver_shot, parent); - - - // DirectedGraph direct_shot; - // BehaviorGoal shot{ - // Behavior::Type::Shot, - // Behavior::Priority::Required, - // ShotParam()}; - - // direct_shot.add_node(shot); - - // Generate 16 move behaviors towards the ball, we'll figure out which - // ones we can fill later - // DirectedGraph simple_move; - // static double j = 0; - // j += kRotationSpeed; - // for (int i = 0; i < 16; i++) { - // double theta = i / 11.0 * 3.14 * 2 + j; - // BehaviorGoal move{ - // BehaviorGoal::Type::MoveToPoint, - // BehaviorGoal::Priority::Required, - // MoveParam( - // (sin(3 * theta) + 1.5) * Eigen::Vector2d{cos(theta), sin( - // theta)} - Eigen::Vector2d{2, 0})}; - // } - - // - // Qual Video Behaviors - // - DirectedGraph qual_goalie_and_shot; - double ball_y = 0; - if (world.get_unique_ball().has_value()) { - ball_y = world.get_unique_ball().value().pos.y(); - } - BehaviorGoal goalie{ - BehaviorGoal::Type::MoveToPoint, - BehaviorGoal::Priority::Required, - MoveParam(Eigen::Vector2d{-5, ball_y})}; - qual_goalie_and_shot.add_node(goalie); - - BehaviorGoal kicker{ - BehaviorGoal::Type::MovingKick, - BehaviorGoal::Priority::Required, - MoveParam(Eigen::Vector2d{0, 1})}; - qual_goalie_and_shot.add_node(kicker); - - for (int i = 2; i < 16; i++) { - BehaviorGoal move{ - BehaviorGoal::Type::MoveToPoint, - BehaviorGoal::Priority::Required, - MoveParam(Eigen::Vector2d{i / -2.0, 4})}; - qual_goalie_and_shot.add_node(move); - } - - // - // Add background behaviors - // - - // std::vector>> possible_behaviors{ - // three_one_touch_shot, direct_shot}; - // for (auto & behavior : possible_behaviors) { - // Behavior goalie{ - // Behavior::Type::MoveToPoint, - // Behavior::Priority::Medium, - // MoveParam({0, 0})}; // Field::OurGoal.center(); - // Behavior forward{ - // Behavior::Type::MoveToPoint, - // Behavior::Priority::Low, - // MoveParam({10, 0})}; // Field::TheirGoal.center(); - // Behavior left_defender{ - // Behavior::Type::MoveToPoint, - // Behavior::Priority::Medium, - // MoveParam({5, 5})}; - // Behavior right_defender{ - // Behavior::Type::MoveToPoint, - // Behavior::Priority::Medium, - // MoveParam({5, -5})}; - - // behavior.get().add_node(goalie); - // behavior.get().add_node(forward); - // behavior.get().add_node(left_defender); - // behavior.get().add_node(right_defender); - // } - - // - // See how that combination of behaviors would be planned and executed - // - // DirectedGraph three_one_touch_shot_feedback = - // behavior_realization.realize_behaviors(three_one_touch_shot, world); - // DirectedGraph direct_shot_feedback = - // behavior_realization.realize_behaviors(direct_shot, world); - - // - // Choose main behavior - // - - // choose direct shot because score chance is better or - // maybe the total behavior completetion time is short - // or maybe the other one can't be completed due to number of robots - DirectedGraph behavior_out = qual_goalie_and_shot; - - return behavior_out; -} diff --git a/ateam_ai/src/behavior/behavior_evaluator.hpp b/ateam_ai/src/behavior/behavior_evaluator.hpp deleted file mode 100644 index eea8a1aff..000000000 --- a/ateam_ai/src/behavior/behavior_evaluator.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef BEHAVIOR__BEHAVIOR_EVALUATOR_HPP_ -#define BEHAVIOR__BEHAVIOR_EVALUATOR_HPP_ - -#include - -#include "behavior/behavior_realization.hpp" -#include "types/behavior_goal.hpp" -#include "types/world.hpp" -#include "util/directed_graph.hpp" - -/** - * Figure out which set of behaviors to run - */ -class BehaviorEvaluator -{ -public: - explicit BehaviorEvaluator(BehaviorRealization & behavior_realization); - - DirectedGraph get_best_behaviors(const World & world); - -private: - BehaviorRealization & behavior_realization; -}; - -#endif // BEHAVIOR__BEHAVIOR_EVALUATOR_HPP_ diff --git a/ateam_ai/src/behavior/behavior_executor.cpp b/ateam_ai/src/behavior/behavior_executor.cpp deleted file mode 100644 index b48a282fc..000000000 --- a/ateam_ai/src/behavior/behavior_executor.cpp +++ /dev/null @@ -1,81 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "behavior/behavior_executor.hpp" - -#include "types/behavior_plan.hpp" -#include "trajectory_generation/trajectory_editor.hpp" - -BehaviorExecutor::BehaviorExecutor(BehaviorRealization & behavior_realization) -: behavior_realization(behavior_realization) {} - -std::array, 16> BehaviorExecutor::execute_behaviors( - const DirectedGraph & behaviors, - const World & world, - BehaviorExecutorState & self_state) -{ - // - // Grab trajectories for everything - // - DirectedGraph behavior_feedback = behavior_realization.realize_behaviors( - behaviors, world); - - // - // Replan course trajectories as we approach their start time - // - - // Long term planning (>10 seconds) is not valid due to the speed of robots - // as we approach some of the later behaviors in time, we need to replan them - // using better planners to dodge these obstacles and everything - - // - // Send trajectories to follow - // - - // Clear world trajectories - for (auto & trajectory : self_state.previous_trajectories) { - trajectory.reset(); - } - - std::array, 16> output_trajectories; - for (const auto & root_id : behavior_feedback.get_root_nodes()) { - // Assume there are no children for now - const auto & root_node = behavior_feedback.get_node(root_id); - - // If the behavior is assigned a robot - if (root_node.assigned_robot_id.has_value()) { - Trajectory trajectory = root_node.trajectory; - std::size_t assigned_robot = root_node.assigned_robot_id.value(); - - if (self_state.previous_trajectories.at(assigned_robot).has_value()) { - trajectory = trajectory_editor::apply_immutable_duration( - self_state.previous_trajectories.at(assigned_robot).value(), - trajectory, - world.immutable_duration, - world.current_time); - } - - output_trajectories.at(root_node.assigned_robot_id.value()) = root_node.trajectory; - self_state.previous_trajectories.at(assigned_robot) = trajectory; - } - } - - return output_trajectories; -} diff --git a/ateam_ai/src/behavior/behavior_executor.hpp b/ateam_ai/src/behavior/behavior_executor.hpp deleted file mode 100644 index 4507c3794..000000000 --- a/ateam_ai/src/behavior/behavior_executor.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef BEHAVIOR__BEHAVIOR_EXECUTOR_HPP_ -#define BEHAVIOR__BEHAVIOR_EXECUTOR_HPP_ - -#include -#include - -#include -#include - -#include "behavior/behavior_realization.hpp" -#include "types/behavior_goal.hpp" -#include "types/world.hpp" -#include "types/trajectory.hpp" -#include "util/directed_graph.hpp" - - -/** - * Given a set of behaviors - * - Replan trajectories as their start time approaches - * - Manage the trajectories - */ -class BehaviorExecutor -{ -public: - explicit BehaviorExecutor(BehaviorRealization & behavior_realization); - - std::array, 16> execute_behaviors( - const DirectedGraph & behaviors, - const World & world, - BehaviorExecutorState & self_state); - -private: - BehaviorRealization & behavior_realization; -}; - -#endif // BEHAVIOR__BEHAVIOR_EXECUTOR_HPP_ diff --git a/ateam_ai/src/behavior/behavior_follower.cpp b/ateam_ai/src/behavior/behavior_follower.cpp deleted file mode 100644 index e6bbb361e..000000000 --- a/ateam_ai/src/behavior/behavior_follower.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "behavior/behavior_follower.hpp" - -#include - -#include "util/pid.hpp" -#include "util/viz.hpp" - -CREATE_PARAM(double, "follower/pid/x_kp", x_kp, 2); -CREATE_PARAM(double, "follower/pid/y_kp", y_kp, 2); -CREATE_PARAM(double, "follower/pid/t_kp", t_kp, 3); - -BehaviorFollower::RobotMotionCommands BehaviorFollower::follow( - const std::array, 16> & robot_trajectories, - World & world) -{ - RobotMotionCommands robot_motion_commands; - for (std::size_t robot_id = 0; robot_id < 16; robot_id++) { - const auto & maybe_trajectory = robot_trajectories.at(robot_id); - - // We need a trajectory that is at least 1 sample long - if (!maybe_trajectory.has_value() || - maybe_trajectory.value().samples.empty()) - { - continue; - } - - auto & robot_controllers = trajectory_controllers.at(robot_id); - auto & x_controller = robot_controllers.at(0); - x_controller.set_kp(x_kp); - auto & y_controller = robot_controllers.at(1); - y_controller.set_kp(y_kp); - auto & t_controller = robot_controllers.at(2); - t_controller.set_kp(t_kp); - - Sample3d command = get_next_command(maybe_trajectory.value(), world.current_time); - - const auto & our_robot = world.our_robots.at(robot_id).value(); - - ateam_msgs::msg::RobotMotionCommand motion_command; - motion_command.twist.linear.x = command.vel.x() + x_controller.execute( - command.pose.x(), our_robot.pos.x()); - motion_command.twist.linear.y = command.vel.y() + y_controller.execute( - command.pose.y(), our_robot.pos.y()); - motion_command.twist.angular.z = command.vel.z() + t_controller.execute( - command.pose.z(), our_robot.theta, true); - - viz::DrawTrajectory(robot_id, maybe_trajectory.value()); - - // TODO(jneiger): move this to a better spot - Eigen::Vector2d robot{world.our_robots.at(robot_id).value().pos.x(), world.our_robots.at( - robot_id).value().pos.y()}; - motion_command.kick = world.get_unique_ball().has_value() && - (world.get_unique_ball().value().pos - robot).norm() < 0.1; - motion_command.kick_speed = 5; // m/s - robot_motion_commands.at(robot_id) = motion_command; - } - - return robot_motion_commands; -} - -Sample3d BehaviorFollower::get_next_command(const Trajectory & t, double current_time) -{ - Sample3d command; - command = t.samples.front(); - - // Find first sample that is either current time or after - for (const auto & sample : t.samples) { - if (sample.time >= current_time) { - command = sample; - break; - } - } - - return command; -} diff --git a/ateam_ai/src/behavior/behavior_follower.hpp b/ateam_ai/src/behavior/behavior_follower.hpp deleted file mode 100644 index 0e81dae0b..000000000 --- a/ateam_ai/src/behavior/behavior_follower.hpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef BEHAVIOR__BEHAVIOR_FOLLOWER_HPP_ -#define BEHAVIOR__BEHAVIOR_FOLLOWER_HPP_ - -#include -#include - -#include -#include - -#include "behavior/behavior_realization.hpp" -#include "types/trajectory.hpp" -#include "types/world.hpp" -#include "util/pid.hpp" - -/** - * Given trajectories as a function of time - * - Follow them as best as possible - */ -class BehaviorFollower -{ -public: - using MaybeRobotMotionCommand = std::optional; - using RobotMotionCommands = std::array; - - RobotMotionCommands follow( - const std::array, 16> & robot_trajectories, - World & world); - -private: - static Sample3d get_next_command(const Trajectory & t, double current_time); - - std::array, 16> trajectory_controllers; -}; - -#endif // BEHAVIOR__BEHAVIOR_FOLLOWER_HPP_ diff --git a/ateam_ai/src/behavior/behavior_realization.cpp b/ateam_ai/src/behavior/behavior_realization.cpp deleted file mode 100644 index a656d2a41..000000000 --- a/ateam_ai/src/behavior/behavior_realization.cpp +++ /dev/null @@ -1,225 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include -#include -#include - -#include "behavior/behavior_realization.hpp" -#include "trajectory_generation/trajectory_generation.hpp" - -#include -#include - -DirectedGraph BehaviorRealization::realize_behaviors( - const DirectedGraph & behaviors, const World & world) -{ - return realize_behaviors_impl(behaviors, world, trajectory_generation::GetPlanFromGoal); -} - -DirectedGraph BehaviorRealization::realize_behaviors_impl( - const DirectedGraph & behaviors, const World & world, - const GetPlanFromGoalFnc & GetPlanFromGoal) -{ - // Generate list of available robots - std::set available_robots = get_available_robots(world); - - // Collect nodes by priority - PriorityGoalListMap priority_to_assignment_group = get_priority_to_assignment_group(behaviors); - - // Remove reserved robots from available (reservation comes from nodes) - // Could do this when we hit section below but if someone changes - // order of priorities that breaks down - for (const auto & behavior_idx : priority_to_assignment_group[BehaviorGoal::Priority::Reserved]) { - available_robots.erase(behaviors.get_node(behavior_idx).reserved_robot_id); - } - // TODO(Cavidano) Define an .at on the behavior dag - - std::map assigned_goals_to_plans; - - auto generate_plans_and_assign = - [this, &behaviors, &world, &GetPlanFromGoal, &available_robots, &priority_to_assignment_group, - &assigned_goals_to_plans](std::vector goals_to_assign) { - if (available_robots.empty()) { - return; - } - - CandidatePlans candidate_plans = generate_candidate_plans( - goals_to_assign, available_robots, behaviors, world, GetPlanFromGoal); - GoalToPlanMap goal_to_plans = assign_goals_to_plans( - goals_to_assign, available_robots, candidate_plans, world); - - for (const auto & [goal_idx, plan] : goal_to_plans) { - assigned_goals_to_plans[goal_idx] = plan; - available_robots.erase(plan.assigned_robot_id.value()); - } - }; - - // Forced singular plan per robot could just use above, But barulic and I both thought we didnt - // need to call through all of that - auto generate_reserved_plans = - [this, &behaviors, &world, &GetPlanFromGoal, &assigned_goals_to_plans] - (std::vector goals_to_assign) { - for (const auto & goal_idx : goals_to_assign) { - assigned_goals_to_plans[goal_idx] = GetPlanFromGoal( - behaviors.get_node(goal_idx), - behaviors.get_node(goal_idx).reserved_robot_id, - world); - } - }; - - // TODO(Cavidano) switch statement later - // Assign all robots to goals accoring to the goal priority - for (const auto & [priority, list_of_goal_idxs_at_priority] : priority_to_assignment_group) { - // For each required node, assign independently - if (priority == BehaviorGoal::Priority::Required) { - for (const auto & goal_idx : list_of_goal_idxs_at_priority) { - generate_plans_and_assign({goal_idx}); - } - } else if (priority == BehaviorGoal::Priority::Reserved) { - generate_reserved_plans(list_of_goal_idxs_at_priority); - } else { - generate_plans_and_assign(list_of_goal_idxs_at_priority); - } - } - - return behaviors.copy_shape_with_new_type(assigned_goals_to_plans); -} - -BehaviorRealization::PriorityGoalListMap BehaviorRealization::get_priority_to_assignment_group( - const DirectedGraph & behaviors) -{ - PriorityGoalListMap priority_to_assignment_group{ - {BehaviorGoal::Priority::Reserved, {}}, - {BehaviorGoal::Priority::Required, {}}, - {BehaviorGoal::Priority::Medium, {}}, - {BehaviorGoal::Priority::Low, {}} - }; - - std::vector root_nodes = behaviors.get_root_nodes(); - for (auto root_node_id : behaviors.get_root_nodes()) { - std::deque nodes_to_add{root_node_id}; - while (nodes_to_add.size() > 0) { - BehaviorGoalNodeIdx front_node = nodes_to_add.front(); - nodes_to_add.pop_front(); - - BehaviorGoal behavior = behaviors.get_node(front_node); - priority_to_assignment_group.at(behavior.priority).emplace_back(front_node); - - std::vector children = behaviors.get_children(front_node); - for (const auto & child_node_id : children) { - nodes_to_add.emplace_back(child_node_id); - } - } - } - - return priority_to_assignment_group; -} - -std::set BehaviorRealization::get_available_robots( - const World & world) -{ - std::set available_robots; - for (std::size_t i = 0; i < 15; i++) { - if (world.our_robots.at(i).has_value()) { - available_robots.insert(i); - } - } - - return available_robots; -} - -BehaviorRealization::CandidatePlans BehaviorRealization::generate_candidate_plans( - const std::vector & goals_node_idxs_to_assign, - const std::set & available_robots, - const DirectedGraph & behaviors, - const World & world, - const GetPlanFromGoalFnc & GetPlanFromGoal) -{ - CandidatePlans candidate_plans; - for (const auto & robot_id : available_robots) { - for (const auto & goal_node_idx : goals_node_idxs_to_assign) { - candidate_plans[robot_id][goal_node_idx] = - GetPlanFromGoal( - behaviors.get_node(goal_node_idx), - robot_id, - world); - } - } - - return candidate_plans; -} - -BehaviorRealization::GoalToPlanMap BehaviorRealization::assign_goals_to_plans( - const std::vector & goals_to_assign, - const std::set & available_robots, - const CandidatePlans & candidate_plans, - const World & world) -{ - if (goals_to_assign.empty() || available_robots.empty()) { - return {}; - } - - ATEAM_CHECK(candidate_plans.size() == available_robots.size(), "Must have one plan per robot"); - ATEAM_CHECK( - candidate_plans.begin()->second.size() == goals_to_assign.size(), - "Must have one plan per goal"); - - // Create cost matrix - std::map row_idx_to_robot_id; - std::map col_idx_to_goal_node_idx; - Eigen::MatrixXd robot_to_goal_cost_matrix(available_robots.size(), goals_to_assign.size()); - - int robot_id_idx = 0; - for (const auto & robot_id : available_robots) { - int goal_idx = 0; - for (const auto & goal : goals_to_assign) { - robot_to_goal_cost_matrix(robot_id_idx, goal_idx) = cost( - candidate_plans.at(robot_id).at( - goal), world); - col_idx_to_goal_node_idx[goal_idx] = goal; - goal_idx++; - } - row_idx_to_robot_id[robot_id_idx] = robot_id; - robot_id_idx++; - } - - // Do assignment - auto assignment = ateam_common::assignment::optimize_assignment(robot_to_goal_cost_matrix); - - // Map cost matrix row/col back to behavior plans / robots - GoalToPlanMap assigned_goals_to_plans; - for (const auto & [row_idx, col_idx] : assignment) { - RobotID robot_to_assign = row_idx_to_robot_id.at(row_idx); - BehaviorGoalNodeIdx goal_to_assign = col_idx_to_goal_node_idx.at(col_idx); - - assigned_goals_to_plans[goal_to_assign] = - candidate_plans.at(robot_to_assign).at(goal_to_assign); - assigned_goals_to_plans[goal_to_assign].assigned_robot_id = robot_to_assign; - } - - return assigned_goals_to_plans; -} - -double BehaviorRealization::cost(const BehaviorPlan & bp, const World & world) -{ - return bp.trajectory.samples.back().time - world.current_time; -} diff --git a/ateam_ai/src/behavior/behavior_realization.hpp b/ateam_ai/src/behavior/behavior_realization.hpp deleted file mode 100644 index 021308690..000000000 --- a/ateam_ai/src/behavior/behavior_realization.hpp +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef BEHAVIOR__BEHAVIOR_REALIZATION_HPP_ -#define BEHAVIOR__BEHAVIOR_REALIZATION_HPP_ - -#include -#include -#include - -#include "types/behavior_goal.hpp" -#include "types/behavior_plan.hpp" -#include "types/world.hpp" -#include "util/directed_graph.hpp" - -/** - * Given a set of behaviors - * - Assign the behaviors to robots - * - Generate trajectories for the robots - * - Figure out relative timings to start the trajectories - */ -class BehaviorRealization -{ -public: - /** - * Generate plans and assign robots to the set of goals in an "optimal" way. - * Each required behavior is assigned in order of when they are added to the DAG - * in a depth first way. - * - * Non-required nodes at each stage are assigned in a "globally optimial" way for all - * available robots and goals in each non-required priority level. - * - * Note that since the required goals are assigned in a highly greedy way, placing too many - * required goals will results in "starvation" of any non-required nodes. - */ - DirectedGraph realize_behaviors( - const DirectedGraph & behaviors, - const World & world); - - // ---------------- internal ---------------- - using GetPlanFromGoalFnc = std::function; - DirectedGraph realize_behaviors_impl( - const DirectedGraph & behaviors, - const World & world, - const GetPlanFromGoalFnc & GetPlanFromGoal); - - using BehaviorGoalNodeIdx = std::size_t; - using RobotID = std::size_t; - using Priority = BehaviorGoal::Priority; - using PriorityGoalListMap = std::map>; - using GoalToPlanMap = std::map; - using CandidatePlans = std::map>; - - /** - * Flattens DAG into a list of goals in each priority - */ - PriorityGoalListMap get_priority_to_assignment_group( - const DirectedGraph & behaviors); - - /** - * Gets list of robot ids on our team that are available to assign - * Eg: All robots we have tracking data for - */ - std::set get_available_robots(const World & world); - - /** - * Generate a plan for each robot to each goal - */ - CandidatePlans generate_candidate_plans( - const std::vector & goals_nodes_idxs_to_assign, - const std::set & available_robots, - const DirectedGraph & behaviors, - const World & world, - const GetPlanFromGoalFnc & GetPlanFromGoal); - - /** - * Assigns goals to their optimal plans in a globally optimal way - */ - GoalToPlanMap assign_goals_to_plans( - const std::vector & goals_to_assign, - const std::set & available_robots, - const CandidatePlans & candidate_plans, - const World & world); - - double cost(const BehaviorPlan & bp, const World & world); -}; - -#endif // BEHAVIOR__BEHAVIOR_REALIZATION_HPP_ diff --git a/ateam_ai/src/trajectory_generation/b_spline.cpp b/ateam_ai/src/trajectory_generation/b_spline.cpp deleted file mode 100644 index 3a49eabd6..000000000 --- a/ateam_ai/src/trajectory_generation/b_spline.cpp +++ /dev/null @@ -1,490 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "trajectory_generation/b_spline.hpp" - -#include -#include - -#include - -namespace BSpline -{ - -Output build_and_sample_spline(const Input & input, const std::size_t num_samples) -{ - std::vector sample_poses = sample_spline(convert_to_spline(input), num_samples); - double length = 0.0; - for (int i = 1; i < sample_poses.size(); i++) { - length += (sample_poses.at(i) - sample_poses.at(i - 1)).norm(); - } - - // TODO(jneiger): Calculate curvature from the derivatives of the spline basis functions - std::deque sample_curvatures; - for (int i = 2; i < sample_poses.size(); i++) { - Eigen::Vector2d diff2 = (sample_poses.at(i) - sample_poses.at(i - 1)); - Eigen::Vector2d diff1 = (sample_poses.at(i - 1) - sample_poses.at(i - 2)); - Eigen::Vector2d avg_diff = (diff1 + diff2) / 2; - Eigen::Vector2d diffdiff = diff2 - diff1; - double curvature = std::abs( - (avg_diff.x() * diffdiff.y() - avg_diff.y() * diffdiff.x()) / - std::pow(avg_diff.squaredNorm(), 1.5)); - sample_curvatures.push_back(curvature); - } - // Because we need the second derivative, we "lose" 2 samples - // Add duplicates at the front and back to keep everything the same length - // TODO(jneiger): Remove this hack after the curvature from spline basis functions - sample_curvatures.push_front(sample_curvatures.front()); - sample_curvatures.push_back(sample_curvatures.back()); - - // Start forward pass - // TODO(jneiger): Revist and make sure the math here is actually correct - std::vector speed{input.initial_vel.norm()}; - std::vector accel{input.max_accel}; - for (int i = 1; i < sample_poses.size(); i++) { - double d = (sample_poses.at(i) - sample_poses.at(i - 1)).norm(); - // given d = v * t + 1/2 * a * t^2, solve for t - // Gives 2 options, time will always be positive - double a = accel.at(i - 1); - double v = speed.at(i - 1); - double t1 = -1 * (sqrt(2 * a * d + v * v) + v) / a; - double t2 = (sqrt(2 * a * d + v * v) - v) / a; - // Quick hack for a == 0 - if (std::abs(a) < 1e-5) { - t1 = d / v; - t2 = d / v; - } - // TODO(jneiger): Redo the t estimations in a better way - double t = std::max(t1, t2); - - // Target vel is just the choice from last step - double target_vel = v + a * t; - // Only accel up to limit - double normal_acceleration_from_curvature = sample_curvatures.at(i) * target_vel * target_vel; - double target_accel = std::max(input.max_accel - normal_acceleration_from_curvature, 0.0); - - // If we are too fast, calculate the max speed at this point and hard set the target vel to that - // and set target accel to 0 - if (sample_curvatures.at(i) * target_vel * target_vel > input.max_accel) { - target_vel = sqrt(input.max_accel / sample_curvatures.at(i)); - target_accel = (target_vel - target_vel) / t; - } - - // Limit vel if too fast - if (target_vel >= input.max_vel) { - // TODO(jneiger): Put the real accel values at this location - target_accel = 0; // Not actually correct but it's easier - target_vel = input.max_vel; - } - - speed.push_back(target_vel); - accel.push_back(target_accel); - } - - // Hard lock the last frame to the end - // Max result in discontinuous samples - speed.back() = input.end_vel.norm(); - accel.back() = -input.max_accel; - - // start backward pass - // TODO(jneiger): Deduplicate code with the above - // TODO(jneiger): Re-look at the math here, I think some indexes are messed up - for (int i = sample_poses.size() - 1; i >= 1; i--) { - double d = (sample_poses.at(i) - sample_poses.at(i - 1)).norm(); - // given d = v * t + 1/2 * a * t^2, solve for t - // Gives 2 options, time will always be positive - double a = std::abs(accel.at(i)); - double v = std::abs(speed.at(i)); - double t1 = -1 * (sqrt(2 * a * d + v * v) + v) / a; - double t2 = (sqrt(2 * a * d + v * v) - v) / a; - if (std::abs(a) < 1e-5) { - t1 = d / v; - t2 = d / v; - } - double t = std::max(t1, t2); - - double normal_acceleration_from_curvature = sample_curvatures.at(i) * v * v; - double target_accel = -std::max(input.max_accel - normal_acceleration_from_curvature, 0.0); - double target_vel = v - target_accel * t; - - // If we are too fast, calculate the max speed at this point and hard set the target vel to that - // and set target accel to 0 - if (sample_curvatures.at(i) * target_vel * target_vel > input.max_accel) { - target_vel = sqrt(input.max_accel / sample_curvatures.at(i)); - target_accel = -(target_vel - target_vel) / t; - } - - // Limit vel - if (target_vel >= input.max_vel) { - target_accel = 0; - target_vel = input.max_vel; - } - - if (target_vel < speed.at(i - 1)) { - speed.at(i - 1) = target_vel; - accel.at(i - 1) = target_accel; - } - } - - // TODO(jneiger): Look into revisiting how the output is formatted - // so it's not this weird f(s) formulation instead of the expected f(t) - // (where s is position along the 1d path, and t is time) - Output out; - for (int i = 0; i < sample_poses.size(); i++) { - out.samples.push_back( - Output::Sample2d{.p = sample_poses.at(i), .v = speed.at(i), - .a = accel.at(i)}); - } - - return out; -} - -std::vector sample_spline( - const InternalState & state, - const std::size_t num_samples) -{ - std::vector samples; - for (int i = 0; i <= num_samples; i++) { - samples.push_back( - de_boors_algorithm( - i * 1.0 / num_samples, 3, state.control_points, - state.knot_sequence_with_multiplicity)); - } - - return samples; -} - -InternalState convert_to_spline(const Input & input) -{ - // See section 9.4 page 154 (pdf page 173) - // p_i represents the data points - // d_i represents the control points - // tau_i represents the knot points - // - // L = K + 2 where K is the number of data points - - // Know d_0 == p_0 and d_L = p_k - // d_1 = d_0 + (tau_1 - tau_0) / 3 * intial_vel - // d_l-1 = d_L - (tau_k - tau_k-1) / 3 * end_vel - - // Need to calculate d_2 through d_l-2 per eq 9.12 - - std::vector tau = get_knot_sequence(input.data_points); - std::vector tau_with_multiplicity = apply_multiplicity(tau, 3); - - // 0 ... K is k+1 data points - std::size_t k = input.data_points.size() - 1; - std::size_t L = k + 2; - - // Front and back data point must match exactly - Eigen::Vector2d d_0 = input.data_points.at(0); - Eigen::Vector2d d_L = input.data_points.at(k); - - // Second and second from last data points determine initial/end speeds - Eigen::Vector2d d_1 = d_0 + (tau.at(1) - tau.at(0)) / 3 * input.initial_vel; - Eigen::Vector2d d_L_1 = d_L - (tau.at(k) - tau.at(k - 1)) / 3 * input.end_vel; - - // Knowing the first and second (and last and second to last) data points result a little more - // specific equations in the least squares - Eigen::Vector2d r_s = input.data_points.at(1) - d_1 * basis_function( - 1, 3, tau.at( - 1), tau_with_multiplicity); - Eigen::Vector2d r_e = input.data_points.at(k - 1) - d_L_1 * basis_function( - L - 2, 3, tau.at( - k - 1), tau_with_multiplicity); - - - // TODO(jneiger): Do the 2/3 data point case in a less weird way - bool skip_middle_points = false; - Eigen::VectorXd x; - Eigen::VectorXd y; - if (input.data_points.size() == 2) { - // If we only have 2 points, we already calculated them - skip_middle_points = true; - } else if (input.data_points.size() == 3) { - // If we only have 3 points, the middle point is the control point (???) - x = Eigen::Vector{input.data_points.at(1).x()}; - y = Eigen::Vector{input.data_points.at(1).y()}; - } else { - // Eq 9.11, don't use 9.12 as it's difficult to figure out - // Note that row 2 (index 1) is centered on the second basis function index (1, 2, 3) - // which is i-1, i, i+1, not i, i+1, i+2. All tridiagonal lines should be non-zero - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(k - 1, k - 1); - A(0, 0) = basis_function(1, 3, tau.at(1), tau_with_multiplicity); - A(0, 1) = basis_function(2, 3, tau.at(1), tau_with_multiplicity); - for (std::size_t i = 2; i <= k - 2; i++) { - for (std::size_t p = 0; p < 3; p++) { - // Get the p==2 on the diagonal - int col_idx = p + i - 2; - int row_idx = i - 1; - - // First and law row start and end outside bounds - if (col_idx < 0 || col_idx >= A.rows()) { - continue; - } - A(row_idx, col_idx) = basis_function(i + p - 1, 3, tau.at(i), tau_with_multiplicity); - } - } - A(k - 2, k - 3) = basis_function(L - 4, 3, tau.at(k - 1), tau_with_multiplicity); - A(k - 2, k - 2) = basis_function(L - 3, 3, tau.at(k - 1), tau_with_multiplicity); - - // See EQ 9.12, vector b of the Ax=b - Eigen::VectorXd b = Eigen::VectorXd::Zero(k - 1); - for (int i = 0; i < b.rows(); i++) { - if (i == 0) { - // i = 0 - b(i) = r_s.x(); - } else if (i == b.rows() - 1) { - // i = K - 1 - b(i) = r_e.x(); - } else { - // i = (1 ... K - 2) - b(i) = input.data_points.at(i + 1).x(); // 2 .. K-2 - } - } - - // Output is the x coordinates of the control points 2..L-2 - // TODO(jneiger): replace with a true "tridiagonal" solver like the following - // https://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm - x = A.colPivHouseholderQr().solve(b); - - for (int i = 0; i < b.rows(); i++) { - if (i == 0) { - // i = 0 - b(i) = r_s.y(); - } else if (i == b.rows() - 1) { - // i = K - 1 - b(i) = r_e.y(); - } else { - // i = (1 ... K - 2) - b(i) = input.data_points.at(i + 1).y(); // 2 .. K-2 - } - } - y = A.colPivHouseholderQr().solve(b); - } - std::vector control_points; - control_points.push_back(d_0); - control_points.push_back(d_1); - if (!skip_middle_points) { - for (int i = 0; i < x.rows(); i++) { - control_points.push_back(Eigen::Vector2d{x(i), y(i)}); - } - } - control_points.push_back(d_L_1); - control_points.push_back(d_L); - - InternalState out; - out.control_points = control_points; - out.knot_sequence = tau; - out.knot_sequence_with_multiplicity = tau_with_multiplicity; - return out; -} - -std::vector apply_multiplicity( - const std::vector & knot_sequence, - const std::size_t multiplicity) -{ - // TODO(jneiger): Swap over to std::fill_n and deques - // https://github.com/SSL-A-Team/software/pull/148#discussion_r1241278472 - std::vector out; - for (std::size_t i = 0; i < multiplicity; i++) { - out.push_back(knot_sequence.front()); - } - for (std::size_t i = 1; i < knot_sequence.size() - 1; i++) { - out.push_back(knot_sequence.at(i)); - } - for (std::size_t i = 0; i < multiplicity; i++) { - out.push_back(knot_sequence.back()); - } - - return out; -} - -std::vector get_knot_sequence(const std::vector & control_points) -{ - return centripetal_spacing(control_points); -} - -std::vector constant_spacing(const std::vector & control_points) -{ - // TODO(jneiger): Swap over to std::ranges::transform - // https://github.com/SSL-A-Team/software/pull/148#discussion_r1241279158 - std::vector out; - double spacing = 1.0 / (control_points.size() - 1); - for (int i = 0; i < control_points.size(); i++) { - out.push_back(i * spacing); - } - - return out; -} - -std::vector chord_length_parametrization_spacing( - const std::vector & control_points) -{ - double total_length = 0; - for (int i = 1; i < control_points.size(); i++) { - total_length += (control_points.at(i) - control_points.at(i - 1)).norm(); - } - - std::vector out; - - // First point is always 0 - out.push_back(0); - for (int i = 1; i < control_points.size(); i++) { - out.push_back( - out.back() + (control_points.at(i) - control_points.at( - i - 1)).norm() / total_length); - } - // Last point should always be 1 - - return out; -} - - -std::vector centripetal_spacing(const std::vector & control_points) -{ - double total_length = 0; - for (int i = 1; i < control_points.size(); i++) { - total_length += std::sqrt((control_points.at(i) - control_points.at(i - 1)).norm()); - } - - std::vector out; - - // First point is always 0 - out.push_back(0); - for (int i = 1; i < control_points.size(); i++) { - out.push_back( - out.back() + std::sqrt( - (control_points.at(i) - control_points.at( - i - 1)).norm()) / total_length); - } - // Last point should always be 1 - - return out; -} - -double basis_function( - const std::size_t i, const std::size_t p, const double u, - const std::vector & knot_sequence) -{ - if (p == 0) { - if (knot_sequence.at(i) <= u && u < knot_sequence.at(i + 1)) { - return 1; - } else { - return 0; - } - } - - // For the sake of calculation, 0/0 = 0 - // See Knots with Positive Multiplicity https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/bspline-ex-1.html - auto coeff = [&knot_sequence](double u, std::size_t i, std::size_t p) { - if (knot_sequence.at(i + p) == knot_sequence.at(i)) { - return 0.0; - } - return (u - knot_sequence.at(i)) / (knot_sequence.at(i + p) - knot_sequence.at(i)); - }; - - // See Definition https://en.wikipedia.org/wiki/B-spline - return coeff(u, i, p) * basis_function(i, p - 1, u, knot_sequence) + - (1 - coeff(u, i + 1, p)) * basis_function(i + 1, p - 1, u, knot_sequence); -} - -std::vector knot_points( - const std::size_t degree, - const std::vector & control_points, - const std::vector & knot_sequence) -{ - std::vector out; - for (const auto & u : knot_sequence) { - Eigen::Vector2d knot_point{0, 0}; - - for (int i = 0; i < control_points.size(); i++) { - knot_point += basis_function(i, degree, u, knot_sequence) * control_points.at(i); - } - out.push_back(knot_point); - } - - return out; -} - -Eigen::Vector2d de_boors_algorithm( - const double u, - const std::size_t degree, - const std::vector & control_points, - const std::vector & knot_sequence) -{ - // Taking the example implementation with the extra programming optimization - // from https://en.wikipedia.org/wiki/De_Boor%27s_algorithm#Example_implementation - - // This feels wrong deduping the knots, I think it's needed though? - std::vector un_dup_knots; - for (const auto & knot : knot_sequence) { - if (un_dup_knots.empty() || un_dup_knots.back() != knot) { - un_dup_knots.push_back(knot); - } - } - - // Need to pad each side with |degree| knots, so multiplicity |degree| + 1 - std::vector t = apply_multiplicity(un_dup_knots, degree + 1); - - // Find the range [u_k, u_k+1) at which u fits within the squence - std::size_t k; - for (k = 0; k < t.size() - 1; k++) { - // If u == 1, return the range [prev_knot, 1) instead of [1,) - if ((u >= t.at(k) && u < t.at(k + 1)) || t.at(k + 1) == 1.0) { - break; - } - } - - // Section below is direct implementation of the algorithms - std::vector d; - for (int j = 0; j < degree + 1; j++) { - // Control point list size, degree, and knot point list size are a function of each other - // Note: that there is size buffering as well on the knot points which make this all confusing - ATEAM_CHECK(j + k - degree >= 0, "Degree and control points sizes dont match"); - ATEAM_CHECK( - j + k - degree < control_points.size(), - "Degree and control points sizes dont match"); - - d.push_back(control_points.at(j + k - degree)); - } - - for (int r = 1; r < degree + 1; r++) { - for (int j = degree; j > r - 1; j--) { - // See above - // Mostly done to crash with error message instead of .at access failures - ATEAM_CHECK(j + k - degree >= 0, "Degree and control points sizes dont match"); - ATEAM_CHECK(j + k - degree < t.size(), "Degree and control points sizes dont match"); - ATEAM_CHECK(j + 1 + k - r >= 0, "Degree and control points sizes dont match"); - ATEAM_CHECK(j + 1 + k - r < t.size(), "Degree and control points sizes dont match"); - ATEAM_CHECK(j >= 0, "Degree and control points sizes dont match"); - ATEAM_CHECK(j < d.size(), "Degree and control points sizes dont match"); - ATEAM_CHECK(j - 1 >= 0, "Degree and control points sizes dont match"); - ATEAM_CHECK(j - 1 < d.size(), "Degree and control points sizes dont match"); - - double alpha = (u - t.at(j + k - degree)) / (t.at(j + 1 + k - r) - t.at(j + k - degree)); - d.at(j) = (1.0 - alpha) * d.at(j - 1) + alpha * d.at(j); - } - } - - return d.at(degree); -} - -} // namespace BSpline diff --git a/ateam_ai/src/trajectory_generation/b_spline.hpp b/ateam_ai/src/trajectory_generation/b_spline.hpp deleted file mode 100644 index 060e9448b..000000000 --- a/ateam_ai/src/trajectory_generation/b_spline.hpp +++ /dev/null @@ -1,153 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef TRAJECTORY_GENERATION__B_SPLINE_HPP_ -#define TRAJECTORY_GENERATION__B_SPLINE_HPP_ - -#include - -#include -#include - -/** - * See https://www.cin.ufpe.br/~mdlm/files/Farin-5a_edicao.pdf as reference - * - * Data points refer to the waypoint inputs - * Control points refer to the points in the spline definition that pull the spline in different directions - * Knot points refer to how to disbribute the basis functions across the length of the curve - * Basis functions deal with the specific weighting of the control points to determine the location of the point on the curve at u=X -*/ -namespace BSpline -{ - -struct Input -{ - std::vector data_points; - Eigen::Vector2d initial_vel; - Eigen::Vector2d end_vel; - - double max_accel; - double max_vel; -}; - -struct InternalState -{ - std::vector control_points; - std::vector knot_sequence; - std::vector knot_sequence_with_multiplicity; -}; - -struct Output -{ - struct Sample2d - { - Eigen::Vector2d p; - double v; - double a; - }; - // Samples are a function of position - std::vector samples; -}; - -/** - * Build and sample the spline |num_samples| number of times -*/ -Output build_and_sample_spline(const Input & input, const std::size_t num_samples); - -/** - * Given the internal control and knot sequences. - * Produce a series of equally sampled points along the spline -*/ -std::vector sample_spline( - const InternalState & state, - const std::size_t num_samples); - -/** - * Convert the data points to a series of control points and knots -*/ -InternalState convert_to_spline(const Input & input); - -/** - * Repeats the first and last knot point |multiplicity| times to force end conditions - * on the start and end control point - * - * |multiplicity| = 3 with velocity constraints produce splines that start/end at the end control_points with the defined velocity -*/ -std::vector apply_multiplicity( - const std::vector & knot_sequence, - const std::size_t multiplicity); - -/** - * Given a sequence of control points, find the spacing of knots. - * This is pre multiplicity. - * - * Spacing is on a scale of 0 ... 1 inclusive -*/ -std::vector get_knot_sequence(const std::vector & control_points); - -/** - * Applies linear spacing of knots -*/ -std::vector constant_spacing(const std::vector & control_points); - -/** - * Applies a distance proportional spacing of knots - * - * Eq 9.16, page 163 -*/ -std::vector chord_length_parametrization_spacing( - const std::vector & control_points); - -/** - * Applies a sqrt of the distance proportional spacing of knots - * - * Eq 9.17, page 163 -*/ -std::vector centripetal_spacing(const std::vector & control_points); - -/** - * Calculates the basis function N_i,p(u) - * - * The basis function is a box car step functions -*/ -double basis_function( - const std::size_t i, const std::size_t p, const double u, - const std::vector & knot_sequence); - -/** - * Calculates the knot points - * - * Knot points are on the B spline curve that split the curve into segments -*/ -std::vector knot_points( - const std::size_t degree, - const std::vector & control_points, - const std::vector & knot_sequence); - -/** - * Samples the curve at location u in range [0, 1] corresponding the the % along the B spline -*/ -Eigen::Vector2d de_boors_algorithm( - const double u, const std::size_t degree, - const std::vector & control_points, - const std::vector & knot_sequence); -} // namespace BSpline - -#endif // TRAJECTORY_GENERATION__B_SPLINE_HPP_ diff --git a/ateam_ai/src/trajectory_generation/b_spline_wrapper.cpp b/ateam_ai/src/trajectory_generation/b_spline_wrapper.cpp deleted file mode 100644 index 3a732a2ab..000000000 --- a/ateam_ai/src/trajectory_generation/b_spline_wrapper.cpp +++ /dev/null @@ -1,166 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "trajectory_generation/b_spline_wrapper.hpp" - -#include - -#include - -#include - -#include "trajectory_generation/b_spline.hpp" -#include "trajectory_generation/trapezoidal_motion_profile.hpp" - -namespace BSplineWrapper -{ - -Trajectory Generate( - const std::vector waypoints, - const double start_heading, - const double end_heading, - const Eigen::Vector3d & start_vel, - const Eigen::Vector3d & end_vel, - const Eigen::Vector3d & max_vel_limits, - const Eigen::Vector3d & max_accel_limits, - const double dt, const double current_time) -{ - ATEAM_CHECK(waypoints.size() >= 2, "Must have at least 2 waypoints"); - - // - // Generate the base "trajectories" - // - - // TODO(jneiger): Split x/y limits to be independent - BSpline::Input b_input; - b_input.data_points = waypoints; - b_input.initial_vel = Eigen::Vector2d{start_vel.x(), start_vel.y()}; - b_input.end_vel = Eigen::Vector2d{end_vel.x(), end_vel.y()}; - b_input.max_accel = max_accel_limits.x(); // Assume X/Y limits are the same - b_input.max_vel = max_vel_limits.x(); - - // Need enough samples that the conversion from curve to a series of line segments - // is still close enough to reality for the trajectory following. - const std::size_t kNumSamples = 40; - // Plan XY trajectory (output is function of position along curve) - BSpline::Output b_output = BSpline::build_and_sample_spline(b_input, kNumSamples); - - // Plan heading trajectory (output is function of time) - double modified_end_heading = start_heading + angles::shortest_angular_distance( - start_heading, - end_heading); - - std::array trajectories; - trajectories.at(2) = - TrapezoidalMotionProfile::Generate1d( - start_heading, - start_vel.z(), - modified_end_heading, - end_vel.z(), - max_vel_limits.z(), - max_accel_limits.z(), dt); - - // - // Sample b spline output to be on the same time series as the heading - // - - // Step through the output and figure out given the speed / accel, the time that segment - // If the time since the start matches the target sample time, interpolate that segment - double prev_segment_t = 0.0; - double target_t = 0.0; - for (int i = 0; i < b_output.samples.size() - 1; i++) { - Eigen::Vector2d diff = b_output.samples.at(i + 1).p - b_output.samples.at(i).p; - double segment_dist = (b_output.samples.at(i + 1).p - b_output.samples.at(i).p).norm(); - double average_v = (b_output.samples.at(i + 1).v - b_output.samples.at(i).v) / 2; - - // This check is probably overly restrictive - // Really just want to include acceleration in the t calc - // with some upper limit so we don't break on bad bspline segment accels at 0 vel samples - // ATEAM_CHECK(average_v == 0, "Average velocity must be non-zero across a segment"); - double end_of_segment_t = prev_segment_t + std::min(segment_dist / std::abs(average_v), 1.0); - - // target_t is in this segment - if (target_t >= prev_segment_t && target_t < end_of_segment_t) { - // Interpolate the actual position / velocity / accel - double coeff = (target_t - prev_segment_t) / (end_of_segment_t - prev_segment_t); - double vel_mag = b_output.samples.at(i).v + (target_t - prev_segment_t) * b_output.samples.at( - i).a; - - TrapezoidalMotionProfile::Sample1d x_sample{ - .time = target_t, - .pos = (b_output.samples.at(i).p + coeff * diff).x(), - .vel = (vel_mag * diff.normalized()).x(), - .accel = (b_output.samples.at(i).a * diff.normalized()).x() - }; - TrapezoidalMotionProfile::Sample1d y_sample{ - .time = target_t, - .pos = (b_output.samples.at(i).p + coeff * diff).y(), - .vel = (vel_mag * diff.normalized()).y(), - .accel = (b_output.samples.at(i).a * diff.normalized()).y() - }; - - trajectories.at(0).samples.push_back(x_sample); - trajectories.at(1).samples.push_back(y_sample); - target_t += dt; - } - - if (target_t >= end_of_segment_t) { - prev_segment_t = end_of_segment_t; - } - } - - // - // Combine the trajectories - // - Trajectory output; - - // TODO(jneiger): Scale the trajectories instead of copying the last - std::size_t t_idx = 0; - double t = current_time; - bool is_more_left = true; - while (is_more_left) { - is_more_left = false; - Sample3d sample; - sample.time = t; - for (std::size_t i = 0; i < trajectories.size(); i++) { - const auto & trajectory = trajectories.at(i); - if (t_idx < trajectory.samples.size()) { - sample.pose(i) = trajectory.samples.at(t_idx).pos; - sample.vel(i) = trajectory.samples.at(t_idx).vel; - sample.accel(i) = trajectory.samples.at(t_idx).accel; - is_more_left = true; - } else if (!trajectory.samples.empty()) { - sample.pose(i) = trajectory.samples.back().pos; - sample.vel(i) = trajectory.samples.back().vel; - sample.accel(i) = trajectory.samples.back().accel; - } - } - - if (is_more_left) { - output.samples.push_back(sample); - } - - t += dt; - t_idx++; - } - - return output; -} -} // namespace BSplineWrapper diff --git a/ateam_ai/src/trajectory_generation/b_spline_wrapper.hpp b/ateam_ai/src/trajectory_generation/b_spline_wrapper.hpp deleted file mode 100644 index 3b36b8d2f..000000000 --- a/ateam_ai/src/trajectory_generation/b_spline_wrapper.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef TRAJECTORY_GENERATION__B_SPLINE_WRAPPER_HPP_ -#define TRAJECTORY_GENERATION__B_SPLINE_WRAPPER_HPP_ - -#include - -#include - -#include "types/trajectory.hpp" - -namespace BSplineWrapper -{ - -Trajectory Generate( - const std::vector waypoints, - const double start_heading, - const double end_heading, - const Eigen::Vector3d & start_vel, - const Eigen::Vector3d & end_vel, - const Eigen::Vector3d & max_vel_limits, - const Eigen::Vector3d & max_accel_limits, - const double dt, const double current_time); - -} // namespace BSplineWrapper - -#endif // TRAJECTORY_GENERATION__B_SPLINE_WRAPPER_HPP_ diff --git a/ateam_ai/src/trajectory_generation/ilqr_problem.hpp b/ateam_ai/src/trajectory_generation/ilqr_problem.hpp deleted file mode 100644 index 578680b17..000000000 --- a/ateam_ai/src/trajectory_generation/ilqr_problem.hpp +++ /dev/null @@ -1,292 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef TRAJECTORY_GENERATION__ILQR_PROBLEM_HPP_ -#define TRAJECTORY_GENERATION__ILQR_PROBLEM_HPP_ - -#include - -#include -#include - -// Defines the abstract definition and solution to the iLQR problem -// https://homes.cs.washington.edu/~todorov/papers/TassaICRA14.pdf -// https://studywolf.wordpress.com/2016/02/03/the-iterative-linear-quadratic-regulator-method/ -// X = num states, U = num inputs, T = num timesteps -template -class iLQRProblem -{ -public: - using State = Eigen::Matrix; - using Input = Eigen::Matrix; - using Jacobian = Eigen::Matrix; - using Cost = double; - using Gradiant = Eigen::Matrix; - using Hessian = Eigen::Matrix; - using Time = int; - - using Trajectory = std::array; - using Actions = std::array; - - using Feedforwards = std::array; - using Feedbacks = std::array, T>; - - // Assume second derivative of dynamics is 0 so we don't have do tensor math - virtual State dynamics(const State & x_t1, const Input & u_t1, Time t) = 0; - virtual Jacobian jacobian(const State & x, const Input & u, Time t) - { - Jacobian out; - - State d = dynamics(x, u, t); - for (std::size_t i = 0; i < X; i++) { - State x_eps = x; - x_eps(i) += eps; - out.col(i) = (dynamics(x_eps, u, t) - d) / eps; - } - - for (std::size_t i = 0; i < U; i++) { - Input u_eps = u; - u_eps(i) += eps; - out.col(i + X) = (dynamics(x, u_eps, t) - d) / eps; - } - - return out; - } - - virtual Cost cost(const State & x, const Input & u, Time t) = 0; - virtual Gradiant gradiant(const State & x, const Input & u, Time t) - { - Gradiant out; - - for (std::size_t i = 0; i < X; i++) { - State x_eps_p = x; - State x_eps_n = x; - x_eps_p(i) += eps; - x_eps_n(i) -= eps; - out(i) = (cost(x_eps_p, u, t) - cost(x_eps_n, u, t)) / (2 * eps); - } - - for (std::size_t i = 0; i < U; i++) { - Input u_eps_p = u; - Input u_eps_n = u; - u_eps_p(i) += eps; - u_eps_n(i) -= eps; - out(i + X) = (cost(x, u_eps_p, t) - cost(x, u_eps_n, t)) / (2 * eps); - } - - return out; - } - - virtual Hessian hessian(const State & x, const Input & u, Time t, const Gradiant & g) - { - Hessian out; - - for (std::size_t i = 0; i < X; i++) { - State x_eps = x; - x_eps(i) += eps; - out.col(i) = (gradiant(x_eps, u, t) - g) / eps; - } - - for (std::size_t i = 0; i < U; i++) { - Input u_eps = u; - u_eps(i) += eps; - out.col(i + X) = (gradiant(x, u_eps, t) - g) / eps; - } - - return out; - } - - std::optional calculate(const State & initial_state) - { - forward_rollout(initial_state); - if (backward_pass()) { - return trajectory; - } else { - return std::nullopt; - } - } - -private: - using Jacobians = std::array; - using Hessians = std::array; - using Gradiants = std::array; - - - void forward_rollout(const State & initial_state) - { - // Step 1: Forward Rollout - trajectory.front() = initial_state; - actions.front().setZero(); - overall_cost = cost(trajectory.front(), actions.front(), 0); - - for (Time t = 1; t < T; t++) { - trajectory.at(t) = dynamics(trajectory.at(t - 1), actions.at(t - 1), t); - actions.at(t).setZero(); - overall_cost += cost(trajectory.at(t), actions.at(t), t); - } - - // Step 2: Calculate jacobian/hessian for x,u - for (Time t = 0; t < T; t++) { - j.at(t) = jacobian(trajectory.at(t), actions.at(t), t); - - g.at(t) = gradiant(trajectory.at(t), actions.at(t), t); - h.at(t) = hessian(trajectory.at(t), actions.at(t), t, g.at(t)); - } - } - - bool backward_pass() - { - for (std::size_t num_iterations = 0; num_iterations < max_num_iterations; num_iterations++) { - // Step 3: Determine best control signal update - Feedforwards k; - Feedbacks K; - - Gradiant Q_dxu; // Derivative of V(f(x, u)) - Hessian Q_ddxu; - State v_dx = g.back().block(0, 0, X, 1); // Derivative of V(x) - Eigen::Matrix v_ddx = h.back().block(0, 0, X, X); - - k.at(T - 1).setZero(); - K.at(T - 1).setZero(); - - for (Time t = T - 2; t >= 0; t--) { - // eq 4a - Q_dxu.block(0, 0, X, 1) = - g.at(t).block(0, 0, X, 1) + - j.at(t).block(0, 0, X, X).transpose() * - v_dx.block(0, 0, X, 1); - // eq 4b - Q_dxu.block(X, 0, U, 1) = - g.at(t).block(X, 0, U, 1) + - j.at(t).block(0, X, X, U).transpose() * - v_dx.block(0, 0, X, 1); - // eq 4c - Q_ddxu.block(0, 0, X, X) = - h.at(t).block(0, 0, X, X) + - j.at(t).block(0, 0, X, X).transpose() * - v_ddx.block(0, 0, X, X) * - j.at(t).block(0, 0, X, X); - // eq 4d - Q_ddxu.block(X, 0, U, X) = - h.at(t).block(X, 0, U, X) + - j.at(t).block(0, X, X, U).transpose() * - v_ddx.block(0, 0, X, X) * - j.at(t).block(0, 0, X, X); - // eq 4e - Q_ddxu.block(X, X, U, U) = - h.at(t).block(X, X, U, U) + - j.at(t).block(0, X, X, U).transpose() * - v_ddx.block(0, 0, X, X) * - j.at(t).block(0, X, X, U); - - // eq 5b - // Solve inverse with regulization to keep it from exploding - Eigen::SelfAdjointEigenSolver> eigensolver(Q_ddxu.block( - X, X, U, - U)); - if (eigensolver.info() != Eigen::Success) { - std::cout << "Failed to solve eigen vectors" << std::endl; - return false; - } - Eigen::Matrix eigen_vals = eigensolver.eigenvalues(); - Eigen::Matrix eigen_vals_diag; - eigen_vals_diag.setZero(); - for (int i = 0; i < U; i++) { - if (eigen_vals[i] < 0) { - eigen_vals[i] = 0; - } - eigen_vals[i] += alpha; - eigen_vals_diag(i, i) = 1.0 / eigen_vals[i]; - } - Eigen::Matrix eigen_vecs = eigensolver.eigenvectors(); - Eigen::Matrix inv = eigen_vecs * eigen_vals_diag * eigen_vecs.transpose(); - - k.at(t) = -1 * inv * Q_dxu.block(X, 0, U, 1); - K.at(t) = -1 * inv * Q_ddxu.block(X, 0, U, X); - - // eq 6b - v_dx = Q_dxu.block(0, 0, X, 1) - K.at(t).transpose() * Q_ddxu.block(X, X, U, U) * k.at(t); - // eq 6c - v_ddx = - Q_ddxu.block(0, 0, X, X) - K.at(t).transpose() * Q_ddxu.block(X, X, U, U) * K.at(t); - } - - // Step 3.5: Update possible control signal, states, costs - Cost test_cost; - Trajectory test_trajectory; - Actions test_actions; - test_trajectory.front() = trajectory.front(); - test_actions.front() = actions.front() + k.front(); // xhat_0 == x_0 so Kt term goes away - test_cost = cost(test_trajectory.front(), test_actions.front(), 0); - for (Time t = 1; t < T; t++) { - test_trajectory.at(t) = dynamics( - test_trajectory.at( - t - 1), test_actions.at(t - 1), t); - test_actions.at(t) = actions.at(t) + k.at(t) + K.at(t) * - (test_trajectory.at(t) - trajectory.at(t)); - test_cost = cost(test_trajectory.at(t), test_actions.at(t), t); - } - - // Step 4: Compare Costs and update update size - if (test_cost < overall_cost) { - trajectory = test_trajectory; - actions = test_actions; - - // If we converge, just return - if (std::abs(test_cost - overall_cost) / test_cost < converge_threshold) { - return true; - } - - overall_cost = test_cost; - - // Step 4.5: Calculate jacobian/hessian for x,u - for (Time t = 0; t < T; t++) { - j.at(t) = jacobian(trajectory.at(t), actions.at(t), t); - - g.at(t) = gradiant(trajectory.at(t), actions.at(t), t); - h.at(t) = hessian(trajectory.at(t), actions.at(t), t, g.at(t)); - } - - alpha *= alpha_change; - } else { - alpha /= alpha_change; - } - } - - return true; - } - - static constexpr std::size_t max_num_iterations = 1000; - static constexpr double converge_threshold = 1e-6; - static constexpr double alpha_change = 0.99; - static constexpr double eps = 1e-3; - - double alpha = 1; // backtracking serach parameter - Trajectory trajectory; - Actions actions; - - Jacobians j; - Gradiants g; - Hessians h; - - Cost overall_cost; -}; - -#endif // TRAJECTORY_GENERATION__ILQR_PROBLEM_HPP_ diff --git a/ateam_ai/src/trajectory_generation/trajectory_editor.cpp b/ateam_ai/src/trajectory_generation/trajectory_editor.cpp deleted file mode 100644 index 593fffa4c..000000000 --- a/ateam_ai/src/trajectory_generation/trajectory_editor.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "trajectory_generation/trajectory_editor.hpp" - -Trajectory trajectory_editor::crop_trajectory( - const Trajectory & a, double new_start_time, - double new_end_time) -{ - Trajectory output; - std::copy_if( - a.samples.begin(), a.samples.end(), std::back_inserter(output.samples), - [new_start_time, new_end_time](Sample3d sample) { - return sample.time >= new_start_time && sample.time < new_end_time; - }); - - return output; -} - -Trajectory trajectory_editor::append_trajectory( - const Trajectory & a, const Trajectory & b) -{ - Trajectory output = a; - - for (auto sample : b.samples) { - output.samples.push_back(sample); - } - - return output; -} - -Trajectory trajectory_editor::apply_immutable_duration( - const Trajectory & last_frame_trajectory, - const Trajectory & this_frame_trajectory, - double immutable_duration, - double current_time) -{ - Trajectory first = crop_trajectory( - last_frame_trajectory, current_time, - current_time + immutable_duration); - Trajectory output = append_trajectory(first, this_frame_trajectory); - - return output; -} - -Robot trajectory_editor::state_at_immutable_duration( - const Trajectory & last_frame_trajectory, double immutable_duration, - double current_time) -{ - assert(!last_frame_trajectory.samples.empty()); - - Sample3d state = last_frame_trajectory.samples.front(); - for (const auto & sample : last_frame_trajectory.samples) { - if (sample.time <= current_time + immutable_duration) { - state = sample; - } else { - break; - } - } - - Robot output; - output.pos = Eigen::Vector2d{state.pose.x(), state.pose.y()}; - output.theta = state.pose.z(); - output.vel = Eigen::Vector2d{state.vel.x(), state.vel.y()}; - output.omega = state.vel.z(); - - return output; -} diff --git a/ateam_ai/src/trajectory_generation/trajectory_editor.hpp b/ateam_ai/src/trajectory_generation/trajectory_editor.hpp deleted file mode 100644 index bc87099f1..000000000 --- a/ateam_ai/src/trajectory_generation/trajectory_editor.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef TRAJECTORY_GENERATION__TRAJECTORY_EDITOR_HPP_ -#define TRAJECTORY_GENERATION__TRAJECTORY_EDITOR_HPP_ - -#include "types/trajectory.hpp" -#include "types/robot.hpp" - -namespace trajectory_editor -{ -/** - * Given a trajectory, crop the trajectory such that it starts at |new_start_time| and ends at |new_end_time|. All - * samples outside this period are removed. - */ -Trajectory crop_trajectory(const Trajectory & a, double new_start_time, double new_end_time); - -/** - * Given two trajectories |a| and |b|, place |a| before |b|, with no editing of sample time - */ -Trajectory append_trajectory(const Trajectory & a, const Trajectory & b); - -/** - * Prepends only the initial |immutable_duration| from |last_frame_trajectory| in front of |this_frame_trajectory| - */ -Trajectory apply_immutable_duration( - const Trajectory & last_frame_trajectory, const Trajectory & this_frame_trajectory, - double immutable_duration, double current_time); - -/** - * Returns the robot state at the last sample before we can change our plan - */ -Robot state_at_immutable_duration( - const Trajectory & last_frame_trajectory, double immutable_duration, double current_time); -} // namespace trajectory_editor - -#endif // TRAJECTORY_GENERATION__TRAJECTORY_EDITOR_HPP_ diff --git a/ateam_ai/src/trajectory_generation/trajectory_generation.cpp b/ateam_ai/src/trajectory_generation/trajectory_generation.cpp deleted file mode 100644 index 31f913fb7..000000000 --- a/ateam_ai/src/trajectory_generation/trajectory_generation.cpp +++ /dev/null @@ -1,186 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "trajectory_generation/trajectory_generation.hpp" - -#include - -#include - -#include - -#include "trajectory_generation/trajectory_editor.hpp" -#include "trajectory_generation/trapezoidal_motion_profile.hpp" - -namespace trajectory_generation -{ -BehaviorPlan GetPlanFromGoal( - BehaviorGoal behavior, int assigned_robot, const World & world) -{ - BehaviorPlan plan; - - switch (behavior.type) { - case BehaviorGoal::Type::MovingKick: - case BehaviorGoal::Type::PivotKick: - // std::get(behavior.params).target_location; - { - Robot current_robot = world.plan_from_our_robots.at(assigned_robot).value(); - - Eigen::Vector3d current, current_vel, target, target_vel; - current.x() = current_robot.pos.x(); - current.y() = current_robot.pos.y(); - current.z() = current_robot.theta; - current_vel.x() = current_robot.vel.x(); - current_vel.y() = current_robot.vel.y(); - current_vel.z() = current_robot.omega; - - const auto & ball = world.get_unique_ball(); - if (!ball.has_value() || ball.value().vel.norm() > 0.05) { - target.x() = current_robot.pos.x(); - target.y() = current_robot.pos.y(); - target.z() = current_robot.theta; - target_vel.x() = 0; - target_vel.y() = 0; - target_vel.z() = 0; - } else { - // robot to ball check to not collide - Eigen::Vector2d ball_pos = ball.value().pos; - Eigen::Vector2d current_robot_pos = current_robot.pos; - Eigen::Vector2d target_goal = Eigen::Vector2d{-6, 0}; - Eigen::Vector2d target_setup_pos = ball_pos + 0.4 * (ball_pos - target_goal).normalized(); - - Eigen::Vector2d robot_to_goal = target_goal - current_robot_pos; - double robot_to_goal_angle = ateam_common::geometry::VectorToAngle(robot_to_goal); - double angle_diff = angles::shortest_angular_distance( - current_robot.theta, robot_to_goal_angle); - - Eigen::Vector2d robot_to_ball = ball_pos - current_robot_pos; - Eigen::Vector2d ball_to_goal = target_goal - ball_pos; - - // Aligned means - // * Ball is directly between the robot and goal - // * Angle difference between heading and robot to goal is low - // * Robot is almost stopped - bool is_aligned = ateam_common::geometry::IsVectorAligned( - robot_to_goal, robot_to_ball, - 0.01); - is_aligned &= std::abs(angle_diff) < 0.05; - is_aligned &= current_robot.vel.norm() < 0.5; - - if (!is_aligned) { - double projection = (target_setup_pos - current_robot_pos).dot( - ball_pos - current_robot_pos) / - ((target_setup_pos - current_robot_pos).norm() * - (target_setup_pos - current_robot_pos).norm()); - projection = std::max(std::min(projection, 1.0), 0.0); - Eigen::Vector2d ball_projected_on_move_line = projection * - (target_setup_pos - current_robot_pos) + current_robot_pos; - double dist = (ball_projected_on_move_line - ball_pos).norm(); - if (dist < 0.1) { - target.x() = target_setup_pos.x(); - target.y() = target_setup_pos.y() - 1; - target.z() = atan2(ball_to_goal.y(), ball_to_goal.x()); - target_vel.x() = 0; - target_vel.y() = 0; - target_vel.z() = 0; - } else { - target.x() = target_setup_pos.x(); - target.y() = target_setup_pos.y(); - target.z() = atan2(ball_to_goal.y(), ball_to_goal.x()); - target_vel.x() = 0; - target_vel.y() = 0; - target_vel.z() = 0; - } - } else { - // Kick - target.x() = ball_pos.x(); - target.y() = ball_pos.y(); - target.z() = atan2(ball_to_goal.y(), ball_to_goal.x()); - target_vel.x() = 0; - target_vel.y() = 0; - target_vel.z() = 0; - } - } - - Eigen::Vector3d max_vel{3, 3, 1}; // TODO(jneiger): Set as params - Eigen::Vector3d max_accel{1, 1, 1}; - double dt = 0.01; // TODO(jneiger): Feed this down from above - Trajectory trajectory = TrapezoidalMotionProfile::Generate3d( - current, current_vel, target, - target_vel, max_vel, max_accel, - dt, world.current_time + world.immutable_duration); - - plan.trajectory = trajectory; - } - break; - - case BehaviorGoal::Type::OneTouchReceiveKick: - case BehaviorGoal::Type::TwoTouchReceiveKick: - // std::get(behavior.params).receive_location; - break; - - case BehaviorGoal::Type::Shot: - break; - - case BehaviorGoal::Type::OneTouchShot: - // std::get(behavior.params).receive_location; - break; - - case BehaviorGoal::Type::MoveToPoint: - { - Robot current_robot = world.plan_from_our_robots.at(assigned_robot).value(); - - Eigen::Vector3d current, current_vel, target, target_vel; - current.x() = current_robot.pos.x(); - current.y() = current_robot.pos.y(); - current.z() = 0; // current_robot.theta; - current_vel.x() = current_robot.vel.x(); - current_vel.y() = current_robot.vel.y(); - current_vel.z() = 0; // current_robot.omega; - - target.x() = std::get(behavior.params).target_location.x(); - target.y() = std::get(behavior.params).target_location.y(); - target.z() = 0; - target_vel.x() = 0; - target_vel.y() = 0; - target_vel.z() = 0; - Eigen::Vector3d max_vel{2, 2, 0.5}; // TODO(jneiger): Set as params - Eigen::Vector3d max_accel{2, 2, 0.5}; - double dt = 0.01; // TODO(jneiger): Feed this down from above - Trajectory trajectory = TrapezoidalMotionProfile::Generate3d( - current, current_vel, target, - target_vel, max_vel, max_accel, - dt, world.current_time + world.immutable_duration); - - plan.trajectory = trajectory; - } - break; - - case BehaviorGoal::Type::CostFunctionPoint: - break; - - default: - break; - } - - plan.assigned_robot_id = assigned_robot; - return plan; -} -} // namespace trajectory_generation diff --git a/ateam_ai/src/trajectory_generation/trajectory_generation.hpp b/ateam_ai/src/trajectory_generation/trajectory_generation.hpp deleted file mode 100644 index 478e476d0..000000000 --- a/ateam_ai/src/trajectory_generation/trajectory_generation.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef TRAJECTORY_GENERATION__TRAJECTORY_GENERATION_HPP_ -#define TRAJECTORY_GENERATION__TRAJECTORY_GENERATION_HPP_ - -#include "types/behavior_goal.hpp" -#include "types/behavior_plan.hpp" -#include "types/world.hpp" - -/** - * Given a behavior and assigned robot, build a motion plan needed to fully execute it - */ -namespace trajectory_generation -{ -BehaviorPlan GetPlanFromGoal( - BehaviorGoal behavior, int assigned_robot, - const World & world); -} - -#endif // TRAJECTORY_GENERATION__TRAJECTORY_GENERATION_HPP_ diff --git a/ateam_ai/src/trajectory_generation/trapezoidal_motion_profile.cpp b/ateam_ai/src/trajectory_generation/trapezoidal_motion_profile.cpp deleted file mode 100644 index 2b22b8c63..000000000 --- a/ateam_ai/src/trajectory_generation/trapezoidal_motion_profile.cpp +++ /dev/null @@ -1,397 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "trajectory_generation/trapezoidal_motion_profile.hpp" - -#include - -#include -#include - -namespace TrapezoidalMotionProfile -{ -/** - * @brief Unwrap heading target to move through the shortest angle distance - * - * @param start Start XYtheta - * @param end End XYtheta - * @return Eigen::Vector3d XYtheta with modified theta - */ -Eigen::Vector3d minimize_angle_goal_target( - const Eigen::Vector3d & start, - const Eigen::Vector3d & end) -{ - Eigen::Vector3d new_end = end; - double angle_diff = angles::shortest_angular_distance(start.z(), end.z()); - new_end.z() = start.z() + angle_diff; - - return new_end; -} - -Trajectory Generate3d( - const Eigen::Vector3d & start, const Eigen::Vector3d & start_vel, - const Eigen::Vector3d & end, const Eigen::Vector3d & end_vel, - const Eigen::Vector3d & max_vel_limits, - const Eigen::Vector3d & max_accel_limits, - const double dt, const double current_time) -{ - // Independently plan for each DOF - std::array trajectories; - Eigen::Vector3d modified_end = minimize_angle_goal_target(start, end); - - // TODO(jneiger): Scale plans to longest DOF trajectory time length - for (std::size_t i = 0; i < trajectories.size(); i++) { - trajectories.at(i) = Generate1d( - start(i), start_vel(i), modified_end(i), end_vel(i), max_vel_limits( - i), max_accel_limits(i), dt); - } - - Trajectory output; - - std::size_t t_idx = 0; - double t = current_time; - bool is_more_left = true; - while (is_more_left) { - is_more_left = false; - Sample3d sample; - sample.time = t; - for (std::size_t i = 0; i < trajectories.size(); i++) { - const auto & trajectory = trajectories.at(i); - if (t_idx < trajectory.samples.size()) { - sample.pose(i) = trajectory.samples.at(t_idx).pos; - sample.vel(i) = trajectory.samples.at(t_idx).vel; - sample.accel(i) = trajectory.samples.at(t_idx).accel; - is_more_left = true; - } else { - sample.pose(i) = trajectory.samples.back().pos; - sample.vel(i) = trajectory.samples.back().vel; - sample.accel(i) = trajectory.samples.back().accel; - } - } - - if (is_more_left) { - output.samples.push_back(sample); - } - - t += dt; - t_idx++; - } - - return output; -} - -/** - * Returns time (s) to accelerate from start vel to end vel given an acceleration limit - */ -double time_to_accelerate(const double start_vel, const double end_vel, const double accel_limit) -{ - return std::abs(end_vel - start_vel) / accel_limit; -} - -/** - * Return average velocity given the start vel and end vel, assuming a constant acceleration - */ -double avg_vel_over_acceleration(const double start_vel, const double end_vel) -{ - return std::midpoint(start_vel, end_vel); -} - -/** - * Returns distance (m) required to accelerate from start_vel to end_vel given an acceleration limit - */ -double dist_covered_during_acceleration( - const double start_vel, const double end_vel, - const double accel_limit) -{ - const double t = time_to_accelerate(start_vel, end_vel, accel_limit); - const double avg_vel = avg_vel_over_acceleration(start_vel, end_vel); - - return t * avg_vel; -} - -struct AccelerationPhaseInputs -{ - double start_vel; // m/s - double end_vel; // m/s - double accel_limit; // abs(limiting_accel_value) == m/s^2 -}; - -struct AccelerationPhaseResults -{ - double time_required; - double distance_required; -}; - -AccelerationPhaseResults get_acceleration_phase_results(const AccelerationPhaseInputs inputs) -{ - AccelerationPhaseResults output; - output.time_required = time_to_accelerate(inputs.start_vel, inputs.end_vel, inputs.accel_limit); - output.distance_required = dist_covered_during_acceleration( - inputs.start_vel, inputs.end_vel, - inputs.accel_limit); - - return output; -} - -struct PhaseCoeffs -{ - double start_time; - double start_dist; - double start_vel; - double duration; - double end_vel; -}; - -struct TrapezoidalCoeffs -{ - // __________ - // / \. - // / \. - // / \. - // ^^^ - // Phase 1 - // ^^^^^^^^^^ - // Phase 2 - // ^^^ - // Phase 3 - // - // Phase 1 - Accel to some "max" velocity - // Phase 2 - Stay at some "max" velocity (optional) - // Phase 3 - Decell to the end velocity - PhaseCoeffs phase_1; - PhaseCoeffs phase_2; - PhaseCoeffs phase_3; -}; - -Trajectory1d sample_trapezoidal_profile( - const TrapezoidalCoeffs coeffs, const double accel_limit, - const double dt) -{ - const auto position = [](const double t, const double v_initial, const double a) { - return v_initial * t + 1. / 2. * a * std::pow(t, 2); - }; - const auto velocity = [](const double t, const double v_initial, const double a) { - return v_initial + a * t; - }; - - Trajectory1d output; - - const double end_time = coeffs.phase_3.start_time + coeffs.phase_3.duration; - for (double t = 0.0; t <= end_time; t += dt) { - Sample1d sample; - sample.time = t; - - if (t < coeffs.phase_2.start_time) { - // Phase 1 - double accel = std::copysign(accel_limit, coeffs.phase_1.end_vel - coeffs.phase_1.start_vel); - sample.pos = position(t, coeffs.phase_1.start_vel, accel); - sample.vel = velocity(t, coeffs.phase_1.start_vel, accel); - sample.accel = accel; - } else if (t < coeffs.phase_3.start_time) { - // Phase 2 - double t_into_phase_2 = t - coeffs.phase_2.start_time; - double d_into_phase_2 = coeffs.phase_2.start_dist; - sample.pos = d_into_phase_2 + position(t_into_phase_2, coeffs.phase_2.start_vel, 0); - sample.vel = velocity(t_into_phase_2, coeffs.phase_2.start_vel, 0); - sample.accel = 0; - } else { - // Phase 3 - double t_into_phase_3 = t - coeffs.phase_3.start_time; - double d_into_phase_3 = coeffs.phase_3.start_dist; - double accel = std::copysign(accel_limit, coeffs.phase_3.end_vel - coeffs.phase_3.start_vel); - sample.pos = d_into_phase_3 + - position(t_into_phase_3, coeffs.phase_3.start_vel, accel); - sample.vel = - velocity(t_into_phase_3, coeffs.phase_3.start_vel, accel); - sample.accel = accel; - } - - output.samples.push_back(sample); - } - - return output; -} - -Trajectory1d Generate1d( - double start_pos, double start_vel, double end_pos, double end_vel, - const double max_vel, const double max_accel, const double dt) -{ - // Catch for us already at the target causing the distance to travel - // being so small that it creates nan values - if (std::abs(start_pos - end_pos) < 1e-6 && std::abs(start_vel - end_vel) < 1e-6) { - Trajectory1d traj; - traj.samples.push_back(Sample1d{.time = 0, .pos = end_pos, .vel = end_vel, .accel = 0}); - return traj; - } - - // Invert so the start is always before the end - // and we only have to deal with positive values - bool invert = start_pos > end_pos; - if (invert) { - start_pos *= -1; - start_vel *= -1; - end_pos *= -1; - end_vel *= -1; - } - - // Note, distances are distance from start_pos internally - // The output will be shifted back into world coordinates - TrapezoidalCoeffs trapezoidal_coeffs; - trapezoidal_coeffs.phase_1.start_time = 0; - trapezoidal_coeffs.phase_1.start_dist = 0; - trapezoidal_coeffs.phase_1.start_vel = start_vel; - - // Phase 1 is start_vel to max_vel - const AccelerationPhaseInputs phase_1_to_vel_limit_input{ - .start_vel = start_vel, - .end_vel = max_vel, - .accel_limit = max_accel}; - - // Phase 3 is max_accel to end_vel - const AccelerationPhaseInputs phase_3_from_vel_limit_input{ - .start_vel = max_vel, - .end_vel = end_vel, - .accel_limit = max_accel}; - - const AccelerationPhaseResults phase_1_to_vel_limit = get_acceleration_phase_results( - phase_1_to_vel_limit_input); - const AccelerationPhaseResults phase_3_from_vel_limit = get_acceleration_phase_results( - phase_3_from_vel_limit_input); - - // Distanced covered by both accels - // Skip phase 2 if that distance is too big - // If it's too big, we need to shrink the max vel limit we accel/decel to/from - const double target_dist = end_pos - start_pos; // Distance to cover - const double dist_covered_by_accels = phase_1_to_vel_limit.distance_required + - phase_3_from_vel_limit.distance_required; - const bool skip_phase_two = dist_covered_by_accels > target_dist; - - if (!skip_phase_two) { - // When phase 2 is included, stay at max speed for X time - - // How far we need phase 2 to cover in distance - double d_covered_by_phase_2 = target_dist - dist_covered_by_accels; - - // Figure out how long that takes to cover that dist at max velocity - double time_at_max_vel = d_covered_by_phase_2 / max_vel; - - trapezoidal_coeffs.phase_1.duration = phase_1_to_vel_limit.time_required; - trapezoidal_coeffs.phase_1.end_vel = max_vel; - - trapezoidal_coeffs.phase_2.start_time = phase_1_to_vel_limit.time_required; - trapezoidal_coeffs.phase_2.start_dist = phase_1_to_vel_limit.distance_required; - trapezoidal_coeffs.phase_2.start_vel = max_vel; - trapezoidal_coeffs.phase_2.duration = time_at_max_vel; - trapezoidal_coeffs.phase_2.end_vel = max_vel; - - trapezoidal_coeffs.phase_3.start_time = trapezoidal_coeffs.phase_2.start_time + time_at_max_vel; - trapezoidal_coeffs.phase_3.start_dist = trapezoidal_coeffs.phase_2.start_dist + - d_covered_by_phase_2; - trapezoidal_coeffs.phase_3.start_vel = max_vel; - trapezoidal_coeffs.phase_3.duration = phase_3_from_vel_limit.time_required; - trapezoidal_coeffs.phase_3.end_vel = end_vel; - } else { - // Figure out the max vel needed to get the target dist - // Solve the above equations for max_velocity - // https://www.wolframalpha.com/input?i=solve+d+%3D+m*+%28m-+s%29+%2F+a%2B+s+*+%28m-+s%29+%2F+a%2B+m*+%28m-+e%29+%2F+a+%2B+e*+%28m-+e%29+%2F+a+for+m - - // max = max vel - // svel = start vel - // evel = end vel - // d = target_distance - - // ts = (max - svel) / max_accel - // te = (max - evel) / max_accel - - // avgs = (max + svel) * ts - // avge = (max + evel) * te - - // d = avgs + avge - // d = (max + svel) * ts + (max + evel) * te - // d = max * ts + svel * ts + max * te + evel * te - // d = max * (max - svel) / max_accel + svel * (max - svel) / max_accel + - // max * (max - evel) / max_accel + evel * (max - evel) / max_accel - // d * max_accel = max * (max - svel) + svel * (max - svel) + - // max * (max - evel) + evel * (max - evel) - // d * max_accel = max^2 - max * svel + svel * max - svel^2 + - // max^2 - max * evel + evel * max - evel^2 - // d * max_accel + svel^2 + evel^2 = 2 * max^2 - // (d * max_accel + svel^2 + evel^2) / 2 = max^2 - // sqrt((d * max_accel + svel^2 + evel^2) / 2) = +-max - - // Don't allow negative speed so we can just assume positive is only correct value - double nonlimit_max_vel = - sqrt((target_dist * max_accel + start_vel * start_vel + end_vel * end_vel) / 2); - - // Phase 1 is start_vel to nonlimit_max_vel - const AccelerationPhaseInputs phase_1_to_nonlimit_vel_input{ - .start_vel = start_vel, - .end_vel = nonlimit_max_vel, - .accel_limit = max_accel}; - - // Phase 3 is max_accel to end_vel - const AccelerationPhaseInputs phase_3_from_nonlimit_vel_input{ - .start_vel = nonlimit_max_vel, - .end_vel = end_vel, - .accel_limit = max_accel}; - - const AccelerationPhaseResults phase_1_to_nonlimit_vel = get_acceleration_phase_results( - phase_1_to_nonlimit_vel_input); - const AccelerationPhaseResults phase_3_from_nonlimit_vel = get_acceleration_phase_results( - phase_3_from_nonlimit_vel_input); - - trapezoidal_coeffs.phase_1.duration = phase_1_to_nonlimit_vel.time_required; - trapezoidal_coeffs.phase_1.end_vel = nonlimit_max_vel; - - // Skip phase 2 - trapezoidal_coeffs.phase_2.start_time = phase_1_to_nonlimit_vel.time_required; - trapezoidal_coeffs.phase_2.start_dist = phase_1_to_nonlimit_vel.distance_required; - trapezoidal_coeffs.phase_2.start_vel = nonlimit_max_vel; - trapezoidal_coeffs.phase_2.duration = 0; - trapezoidal_coeffs.phase_2.end_vel = nonlimit_max_vel; - - trapezoidal_coeffs.phase_3.start_time = trapezoidal_coeffs.phase_2.start_time; - trapezoidal_coeffs.phase_3.start_dist = trapezoidal_coeffs.phase_2.start_dist; - trapezoidal_coeffs.phase_3.start_vel = nonlimit_max_vel; - trapezoidal_coeffs.phase_3.duration = phase_3_from_nonlimit_vel.time_required; - trapezoidal_coeffs.phase_3.end_vel = end_vel; - } - - Trajectory1d output = sample_trapezoidal_profile(trapezoidal_coeffs, max_accel, dt); - - // Add offset start pos - std::for_each( - output.samples.begin(), - output.samples.end(), - [start_pos](Sample1d & sample) { - sample.pos += start_pos; - }); - - if (invert) { - for (auto & sample : output.samples) { - sample.pos *= -1; - sample.vel *= -1; - sample.accel *= -1; - } - } - - return output; -} -} // namespace TrapezoidalMotionProfile diff --git a/ateam_ai/src/trajectory_generation/trapezoidal_motion_profile.hpp b/ateam_ai/src/trajectory_generation/trapezoidal_motion_profile.hpp deleted file mode 100644 index 21c1c3bdd..000000000 --- a/ateam_ai/src/trajectory_generation/trapezoidal_motion_profile.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef TRAJECTORY_GENERATION__TRAPEZOIDAL_MOTION_PROFILE_HPP_ -#define TRAJECTORY_GENERATION__TRAPEZOIDAL_MOTION_PROFILE_HPP_ - -#include - -#include - -#include "types/trajectory.hpp" - -namespace TrapezoidalMotionProfile -{ -/** - * Generate a trapezoidal motion profile given the initial and final states - * and the vel/accel limits for 3 DOF - */ -Trajectory Generate3d( - const Eigen::Vector3d & start, const Eigen::Vector3d & start_vel, - const Eigen::Vector3d & end, const Eigen::Vector3d & end_vel, - const Eigen::Vector3d & max_vel_limits, - const Eigen::Vector3d & max_accel_limits, - const double dt, const double current_time); - -struct Sample1d -{ - double time; // T=0 is current time - double pos; - double vel; - double accel; -}; - -struct Trajectory1d -{ - std::vector samples; // First element is at start pos -}; - -/** - * Generate a 1d trapezoidal motion for a single dimension - */ -Trajectory1d Generate1d( - double start_pos, double start_vel, double end_pos, double end_vel, - const double max_vel, const double max_accel, const double dt); -} // namespace TrapezoidalMotionProfile - -#endif // TRAJECTORY_GENERATION__TRAPEZOIDAL_MOTION_PROFILE_HPP_ diff --git a/ateam_ai/src/types/ball.hpp b/ateam_ai/src/types/ball.hpp deleted file mode 100644 index 18f765fba..000000000 --- a/ateam_ai/src/types/ball.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - - -#ifndef TYPES__BALL_HPP_ -#define TYPES__BALL_HPP_ - -#include - -struct Ball -{ - Eigen::Vector2d pos; - Eigen::Vector2d vel; -}; - -#endif // TYPES__BALL_HPP_ diff --git a/ateam_ai/src/types/behavior_goal.hpp b/ateam_ai/src/types/behavior_goal.hpp deleted file mode 100644 index 7c8b0e066..000000000 --- a/ateam_ai/src/types/behavior_goal.hpp +++ /dev/null @@ -1,113 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef TYPES__BEHAVIOR_GOAL_HPP_ -#define TYPES__BEHAVIOR_GOAL_HPP_ - -#include - -#include -#include -#include - -struct KickParam -{ - Eigen::Vector2d target_location; - - explicit KickParam(Eigen::Vector2d target_location) - : target_location(target_location) {} -}; - -struct ReceiveParam -{ - Eigen::Vector2d receive_location; - Eigen::Vector2d target_location; - - ReceiveParam(Eigen::Vector2d receive_location, Eigen::Vector2d target_location) - : receive_location(receive_location), target_location(target_location) {} -}; - -struct ShotParam {}; - -struct ReceiveShotParam -{ - Eigen::Vector2d receive_location; - - explicit ReceiveShotParam(Eigen::Vector2d receive_location) - : receive_location(receive_location) {} -}; - -struct MoveParam -{ - Eigen::Vector2d target_location; - - explicit MoveParam(Eigen::Vector2d target_location) - : target_location(target_location) {} -}; - -struct CostParam -{ - std::function cost; - std::optional> gradient; -}; - -struct BehaviorGoal -{ - enum Type - { - // Straight kick - MovingKick, - PivotKick, - - // Receive + kick - OneTouchReceiveKick, - TwoTouchReceiveKick, - - // Kick + instantaneous target selection near kick time - Shot, - OneTouchShot, - - // Normal moves - MoveToPoint, - CostFunctionPoint - } type; - - // Priority enum must be decreasing in order of high priority to low priority - // Hard assumptions exist on the enum ordering of ints - enum Priority - { - Reserved, - Required, - Medium, - Low - } priority; - - std::size_t reserved_robot_id = 0; - - using Params = std::variant; - Params params; - - // TODO(jneiger): Add type<->param checking for consistency - BehaviorGoal(Type type, Priority priority, Params params, std::size_t reserved_robot_id = 0) - : type(type), priority(priority), params(params), reserved_robot_id(reserved_robot_id) {} -}; - -#endif // TYPES__BEHAVIOR_GOAL_HPP_ diff --git a/ateam_ai/src/types/behavior_plan.hpp b/ateam_ai/src/types/behavior_plan.hpp deleted file mode 100644 index 39228741b..000000000 --- a/ateam_ai/src/types/behavior_plan.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef TYPES__BEHAVIOR_PLAN_HPP_ -#define TYPES__BEHAVIOR_PLAN_HPP_ - -#include - -#include -#include -#include - -#include "types/trajectory.hpp" - -struct KickFeedback {}; -struct ReceiveFeedback {}; -struct ShotFeedback {}; -struct ReceiveShotFeedback {}; -struct MoveFeedback {}; -struct CostFeedback {}; - -struct BehaviorPlan -{ - std::optional assigned_robot_id; - Trajectory trajectory; - DribblerAndKickerBehavior dribbler_and_kicker_behavior; - - using SpecificFeedback = std::variant; - std::variant specific_feedback; -}; - -#endif // TYPES__BEHAVIOR_PLAN_HPP_ diff --git a/ateam_ai/src/types/field.hpp b/ateam_ai/src/types/field.hpp deleted file mode 100644 index 8cffe1bb7..000000000 --- a/ateam_ai/src/types/field.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef TYPES__FIELD_HPP_ -#define TYPES__FIELD_HPP_ - -#include -#include - -#include - -struct FieldSidedInfo -{ - // could be non axis aligned which was the whole point so didnt want to use our types yet - std::array goalie_box; - std::array goal; -}; -struct Field -{ - // we will definetly change the format of this at some point this is preliminary - // since we dont really have a geometry library yet - // all of this should be in our coordinate system - // There is a mirroring ros message of this type - float field_length; - float field_width; - float goal_width; - float goal_depth; - float boundary_width; - ateam_geometry::Circle center_circle; - std::array field_corners; - FieldSidedInfo ours; - FieldSidedInfo theirs; -}; - - -#endif // TYPES__FIELD_HPP_ diff --git a/ateam_ai/src/types/referee_info.hpp b/ateam_ai/src/types/referee_info.hpp deleted file mode 100644 index ca91e3f03..000000000 --- a/ateam_ai/src/types/referee_info.hpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - - -#ifndef TYPES__REFEREE_INFO_HPP_ -#define TYPES__REFEREE_INFO_HPP_ - -#include - -struct RefereeInfo -{ - int our_goalie_id; - int their_goalie_id; - ateam_common::GameStage current_game_stage; - ateam_common::GameCommand running_command; -}; - -#endif // TYPES__REFEREE_INFO_HPP_ diff --git a/ateam_ai/src/types/robot.hpp b/ateam_ai/src/types/robot.hpp deleted file mode 100644 index 29ff71885..000000000 --- a/ateam_ai/src/types/robot.hpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - - -#ifndef TYPES__ROBOT_HPP_ -#define TYPES__ROBOT_HPP_ - -#include - -struct Robot -{ - Eigen::Vector2d pos; - double theta; - Eigen::Vector2d vel; - double omega; -}; - -#endif // TYPES__ROBOT_HPP_ diff --git a/ateam_ai/src/types/trajectory.hpp b/ateam_ai/src/types/trajectory.hpp deleted file mode 100644 index 159fb5916..000000000 --- a/ateam_ai/src/types/trajectory.hpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef TYPES__TRAJECTORY_HPP_ -#define TYPES__TRAJECTORY_HPP_ - -#include - -#include - -struct Sample3d -{ - double time; - Eigen::Vector3d pose; - Eigen::Vector3d vel; - Eigen::Vector3d accel; -}; - -struct Trajectory -{ - std::vector samples; -}; -struct DribblerAndKickerBehavior {}; - -#endif // TYPES__TRAJECTORY_HPP_ diff --git a/ateam_ai/src/types/world.hpp b/ateam_ai/src/types/world.hpp deleted file mode 100644 index ada95959f..000000000 --- a/ateam_ai/src/types/world.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - - -#ifndef TYPES__WORLD_HPP_ -#define TYPES__WORLD_HPP_ - -#include -#include -#include - -#include "types/ball.hpp" -#include "types/field.hpp" -#include "types/referee_info.hpp" -#include "types/robot.hpp" -#include "types/trajectory.hpp" - -struct BehaviorExecutorState -{ - // Trajectory of each robot last frame - std::array, 16> previous_trajectories; -}; - -struct World -{ - const double immutable_duration = 0.1; // s - double current_time = 0.0; // s - - Field field; - RefereeInfo referee_info; - - std::optional get_unique_ball() const - { - if (balls.size() == 1) { - return balls.front(); - } else { - return std::nullopt; - } - } - - std::vector balls; - std::array, 16> our_robots; - std::array, 16> their_robots; - - std::array, 16> plan_from_our_robots; - - BehaviorExecutorState behavior_executor_state; -}; - -#endif // TYPES__WORLD_HPP_ diff --git a/ateam_ai/src/util/directed_graph.hpp b/ateam_ai/src/util/directed_graph.hpp deleted file mode 100644 index e92b542de..000000000 --- a/ateam_ai/src/util/directed_graph.hpp +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef UTIL__DIRECTED_GRAPH_HPP_ -#define UTIL__DIRECTED_GRAPH_HPP_ - -#include -#include -#include - -#include - -template -class DirectedGraph; - -/** - * Directed graph of nodes. The graph must not be looping - */ -template -class DirectedGraph -{ -public: - std::size_t add_node(Node node) - { - std::size_t node_idx = nodes.size(); - nodes.push_back(node); - root_nodes.push_back(node_idx); - parent_to_child_relationship[node_idx] = {}; - - return node_idx; - } - - std::size_t add_node(Node node, std::size_t parent_idx) - { - return add_node(node, std::vector{parent_idx}); - } - - std::size_t add_node(Node node, std::vector parent_idxs) - { - std::size_t child_idx = nodes.size(); - nodes.push_back(node); - parent_to_child_relationship[child_idx] = {}; - - for (auto parent_idx : parent_idxs) { - ATEAM_CHECK( - parent_to_child_relationship.count( - parent_idx) > 0, "Given parent ID does not exist in parent->child relationship"); - parent_to_child_relationship.at(parent_idx).push_back(child_idx); - } - - return child_idx; - } - - Node get_node(std::size_t node_idx) const - { - ATEAM_CHECK(node_idx < nodes.size(), "node_idx must be smaller than the number of nodes"); - return nodes.at(node_idx); - } - - std::vector get_root_nodes() const - { - return root_nodes; - } - - std::vector get_children(std::size_t node_idx) const - { - return parent_to_child_relationship.at(node_idx); - } - - // Self friend because template classes of different types are "difference classes" - // so we need to friend ourself so we can access the private members of all - // templated versions of the DirectedGraph from DirectedGraph - template - friend class DirectedGraph; - - template - DirectedGraph copy_shape_with_new_type(std::map new_nodes) const - { - DirectedGraph new_graph; - new_graph.root_nodes = root_nodes; - new_graph.parent_to_child_relationship = parent_to_child_relationship; - for (std::size_t i = 0; i < nodes.size(); i++) { - if (new_nodes.count(i) > 0) { - new_graph.nodes.push_back(new_nodes.at(i)); - } else { - new_graph.nodes.push_back(T()); - } - } - - return new_graph; - } - -private: - std::vector nodes; - std::vector root_nodes; - std::unordered_map> parent_to_child_relationship; -}; - - -#endif // UTIL__DIRECTED_GRAPH_HPP_ diff --git a/ateam_ai/src/util/message_conversions.cpp b/ateam_ai/src/util/message_conversions.cpp deleted file mode 100644 index 65698585d..000000000 --- a/ateam_ai/src/util/message_conversions.cpp +++ /dev/null @@ -1,142 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "util/message_conversions.hpp" - -#include - -#include - -namespace ateam_ai::message_conversions -{ - -ateam_msgs::msg::Sample3d toMsg(const Sample3d & obj) -{ - ateam_msgs::msg::Sample3d sample_3d_msg; - sample_3d_msg.time.sec = std::floor(obj.time); - sample_3d_msg.time.nanosec = std::floor((obj.time - std::floor(obj.time)) * 1e9); - sample_3d_msg.pose.position.x = obj.pose.x(); - sample_3d_msg.pose.position.y = obj.pose.y(); - sample_3d_msg.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), obj.pose.z())); - sample_3d_msg.twist.linear.x = obj.vel.x(); - sample_3d_msg.twist.linear.y = obj.vel.y(); - sample_3d_msg.twist.angular.z = obj.vel.z(); - sample_3d_msg.accel.linear.x = obj.accel.x(); - sample_3d_msg.accel.linear.y = obj.accel.y(); - sample_3d_msg.accel.angular.z = obj.accel.z(); - - return sample_3d_msg; -} - -ateam_msgs::msg::Trajectory toMsg(const Trajectory & obj) -{ - ateam_msgs::msg::Trajectory trajectory_msg; - for (const auto & sample : obj.samples) { - trajectory_msg.samples.push_back(toMsg(sample)); - } - - return trajectory_msg; -} - -ateam_msgs::msg::BehaviorExecutorState toMsg(const BehaviorExecutorState & obj) -{ - ateam_msgs::msg::BehaviorExecutorState behavior_executor_state_msg; - - for (const auto & maybe_trajectory : obj.previous_trajectories) { - if (maybe_trajectory.has_value()) { - behavior_executor_state_msg.previous_trajectories.push_back(toMsg(maybe_trajectory.value())); - } else { - behavior_executor_state_msg.previous_trajectories.push_back(ateam_msgs::msg::Trajectory()); - } - } - - return behavior_executor_state_msg; -} - -ateam_msgs::msg::BallState toMsg(const Ball & obj) -{ - ateam_msgs::msg::BallState ball_state_msg; - ball_state_msg.pose.position.x = obj.pos.x(); - ball_state_msg.pose.position.y = obj.pos.y(); - ball_state_msg.twist.linear.x = obj.vel.x(); - ball_state_msg.twist.linear.y = obj.vel.y(); - - return ball_state_msg; -} - -ateam_msgs::msg::RobotState toMsg(const Robot & obj) -{ - ateam_msgs::msg::RobotState robot_state_msg; - robot_state_msg.pose.position.x = obj.pos.x(); - robot_state_msg.pose.position.y = obj.pos.y(); - robot_state_msg.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), obj.theta)); - robot_state_msg.twist.linear.x = obj.vel.x(); - robot_state_msg.twist.linear.y = obj.vel.y(); - robot_state_msg.twist.angular.z = obj.omega; - - return robot_state_msg; -} - -ateam_msgs::msg::World toMsg(const World & obj) -{ - ateam_msgs::msg::World world_msg; - - world_msg.current_time.sec = std::floor(obj.current_time); - world_msg.current_time.nanosec = std::floor( - (obj.current_time - std::floor( - obj.current_time)) * 1e9); - - // world_msg.field = toMsg(obj.field) - // world_msg.referee_info = toMsg(obj.referee_info) - - world_msg.behavior_executor_state = toMsg(obj.behavior_executor_state); - - for (const auto & ball : obj.balls) { - world_msg.balls.push_back(toMsg(ball)); - } - - for (const auto & maybe_robot : obj.our_robots) { - if (maybe_robot.has_value()) { - world_msg.our_robots.push_back(toMsg(maybe_robot.value())); - } else { - world_msg.our_robots.push_back(ateam_msgs::msg::RobotState()); - } - } - - for (const auto & maybe_robot : obj.their_robots) { - if (maybe_robot.has_value()) { - world_msg.their_robots.push_back(toMsg(maybe_robot.value())); - } else { - world_msg.their_robots.push_back(ateam_msgs::msg::RobotState()); - } - } - - for (const auto & maybe_robot : obj.plan_from_our_robots) { - if (maybe_robot.has_value()) { - world_msg.plan_from_our_robots.push_back(toMsg(maybe_robot.value())); - } else { - world_msg.plan_from_our_robots.push_back(ateam_msgs::msg::RobotState()); - } - } - - return world_msg; -} - -} // namespace ateam_ai::message_conversions diff --git a/ateam_ai/src/util/message_conversions.hpp b/ateam_ai/src/util/message_conversions.hpp deleted file mode 100644 index 1e47a59cd..000000000 --- a/ateam_ai/src/util/message_conversions.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - - -#ifndef UTIL__MESSAGE_CONVERSIONS_HPP_ -#define UTIL__MESSAGE_CONVERSIONS_HPP_ - -#include -#include -#include -#include -#include - -#include "types/world.hpp" - -namespace ateam_ai::message_conversions -{ - -ateam_msgs::msg::Sample3d toMsg(const Sample3d & obj); -ateam_msgs::msg::Trajectory toMsg(const Trajectory & obj); -ateam_msgs::msg::BehaviorExecutorState toMsg(const BehaviorExecutorState & obj); - -ateam_msgs::msg::BallState toMsg(const Ball & obj); -ateam_msgs::msg::RobotState toMsg(const Robot & obj); - -ateam_msgs::msg::World toMsg(const World & obj); - -} // namespace ateam_ai::message_conversions - -#endif // UTIL__MESSAGE_CONVERSIONS_HPP_ diff --git a/ateam_ai/src/util/pid.hpp b/ateam_ai/src/util/pid.hpp deleted file mode 100644 index 344a4f863..000000000 --- a/ateam_ai/src/util/pid.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef UTIL__PID_HPP_ -#define UTIL__PID_HPP_ - -#include - -class PID -{ -public: - void set_kp(double kp) - { - this->kp = kp; - } - - void set_ki(double ki) - { - this->ki = ki; - } - - void set_kd(double kd) - { - this->kd = kd; - } - - double execute(double target, double current, bool is_angle = false) - { - if (!is_angle) { - double error = target - current; - return kp * error; - } else { - double error = angles::shortest_angular_distance(current, target); - return kp * error; - } - } - -private: - double kp = 1; - double ki = 0; - double kd = 0; -}; - -#endif // UTIL__PID_HPP_ diff --git a/ateam_ai/src/util/viz.hpp b/ateam_ai/src/util/viz.hpp deleted file mode 100644 index 4a92ade7e..000000000 --- a/ateam_ai/src/util/viz.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef UTIL__VIZ_HPP_ -#define UTIL__VIZ_HPP_ - -#include -#include - -#include - -#include "types/trajectory.hpp" - -namespace viz -{ -void DrawTrajectory(const int robot_id, const Trajectory & trajectory) -{ - if (trajectory.samples.size() <= 1) { - return; - } - - std::vector line_pts; - for (const auto & sample : trajectory.samples) { - line_pts.push_back(Eigen::Vector2d{sample.pose.x(), sample.pose.y()}); - } - - ateam_common::Overlay::GetOverlay().DrawLine(line_pts, "trajectory_" + std::to_string(robot_id)); -} - -} // namespace viz - -#endif // UTIL__VIZ_HPP_ diff --git a/ateam_ai/test/behavior/behavior_realization_test.cpp b/ateam_ai/test/behavior/behavior_realization_test.cpp deleted file mode 100644 index a419ffdaa..000000000 --- a/ateam_ai/test/behavior/behavior_realization_test.cpp +++ /dev/null @@ -1,251 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "behavior/behavior_realization.hpp" - -#include - -BehaviorGoal create_behavior( - BehaviorGoal::Priority priority, - Eigen::Vector2d target = Eigen::Vector2d{0, 0}) -{ - return BehaviorGoal(BehaviorGoal::Type::MoveToPoint, priority, MoveParam(target)); -} - -World create_world_with_x_robots(int num_robots) -{ - World w; - for (int i = 0; i < num_robots; i++) { - w.our_robots.at(i) = Robot(); - } - return w; -} - -BehaviorPlan create_behavior_plan_with_t_end(double t_end) -{ - BehaviorPlan bp; - bp.trajectory.samples.push_back(Sample3d{.time = t_end}); - - return bp; -} - -TEST(BehaviorRealization, get_priority_to_assignment_group_ReturnEmpty_WhenNoBehaviors) -{ - BehaviorRealization realization; - DirectedGraph behaviors; - - BehaviorRealization::PriorityGoalListMap ret = realization.get_priority_to_assignment_group( - behaviors); - - ASSERT_EQ(ret.size(), 4); - EXPECT_TRUE(ret.at(BehaviorGoal::Priority::Required).empty()); - EXPECT_TRUE(ret.at(BehaviorGoal::Priority::Medium).empty()); - EXPECT_TRUE(ret.at(BehaviorGoal::Priority::Low).empty()); -} - -TEST(BehaviorRealization, get_priority_to_assignment_group_ReturnRequired_WhenOnlyRootRequired) -{ - BehaviorRealization realization; - DirectedGraph behaviors; - BehaviorRealization::BehaviorGoalNodeIdx beh1 = - behaviors.add_node(create_behavior(BehaviorGoal::Required)); - BehaviorRealization::BehaviorGoalNodeIdx beh2 = - behaviors.add_node(create_behavior(BehaviorGoal::Required)); - - BehaviorRealization::PriorityGoalListMap ret = realization.get_priority_to_assignment_group( - behaviors); - - ASSERT_EQ(ret.size(), 4); - ASSERT_EQ(ret.at(BehaviorGoal::Priority::Required).size(), 2); - EXPECT_EQ(ret.at(BehaviorGoal::Priority::Required).at(0), beh1); - EXPECT_EQ(ret.at(BehaviorGoal::Priority::Required).at(1), beh2); - EXPECT_TRUE(ret.at(BehaviorGoal::Priority::Medium).empty()); - EXPECT_TRUE(ret.at(BehaviorGoal::Priority::Low).empty()); -} - -TEST(BehaviorRealization, get_priority_to_assignment_group_ReturnRequired_WhenOnlyNestedRequired) -{ - BehaviorRealization realization; - DirectedGraph behaviors; - BehaviorRealization::BehaviorGoalNodeIdx beh1 = behaviors.add_node( - create_behavior(BehaviorGoal::Required)); - BehaviorRealization::BehaviorGoalNodeIdx beh2 = behaviors.add_node( - create_behavior(BehaviorGoal::Required), beh1); - - BehaviorRealization::PriorityGoalListMap ret = realization.get_priority_to_assignment_group( - behaviors); - - ASSERT_EQ(ret.size(), 4); - ASSERT_EQ(ret.at(BehaviorGoal::Priority::Required).size(), 2); - EXPECT_EQ(ret.at(BehaviorGoal::Priority::Required).at(0), beh1); - EXPECT_EQ(ret.at(BehaviorGoal::Priority::Required).at(1), beh2); - EXPECT_TRUE(ret.at(BehaviorGoal::Priority::Medium).empty()); - EXPECT_TRUE(ret.at(BehaviorGoal::Priority::Low).empty()); -} - -TEST(BehaviorRealization, get_priority_to_assignment_group_ReturnAll_WhenAll) -{ - BehaviorRealization realization; - DirectedGraph behaviors; - BehaviorRealization::BehaviorGoalNodeIdx beh1 = behaviors.add_node( - create_behavior(BehaviorGoal::Required)); - BehaviorRealization::BehaviorGoalNodeIdx beh2 = behaviors.add_node( - create_behavior(BehaviorGoal::Required), beh1); - BehaviorRealization::BehaviorGoalNodeIdx beh3 = behaviors.add_node( - create_behavior(BehaviorGoal::Medium)); - BehaviorRealization::BehaviorGoalNodeIdx beh4 = behaviors.add_node( - create_behavior(BehaviorGoal::Medium), beh3); - BehaviorRealization::BehaviorGoalNodeIdx beh5 = behaviors.add_node( - create_behavior(BehaviorGoal::Low)); - BehaviorRealization::BehaviorGoalNodeIdx beh6 = behaviors.add_node( - create_behavior(BehaviorGoal::Low), beh5); - - BehaviorRealization::PriorityGoalListMap ret = realization.get_priority_to_assignment_group( - behaviors); - - ASSERT_EQ(ret.size(), 4); - ASSERT_EQ(ret.at(BehaviorGoal::Priority::Required).size(), 2); - EXPECT_EQ(ret.at(BehaviorGoal::Priority::Required).at(0), beh1); - EXPECT_EQ(ret.at(BehaviorGoal::Priority::Required).at(1), beh2); - ASSERT_EQ(ret.at(BehaviorGoal::Priority::Medium).size(), 2); - EXPECT_EQ(ret.at(BehaviorGoal::Priority::Medium).at(0), beh3); - EXPECT_EQ(ret.at(BehaviorGoal::Priority::Medium).at(1), beh4); - ASSERT_EQ(ret.at(BehaviorGoal::Priority::Low).size(), 2); - EXPECT_EQ(ret.at(BehaviorGoal::Priority::Low).at(0), beh5); - EXPECT_EQ(ret.at(BehaviorGoal::Priority::Low).at(1), beh6); -} - -TEST(BehaviorRealization, get_available_robots_ReturnX_WhenX) -{ - for (int i = 0; i < 16; i++) { - BehaviorRealization realization; - World w = create_world_with_x_robots(i); - - std::set ret = realization.get_available_robots(w); - - ASSERT_EQ(ret.size(), i); - for (int j = 0; j < 16; j++) { - EXPECT_EQ(ret.count(j) > 0, j < i ? 1 : 0); - } - } -} - -TEST(BehaviorRealization, get_available_robots_ReturnsCorrectMapping_WhenNonZero) -{ - BehaviorRealization realization; - DirectedGraph behaviors; - std::vector idxs_to_plan; - for (int i = 0; i < 16; i++) { - behaviors.add_node(create_behavior(BehaviorGoal::Required)); - idxs_to_plan.push_back(i); - } - - std::set available_robots; - for (int i = 0; i < 16; i++) { - available_robots.insert(i); - } - - std::map count_of_calls; - for (int i = 0; i < 16; i++) { - count_of_calls[i] = 0; - } - auto plan_fnc = [&count_of_calls](BehaviorGoal goal, int robot_id, const World & world) { - count_of_calls.at(robot_id)++; - return BehaviorPlan(); - }; - - BehaviorRealization::CandidatePlans ret = realization.generate_candidate_plans( - idxs_to_plan, available_robots, behaviors, create_world_with_x_robots(16), plan_fnc); - - for (auto call_count_pair : count_of_calls) { - EXPECT_EQ(call_count_pair.second, idxs_to_plan.size()); - } -} - -TEST(BehaviorRealization, assign_goals_to_plans_ReturnEmpty_WhenEmpty) { - BehaviorRealization realization; - - BehaviorRealization::GoalToPlanMap ret = realization.assign_goals_to_plans( - {}, {}, {{}}, World()); - - EXPECT_EQ(ret.size(), 0); -} - -TEST(BehaviorRealization, assign_goals_to_plans_ReturnCorrect_WithFakePlans) { - BehaviorRealization realization; - std::vector goals_to_assign{0, 2}; - std::set available_robots{1, 3}; - BehaviorRealization::CandidatePlans candidate_plans; - candidate_plans[1][0] = create_behavior_plan_with_t_end(100); - candidate_plans[1][2] = create_behavior_plan_with_t_end(10); - candidate_plans[3][0] = create_behavior_plan_with_t_end(11); - candidate_plans[3][2] = create_behavior_plan_with_t_end(100); - World world; - world.current_time = 0.0; - - BehaviorRealization::GoalToPlanMap ret = realization.assign_goals_to_plans( - goals_to_assign, - available_robots, - candidate_plans, - world); - - EXPECT_EQ(ret.size(), 2); - EXPECT_EQ(ret.at(0).trajectory.samples.front().time, 11); - EXPECT_EQ(ret.at(0).assigned_robot_id.value(), 3); - EXPECT_EQ(ret.at(2).trajectory.samples.front().time, 10); - EXPECT_EQ(ret.at(2).assigned_robot_id.value(), 1); -} - -TEST(BehaviorRealization, realize_behaviors_ReturnCorrect_WithRealisticInput) { - BehaviorRealization realization; - DirectedGraph behaviors; - BehaviorRealization::BehaviorGoalNodeIdx beh1 = - behaviors.add_node(create_behavior(BehaviorGoal::Required, Eigen::Vector2d{0, 0})); - BehaviorRealization::BehaviorGoalNodeIdx beh2 = - behaviors.add_node(create_behavior(BehaviorGoal::Required, Eigen::Vector2d{10, 0}), beh1); - BehaviorRealization::BehaviorGoalNodeIdx beh3 = - behaviors.add_node(create_behavior(BehaviorGoal::Medium, Eigen::Vector2d{20, 0})); - BehaviorRealization::BehaviorGoalNodeIdx beh4 = - behaviors.add_node(create_behavior(BehaviorGoal::Low, Eigen::Vector2d{30, 0})); - - World world = create_world_with_x_robots(3); - world.current_time = 0.0; - world.our_robots.at(0).value().pos = Eigen::Vector2d{30, 1}; - world.our_robots.at(1).value().pos = Eigen::Vector2d{20, 1}; - world.our_robots.at(2).value().pos = Eigen::Vector2d{19, 1}; - - auto get_plan_from_goal = [](BehaviorGoal bg, int id, const World & world) { - // Trajectory time is directly related to the position different - double diff = (std::get(bg.params).target_location - world.our_robots.at( - id).value().pos).norm(); - return create_behavior_plan_with_t_end(diff); - }; - - DirectedGraph plans = realization.realize_behaviors_impl( - behaviors, world, - get_plan_from_goal); - ASSERT_TRUE(plans.get_node(beh1).assigned_robot_id.has_value()); - ASSERT_TRUE(plans.get_node(beh2).assigned_robot_id.has_value()); - ASSERT_TRUE(plans.get_node(beh3).assigned_robot_id.has_value()); - EXPECT_EQ(plans.get_node(beh1).assigned_robot_id.value(), 2); - EXPECT_EQ(plans.get_node(beh2).assigned_robot_id.value(), 1); - EXPECT_EQ(plans.get_node(beh3).assigned_robot_id.value(), 0); - EXPECT_FALSE(plans.get_node(beh4).assigned_robot_id.has_value()); -} diff --git a/ateam_ai/test/trajectory_generation/b_spline_test.cpp b/ateam_ai/test/trajectory_generation/b_spline_test.cpp deleted file mode 100644 index 787d831fc..000000000 --- a/ateam_ai/test/trajectory_generation/b_spline_test.cpp +++ /dev/null @@ -1,263 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "trajectory_generation/b_spline.hpp" - -#include - -#include - -TEST(b_spline, apply_multiplicity_ShouldRepeat_2point) -{ - std::vector knot_points{0, 1}; - - auto ret = BSpline::apply_multiplicity(knot_points, 3); - - EXPECT_EQ(ret.size(), 6); - for (int i = 0; i < 3; i++) { - EXPECT_EQ(ret.at(i), knot_points.front()); - } - for (int i = 3; i < 6; i++) { - EXPECT_EQ(ret.at(i), knot_points.back()); - } -} - -TEST(b_spline, constant_spacing_ShouldReturn1_2point) -{ - std::vector control_points{ - Eigen::Vector2d{0, 0}, - Eigen::Vector2d{1, 1} - }; - - auto out = BSpline::constant_spacing(control_points); - - EXPECT_EQ(out.size(), 2); - EXPECT_EQ(out.front(), 0.0); - EXPECT_EQ(out.back(), 1.0); -} - -TEST(b_spline, constant_spacing_ShouldReturnHalf_3point) -{ - std::vector control_points{ - Eigen::Vector2d{0, 0}, - Eigen::Vector2d{1, 1}, - Eigen::Vector2d{2, 2} - }; - - auto out = BSpline::constant_spacing(control_points); - - EXPECT_EQ(out.size(), 3); - EXPECT_EQ(out.front(), 0.0); - EXPECT_EQ(out.at(1), 0.5); - EXPECT_EQ(out.back(), 1.0); -} - -TEST(b_spline, chord_length_ShouldReturnExample_4point) -{ - // Example taken from https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/INT-APP/PARA-chord-length.html - std::vector control_points{ - Eigen::Vector2d{0, 0}, - Eigen::Vector2d{1, 2}, - Eigen::Vector2d{3, 4}, - Eigen::Vector2d{4, 0} - }; - - auto out = BSpline::chord_length_parametrization_spacing(control_points); - - EXPECT_EQ(out.size(), 4); - EXPECT_EQ(out.front(), 0.0); - EXPECT_NEAR(out.at(1), 0.2434, 1e-3); - EXPECT_NEAR(out.at(2), 0.5512, 1e-3); - EXPECT_NEAR(out.back(), 1.0, 1e-3); -} - -TEST(b_spline, centripetal_ShouldReturnExample_4point) -{ - // Example taken from https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/INT-APP/PARA-chord-length.html - std::vector control_points{ - Eigen::Vector2d{0, 0}, - Eigen::Vector2d{1, 2}, - Eigen::Vector2d{3, 4}, - Eigen::Vector2d{4, 0} - }; - - auto out = BSpline::centripetal_spacing(control_points); - - EXPECT_EQ(out.size(), 4); - EXPECT_EQ(out.front(), 0.0); - EXPECT_NEAR(out.at(1), 0.2871, 1e-3); - EXPECT_NEAR(out.at(2), 0.6101, 1e-3); - EXPECT_NEAR(out.back(), 1.0, 1e-3); -} - -TEST(b_spline, basis_function_ShouldHalfUSq_WhenBetween01) -{ - // Example from https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/bspline-basis.html - double u = 0.5; - std::vector knot_sequence{0, 1, 2, 3}; - auto out = BSpline::basis_function(0, 2, u, knot_sequence); - - EXPECT_NEAR(out, 0.5 * u * u, 1e-3); -} - -TEST(b_spline, basis_function_ShouldWack_WhenBetween12) -{ - // Example from https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/bspline-basis.html - double u = 1.5; - std::vector knot_sequence{0, 1, 2, 3}; - auto out = BSpline::basis_function(0, 2, u, knot_sequence); - - EXPECT_NEAR(out, 0.5 * (-3 + 6 * u - 2 * u * u), 1e-3); -} - -TEST(b_spline, basis_function_ShouldWack_WhenBetween23) -{ - // Example from https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/bspline-basis.html - double u = 2.5; - std::vector knot_sequence{0, 1, 2, 3}; - auto out = BSpline::basis_function(0, 2, u, knot_sequence); - - EXPECT_NEAR(out, 0.5 * (3 - u) * (3 - u), 1e-3); -} - -TEST(b_spline, basis_function_ShouldZero_WhenMultiple) -{ - // Example from https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/bspline-basis.html - double u = 0.5; - std::vector knot_sequence{0, 0, 0, 1}; - auto out = BSpline::basis_function(0, 0, u, knot_sequence); - - EXPECT_NEAR(out, 0.0, 1e-3); -} - -TEST(b_spline, knot_points_ShouldCircle_WhenOverlaping) -{ - // Example from https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/bspline-curve-closed.html - std::size_t p = 0; - std::size_t n = 20; - std::size_t m = n + p + 1; - std::vector control_points; - std::vector knot_sequence; - for (int i = 0; i < n + 1; i++) { - double percent_circle = static_cast(i) / (n - p + 1); - double angle = 2 * M_PI * percent_circle; - control_points.push_back(Eigen::Vector2d{std::cos(angle), std::sin(angle)}); - } - - for (int i = 0; i < m + 1; i++) { - knot_sequence.push_back(static_cast(i) / m); - } - - - auto out = BSpline::knot_points(p, control_points, knot_sequence); - - ASSERT_EQ(out.size(), m + 1); - // Note: domain of curve is u_p to u_n-p - // This is because the spline is "open" (no duplicated end control points) - // which means the ends of the knot points don't have enough - // coverage by the basis functions - // https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/bspline-curve-open.html - for (int i = p; i < n - p + 1; i++) { - const auto point = out.at(i); - EXPECT_NEAR(point.norm(), 1, 1e-3); - } -} - -TEST(b_spline, de_boors_algorithm_ShouldExmaple_WhenNontrivial) -{ - // Example from https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/de-Boor.html - std::size_t p = 3; - Eigen::Vector2d P_0_0{0, 0}; - Eigen::Vector2d P_1_0{1, 0}; - Eigen::Vector2d P_2_0{2, 0}; - Eigen::Vector2d P_3_0{3, 0}; - Eigen::Vector2d P_4_0{4, 0}; - Eigen::Vector2d P_5_0{5, 0}; - Eigen::Vector2d P_6_0{6, 0}; - std::vector control_points{ - P_0_0, - P_1_0, - P_2_0, - P_3_0, - P_4_0, - P_5_0, - P_6_0 - }; - std::vector knot_sequence{0, 0, 0, 0, 0.25, 0.5, 0.75, 1, 1, 1, 1}; - - auto out = BSpline::de_boors_algorithm(0.4, p, control_points, knot_sequence); - - - Eigen::Vector2d P_4_1 = 0.8 * P_3_0 + 0.2 * P_4_0; - Eigen::Vector2d P_3_1 = 0.47 * P_2_0 + 0.53 * P_3_0; - Eigen::Vector2d P_2_1 = 0.2 * P_1_0 + 0.8 * P_2_0; - - Eigen::Vector2d P_4_2 = 0.7 * P_3_1 + 0.3 * P_4_1; - Eigen::Vector2d P_3_2 = 0.2 * P_2_1 + 0.8 * P_3_1; - - Eigen::Vector2d P_4_3 = 0.4 * P_3_2 + 0.6 * P_4_2; - - EXPECT_NEAR(out.x(), P_4_3.x(), 1e-2); - EXPECT_NEAR(out.y(), P_4_3.y(), 1e-2); -} - -TEST(b_spline, convert_to_spline) -{ - BSpline::Input i; - i.data_points = {{0, 0}, {1, 1}, {2, 2}, {3, 3}, {4, 4}, {5, 5}}; - i.initial_vel = {0, 0}; - i.end_vel = {0, 0}; - - auto out = BSpline::convert_to_spline(i); -} - -TEST(b_spline, sample_spline_ShouldReturnIncrease_When6Points) -{ - BSpline::Input i; - i.data_points = {{0, 0}, {1, 1}, {2, 2}, {3, 3}, {4, 4}, {5, 5}}; - i.initial_vel = {0, 0}; - i.end_vel = {0, 0}; - - auto out = BSpline::sample_spline(BSpline::convert_to_spline(i), 20); - - for (int i = 1; i < out.size(); i++) { - Eigen::Vector2d diff = out.at(i) - out.at(i - 1); - EXPECT_EQ(diff.x(), diff.y()); - EXPECT_GT(diff.x(), 0); - EXPECT_GT(diff.y(), 0); - } -} - -TEST(b_spline, build_and_sample_spline) -{ - BSpline::Input i; - i.data_points = {{0, 0}, {1, 1}, {2, 2}, {3, 3}, {4, 4}, {5, 5}}; - i.initial_vel = {1, 1}; - i.end_vel = {0, 0}; - i.max_accel = 1; - i.max_vel = 2; - - auto out = BSpline::build_and_sample_spline(i, 30); - - for (int i = 0; i < out.samples.size(); i++) { - std::cout << out.samples.at(i).p.x() << " " << out.samples.at(i).p.y() << " " << out.samples.at( - i).v << " " << out.samples.at(i).a << std::endl; - } -} diff --git a/ateam_ai/test/trajectory_generation/b_spline_wrapper_test.cpp b/ateam_ai/test/trajectory_generation/b_spline_wrapper_test.cpp deleted file mode 100644 index bafdd8ef5..000000000 --- a/ateam_ai/test/trajectory_generation/b_spline_wrapper_test.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "trajectory_generation/b_spline_wrapper.hpp" - -#include - -TEST(b_spline_wrapper, basic_test) -{ - std::vector waypoints{ - Eigen::Vector2d{0, 0}, - Eigen::Vector2d{5, 3}, - Eigen::Vector2d{6, 2}, - Eigen::Vector2d{15, 3}, - }; - auto out = BSplineWrapper::Generate( - waypoints, 0, 1, Eigen::Vector3d{0, 0, 0}, Eigen::Vector3d{0, - 0, 0}, Eigen::Vector3d{2, 2, 2}, Eigen::Vector3d{3, 3, 3}, - 0.1, 1); - - std::cout << "output" << std::endl; - for (int i = 0; i < out.samples.size(); i++) { - std::cout << out.samples.at(i).time << "\t" << out.samples.at(i).pose.x() << " " << - out.samples.at(i).pose.y() << " " << out.samples.at(i).pose.z() << "\t" << - out.samples.at(i).vel.x() << " " << out.samples.at(i).vel.y() << " " << - out.samples.at(i).vel.z() << std::endl; - } -} diff --git a/ateam_ai/test/trajectory_generation/ilqr_problem_test.cpp b/ateam_ai/test/trajectory_generation/ilqr_problem_test.cpp deleted file mode 100644 index bbaa7c752..000000000 --- a/ateam_ai/test/trajectory_generation/ilqr_problem_test.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "trajectory_generation/ilqr_problem.hpp" - -#include -#include - -constexpr int num_samples = 100; -constexpr double dt = 0.01; -class PointMass1dProblem : public iLQRProblem<2, 1, num_samples> -{ -public: - State dynamics(const State & x_t1, const Input & u_t1, Time t) override - { - Eigen::Matrix A; A << 0, 1, 0, 0; - Eigen::Matrix B; B << 0, 1; - - return x_t1 + dt * (A * x_t1 + B * u_t1); - } - - Cost cost(const State & x, const Input & u, Time t) override - { - constexpr double final_gain = 1e6; - if (t == num_samples - 1) { - Eigen::Vector2d target{10, 0}; - Eigen::Vector2d weights{1, 1}; - return final_gain * (target - x).dot(target - x); - } else { - return dt * u.dot(u); - } - } -}; - -TEST(iLQRProblem, SamplePointMass) -{ - PointMass1dProblem problem; - auto maybe_trajectory = problem.calculate(Eigen::Vector2d{0, 0}); - - ASSERT_TRUE(maybe_trajectory.has_value()); - EXPECT_NEAR(maybe_trajectory.value().back().x(), 10, 1e-1); - EXPECT_NEAR(maybe_trajectory.value().back().y(), 0, 1e-4); -} diff --git a/ateam_ai/test/trajectory_generation/trajectory_editor_test.cpp b/ateam_ai/test/trajectory_generation/trajectory_editor_test.cpp deleted file mode 100644 index 8abc9c927..000000000 --- a/ateam_ai/test/trajectory_generation/trajectory_editor_test.cpp +++ /dev/null @@ -1,170 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "trajectory_generation/trajectory_editor.hpp" - -#include - -TEST(trajectory_editor, crop_trajectory_ShouldSame_WhenBoundsInclude) -{ - Trajectory t; - for (double i = 0; i < 100; i++) { - t.samples.push_back({.time = i}); - } - - double start = -1; - double end = 100; - Trajectory ret = trajectory_editor::crop_trajectory(t, start, end); - - ASSERT_LE(start, ret.samples.front().time); - ASSERT_GE(end, ret.samples.back().time); -} - -TEST(trajectory_editor, crop_trajectory_ShouldLeftHalf_WhenBoundsPartial) -{ - Trajectory t; - for (double i = 0; i < 100; i++) { - t.samples.push_back({.time = i}); - } - - double start = -1; - double end = 50; - Trajectory ret = trajectory_editor::crop_trajectory(t, start, end); - - ASSERT_LE(start, ret.samples.front().time); - ASSERT_GE(end, ret.samples.back().time); -} - -TEST(trajectory_editor, crop_trajectory_ShouldRightHalf_WhenBoundsPartial) -{ - Trajectory t; - for (double i = 0; i < 100; i++) { - t.samples.push_back({.time = i}); - } - - double start = 25; - double end = 100; - Trajectory ret = trajectory_editor::crop_trajectory(t, start, end); - - ASSERT_LE(start, ret.samples.front().time); - ASSERT_GE(end, ret.samples.back().time); -} - -TEST(trajectory_editor, crop_trajectory_ShouldBoth_WhenBoundsPartial) -{ - Trajectory t; - for (double i = 0; i < 100; i++) { - t.samples.push_back({.time = i}); - } - - double start = 25; - double end = 50; - Trajectory ret = trajectory_editor::crop_trajectory(t, start, end); - - ASSERT_LE(start, ret.samples.front().time); - ASSERT_GE(end, ret.samples.back().time); -} - -TEST(trajectory_editor, crop_trajectory_ShouldNeither_WhenBoundsExclude) -{ - Trajectory t; - for (double i = 0; i < 100; i++) { - t.samples.push_back({.time = i}); - } - - double start = 125; - double end = 150; - Trajectory ret = trajectory_editor::crop_trajectory(t, start, end); - - ASSERT_TRUE(ret.samples.empty()); -} - -TEST(trajectory_editor, crop_trajectory_ShouldNothing_WhenNothingInput) -{ - Trajectory t; - - double start = 125; - double end = 150; - Trajectory ret = trajectory_editor::crop_trajectory(t, start, end); - - ASSERT_TRUE(ret.samples.empty()); -} - -TEST(trajectory_editor, append_trajectory_ShouldEmpty_WhenBothEmpty) -{ - Trajectory a; - Trajectory b; - - Trajectory ret = trajectory_editor::append_trajectory(a, b); - - ASSERT_TRUE(ret.samples.empty()); -} - -TEST(trajectory_editor, append_trajectory_ShouldA_WhenBEmpty) -{ - Trajectory a; - a.samples.push_back({.time = 0}); - Trajectory b; - - Trajectory ret = trajectory_editor::append_trajectory(a, b); - - ASSERT_EQ(ret.samples.size(), 1); -} - -TEST(trajectory_editor, append_trajectory_ShouldB_WhenAEmpty) -{ - Trajectory a; - Trajectory b; - b.samples.push_back({.time = 0}); - - Trajectory ret = trajectory_editor::append_trajectory(a, b); - - ASSERT_EQ(ret.samples.size(), 1); -} - -TEST(trajectory_editor, append_trajectory_ShouldBoth_WhenBothFilled) -{ - Trajectory a; - Trajectory b; - a.samples.push_back({.time = 0}); - b.samples.push_back({.time = 0}); - - Trajectory ret = trajectory_editor::append_trajectory(a, b); - - ASSERT_EQ(ret.samples.size(), 2); -} - -TEST(trajectory_editor, append_trajectory_ShouldIncreaseInTime_WhenBothNoneZero) -{ - Trajectory a; - Trajectory b; - a.samples.push_back({.time = -10}); - a.samples.push_back({.time = -9}); - b.samples.push_back({.time = 8}); - b.samples.push_back({.time = 9}); - - Trajectory ret = trajectory_editor::append_trajectory(a, b); - - ASSERT_EQ(ret.samples.size(), 4); - EXPECT_EQ(ret.samples.at(0).time, -10); - EXPECT_EQ(ret.samples.at(1).time, -9); - EXPECT_EQ(ret.samples.at(2).time, 8); - EXPECT_EQ(ret.samples.at(3).time, 9); -} diff --git a/ateam_ai/test/trajectory_generation/trapezoidal_motion_profile_test.cpp b/ateam_ai/test/trajectory_generation/trapezoidal_motion_profile_test.cpp deleted file mode 100644 index 7968b74a2..000000000 --- a/ateam_ai/test/trajectory_generation/trapezoidal_motion_profile_test.cpp +++ /dev/null @@ -1,275 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "trajectory_generation/trapezoidal_motion_profile.hpp" - -#include - -#include - -testing::AssertionResult IsValidTrajectory( - TrapezoidalMotionProfile::Trajectory1d trajectory, - double max_vel, double max_accel, double dt) -{ - // Pos difference matches vel - for (std::size_t i = 0; i < trajectory.samples.size() - 1; i++) { - const auto & cur_sample = trajectory.samples.at(i); - const auto & next_sample = trajectory.samples.at(i + 1); - double sample_vel = (next_sample.pos - cur_sample.pos) / dt; - double avg_vel = (cur_sample.vel + next_sample.vel) / 2; - if (std::abs(sample_vel - avg_vel) > max_vel + 1e-3) { - return testing::AssertionFailure() << sample_vel << " (" << next_sample.pos << - " - " << cur_sample.pos << ") pos difference doesn't match average vel " << - avg_vel << " at T=" << cur_sample.time; - } - } - - // Vel difference matches accel - for (std::size_t i = 0; i < trajectory.samples.size() - 1; i++) { - const auto & cur_sample = trajectory.samples.at(i); - const auto & next_sample = trajectory.samples.at(i + 1); - double sample_accel = (next_sample.vel - cur_sample.vel) / dt; - double avg_accel = (cur_sample.accel + next_sample.accel) / 2; - if (std::abs(sample_accel - avg_accel) > max_accel + 1e-3) { - return testing::AssertionFailure() << sample_accel << " (" << next_sample.vel << - " - " << cur_sample.vel << ") vel difference doesn't match avg accel " << - avg_accel << " at T=" << cur_sample.time; - } - } - - return testing::AssertionSuccess(); -} - -TEST(TrapezoidalMotionProfile, Trajectory1d_ShouldReturnValidTrajectory_WhenSimpleInput) -{ - double start = 0; - double start_vel = 0; - double end = 10; - double end_vel = 0; - double max_vel = 1; - double max_accel = 1; - double dt = 0.1; - - TrapezoidalMotionProfile::Trajectory1d ret = TrapezoidalMotionProfile::Generate1d( - start, - start_vel, - end, - end_vel, - max_vel, - max_accel, - dt); - - ASSERT_TRUE(IsValidTrajectory(ret, max_vel, max_accel, dt)); -} - -TEST(TrapezoidalMotionProfile, Trajectory1d_ShouldReturnValidTrajectory_WhenNegativeInput) -{ - double start = 10; - double start_vel = 0; - double end = 0; - double end_vel = 0; - double max_vel = 1; - double max_accel = 1; - double dt = 0.1; - - TrapezoidalMotionProfile::Trajectory1d ret = TrapezoidalMotionProfile::Generate1d( - start, - start_vel, - end, - end_vel, - max_vel, - max_accel, - dt); - - ASSERT_TRUE(IsValidTrajectory(ret, max_vel, max_accel, dt)); -} - -TEST(TrapezoidalMotionProfile, Trajectory1d_ShouldReturnValidTrajectory_WhenNegativeSpeed) -{ - double start = 0; - double start_vel = -1; - double end = 10; - double end_vel = 0; - double max_vel = 1; - double max_accel = 1; - double dt = 0.1; - - TrapezoidalMotionProfile::Trajectory1d ret = TrapezoidalMotionProfile::Generate1d( - start, - start_vel, - end, - end_vel, - max_vel, - max_accel, - dt); - - ASSERT_TRUE(IsValidTrajectory(ret, max_vel, max_accel, dt)); -} - -TEST(TrapezoidalMotionProfile, Trajectory1d_ShouldReturnValidTrajectory_WhenShort) -{ - double start = 0; - double start_vel = 0; - double end = 1; - double end_vel = 0; - double max_vel = 1; - double max_accel = 1; - double dt = 0.1; - - TrapezoidalMotionProfile::Trajectory1d ret = TrapezoidalMotionProfile::Generate1d( - start, - start_vel, - end, - end_vel, - max_vel, - max_accel, - dt); - - ASSERT_TRUE(IsValidTrajectory(ret, max_vel, max_accel, dt)); -} - -TEST(TrapezoidalMotionProfile, Trajectory1d_ShouldReturnValidTrajectory_WhenShortNegative) -{ - double start = 0; - double start_vel = -1; - double end = 0; - double end_vel = 0; - double max_vel = 1; - double max_accel = 1; - double dt = 0.1; - - TrapezoidalMotionProfile::Trajectory1d ret = TrapezoidalMotionProfile::Generate1d( - start, - start_vel, - end, - end_vel, - max_vel, - max_accel, - dt); - - ASSERT_TRUE(IsValidTrajectory(ret, max_vel, max_accel, dt)); -} - -TEST(TrapezoidalMotionProfile, Trajectory3d_ShouldReturnOneLong_WhenAlreadyAtTarget) -{ - Eigen::Vector3d start{10, 10, 1}; - Eigen::Vector3d end{10, 10, 1}; - Eigen::Vector3d start_vel{10, 10, 1}; - Eigen::Vector3d end_vel{10, 10, 1}; - Eigen::Vector3d max_vel{10, 10, 1}; - Eigen::Vector3d max_accel{10, 10, 1}; - double dt = 0.1; - double current_time = 1; - - Trajectory ret = TrapezoidalMotionProfile::Generate3d( - start, - start_vel, - end, - end_vel, - max_vel, - max_accel, - dt, - current_time); - - ASSERT_GT(ret.samples.size(), 0); - EXPECT_EQ(ret.samples.front().pose, start); - EXPECT_EQ(ret.samples.front().pose, end); - EXPECT_EQ(ret.samples.front().vel, start_vel); - EXPECT_EQ(ret.samples.front().vel, end_vel); - EXPECT_EQ(ret.samples.front().accel, Eigen::Vector3d::Zero()); -} - -TEST(TrapezoidalMotionProfile, Trajectory3d_ShouldGoShortAngle_WhenLargeNegToPos) -{ - Eigen::Vector3d start{10, 10, -3}; - Eigen::Vector3d end{11, 11, 3}; - Eigen::Vector3d start_vel{0, 0, 0}; - Eigen::Vector3d end_vel{0, 0, 0}; - Eigen::Vector3d max_vel{10, 10, 1}; - Eigen::Vector3d max_accel{10, 10, 1}; - double dt = 0.1; - double current_time = 1; - - Trajectory ret = TrapezoidalMotionProfile::Generate3d( - start, - start_vel, - end, - end_vel, - max_vel, - max_accel, - dt, - current_time); - - ASSERT_GT(ret.samples.size(), 1); - double prev_angle = start.z(); - bool has_wrapped_angle = false; - for (std::size_t i = 1; i < ret.samples.size(); i++) { - double cur_angle = ret.samples.at(i).pose.z(); - - // Wrap happens if prev angle is near -PI and cur angle is near PI - // for one frame - if (cur_angle > prev_angle && cur_angle > 3 && prev_angle < -3) { - EXPECT_FALSE(has_wrapped_angle); // Only wrap once - has_wrapped_angle = true; - } else { - EXPECT_LT(cur_angle, prev_angle); - } - prev_angle = cur_angle; - } -} - -TEST(TrapezoidalMotionProfile, Trajectory3d_ShouldGoShortAngle_WhenLargePosToNeg) -{ - Eigen::Vector3d start{10, 10, 3}; - Eigen::Vector3d end{11, 11, -3}; - Eigen::Vector3d start_vel{0, 0, 0}; - Eigen::Vector3d end_vel{0, 0, 0}; - Eigen::Vector3d max_vel{10, 10, 1}; - Eigen::Vector3d max_accel{10, 10, 1}; - double dt = 0.1; - double current_time = 1; - - Trajectory ret = TrapezoidalMotionProfile::Generate3d( - start, - start_vel, - end, - end_vel, - max_vel, - max_accel, - dt, - current_time); - - ASSERT_GT(ret.samples.size(), 1); - double prev_angle = start.z(); - bool has_wrapped_angle = false; - for (std::size_t i = 1; i < ret.samples.size(); i++) { - double cur_angle = ret.samples.at(i).pose.z(); - - // Wrap happens if perv angle is near PI and cur angle is near -PI - // for one frame - if (cur_angle < prev_angle && cur_angle < -3 && prev_angle > 3) { - EXPECT_FALSE(has_wrapped_angle); // Only wrap once - has_wrapped_angle = true; - } else { - EXPECT_GT(cur_angle, prev_angle); - } - prev_angle = cur_angle; - } -} diff --git a/ateam_ai/test/util/pid_test.cpp b/ateam_ai/test/util/pid_test.cpp deleted file mode 100644 index 90dd3f58f..000000000 --- a/ateam_ai/test/util/pid_test.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include - -#include "util/pid.hpp" - -TEST(PID, AngleControllerValid) -{ - PID pid; - EXPECT_NEAR(pid.execute(1, 0, true), 1, 1e-5); - EXPECT_NEAR(pid.execute(-1, 0, true), -1, 1e-5); - EXPECT_NEAR(pid.execute(-2, -1, true), -1, 1e-5); - EXPECT_NEAR(pid.execute(3, -3, true), -(2 * M_PI - 6), 1e-5); - EXPECT_NEAR(pid.execute(-3, 3, true), 2 * M_PI - 6, 1e-5); -} diff --git a/ateam_common/CMakeLists.txt b/ateam_common/CMakeLists.txt index 5afb19002..617a8a615 100644 --- a/ateam_common/CMakeLists.txt +++ b/ateam_common/CMakeLists.txt @@ -1,10 +1,6 @@ cmake_minimum_required(VERSION 3.5) project(ateam_common) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) @@ -18,18 +14,16 @@ find_package(angles REQUIRED) find_package(ssl_league_msgs REQUIRED) add_library(${PROJECT_NAME} SHARED - src/assignment.cpp src/bi_directional_udp.cpp src/cache_directory.cpp src/get_ip_addresses.cpp src/multicast_receiver.cpp - src/parameters.cpp - src/status.cpp src/game_controller_listener.cpp src/km_assignment.cpp src/time.cpp ) -set_target_properties(${PROJECT_NAME} PROPERTIES CXX_STANDARD 20) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_20) +target_compile_options(${PROJECT_NAME} PRIVATE -Werror -Wall -Wextra -Wpedantic) ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_components diff --git a/ateam_common/include/ateam_common/assignment.hpp b/ateam_common/include/ateam_common/assignment.hpp deleted file mode 100644 index 55e03281f..000000000 --- a/ateam_common/include/ateam_common/assignment.hpp +++ /dev/null @@ -1,145 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_COMMON__ASSIGNMENT_HPP_ -#define ATEAM_COMMON__ASSIGNMENT_HPP_ - -#include - -#include -#include - -namespace ateam_common -{ -namespace assignment -{ -/** - * Given a Matrix of N rows and M columns, where... - * * each of the N rows is a robot to assign - * * each of the M columns is a role to be assigned - * * each cell at (i,j) is the cost of robot i performing role j - * - * @return List of (i, j) mappings for the most optimial assignment - * - * @note Each role/robot will only be assigned at most once -*/ -std::unordered_map optimize_assignment( - const Eigen::MatrixXd & cost_matrix); - -namespace internal -{ -/** - * Implements actual algorithm steps in order -*/ -std::unordered_map optimize_assignment_impl( - const Eigen::MatrixXd & cost_matrix); - -// Unfilled data when squarizing the matrix will be set to this value -constexpr double UNFILLED_LARGE_VALUE = 1e10; - -// Marked zeros can have multiple types -enum ZerosType -{ - NONE = 0, - STARRED = 1, - PRIMED = 2 -}; - -struct CostMarkCovers -{ - Eigen::MatrixXd cost_matrix; // Valid at all times - Eigen::MatrixXi mark_matrix; // Valid after Step 3 - Eigen::VectorXi row_covers; // Valid after Step 4 - Eigen::VectorXi col_covers; // Valid after Step 4 -}; - -/** - * Given a non-square matrix, make the matrix square by adding the least number of rows/cols needed. - * All new cells will be filled with some "large" cost -*/ -Eigen::MatrixXd squarize_matrix(const Eigen::MatrixXd & matrix); - -/** - * @return True if there is an unique and optimial assignment -*/ -bool has_unique_assignments(const CostMarkCovers & cmc); - -/** - * For each row in the cost matrix, the min element is subtracted from every element in that row -*/ -void apply_step_1(CostMarkCovers & cmc); - -/** - * For each col in the cost matrix, the min element is subtracted from every element in that row -*/ -void apply_step_2(CostMarkCovers & cmc); - -/** - * Creates the mark matrix, each mark will be unique for that col/row - * * Only zeros are marked - * * From top left to bottom right, row-wise, each zero will be marked - * * Only 1 mark in each row and column is allowed -*/ -void apply_step_3(CostMarkCovers & cmc); - -/** - * Create the col covers for all starred zeros in the mark matrix - */ -void star_zero_cols(CostMarkCovers & cmc); - -/** - * Returns the matrix coordinates of the next uncovered zero - * If none is found, return -1, -1 -*/ -std::optional next_uncovered_zero(const CostMarkCovers & cmc); - -/** - * Returns the matrix coordinates of the zero type requested in the same row as the start location - * If none is found, return -1, -1 -*/ -std::optional find_zero_type_in_row( - const CostMarkCovers & cmc, - const Eigen::Vector2i & start, - const ZerosType & target_type); - -/** - * Returns the matrix coordinates of the zero type requested in the same col as the start location - * If none is found, return -1, -1 -*/ -std::optional find_zero_type_in_col( - const CostMarkCovers & cmc, - const Eigen::Vector2i & start, - const ZerosType & target_type); - -/** - * Using the mark matrix, optimize line coverage to as few lines as possible. - * Lines refer to the col/row covers -*/ -void apply_step_4(CostMarkCovers & cmc); - -/** - * Find the lowest uncovered value. Subtract this from every unmarked element and add it to every element covered by two lines. -*/ -void apply_step_5(CostMarkCovers & cmc); -} // namespace internal -} // namespace assignment -} // namespace ateam_common - -#endif // ATEAM_COMMON__ASSIGNMENT_HPP_ diff --git a/ateam_common/include/ateam_common/bi_directional_udp.hpp b/ateam_common/include/ateam_common/bi_directional_udp.hpp index 322d1a22b..c18e2c2a5 100644 --- a/ateam_common/include/ateam_common/bi_directional_udp.hpp +++ b/ateam_common/include/ateam_common/bi_directional_udp.hpp @@ -59,7 +59,7 @@ class BiDirectionalUDP std::array receive_buffer_; std::thread io_service_thread_; - void HandleUDPSendTo(const boost::system::error_code & error, std::size_t bytes_transferred); + void HandleUDPSendTo(const boost::system::error_code & error); void HandleUDPReceiveFrom(const boost::system::error_code & error, std::size_t bytes_transferred); }; diff --git a/ateam_common/include/ateam_common/logger.hpp b/ateam_common/include/ateam_common/logger.hpp deleted file mode 100644 index fd6ca143a..000000000 --- a/ateam_common/include/ateam_common/logger.hpp +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_COMMON__LOGGER_HPP_ -#define ATEAM_COMMON__LOGGER_HPP_ - -#include -#include -#include - -#include -#include - -/** - * @brief Sets up the global logger to redirect all logged output through ROS. - * - * Recommended use: - * Call this macro in the constructor of your node. - * @code{.cpp} - * SET_ROS_LOG_HANDLER("my_node"); - * @endcode - */ -#define SET_ROS_LOG_HANDLER(logger_name) \ - ateam_common::Logger::GetLogger().SetRosLogHandler(logger_name); - -/** - * @brief Logs INFO/WARN/ERROR/FATAL using the standard printf style format + args setup. The filename and line number are automatically added as a prefix. - */ -#define LOG_INFO(format, ...) \ - _LOG_LINE(ateam_common::Logger::Level::INFO, format, __VA_ARGS__); -#define LOG_WARN(format, ...) \ - _LOG_LINE(ateam_common::Logger::Level::WARN, format, __VA_ARGS__); -#define LOG_ERROR(format, ...) \ - _LOG_LINE(ateam_common::Logger::Level::ERROR, format, __VA_ARGS__); -#define LOG_FATAL(format, ...) \ - _LOG_LINE(ateam_common::Logger::Level::FATAL, format, __VA_ARGS__); - -#define _LOG_LINE(level, format, ...) \ - ateam_common::Logger::GetLogger().Log( \ - level, std::string("[%s:%d] ") + std::string( \ - format), __FILE__, __LINE__, __VA_ARGS__); - -namespace ateam_common -{ -class Logger -{ -public: - enum Level - { - INFO, - WARN, - ERROR, - FATAL - }; - - static Logger & GetLogger() - { - static Logger l; - return l; - } - - void SetRosLogHandler(const std::string & logger_name) - { - ros_logger = rclcpp::get_logger(logger_name); - } - - template - void Log(const Level & level, const std::string & format, Args && ... args) - { - if (ros_logger.has_value()) { - switch (level) { - case Level::INFO: - RCLCPP_INFO(ros_logger.value(), format.c_str(), std::forward(args)...); - break; - case Level::WARN: - RCLCPP_WARN(ros_logger.value(), format.c_str(), std::forward(args)...); - break; - case Level::ERROR: - RCLCPP_ERROR(ros_logger.value(), format.c_str(), std::forward(args)...); - break; - case Level::FATAL: - RCLCPP_FATAL(ros_logger.value(), format.c_str(), std::forward(args)...); - break; - } - } else { - // Support logging in non-ros contexts (like unit testing) - printf(format.c_str(), std::forward(args)...); - printf("\n\r"); - } - } - -private: - Logger() = default; - - std::optional ros_logger; -}; -} // namespace ateam_common - -#endif // ATEAM_COMMON__LOGGER_HPP_ diff --git a/ateam_common/include/ateam_common/overlay.hpp b/ateam_common/include/ateam_common/overlay.hpp deleted file mode 100644 index a1f3d96a3..000000000 --- a/ateam_common/include/ateam_common/overlay.hpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_COMMON__OVERLAY_HPP_ -#define ATEAM_COMMON__OVERLAY_HPP_ - -#include - -#include -#include -#include -#include - -#include - -namespace ateam_common -{ -class Overlay -{ -public: - static Overlay & GetOverlay() - { - static Overlay o; - return o; - } - - void SetOverlayPublishCallback(std::function cb) - { - this->cb = cb; - } - - void SetNamespace(const std::string & ns) - { - this->ns = ns; - } - - void DrawLine(const std::vector & line, const std::string name) - { - ateam_msgs::msg::Overlay overlay_msg; - overlay_msg.ns = ns; - overlay_msg.name = name; - overlay_msg.visible = true; - overlay_msg.type = ateam_msgs::msg::Overlay::LINE; - overlay_msg.command = ateam_msgs::msg::Overlay::REPLACE; - overlay_msg.position.x = 0; - overlay_msg.position.y = 0; - overlay_msg.stroke_color = "white"; - overlay_msg.lifetime = 100; - overlay_msg.stroke_width = 10; - overlay_msg.depth = 1; - - for (const auto & pt : line) { - geometry_msgs::msg::Point p; - p.x = pt.x(); - p.y = pt.y(); - overlay_msg.points.push_back(p); - } - - cb(overlay_msg); - } - -private: - std::function cb; - std::string ns = ""; -}; -} // namespace ateam_common - -#endif // ATEAM_COMMON__OVERLAY_HPP_ diff --git a/ateam_common/include/ateam_common/parameters.hpp b/ateam_common/include/ateam_common/parameters.hpp deleted file mode 100644 index 4c61cab45..000000000 --- a/ateam_common/include/ateam_common/parameters.hpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_COMMON__PARAMETERS_HPP_ -#define ATEAM_COMMON__PARAMETERS_HPP_ - -#include -#include -#include - -#include -#include - -using OnSetParamCBPtr = rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr; - -// CREATE_PARAM(double, "ilqr/constants/", kMaxIterations, 100); -// Must do outside a class -#define CREATE_PARAM(type, ros_namespace, param_name, default_value) \ - ateam_common::Parameter param_name(default_value, ros_namespace #param_name) - -// Must do first in node constructor -#define REGISTER_NODE_PARAMS(node_ptr) \ - static OnSetParamCBPtr param_cb_handler = \ - ateam_common::ParameterRegistry::Get().ApplyAllRegistrations(node_ptr); - -namespace ateam_common -{ -template -class Parameter; - -/** - * Keep track of all the parameters that exists. When connected to a ros node, - * registers those parameters and updates the specific parameters when they change - */ -class ParameterRegistry -{ -public: - // Registers a parameter with some fully qualified ros parameter name - void RegisterParam(const std::string & parameter_name, Parameter * parameter); - void RegisterParam(const std::string & parameter_name, Parameter * parameter); - void RegisterParam(const std::string & parameter_name, Parameter * parameter); - - // Registers all known params with the given ros node - // Do this first inside the constructor for a ros node to construct - // all the param bindings correctly as well as register the callbacks correctly - OnSetParamCBPtr ApplyAllRegistrations(rclcpp::Node * node); - - static ParameterRegistry & Get(); - -private: - ParameterRegistry() = default; - - // Normal ros node param callback - rcl_interfaces::msg::SetParametersResult NodeParamCallback( - const std::vector & parameters); - - std::unordered_map *> int_params; - std::unordered_map *> double_params; - std::unordered_map *> bool_params; -}; - -// Type of parameter that the user is exposed to -template -class Parameter -{ -public: - Parameter(const T & default_value, const std::string & ros_name) - : t_{default_value} - { - ParameterRegistry::Get().RegisterParam(ros_name, this); - } - - operator T() const { - return t_; - } - -private: - // Only allow the callback to set the value of the param after initialization - friend class ParameterRegistry; - void set(const T & new_value) - { - t_ = new_value; - } - - T t_; -}; - -} // namespace ateam_common - -#endif // ATEAM_COMMON__PARAMETERS_HPP_ diff --git a/ateam_common/include/ateam_common/status.hpp b/ateam_common/include/ateam_common/status.hpp deleted file mode 100644 index f9cdafff4..000000000 --- a/ateam_common/include/ateam_common/status.hpp +++ /dev/null @@ -1,154 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#ifndef ATEAM_COMMON__STATUS_HPP_ -#define ATEAM_COMMON__STATUS_HPP_ - -// Use backtrace so we get line numbers and files -#define BOOST_STACKTRACE_USE_ADDR2LINE - -#include -#include -#include - -#include -#include - -// Utilities to improve the error checking workflow -// -// We now have access to a ateam::Status or ateam::StatusOr type. -// Both are used as a return from a function to signal success or faliure -// -// Eg: -// ateam::Status log_to_console(std::string); -// or -// ateam::StatusOr sqrt(double); -// -// The sqrt function will then be able to return a failure if the input is invalid. -// Eg: -// ateam::StatusOr sqrt(double input) { -// ATEAM_SCHECK(input >= 0, "input must be non-negative"); -// return math.sqrt(input); -// } -// -// This will allow significant improvements to safety surrounding assumptions a function -// can ensure. - - -// From the user POV, we can simplify catching and throwing these exceptions. -// There are two main classes user functions, ATEAM_ASSIGN_OR_* and ATEAM_*CHECK -// -// ATEAM_ASSIGN_OR_RETURN(auto var, func_that_returns_statusor(), "failure string"); -// will set var to the result of the function if and only if, the function doesn't fail. -// If it fails, it will propegate the failure up the stack -// -// ATEAM_ASSIGN_OR_THROW(auto var, func_that_returns_statusor(), "failure string"); -// will set var to the result of the function if and only if, the function doesn't fail. -// If it fails, it will throw an exception -// -// ATEAM_SCHECK(condition, "failure string") will check that the condition is true. -// If the condition is false, it will return a status::Failure with the given failure string. -// -// ATEAM_CHECK(condition, "failure string") will check that the condition is true. -// If the condition is false, it will throw an exception with the given failure string. -// -// In most code, you will always use ATEAM_ASSIGN_OR_RETURN or ATEAM_SCHECK to propogate the -// errors correctly. - - -namespace ateam -{ - -class ErrorType -{ -public: - std::string cause; - std::stringstream stack_trace; - - ErrorType() {} - - explicit ErrorType(std::string cause) - : cause(cause) - { - stack_trace << boost::stacktrace::stacktrace() << std::endl; - } -}; - -std::ostream & operator<<(std::ostream & os, const ErrorType & error); - -using Status = BOOST_OUTCOME_V2_NAMESPACE::result; - -template -using StatusOr = BOOST_OUTCOME_V2_NAMESPACE::result; - -// Wrappers around success / failure so we can simplify the interface -Status Ok(); -Status Failure(std::string && s); - -template -StatusOr Ok(T && s) -{ - return BOOST_OUTCOME_V2_NAMESPACE::success(std::move(s)); -} -template -StatusOr Failure(std::string && s) -{ - return ErrorType(std::move(s)); -} - -#define ATEAM_ASSIGN_OR_RETURN(lhs, rhs, text) \ - lhs = ({auto __status_or_lhs = rhs; \ - if (!__status_or_lhs) { \ - return ateam::Failure(text); \ - } \ - __status_or_lhs.value()}); - -#define ATEAM_ASSIGN_OR_THROW(lhs, rhs, text) \ - lhs = ({auto __status_or_lhs = rhs; \ - if (!__status_or_lhs) { \ - std::stringstream s; \ - s << text << "\n" << __status_or_lhs.error() << std::endl; \ - std::cerr << s.str(); \ - throw s.str(); \ - } __status_or_lhs.value();}); - -#define ATEAM_SCHECK(x, text) \ - do { \ - auto __value = x; \ - if (!__value) { \ - return ateam::Failure(text); \ - } \ - } while (false); - -#define ATEAM_CHECK(x, text) \ - do { \ - auto __value = x; \ - if (!__value) { \ - std::stringstream s; \ - s << ateam::Failure(text).error() << std::endl; \ - std::cerr << s.str(); \ - throw s.str(); \ - } \ - } while (false); - -} // namespace ateam -#endif // ATEAM_COMMON__STATUS_HPP_ diff --git a/ateam_common/src/assignment.cpp b/ateam_common/src/assignment.cpp deleted file mode 100644 index a79f6eed2..000000000 --- a/ateam_common/src/assignment.cpp +++ /dev/null @@ -1,320 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "ateam_common/assignment.hpp" - -#include -#include - -#include - -namespace ateam_common::assignment -{ -std::unordered_map optimize_assignment( - const Eigen::MatrixXd & cost_matrix) -{ - if (cost_matrix.rows() > cost_matrix.cols()) { - auto transpose_ret = internal::optimize_assignment_impl(cost_matrix.transpose()); - std::unordered_map ret; - for (const auto & [key, value] : transpose_ret) { - ret[value] = key; - } - return ret; - } else { - return internal::optimize_assignment_impl(cost_matrix); - } -} - -std::unordered_map internal::optimize_assignment_impl( - const Eigen::MatrixXd & cost_matrix) -{ - CostMarkCovers cmc; - cmc.cost_matrix = squarize_matrix(cost_matrix); - - apply_step_1(cmc); - apply_step_2(cmc); - apply_step_3(cmc); - - do { - apply_step_4(cmc); - apply_step_5(cmc); - } while (!has_unique_assignments(cmc)); - - std::unordered_map assignment; - for (int i = 0; i < cmc.mark_matrix.rows(); i++) { - bool is_row_inbounds = i < cost_matrix.rows(); - if (!is_row_inbounds) { - continue; - } - - for (int j = 0; j < cmc.mark_matrix.cols(); j++) { - bool is_col_inbounds = j < cost_matrix.cols(); - if (!is_col_inbounds) { - continue; - } - - bool is_starred = cmc.mark_matrix(i, j) == ZerosType::STARRED; - if (is_starred) { - assignment[i] = j; - } - } - } - return assignment; -} - -namespace internal -{ -Eigen::MatrixXd squarize_matrix(const Eigen::MatrixXd & matrix) -{ - if (matrix.rows() == matrix.cols()) { - return matrix; - } - - std::size_t new_row_col = std::max(matrix.rows(), matrix.cols()); - Eigen::MatrixXd new_mat = - Eigen::MatrixXd::Constant(new_row_col, new_row_col, UNFILLED_LARGE_VALUE); - - new_mat.block(0, 0, matrix.rows(), matrix.cols()) = matrix; - - return new_mat; -} - -bool has_unique_assignments(const CostMarkCovers & cmc) -{ - // The count of marked rows and columns should be the size of the matrix - int num_marked = 0; - for (int i = 0; i < cmc.cost_matrix.rows(); i++) { - num_marked += cmc.row_covers(i); - num_marked += cmc.col_covers(i); - } - - return num_marked == cmc.cost_matrix.rows(); -} - -void apply_step_1(CostMarkCovers & cmc) -{ - Eigen::VectorXd min_coeff = cmc.cost_matrix.rowwise().minCoeff(); - for (int i = 0; i < cmc.cost_matrix.rows(); i++) { - cmc.cost_matrix.row(i) -= Eigen::MatrixXd::Constant(1, cmc.cost_matrix.cols(), min_coeff(i)); - } -} - -void apply_step_2(CostMarkCovers & cmc) -{ - Eigen::VectorXd min_coeff = cmc.cost_matrix.colwise().minCoeff(); - for (int i = 0; i < cmc.cost_matrix.cols(); i++) { - cmc.cost_matrix.col(i) -= Eigen::MatrixXd::Constant(cmc.cost_matrix.rows(), 1, min_coeff(i)); - } -} - -void apply_step_3(CostMarkCovers & cmc) -{ - cmc.row_covers = Eigen::VectorXi::Zero(cmc.cost_matrix.rows()); - cmc.col_covers = Eigen::VectorXi::Zero(cmc.cost_matrix.cols()); - cmc.mark_matrix = Eigen::MatrixXi::Zero(cmc.cost_matrix.rows(), cmc.cost_matrix.cols()); - - // Mark uncovered zeros one at a time - for (int i = 0; i < cmc.cost_matrix.rows(); i++) { - bool is_row_covered = cmc.row_covers(i) > 0; - if (is_row_covered) { - continue; - } - for (int j = 0; j < cmc.cost_matrix.cols(); j++) { - // Can't count zeros that are already covered - bool is_col_covered = cmc.col_covers(j) > 0; - if (is_col_covered) { - continue; - } - - bool is_zero = cmc.cost_matrix(i, j) == 0; - if (is_zero) { - cmc.mark_matrix(i, j) = ZerosType::STARRED; - cmc.row_covers(i) = 1; - cmc.col_covers(j) = 1; - break; - } - } - } -} - -void star_zero_cols(CostMarkCovers & cmc) -{ - cmc.col_covers = Eigen::VectorXi::Zero(cmc.mark_matrix.cols()); - Eigen::VectorXi marked_col_coeff = cmc.mark_matrix.colwise().maxCoeff(); - for (int j = 0; j < cmc.mark_matrix.cols(); j++) { - bool is_starred_zero = marked_col_coeff(j) == ZerosType::STARRED; - if (is_starred_zero) { - cmc.col_covers(j) = 1; - } - } -} - -std::optional next_uncovered_zero( - const CostMarkCovers & cmc) -{ - for (int i = 0; i < cmc.cost_matrix.rows(); i++) { - bool is_covered_row = cmc.row_covers(i) == 1; - if (is_covered_row) { - continue; - } - for (int j = 0; j < cmc.cost_matrix.cols(); j++) { - bool is_covered_col = cmc.col_covers(j) == 1; - if (is_covered_col) { - continue; - } - - bool is_zero = cmc.cost_matrix(i, j) == 0; - if (is_zero) { - return Eigen::Vector2i{i, j}; - } - } - } - return std::nullopt; -} - -std::optional find_zero_type_in_row( - const CostMarkCovers & cmc, - const Eigen::Vector2i & start, - const ZerosType & target_type) -{ - for (int j = 0; j < cmc.mark_matrix.cols(); j++) { - if (cmc.mark_matrix(start.x(), j) == target_type) { - return Eigen::Vector2i{start.x(), j}; - } - } - return std::nullopt; -} - -std::optional find_zero_type_in_col( - const CostMarkCovers & cmc, - const Eigen::Vector2i & start, - const ZerosType & target_type) -{ - for (int i = 0; i < cmc.mark_matrix.cols(); i++) { - if (cmc.mark_matrix(i, start.y()) == target_type) { - return Eigen::Vector2i{i, start.y()}; - } - } - return std::nullopt; -} - -void apply_step_4(CostMarkCovers & cmc) -{ - // Cover all columns containing a (starred) zero. - cmc.row_covers = Eigen::VectorXi::Zero(cmc.cost_matrix.rows()); - cmc.col_covers = Eigen::VectorXi::Zero(cmc.cost_matrix.cols()); - - // Cover all cols containing a starred 0 - star_zero_cols(cmc); - - while (true) { - // Find a non-covered zero and prime it. (If all zeroes are covered, skip to step 5.) - auto uncovered_zero = next_uncovered_zero(cmc); - if (!uncovered_zero.has_value()) { - return; - } - cmc.mark_matrix(uncovered_zero.value().x(), uncovered_zero.value().y()) = ZerosType::PRIMED; - - // If the zero is on the same row as a starred zero, - // cover the corresponding row, and uncover the column of the starred zero. - // Then, GOTO "Find a non-covered zero and prime it." - auto starred_in_row = find_zero_type_in_row( - cmc, - uncovered_zero.value(), - ZerosType::STARRED); - if (starred_in_row.has_value()) { - cmc.row_covers(uncovered_zero.value().x()) = 1; - cmc.col_covers(starred_in_row.value().y()) = 0; - continue; - } - - // Else the non-covered zero has no assigned zero on its row. - // We make a path starting from the zero by performing the following steps: - std::vector path{uncovered_zero.value()}; - while (true) { - // Substep 1: Find a starred zero on the corresponding column. - // If there is one, go to Substep 2, else, stop. - auto starred_in_row = find_zero_type_in_col(cmc, path.back(), ZerosType::STARRED); - if (!starred_in_row.has_value()) { - break; - } - path.push_back(starred_in_row.value()); - - // Substep 2: Find a primed zero on the corresponding row (there should always be one). - // Go to Substep 1. - auto prime_in_row = find_zero_type_in_row(cmc, path.back(), ZerosType::PRIMED); - path.push_back(prime_in_row.value()); - } - - // For all zeros encountered during the path, star primed zeros and unstar starred zeros. - for (std::size_t k = 0; k < path.size(); k++) { - if (cmc.mark_matrix(path.at(k).x(), path.at(k).y()) == ZerosType::PRIMED) { - cmc.mark_matrix(path.at(k).x(), path.at(k).y()) = ZerosType::STARRED; - } else if (cmc.mark_matrix(path.at(k).x(), path.at(k).y()) == ZerosType::STARRED) { - cmc.mark_matrix(path.at(k).x(), path.at(k).y()) = ZerosType::NONE; - } - } - - // Unprime all primed zeroes and uncover all lines. - for (int i = 0; i < cmc.mark_matrix.rows(); i++) { - for (int j = 0; j < cmc.mark_matrix.cols(); j++) { - if (cmc.mark_matrix(i, j) == ZerosType::PRIMED) { - cmc.mark_matrix(i, j) = ZerosType::NONE; - } - } - } - cmc.row_covers = Eigen::VectorXi::Zero(cmc.cost_matrix.rows()); - cmc.col_covers = Eigen::VectorXi::Zero(cmc.cost_matrix.cols()); - - // Cover all columns containing a (starred) zero. - star_zero_cols(cmc); - } -} - -void apply_step_5(CostMarkCovers & cmc) -{ - // Find the lowest uncovered value - double min_value = UNFILLED_LARGE_VALUE; - for (int i = 0; i < cmc.cost_matrix.rows(); i++) { - for (int j = 0; j < cmc.cost_matrix.cols(); j++) { - bool is_row_covered = cmc.row_covers(i) == 1; - bool is_col_covered = cmc.col_covers(j) == 1; - if (!is_row_covered && !is_col_covered && cmc.cost_matrix(i, j) < min_value) { - min_value = cmc.cost_matrix(i, j); - } - } - } - - // Subtract this from every unmarked element and add it to every element covered by two lines - for (int i = 0; i < cmc.cost_matrix.rows(); i++) { - for (int j = 0; j < cmc.cost_matrix.cols(); j++) { - bool is_row_covered = cmc.row_covers(i) == 1; - bool is_col_covered = cmc.col_covers(j) == 1; - if (!is_row_covered && !is_col_covered) { - cmc.cost_matrix(i, j) -= min_value; - } else if (is_row_covered && is_col_covered) { - cmc.cost_matrix(i, j) += min_value; - } - } - } -} - -} // namespace internal -} // namespace ateam_common::assignment diff --git a/ateam_common/src/bi_directional_udp.cpp b/ateam_common/src/bi_directional_udp.cpp index 800c6782d..30c7be951 100644 --- a/ateam_common/src/bi_directional_udp.cpp +++ b/ateam_common/src/bi_directional_udp.cpp @@ -82,12 +82,8 @@ void BiDirectionalUDP::send(const uint8_t * const data, const size_t length) // With the if statement above, this should never happen memcpy(send_buffer_.data(), data, std::min(send_buffer_.size(), length)); - udp_socket_.async_send_to( - boost::asio::buffer(send_buffer_, length), - endpoint_, - boost::bind( - &BiDirectionalUDP::HandleUDPSendTo, this, - boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred)); + udp_socket_.async_send_to(boost::asio::buffer(send_buffer_, length), endpoint_, + boost::bind(&BiDirectionalUDP::HandleUDPSendTo, this, boost::asio::placeholders::error)); } @@ -111,9 +107,7 @@ std::string BiDirectionalUDP::GetRemoteIPAddress() const return endpoint_.address().to_string(); } -void BiDirectionalUDP::HandleUDPSendTo( - const boost::system::error_code & error, - std::size_t bytes_transferred) +void BiDirectionalUDP::HandleUDPSendTo(const boost::system::error_code & error) { if (error) { // RCLCPP_ERROR(get_logger(), "Error during udp send"); diff --git a/ateam_common/src/km_assignment.cpp b/ateam_common/src/km_assignment.cpp index 6f9c07187..3324194a0 100644 --- a/ateam_common/src/km_assignment.cpp +++ b/ateam_common/src/km_assignment.cpp @@ -347,7 +347,7 @@ std::vector km_assignment( } } - for (size_t x = 0; x < cost.rows(); ++x) { + for (auto x = 0l; x < cost.rows(); ++x) { // If we assign an x a y that did not exist in the original, // (or vice versa) set it to not assigned (-1) if (xy[x] >= static_cast(cost_matrix.cols())) { diff --git a/ateam_common/src/parameters.cpp b/ateam_common/src/parameters.cpp deleted file mode 100644 index 270bbdfd5..000000000 --- a/ateam_common/src/parameters.cpp +++ /dev/null @@ -1,131 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "ateam_common/parameters.hpp" - -#include -#include -#include -#include - -namespace ateam_common -{ -void ParameterRegistry::RegisterParam( - const std::string & parameter_name, - Parameter * parameter) -{ - if (int_params.count(parameter_name) > 0) { - std::cout << "Multiple parameters with name " << parameter_name << std::endl; - } else { - int_params[parameter_name] = parameter; - } -} - -void ParameterRegistry::RegisterParam( - const std::string & parameter_name, - Parameter * parameter) -{ - if (double_params.count(parameter_name) > 0) { - std::cout << "Multiple parameters with name " << parameter_name << std::endl; - } else { - double_params[parameter_name] = parameter; - } -} - -void ParameterRegistry::RegisterParam( - const std::string & parameter_name, - Parameter * parameter) -{ - if (double_params.count(parameter_name) > 0) { - std::cout << "Multiple parameters with name " << parameter_name << std::endl; - } else { - bool_params[parameter_name] = parameter; - } -} - -OnSetParamCBPtr ParameterRegistry::ApplyAllRegistrations(rclcpp::Node * node) -{ - for (const auto & param : int_params) { - node->declare_parameter(param.first, static_cast(*param.second)); - } - for (const auto & param : double_params) { - node->declare_parameter(param.first, static_cast(*param.second)); - } - for (const auto & param : bool_params) { - node->declare_parameter(param.first, static_cast(*param.second)); - } - - return node->add_on_set_parameters_callback( - std::bind(&ParameterRegistry::NodeParamCallback, this, std::placeholders::_1)); -} - -ParameterRegistry & ParameterRegistry::Get() -{ - static ParameterRegistry p; - return p; -} - -rcl_interfaces::msg::SetParametersResult ParameterRegistry::NodeParamCallback( - const std::vector & parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; - - for (const auto & param : parameters) { - int num_params_set = 0; - num_params_set += int_params.count(param.get_name()); - num_params_set += double_params.count(param.get_name()); - num_params_set += bool_params.count(param.get_name()); - - if (num_params_set == 1) { - if (int_params.count(param.get_name()) > 0 && - param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) - { - int_params.at(param.get_name())->set(param.as_int()); - continue; - } - if (double_params.count(param.get_name()) > 0 && - param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) - { - double_params.at(param.get_name())->set(param.as_double()); - continue; - } - if (bool_params.count(param.get_name()) > 0 && - param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) - { - bool_params.at(param.get_name())->set(param.as_bool()); - continue; - } - - result.successful = false; - result.reason = "Wrong type for parameter with name: " + param.get_name(); - break; - } else { - result.successful = false; - result.reason = "Failed to find parameter with name: " + param.get_name(); - break; - } - } - - return result; -} - -} // namespace ateam_common diff --git a/ateam_common/src/status.cpp b/ateam_common/src/status.cpp deleted file mode 100644 index c73e1f5e4..000000000 --- a/ateam_common/src/status.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include "ateam_common/status.hpp" - -std::ostream & ateam::operator<<(std::ostream & os, const ErrorType & error) -{ - os << error.cause << "\n\n" << error.stack_trace.str(); - return os; -} - -ateam::Status ateam::Ok() -{ - return BOOST_OUTCOME_V2_NAMESPACE::success(); -} -ateam::Status ateam::Failure(std::string && s) -{ - return ateam::ErrorType(std::move(s)); -} diff --git a/ateam_common/test/CMakeLists.txt b/ateam_common/test/CMakeLists.txt index a45086e8c..803bfaa2e 100644 --- a/ateam_common/test/CMakeLists.txt +++ b/ateam_common/test/CMakeLists.txt @@ -3,23 +3,8 @@ find_package(ament_cmake_gtest REQUIRED) ament_add_gmock(test_protobuf_logging test_protobuf_logging.cpp) target_link_libraries(test_protobuf_logging ${PROJECT_NAME}) -add_compile_options(-g) -ament_add_gtest(test_status test_status.cpp) -target_link_libraries(test_status ${PROJECT_NAME}) -set_property(TARGET test_status PROPERTY ENABLE_EXPORTS ON) - -ament_add_gmock(test_logging test_logging.cpp) -set_target_properties(test_logging PROPERTIES CXX_STANDARD 17) -target_link_libraries(test_logging ${PROJECT_NAME}) - -ament_add_gtest(test_parameters test_parameters.cpp) -target_link_libraries(test_parameters ${PROJECT_NAME}) - ament_add_gtest(test_geometry test_angle.cpp) target_link_libraries(test_geometry ${PROJECT_NAME}) -ament_add_gmock(test_assignment test_assignment.cpp) -target_link_libraries(test_assignment ${PROJECT_NAME}) - ament_add_gmock(test_km_assignment test_km_assignment.cpp) target_link_libraries(test_km_assignment ${PROJECT_NAME}) \ No newline at end of file diff --git a/ateam_common/test/test_assignment.cpp b/ateam_common/test/test_assignment.cpp deleted file mode 100644 index 907ca4f4b..000000000 --- a/ateam_common/test/test_assignment.cpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include - -#include "ateam_common/assignment.hpp" - -namespace assignment = ateam_common::assignment; -namespace ainternal = ateam_common::assignment::internal; - -TEST(Assignment, optimize_assignment) { - Eigen::Matrix cost3x3{{8, 4, 7}, {5, 2, 3}, {9, 4, 8}}; - auto out3x3 = assignment::optimize_assignment(cost3x3); - EXPECT_EQ(out3x3.size(), 3); - EXPECT_EQ(out3x3.at(0), 0); - EXPECT_EQ(out3x3.at(1), 2); - EXPECT_EQ(out3x3.at(2), 1); - - Eigen::Matrix cost5x5{ - {10, 5, 13, 15, 16}, - {3, 9, 18, 13, 6}, - {10, 7, 2, 2, 2}, - {7, 11, 9, 7, 12}, - {7, 9, 10, 4, 12} - }; - auto out5x5 = assignment::optimize_assignment(cost5x5); - EXPECT_EQ(out5x5.size(), 5); - EXPECT_EQ(out5x5.at(0), 1); - EXPECT_EQ(out5x5.at(1), 0); - EXPECT_EQ(out5x5.at(2), 4); - EXPECT_EQ(out5x5.at(3), 2); - EXPECT_EQ(out5x5.at(4), 3); - - Eigen::Matrix cost3x2{ - {1, 10}, - {10, 1}, - {5, 5} - }; - auto out3x2 = assignment::optimize_assignment(cost3x2); - EXPECT_EQ(out3x2.size(), 2); - EXPECT_EQ(out3x2.at(0), 0); - EXPECT_EQ(out3x2.at(1), 1); - - Eigen::Matrix cost2x3{ - {1, 10, 5}, - {10, 1, 5}, - }; - auto out2x3 = assignment::optimize_assignment(cost2x3); - EXPECT_EQ(out2x3.size(), 2); - EXPECT_EQ(out2x3.at(0), 0); - EXPECT_EQ(out2x3.at(1), 1); - - Eigen::Matrix cost8x1{{3.1}, {3.09}, {3.64}, {3.44}, {3.47}, {3.44}, {3.85}, - {3.44}}; - auto out8x1 = assignment::optimize_assignment(cost8x1.transpose()); - EXPECT_EQ(out8x1.size(), 1); - EXPECT_EQ(out8x1.at(0), 1); -} - -MATCHER_P(IsSameSize, m, "") { - *result_listener << "where the size (" << arg.rows() << ", " << arg.cols() << ") "; - *result_listener << "!= expected (" << m.rows() << ", " << m.cols() << ")"; - return arg.rows() == m.rows() && arg.cols() == m.cols(); -} - -MATCHER_P(HasOriginalData, original, "") { - // The original data should stay the same - *result_listener << "where the new data:\n\r" << arg << "\n\r"; - *result_listener << "is not the original data:\n\r" << original << "\n\r"; - return (arg.block( - 0, 0, original.rows(), - original.cols()).array() - original.array()).matrix().sum() < 1e-6; -} - -MATCHER_P(HasUnfilledData, original, "") { - *result_listener << "where the unfilled data:\n\r" << arg << "\n\r"; - *result_listener << "is incorrect compared to original data:\n\r" << original << "\n\r"; - if (arg.size() == original.size()) { - return true; - } - - // Average cell value in the non-original cells should be UNFILLED_LARGE_VALUE - Eigen::MatrixXd temp = arg; - std::size_t diff = arg.size() - original.size(); - temp.block(0, 0, original.rows(), original.cols()) = Eigen::MatrixXd::Zero( - original.rows(), original.cols()); - return temp.sum() / diff == ainternal::UNFILLED_LARGE_VALUE; -} - -TEST(Assignment, SquareizeMatrix) -{ - // No-op with square input - Eigen::Matrix square1x1{{1}}; - EXPECT_THAT(ainternal::squarize_matrix(square1x1), IsSameSize(square1x1)); - EXPECT_THAT(ainternal::squarize_matrix(square1x1), HasOriginalData(square1x1)); - EXPECT_THAT(ainternal::squarize_matrix(square1x1), HasUnfilledData(square1x1)); - - Eigen::Matrix2d square2x2{{1, 2}, {3, 4}}; - EXPECT_THAT(ainternal::squarize_matrix(square2x2), IsSameSize(square2x2)); - EXPECT_THAT(ainternal::squarize_matrix(square2x2), HasOriginalData(square2x2)); - EXPECT_THAT(ainternal::squarize_matrix(square2x2), HasUnfilledData(square2x2)); - - Eigen::Matrix3d square3x3{{1, 2, 3}, {4, 5, 6}, {7, 8, 9}}; - EXPECT_THAT(ainternal::squarize_matrix(square3x3), IsSameSize(square3x3)); - EXPECT_THAT(ainternal::squarize_matrix(square3x3), HasOriginalData(square3x3)); - EXPECT_THAT(ainternal::squarize_matrix(square3x3), HasUnfilledData(square3x3)); - - // Too many rows - Eigen::Matrix row2x1{{1}, {2}}; - EXPECT_THAT(ainternal::squarize_matrix(row2x1), IsSameSize(square2x2)); - EXPECT_THAT(ainternal::squarize_matrix(row2x1), HasOriginalData(row2x1)); - EXPECT_THAT(ainternal::squarize_matrix(row2x1), HasUnfilledData(row2x1)); - - Eigen::Matrix row3x1{{1}, {2}, {3}}; - EXPECT_THAT(ainternal::squarize_matrix(row3x1), IsSameSize(square3x3)); - EXPECT_THAT(ainternal::squarize_matrix(row3x1), HasOriginalData(row3x1)); - EXPECT_THAT(ainternal::squarize_matrix(row3x1), HasUnfilledData(row3x1)); - - // Too many cols - Eigen::Matrix col1x2{{1, 2}}; - EXPECT_THAT(ainternal::squarize_matrix(col1x2), IsSameSize(square2x2)); - EXPECT_THAT(ainternal::squarize_matrix(col1x2), HasOriginalData(col1x2)); - EXPECT_THAT(ainternal::squarize_matrix(col1x2), HasUnfilledData(col1x2)); - - Eigen::Matrix col1x3{{1, 2, 3}}; - EXPECT_THAT(ainternal::squarize_matrix(col1x3), IsSameSize(square3x3)); - EXPECT_THAT(ainternal::squarize_matrix(col1x3), HasOriginalData(col1x3)); - EXPECT_THAT(ainternal::squarize_matrix(col1x3), HasUnfilledData(col1x3)); -} - -MATCHER_P(Has0ReferencedRows, original, "") { - *result_listener << "where the rows aren't relative to the min value:\n\r" << arg << "\n\r"; - *result_listener << "with the original matrix:\n\r" << original << "\n\r"; - bool valid = true; - // For each row, All data in that row should be relative to the min value in the original matrix - Eigen::VectorXd min_coeff = original.rowwise().minCoeff(); - for (int i = 0; i < arg.rows(); i++) { - for (int j = 0; j < arg.cols(); j++) { - if (original(i, j) - min_coeff(i) != arg(i, j)) { - *result_listener << "at (row, col): " << i << " " << j << "\n\r"; - valid = false; - } - } - } - - return valid; -} - -TEST(Assignment, apply_step_1) -{ - ainternal::CostMarkCovers cmc1x1; - cmc1x1.cost_matrix = Eigen::Matrix{{1}}; - ainternal::apply_step_1(cmc1x1); - EXPECT_THAT(cmc1x1.cost_matrix, Has0ReferencedRows(Eigen::Matrix{{1}})); - - ainternal::CostMarkCovers cmc2x2; - cmc2x2.cost_matrix = Eigen::Matrix2d{{1, 2}, {3, 4}}; - ainternal::apply_step_1(cmc2x2); - EXPECT_THAT(cmc2x2.cost_matrix, Has0ReferencedRows(Eigen::Matrix2d{{1, 2}, {3, 4}})); - - ainternal::CostMarkCovers cmc3x3; - cmc3x3.cost_matrix = Eigen::Matrix3d{{1, 2, 3}, {4, 5, 6}, {7, 8, 9}}; - ainternal::apply_step_1(cmc3x3); - EXPECT_THAT( - cmc3x3.cost_matrix, - Has0ReferencedRows(Eigen::Matrix3d{{1, 2, 3}, {4, 5, 6}, {7, 8, 9}})); -} - -MATCHER_P(Has0ReferencedCols, original, "") { - *result_listener << "where the rows aren't relative to the min value:\n\r" << arg << "\n\r"; - *result_listener << "with the original matrix:\n\r" << original << "\n\r"; - bool valid = true; - // For each col, All data in that col should be relative to the min value in the original matrix - Eigen::VectorXd min_coeff = original.colwise().minCoeff(); - for (int j = 0; j < arg.cols(); j++) { - for (int i = 0; i < arg.rows(); i++) { - if (original(i, j) - min_coeff(j) != arg(i, j)) { - *result_listener << "at (row, col): " << i << " " << j << "\n\r"; - valid = false; - } - } - } - - return valid; -} - -TEST(Assignment, apply_step_2) -{ - ainternal::CostMarkCovers cmc1x1; - cmc1x1.cost_matrix = Eigen::Matrix{{1}}; - ainternal::apply_step_2(cmc1x1); - EXPECT_THAT(cmc1x1.cost_matrix, Has0ReferencedCols(Eigen::Matrix{{1}})); - - ainternal::CostMarkCovers cmc2x2; - cmc2x2.cost_matrix = Eigen::Matrix2d{{1, 2}, {3, 4}}; - ainternal::apply_step_2(cmc2x2); - EXPECT_THAT(cmc2x2.cost_matrix, Has0ReferencedCols(Eigen::Matrix2d{{1, 2}, {3, 4}})); - - ainternal::CostMarkCovers cmc3x3; - cmc3x3.cost_matrix = Eigen::Matrix3d{{1, 2, 3}, {4, 5, 6}, {7, 8, 9}}; - ainternal::apply_step_2(cmc3x3); - EXPECT_THAT( - cmc3x3.cost_matrix, - Has0ReferencedCols(Eigen::Matrix3d{{1, 2, 3}, {4, 5, 6}, {7, 8, 9}})); -} - -MATCHER_P(IsSame, original, "") { - *result_listener << "where :\n\r" << arg << "\n\r"; - *result_listener << "is not equal to:\n\r" << original << "\n\r"; - bool valid = true; - for (int j = 0; j < arg.cols(); j++) { - for (int i = 0; i < arg.rows(); i++) { - if (original(i, j) != arg(i, j)) { - *result_listener << "at (row, col): " << i << " " << j << "\n\r"; - valid = false; - } - } - } - - return valid; -} - -TEST(Assignment, apply_step_3) -{ - // Straightforward marking - ainternal::CostMarkCovers cmc1x1; - cmc1x1.cost_matrix = Eigen::Matrix{{0}}; - ainternal::apply_step_3(cmc1x1); - EXPECT_THAT(cmc1x1.mark_matrix, IsSame(Eigen::Matrix{{1}})); - - ainternal::CostMarkCovers cmc2x2; - cmc2x2.cost_matrix = Eigen::Matrix2d{{0, 2}, {3, 0}}; - ainternal::apply_step_3(cmc2x2); - EXPECT_THAT(cmc2x2.mark_matrix, IsSame(Eigen::Matrix2d{{1, 0}, {0, 1}})); - - ainternal::CostMarkCovers cmc3x3; - cmc3x3.cost_matrix = Eigen::Matrix3d{{1, 0, 3}, {0, 5, 6}, {7, 8, 0}}; - ainternal::apply_step_3(cmc3x3); - EXPECT_THAT(cmc3x3.mark_matrix, IsSame(Eigen::Matrix3d{{0, 1, 0}, {1, 0, 0}, {0, 0, 1}})); - - // More complicated marking - ainternal::CostMarkCovers zero2x2; - zero2x2.cost_matrix = Eigen::Matrix2d{{0, 0}, {0, 0}}; - ainternal::apply_step_3(zero2x2); - EXPECT_THAT(zero2x2.mark_matrix, IsSame(Eigen::Matrix2d{{1, 0}, {0, 1}})); - - ainternal::CostMarkCovers inv2x2; - inv2x2.cost_matrix = Eigen::Matrix2d{{1, 0}, {0, 1}}; - ainternal::apply_step_3(inv2x2); - EXPECT_THAT(inv2x2.mark_matrix, IsSame(Eigen::Matrix2d{{0, 1}, {1, 0}})); -} - -TEST(Assignment, apply_step_4) -{ - ainternal::CostMarkCovers cmc1x1; - cmc1x1.cost_matrix = Eigen::Matrix{{0}}; - cmc1x1.mark_matrix = Eigen::Matrix{{0}}; - ainternal::apply_step_4(cmc1x1); - EXPECT_THAT(cmc1x1.row_covers, IsSame(Eigen::Vector{0})); - EXPECT_THAT(cmc1x1.col_covers, IsSame(Eigen::Vector{1})); - - ainternal::CostMarkCovers cmc4x4; - cmc4x4.cost_matrix = - Eigen::Matrix{{0, 12, 0, 14}, {21, 0, 23, 0}, {0, 32, 33, 34}, {0, 42, 43, 44}}; - cmc4x4.mark_matrix = - Eigen::Matrix{{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; - ainternal::apply_step_4(cmc4x4); - EXPECT_THAT(cmc4x4.row_covers, IsSame(Eigen::Vector{0, 1, 0, 0})); - EXPECT_THAT(cmc4x4.col_covers, IsSame(Eigen::Vector{1, 0, 1, 0})); -} - -TEST(Assignment, apply_step_5) -{ - ainternal::CostMarkCovers cmc1x1; - cmc1x1.cost_matrix = Eigen::Matrix{{1}}; - cmc1x1.mark_matrix = Eigen::Matrix{{0}}; - cmc1x1.row_covers = Eigen::Vector{0}; - cmc1x1.col_covers = Eigen::Vector{0}; - ainternal::apply_step_5(cmc1x1); - EXPECT_THAT(cmc1x1.cost_matrix, IsSame(Eigen::Matrix{{0}})); - - ainternal::CostMarkCovers cmc4x4; - cmc4x4.cost_matrix = - Eigen::Matrix{{0, 12, 0, 14}, {21, 0, 23, 0}, {0, 32, 33, 34}, {0, 42, 43, 44}}; - cmc4x4.mark_matrix = Eigen::Matrix::Zero(); - cmc4x4.row_covers = Eigen::Vector{0, 1, 0, 0}; - cmc4x4.col_covers = Eigen::Vector{1, 0, 1, 0}; - ainternal::apply_step_5(cmc4x4); - EXPECT_THAT( - cmc4x4.cost_matrix, - IsSame( - Eigen::Matrix{{0, 0, 0, 2}, {33, 0, 35, 0}, {0, 20, 33, 22}, - {0, 30, 43, 32}})); -} diff --git a/ateam_common/test/test_logging.cpp b/ateam_common/test/test_logging.cpp deleted file mode 100644 index 7ecbb2a2c..000000000 --- a/ateam_common/test/test_logging.cpp +++ /dev/null @@ -1,125 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include -#include -#include "ateam_common/logger.hpp" - -const char * logger_name = "test"; -size_t log_calls = 0; -rclcpp::Logger logger = rclcpp::get_logger(logger_name); - -struct LogEvent -{ - const rcutils_log_location_t * location; - int level; - std::string name; - rcutils_time_point_value_t timestamp; - std::string message; -}; -LogEvent last_log_event; - -class TestLogging : public testing::Test -{ -public: - rcutils_logging_output_handler_t previous_output_handler; - - void SetUp() - { - log_calls = 0; - ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_initialize()); - rcutils_logging_set_default_logger_level(RCUTILS_LOG_SEVERITY_DEBUG); - - auto output_handler = [](const rcutils_log_location_t * location, - int level, const char * name, rcutils_time_point_value_t timestamp, - const char * format, va_list * args) { - ++log_calls; - last_log_event = LogEvent{ - location, - level, - (name ? name : ""), - timestamp, - "" - }; - char buffer[1024]; - vsnprintf(buffer, sizeof(buffer), format, *args); - last_log_event.message = buffer; - }; - - previous_output_handler = rcutils_logging_get_output_handler(); - rcutils_logging_set_output_handler(output_handler); - } - - void TearDown() - { - rcutils_logging_set_output_handler(previous_output_handler); - ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_shutdown()); - EXPECT_FALSE(g_rcutils_logging_initialized); - } -}; - -TEST_F(TestLogging, test_info) -{ - SET_ROS_LOG_HANDLER(logger_name); - LOG_INFO("Hello %s", "World"); - EXPECT_EQ(log_calls, 1u); - EXPECT_EQ(last_log_event.level, RCUTILS_LOG_SEVERITY_INFO); - EXPECT_EQ(last_log_event.name, logger_name); - EXPECT_THAT( - last_log_event.message, - ::testing::MatchesRegex("\\[.*test_logging\\.cpp\\:[0-9]+\\] Hello World")); -} - -TEST_F(TestLogging, test_warn) -{ - SET_ROS_LOG_HANDLER(logger_name); - LOG_WARN("Hello %s", "World"); - EXPECT_EQ(log_calls, 1u); - EXPECT_EQ(last_log_event.level, RCUTILS_LOG_SEVERITY_WARN); - EXPECT_EQ(last_log_event.name, logger_name); - EXPECT_THAT( - last_log_event.message, - ::testing::MatchesRegex("\\[.*test_logging\\.cpp\\:[0-9]+\\] Hello World")); -} - -TEST_F(TestLogging, test_error) -{ - SET_ROS_LOG_HANDLER(logger_name); - LOG_ERROR("Hello %s", "World"); - EXPECT_EQ(log_calls, 1u); - EXPECT_EQ(last_log_event.level, RCUTILS_LOG_SEVERITY_ERROR); - EXPECT_EQ(last_log_event.name, logger_name); - EXPECT_THAT( - last_log_event.message, - ::testing::MatchesRegex("\\[.*test_logging\\.cpp\\:[0-9]+\\] Hello World")); -} - -TEST_F(TestLogging, test_fatal) -{ - SET_ROS_LOG_HANDLER(logger_name); - LOG_FATAL("Hello %s", "World"); - EXPECT_EQ(log_calls, 1u); - EXPECT_EQ(last_log_event.level, RCUTILS_LOG_SEVERITY_FATAL); - EXPECT_EQ(last_log_event.name, logger_name); - EXPECT_THAT( - last_log_event.message, - ::testing::MatchesRegex("\\[.*test_logging\\.cpp\\:[0-9]+\\] Hello World")); -} diff --git a/ateam_common/test/test_parameters.cpp b/ateam_common/test/test_parameters.cpp deleted file mode 100644 index 6c54a421c..000000000 --- a/ateam_common/test/test_parameters.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include "ateam_common/parameters.hpp" - -CREATE_PARAM(int, "int/", intparam, 10); -CREATE_PARAM(double, "double/", doubleparam, 5.6); -CREATE_PARAM(bool, "bool/", boolparam, true); - -TEST(TestParameters, BasicParametersAreDefaultValues) -{ - EXPECT_EQ(intparam, 10); - EXPECT_EQ(doubleparam, 5.6); - EXPECT_TRUE(boolparam); -} diff --git a/ateam_common/test/test_robot_assignment.cpp b/ateam_common/test/test_robot_assignment.cpp deleted file mode 100644 index 886b973dd..000000000 --- a/ateam_common/test/test_robot_assignment.cpp +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include - -#include "ateam_common/robot_assignment.hpp" - -using assign = ateam_common::robot_assignment::assign; - -TEST(RobotAssignment, assign_simple) { - // goals should be assigned in robot order as the robots are on top of each goal - std::vector robots1 {{ - {{1, 0, 0}, {}, {}, 0}, - {{2, 0, 0}, {}, {}, 1}, - {{3, 0, 0}, {}, {}, 2}, - {{4, 0, 0}, {}, {}, 3} - }}; - - std::vector goals1 {{ - {1, 0}, - {2, 0}, - {3, 0}, - {4, 0} - }}; - - std::map exp_result1 {{ - {0, 0}, - {1, 1}, - {2, 2}, - {3, 3}, - }}; - auto result1 = assign(robots1, goals1); - EXPECT_EQ(result1, exp_result1); -} - -TEST(RobotAssignment, less_goals_than_robots) { - // less goals than robots - std::vector robots2 {{ - {{1, 0, 0}, {}, {}, 0}, - {{2, 0, 0}, {}, {}, 1}, - {{3, 0, 0}, {}, {}, 2}, - {{4, 0, 0}, {}, {}, 3} - }}; - - std::vector goals2 {{ - {1, 0}, - {2, 0}, - }}; - - std::map exp_result2 {{ - {0, 0}, - {1, 1}, - }}; - - auto result2 = assign(robots2, goals2); - EXPECT_EQ(result2, exp_result2); -} - -TEST(RobotAssignment, more_goals_than_robots) { - // More goals than robots - std::vector robots3 {{ - {{3, 0, 0}, {}, {}, 0}, - {{4, 0, 0}, {}, {}, 1}, - }}; - - std::vector goals3 {{ - {1, 0}, - {2, 0}, - {3, 0}, - {4, 0} - }}; - - std::map exp_result3 {{ - {0, 2}, - {1, 3}, - }}; - - auto result3 = assign(robots3, goals3); - EXPECT_EQ(result3, exp_result3); -} diff --git a/ateam_common/test/test_status.cpp b/ateam_common/test/test_status.cpp deleted file mode 100644 index 5e0191999..000000000 --- a/ateam_common/test/test_status.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2021 A Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. - -#include -#include -#include "ateam_common/status.hpp" - -ateam::Status AlwaysFailStatus() -{ - return ateam::Failure("Fail string"); -} - -ateam::Status AlwaysPassStatus() -{ - return ateam::Ok(); -} - -ateam::StatusOr AlwaysFailInt() -{ - return ateam::Failure("Fail string"); -} - -ateam::StatusOr Always10Int() -{ - return ateam::Ok(10); -} - -TEST(Status, assign_or_throw_bad) -{ - EXPECT_THROW(ATEAM_ASSIGN_OR_THROW(auto a, AlwaysFailInt(), "Oops failed1"), std::string); - EXPECT_THROW(ATEAM_ASSIGN_OR_THROW(auto b, AlwaysFailInt(), "Oops failed2"), std::string); -} - -TEST(Status, assign_or_throw_good) -{ - ATEAM_ASSIGN_OR_THROW(auto a, Always10Int(), "Oops failed"); - EXPECT_EQ(a, 10); - ATEAM_ASSIGN_OR_THROW(auto b, Always10Int(), "Oops failed"); - EXPECT_EQ(b, 10); -} - -TEST(Status, always) -{ - EXPECT_NO_THROW(ATEAM_CHECK(AlwaysPassStatus(), "Oops failed1")); - EXPECT_THROW(ATEAM_CHECK(AlwaysFailStatus(), "Oops failed1"), std::string); -} diff --git a/ateam_kenobi/src/core/motion/motion_controller.cpp b/ateam_kenobi/src/core/motion/motion_controller.cpp index 9d4aaed2c..7f062a378 100644 --- a/ateam_kenobi/src/core/motion/motion_controller.cpp +++ b/ateam_kenobi/src/core/motion/motion_controller.cpp @@ -29,23 +29,10 @@ #include #include #include -#include #include "ateam_geometry/ateam_geometry.hpp" #include "ateam_common/robot_constants.hpp" #include "pid.hpp" -/* -// PID gains -CREATE_PARAM(double, "motion/pid/x_kp", x_kp, 2); -CREATE_PARAM(double, "motion/pid/y_kp", y_kp, 2); -CREATE_PARAM(double, "motion/pid/t_kp", t_kp, 3); - -// PID velocity limits -CREATE_PARAM(double, "motion/pid/x_max", x_max, 2); -CREATE_PARAM(double, "motion/pid/y_max", y_max, 2); -CREATE_PARAM(double, "motion/pid/t_max", t_max, 4); -*/ - MotionController::MotionController(rclcpp::Logger logger) : logger_(logger)