diff --git a/.env b/.env index 77fd8eed0..f85ed6070 100644 --- a/.env +++ b/.env @@ -12,7 +12,7 @@ PROJECT_NAME="airstack" # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version. -DOCKER_IMAGE_TAG="0.14.0" +DOCKER_IMAGE_TAG="8b5529e" # Can replace with your docker hub username PROJECT_DOCKER_REGISTRY="airlab-storage.andrew.cmu.edu:5001/shared" # ============================================ diff --git a/docs/development/docker_usage.md b/docs/development/docker_usage.md index ece633f1e..dbb04fb1c 100644 --- a/docs/development/docker_usage.md +++ b/docs/development/docker_usage.md @@ -71,6 +71,8 @@ docker compose up isaac-sim -d docker compose up gcs -d ``` + + ### Isaac Sim Start a bash shell in the Isaac Sim container: @@ -164,6 +166,20 @@ graph TD ``` +## Automated Testing + +To perform automated tests for the configured packages, please use the `autotest` service which +extends the `robot` service with testing specific commands. Presently only `takeoff_landing_planner` +is configured to be tested. + +```bash +# On your development PC, do: + +docker compose up autotest +``` + +This command will spin up a `robot` container, build the ROS2 workspace, source the workspace and run all the configured tests for the provided packages using `colcon test`. Excessive output log from the build process is presently piped away to preserve readability. + ## Docker Compose Variable Overrides Sometimes you may want to test different configurations of the autonomy stack. For example, you may want to disable automatically playing the sim on startup, or to change a child launch file. @@ -188,7 +204,4 @@ docker compose --env-file .env --env-file overrides/no_macvo.env up -d ``` -When overriding, the default `.env` file must be loaded first. The overrides are applied on top of it. - - - +When overriding, the default `.env` file must be loaded first. The overrides are applied on top of it. \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/mission_manager/src/BeliefMap.cpp b/ground_control_station/ros_ws/src/mission_manager/src/BeliefMap.cpp index 889868e25..4bbc1d9a6 100644 --- a/ground_control_station/ros_ws/src/mission_manager/src/BeliefMap.cpp +++ b/ground_control_station/ros_ws/src/mission_manager/src/BeliefMap.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 "mission_manager/BeliefMap.h" @@ -132,4 +152,4 @@ bool BeliefMap::update_map(rclcpp::Logger logger, const airstack_msgs::msg::Beli cur_index_num++; } return true; -} +} diff --git a/ground_control_station/ros_ws/src/mission_manager/src/MissionManager.cpp b/ground_control_station/ros_ws/src/mission_manager/src/MissionManager.cpp index 0e99e798b..d24e78484 100644 --- a/ground_control_station/ros_ws/src/mission_manager/src/MissionManager.cpp +++ b/ground_control_station/ros_ws/src/mission_manager/src/MissionManager.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 "mission_manager/MissionManager.h" #include "mission_manager/visualization.h" @@ -426,4 +446,4 @@ std::vector MissionManager::assign_tasks( scenario_coutner_++; return task_assignments; -} \ No newline at end of file +} diff --git a/ground_control_station/ros_ws/src/mission_manager/src/example_search_request.cpp b/ground_control_station/ros_ws/src/mission_manager/src/example_search_request.cpp index 4e4d669d9..1dde5d8da 100644 --- a/ground_control_station/ros_ws/src/mission_manager/src/example_search_request.cpp +++ b/ground_control_station/ros_ws/src/mission_manager/src/example_search_request.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 @@ -167,4 +187,4 @@ int main(int argc, char * argv[]) y: -10 radius: 8 -*/ \ No newline at end of file +*/ diff --git a/ground_control_station/ros_ws/src/mission_manager/src/mission_manager_node.cpp b/ground_control_station/ros_ws/src/mission_manager/src/mission_manager_node.cpp index d3038e534..f1a342f33 100644 --- a/ground_control_station/ros_ws/src/mission_manager/src/mission_manager_node.cpp +++ b/ground_control_station/ros_ws/src/mission_manager/src/mission_manager_node.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 "mission_manager/mission_manager_node.h" @@ -7,4 +27,4 @@ int main(int argc, char * argv[]) rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} +} diff --git a/mkdocs.yml b/mkdocs.yml index 3580bb57c..7376b6173 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -3,7 +3,7 @@ copyright: Copyright © 2024 - 2030 AirLab CMU docs_dir: . site_name: AirStack site_dir: ../site -site_url: 'https://docs.theairlab.org/docs/' # Trailing slash is recommended +site_url: "https://docs.theairlab.org/docs/" # Trailing slash is recommended exclude_docs: | **/ros_ws/build **/ros_ws/install @@ -66,39 +66,41 @@ nav: - Robot: - docs/robot/index.md - Autonomy Modules: - - Robot Interface: + - Robot Interface: - docs/robot/autonomy/0_interface/index.md - Sensors: - docs/robot/autonomy/1_sensors/index.md - - docs/robot/autonomy/1_sensors/gimbal.md + - docs/robot/autonomy/1_sensors/gimbal.md - Perception: - docs/robot/autonomy/2_perception/index.md - docs/robot/autonomy/2_perception/state_estimation.md - Local: - docs/robot/autonomy/3_local/index.md - World Model: - - docs/robot/autonomy/3_local/world_model/index.md - - DROAN (Obstacle Avoidance World Model): - - robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/README.md - - robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/README.md - - robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/README.md + - docs/robot/autonomy/3_local/world_model/index.md + - DROAN (Obstacle Avoidance World Model): + - robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/README.md + - robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/README.md + - robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/README.md - Planning: - - docs/robot/autonomy/3_local/planning/index.md - - robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/README.md - - robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/README.md - - DROAN (Obstacle Avoidance Planner): - - robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/README.md + - docs/robot/autonomy/3_local/planning/index.md + - robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/README.md + - Takeoff Landing Planner: + - robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/README.md + - robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/test/README.md + - DROAN (Obstacle Avoidance Planner): + - robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/README.md - Controls: - - docs/robot/autonomy/3_local/controls/index.md - - robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/README.md + - docs/robot/autonomy/3_local/controls/index.md + - robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/README.md - Global: - docs/robot/autonomy/4_global/index.md - World Model: - - docs/robot/autonomy/4_global/world_model/index.md - - robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/README.md + - docs/robot/autonomy/4_global/world_model/index.md + - robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/README.md - Planning: - - docs/robot/autonomy/4_global/planning/index.md - - robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/README.md + - docs/robot/autonomy/4_global/planning/index.md + - robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/README.md - Behavior: - docs/robot/autonomy/5_behavior/index.md - docs/robot/autonomy/5_behavior/behavior_tree.md @@ -170,4 +172,4 @@ theme: scheme: slate toggle: icon: material/brightness-4 - name: Switch to light mode \ No newline at end of file + name: Switch to light mode diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml index 956312f76..643853d65 100644 --- a/robot/docker/docker-compose.yaml +++ b/robot/docker/docker-compose.yaml @@ -72,3 +72,18 @@ services: network_mode: host volumes: - /media/airlab/Storage/airstack_collection:/bags:rw + +# =================================================================================================================== + autotest: + extends: + service: robot + command: > + bash -il -c " + echo 'Building and sourcing workspace...'; + bws &> /dev/null && sws &> /dev/null && + echo 'Starting tests for packages: takeoff_landing_planner'; + colcon test --packages-select takeoff_landing_planner --event-handlers=console_direct+; + TEST_EXIT_CODE=$$?; + echo \"Tests completed with exit code: $$TEST_EXIT_CODE\"; + exit $$TEST_EXIT_CODE" + entrypoint: [] diff --git a/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp index f9aee5bf1..48e11fc1c 100644 --- a/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp +++ b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + #pragma once #include @@ -19,4 +39,4 @@ class TimeChecker{ TimeChecker(); void update(rclcpp::Time time); double elapsed_since_last_update(rclcpp::Time time); -}; +}; diff --git a/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/src/drone_safety_monitor.cpp b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/src/drone_safety_monitor.cpp index c8a18e4d8..86c21c94f 100644 --- a/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/src/drone_safety_monitor.cpp +++ b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/src/drone_safety_monitor.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 TimeChecker::TimeChecker() @@ -61,4 +81,4 @@ int main(int argc, char **argv) rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} +} diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp index ba79d4195..a82ef10c7 100644 --- a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp +++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + /** * @file mavros_interface.cpp * @author John Keller (jkeller2@andrew.cmu.edu), Andrew Jong @@ -669,4 +689,4 @@ class MAVROSInterface : public robot_interface::RobotInterface { } // namespace mavros_interface #include -PLUGINLIB_EXPORT_CLASS(mavros_interface::MAVROSInterface, robot_interface::RobotInterface) +PLUGINLIB_EXPORT_CLASS(mavros_interface::MAVROSInterface, robot_interface::RobotInterface) diff --git a/robot/ros_ws/src/autonomy/0_interface/robot_interface/include/robot_interface/robot_interface.hpp b/robot/ros_ws/src/autonomy/0_interface/robot_interface/include/robot_interface/robot_interface.hpp index 21f10a632..11428c463 100644 --- a/robot/ros_ws/src/autonomy/0_interface/robot_interface/include/robot_interface/robot_interface.hpp +++ b/robot/ros_ws/src/autonomy/0_interface/robot_interface/include/robot_interface/robot_interface.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + /** * * @file robot_interface.hpp @@ -142,4 +162,4 @@ namespace robot_interface { public: virtual ~RobotInterface() {} }; -} // namespace robot_interface +} // namespace robot_interface diff --git a/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/odometry_conversion.cpp b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/odometry_conversion.cpp index 7be1eccb4..8f17ca41f 100644 --- a/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/odometry_conversion.cpp +++ b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/odometry_conversion.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 @@ -141,4 +161,4 @@ int main(int argc, char* argv[]) { rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} +} diff --git a/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface.cpp b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface.cpp index 6db53fce8..822b5ab26 100644 --- a/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface.cpp +++ b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + /** * @file robot_interface.cpp * @author your name (you@domain.com) @@ -13,4 +33,4 @@ */ #include -int blank() { return 0; } +int blank() { return 0; } diff --git a/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface_node.cpp b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface_node.cpp index 5d20ba332..3aefb6497 100644 --- a/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface_node.cpp +++ b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface_node.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + /** * @file robot_interface_node.cpp * @author John Keller (jkeller2@andrew.cmu.edu), Andrew Jong @@ -148,4 +168,4 @@ int main(int argc, char** argv) { } return 0; -} +} diff --git a/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/stabilized_tf_node.cpp b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/stabilized_tf_node.cpp index c77c4db1f..88f45ff0f 100644 --- a/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/stabilized_tf_node.cpp +++ b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/stabilized_tf_node.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 @@ -76,4 +96,4 @@ int main(int argc, char *argv[]) rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} +} diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/cost_map_interface/include/cost_map_interface/cost_map_interface.hpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/cost_map_interface/include/cost_map_interface/cost_map_interface.hpp index e05386597..cec1f0de2 100644 --- a/robot/ros_ws/src/autonomy/3_local/a_world_models/cost_map_interface/include/cost_map_interface/cost_map_interface.hpp +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/cost_map_interface/include/cost_map_interface/cost_map_interface.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + #pragma once #include @@ -48,4 +68,4 @@ class CostMapInterface { } }; -} // namespace cost_map_interface +} // namespace cost_map_interface diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/cost_map_interface/src/cost_map_interface.cpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/cost_map_interface/src/cost_map_interface.cpp index efa55f3e8..ae5fe61b7 100644 --- a/robot/ros_ws/src/autonomy/3_local/a_world_models/cost_map_interface/src/cost_map_interface.cpp +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/cost_map_interface/src/cost_map_interface.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 -int blank() { return 0; } +int blank() { return 0; } diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp index 0403fa8a5..e8d41dccd 100644 --- a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + #pragma once #include @@ -98,4 +118,4 @@ class DisparityExpansionNode : public rclcpp::Node { const cv_bridge::CvImagePtr& bg_msg); void publish_frustum(const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg); -}; +}; diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp old mode 100755 new mode 100644 index 410e74351..dcfb92cd0 --- a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp @@ -1,9 +1,22 @@ -/* - * Copyright (c) 2016 Carnegie Mellon University, Author - * - * For License information please see the LICENSE file in the root directory. - * - */ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. // Applies C-Space expansion on disparity images. #include @@ -671,4 +684,4 @@ int main(int argc, char** argv) { rclcpp::spin(node); rclcpp::shutdown(); return 0; -} +} diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_pcd.cpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_pcd.cpp index 29d6dbe77..88f9fa655 100644 --- a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_pcd.cpp +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_pcd.cpp @@ -1,11 +1,22 @@ -/* -* Copyright (c) 2016 Carnegie Mellon University, Author -* -* For License information please see the LICENSE file in the root directory. -* -*/ - -//Outputs a point cloud using disparity images. +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 "ros/ros.h" #include "sensor_msgs/Image.h" @@ -306,4 +317,4 @@ int main(int argc, char** argv) rclcpp::shutdown(); return 0; -} +} diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp index 00f11548c..08150e0e2 100644 --- a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + #pragma once #include @@ -490,4 +510,4 @@ class DisparityGraph { float orig_z; }; -} // namespace disparity_graph +} // namespace disparity_graph diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/src/disparity_graph.cpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/src/disparity_graph.cpp index 2e1006d0b..e5ac3cda7 100644 --- a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/src/disparity_graph.cpp +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/src/disparity_graph.cpp @@ -1,6 +1,26 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 "disparity_graph/disparity_graph.hpp" namespace disparity_graph { -} // namespace disparity_graph +} // namespace disparity_graph diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/include/disparity_graph_cost_map/disparity_graph_cost_map.hpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/include/disparity_graph_cost_map/disparity_graph_cost_map.hpp index eea3c7042..ed9036166 100644 --- a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/include/disparity_graph_cost_map/disparity_graph_cost_map.hpp +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/include/disparity_graph_cost_map/disparity_graph_cost_map.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + #pragma once #include #include @@ -85,4 +105,4 @@ class DisparityGraphCostMap : public cost_map_interface::CostMapInterface { virtual void initialize(const rclcpp::Node::SharedPtr& node_ptr, const std::shared_ptr tf_buffer_ptr) override; }; -} // namespace disparity_graph_cost_map +} // namespace disparity_graph_cost_map diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/src/disparity_graph_cost_map.cpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/src/disparity_graph_cost_map.cpp index 7d71c48a8..b1cdecd6c 100644 --- a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/src/disparity_graph_cost_map.cpp +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/src/disparity_graph_cost_map.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 namespace disparity_graph_cost_map { DisparityGraphCostMap::DisparityGraphCostMap() : CostMapInterface(), disp_graph() { @@ -235,4 +255,4 @@ const visualization_msgs::msg::MarkerArray& DisparityGraphCostMap::get_debug_mar #include PLUGINLIB_EXPORT_CLASS(disparity_graph_cost_map::DisparityGraphCostMap, - cost_map_interface::CostMapInterface) + cost_map_interface::CostMapInterface) diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp index dc81ac209..04a1ec076 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + #pragma once #include @@ -658,4 +678,4 @@ class DroanLocalPlanner : public rclcpp::Node { "tracking point received with frame id: " << odom->header.frame_id); tracking_point_odom = *odom; } -}; +}; diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp index b15715896..4a144a132 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 @@ -9,4 +29,4 @@ int main(int argc, char** argv) { rclcpp::spin(node); rclcpp::shutdown(); return 0; -} +} diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt index 506ac7d5f..1adb3320b 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt @@ -42,16 +42,35 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY + test + DESTINATION share/${PROJECT_NAME}) + +find_package(ament_cmake_ros REQUIRED) +find_package(launch_testing_ament_cmake REQUIRED) + +# Isolated Integration Testing from (https://arnebaeyens.com/blog/2024/ros2-integration-testing/) +function(add_ros_isolated_launch_test path) + set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py") + add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) +endfunction() + if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) + # find_package(ament_lint_auto REQUIRED) + # ament_lint_auto_find_test_dependencies() + # the following line skips the linter which checks for copyrights # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) + # set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + # set(ament_cmake_cpplint_FOUND TRUE) + + add_ros_isolated_launch_test( + test/scripts/test_takeoff_landing_planner.py + ) endif() ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/README.md b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/README.md index 9223a4a9f..3cb5ca33e 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/README.md +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/README.md @@ -1,6 +1,117 @@ # Takeoff Landing Planner +## Overview -Author: John Keller +The TakeoffLandingPlanner provides a rich interface for handling takeoff/landing commands, with support for both trajectory-based and ArduPilot command-based modes. It monitors the robot's position, generates appropriate trajectories, and tracks the completion status of takeoff and landing maneuvers. -DOCS TODO. Help appreciated. +## Highlights + +- Two takeoff modes: standard and high altitude +- Configurable takeoff and landing velocities +- Configurable takeoff trajectory direction +- Trajectory-based takeoff and landing with position tracking +- Direct ArduPilot takeoff command support +- Completion monitoring with distance and time thresholds +- State publishing for integration with higher-level systems + +## Topics + +### Subscriptions + +| Topic | Type | Description | +| ---------------------------------- | ------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------ | +| `trajectory_completion_percentage` | `std_msgs/Float32` | Current completion percentage of trajectory (from [trajectory_controller](../../c_controls/trajectory_controller/src/trajectory_controller.cpp)) | +| `tracking_point` | `airstack_msgs/Odometry` | Current tracking point for trajectory (from [trajectory_controller](../../c_controls/trajectory_controller/src/trajectory_controller.cpp)) | +| `odometry` | `nav_msgs/Odometry` | Robot odometry data | +| `ekf_active` | `std_msgs/Bool` | EKF status flag | +| `high_takeoff` | `std_msgs/Bool` | Flag to trigger high altitude takeoff | + +### Publications + +| Topic | Type | Description | +| --------------------- | --------------------------------- | -------------------------------------------------------- | +| `trajectory_override` | `airstack_msgs/TrajectoryXYZVYaw` | Generated takeoff/landing trajectory | +| `takeoff_state` | `std_msgs/String` | Current takeoff state ("NONE", "TAKING_OFF", "COMPLETE") | +| `landing_state` | `std_msgs/String` | Current landing state ("NONE", "LANDING", "COMPLETE") | + +## Services + +| Service | Type | Description | +| ----------------------------- | ------------------------------------- | ----------------------------------------- | +| `set_takeoff_landing_command` | `airstack_msgs/TakeoffLandingCommand` | Sets command mode (NONE, TAKEOFF, LAND) | +| `ardupilot_takeoff` | `std_srvs/Trigger` | Triggers direct ArduPilot takeoff command | + +## Parameters + +| Parameter | Type | Default | Description | +| -------------------------------------- | ----- | ------- | ------------------------------------------------------- | +| `takeoff_height` | float | 0.5 | Standard takeoff height in meters | +| `high_takeoff_height` | float | 1.2 | High altitude takeoff height in meters | +| `takeoff_velocity` | float | 0.3 | Velocity during takeoff in m/s | +| `landing_velocity` | float | 0.3 | Velocity during landing in m/s | +| `takeoff_acceptance_distance` | float | 0.1 | Maximum distance to consider takeoff complete in meters | +| `takeoff_acceptance_time` | float | 2.0 | Required time at acceptance distance in seconds | +| `landing_stationary_distance` | float | 0.02 | Maximum movement distance to consider landed in meters | +| `landing_acceptance_time` | float | 5.0 | Required time at stationary distance in seconds | +| `landing_tracking_point_ahead_time` | float | 5.0 | How far ahead the tracking point should be | +| `takeoff_path_roll` | float | 0.0 | Roll angle for takeoff path in degrees | +| `takeoff_path_pitch` | float | 0.0 | Pitch angle for takeoff path in degrees | +| `takeoff_path_relative_to_orientation` | bool | false | Whether to use robot's current orientation for takeoff | + +## Usage + +### Basic Usage + +To launch the TakeoffLandingPlanner node: + +```bash +ros2 run takeoff_landing_planner takeoff_landing_planner +``` + +### Initiating Takeoff + +To command a takeoff: + +```bash +ros2 service call /set_takeoff_landing_command airstack_msgs/srv/TakeoffLandingCommand "{command: 1}" +``` + +### Initiating Landing + +To command a landing: + +```bash +ros2 service call /set_takeoff_landing_command airstack_msgs/srv/TakeoffLandingCommand "{command: 2}" +``` + +### ArduPilot Takeoff + +To use direct ArduPilot takeoff commands: + +```bash +ros2 service call /ardupilot_takeoff std_srvs/srv/Trigger "{}" +``` + +## State Monitoring + +The node publishes the current state of takeoff and landing operations through the `takeoff_state` and `landing_state` topics. Monitor these topics to track the progress and completion of operations: + +```bash +ros2 topic echo /takeoff_state +ros2 topic echo /landing_state +``` + +## Dependencies + +- airstack_msgs +- nav_msgs +- mavros_msgs +- std_msgs +- std_srvs +- tf2_ros + +## Notes + +- The node uses TF2 to track relative positions between the robot and tracking points +- Landing completion detection relies on both position stability and tracking point metrics +- The default landing target is hardcoded (-10000.0) and should be parameterized in future versions diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp index 3cd6bf7b8..76b7337b5 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + #pragma once #include @@ -13,75 +33,77 @@ #include #include -class TakeoffLandingPlanner : public rclcpp::Node { - private: - // parameters - float takeoff_height, high_takeoff_height, takeoff_velocity, landing_velocity; - float takeoff_acceptance_distance, takeoff_acceptance_time; - float landing_stationary_distance, landing_acceptance_time; - float landing_tracking_point_ahead_time; - float takeoff_path_roll, takeoff_path_pitch; - bool takeoff_path_relative_to_orientation; +class TakeoffLandingPlanner : public rclcpp::Node +{ +private: + // parameters + float takeoff_height, high_takeoff_height, takeoff_velocity, landing_velocity; + float takeoff_acceptance_distance, takeoff_acceptance_time; + float landing_stationary_distance, landing_acceptance_time; + float landing_tracking_point_ahead_time; + float takeoff_path_roll, takeoff_path_pitch; + bool takeoff_path_relative_to_orientation; + + // variables + bool got_completion_percentage, is_tracking_point_received, got_robot_odom; + nav_msgs::msg::Odometry robot_odom; + airstack_msgs::msg::Odometry tracking_point_odom; + float completion_percentage; + // airstack_msgs::srv::TrajectoryMode track_mode_srv; + uint8_t current_command; - // variables - bool got_completion_percentage, is_tracking_point_received, got_robot_odom; - nav_msgs::msg::Odometry robot_odom; - airstack_msgs::msg::Odometry tracking_point_odom; - float completion_percentage; - // airstack_msgs::srv::TrajectoryMode track_mode_srv; - uint8_t current_command; + // takeoff variables + bool takeoff_is_newly_active; + bool takeoff_distance_check; + bool ekf_active; + rclcpp::Time takeoff_acceptance_start; + bool high_takeoff; + TakeoffTrajectory * takeoff_traj_gen; + TakeoffTrajectory * high_takeoff_traj_gen; - // takeoff variables - bool takeoff_is_newly_active; - bool takeoff_distance_check; - bool ekf_active; - rclcpp::Time takeoff_acceptance_start; - bool high_takeoff; - TakeoffTrajectory* takeoff_traj_gen; - TakeoffTrajectory* high_takeoff_traj_gen; + // land variables + bool land_is_newly_active; + std::list robot_odoms; + TakeoffTrajectory * landing_traj_gen; - // land variables - bool land_is_newly_active; - std::list robot_odoms; - TakeoffTrajectory* landing_traj_gen; + // subscribers + rclcpp::Subscription::SharedPtr completion_percentage_sub; + rclcpp::Subscription::SharedPtr tracking_point_sub; + rclcpp::Subscription::SharedPtr robot_odom_sub; + rclcpp::Subscription::SharedPtr ekf_active_sub; + rclcpp::Subscription::SharedPtr high_takeoff_sub; + tf2_ros::Buffer * tf_buffer; + tf2_ros::TransformListener * tf_listener; - // subscribers - rclcpp::Subscription::SharedPtr completion_percentage_sub; - rclcpp::Subscription::SharedPtr tracking_point_sub; - rclcpp::Subscription::SharedPtr robot_odom_sub; - rclcpp::Subscription::SharedPtr ekf_active_sub; - rclcpp::Subscription::SharedPtr high_takeoff_sub; - tf2_ros::Buffer* tf_buffer; - tf2_ros::TransformListener* tf_listener; + // publishers + rclcpp::Publisher::SharedPtr traj_override_pub; + rclcpp::Publisher::SharedPtr takeoff_state_pub; + rclcpp::Publisher::SharedPtr landing_state_pub; - // publishers - rclcpp::Publisher::SharedPtr traj_override_pub; - rclcpp::Publisher::SharedPtr takeoff_state_pub; - rclcpp::Publisher::SharedPtr landing_state_pub; + // services + rclcpp::CallbackGroup::SharedPtr service_callback_group; + rclcpp::Service::SharedPtr command_server; + rclcpp::Service::SharedPtr ardupilot_takeoff_server; + rclcpp::Client::SharedPtr takeoff_client; - // services - rclcpp::CallbackGroup::SharedPtr service_callback_group; - rclcpp::Service::SharedPtr command_server; - rclcpp::Service::SharedPtr ardupilot_takeoff_server; - rclcpp::Client::SharedPtr takeoff_client; + // timers + rclcpp::TimerBase::SharedPtr timer; - // timers - rclcpp::TimerBase::SharedPtr timer; + // callbacks + void completion_percentage_callback(std_msgs::msg::Float32::SharedPtr msg); + void tracking_point_callback(airstack_msgs::msg::Odometry::SharedPtr msg); + void robot_odom_callback(nav_msgs::msg::Odometry::SharedPtr msg); + void ekf_active_callback(std_msgs::msg::Bool::SharedPtr msg); + void high_takeoff_callback(std_msgs::msg::Bool::SharedPtr msg); - // callbacks - void completion_percentage_callback(std_msgs::msg::Float32::SharedPtr msg); - void tracking_point_callback(airstack_msgs::msg::Odometry::SharedPtr msg); - void robot_odom_callback(nav_msgs::msg::Odometry::SharedPtr msg); - void ekf_active_callback(std_msgs::msg::Bool::SharedPtr msg); - void high_takeoff_callback(std_msgs::msg::Bool::SharedPtr msg); + void set_takeoff_landing_command( + const airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr request, + airstack_msgs::srv::TakeoffLandingCommand::Response::SharedPtr response); + void ardupilot_takeoff( + const std::shared_ptr request, + std::shared_ptr response); - void set_takeoff_landing_command( - const airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr request, - airstack_msgs::srv::TakeoffLandingCommand::Response::SharedPtr response); - void ardupilot_takeoff(const std::shared_ptr request, - std::shared_ptr response); - - public: - TakeoffLandingPlanner(); - void timer_callback(); -}; +public: + TakeoffLandingPlanner(); + void timer_callback(); +}; diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp index bdcc04683..86b26ea5a 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp @@ -1,282 +1,328 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 -TakeoffLandingPlanner::TakeoffLandingPlanner() : rclcpp::Node("takeoff_landing_planner") { - // init parameters - takeoff_height = airstack::get_param(this, "takeoff_height", 0.5); - high_takeoff_height = airstack::get_param(this, "high_takeoff_height", 1.2); - takeoff_velocity = airstack::get_param(this, "takeoff_velocity", 0.3); - landing_velocity = airstack::get_param(this, "landing_velocity", 0.3); - takeoff_acceptance_distance = airstack::get_param(this, "takeoff_acceptance_distance", 0.1); - takeoff_acceptance_time = airstack::get_param(this, "takeoff_acceptance_time", 2.0); - landing_stationary_distance = airstack::get_param(this, "landing_stationary_distance", 0.02); - landing_acceptance_time = airstack::get_param(this, "landing_acceptance_time", 5.0); - landing_tracking_point_ahead_time = - airstack::get_param(this, "landing_tracking_point_ahead_time", 5.0); - takeoff_path_roll = airstack::get_param(this, "takeoff_path_roll", 0.) * M_PI / 180.; - takeoff_path_pitch = airstack::get_param(this, "takeoff_path_pitch", 0.) * M_PI / 180.; - takeoff_path_relative_to_orientation = - airstack::get_param(this, "takeoff_path_relative_to_orientation", false); - - // init subscribers - tf_buffer = new tf2_ros::Buffer(this->get_clock()); - tf_listener = new tf2_ros::TransformListener(*tf_buffer); - completion_percentage_sub = this->create_subscription( - "trajectory_completion_percentage", 1, - std::bind(&TakeoffLandingPlanner::completion_percentage_callback, this, - std::placeholders::_1)); - tracking_point_sub = this->create_subscription( - "tracking_point", 1, - std::bind(&TakeoffLandingPlanner::tracking_point_callback, this, std::placeholders::_1)); - robot_odom_sub = this->create_subscription( - "odometry", 1, - std::bind(&TakeoffLandingPlanner::robot_odom_callback, this, std::placeholders::_1)); - ekf_active_sub = this->create_subscription( - "ekf_active", 1, - std::bind(&TakeoffLandingPlanner::ekf_active_callback, this, std::placeholders::_1)); - high_takeoff_sub = this->create_subscription( - "high_takeoff", 1, - std::bind(&TakeoffLandingPlanner::high_takeoff_callback, this, std::placeholders::_1)); - - // init publishers - traj_override_pub = - this->create_publisher("trajectory_override", 1); - takeoff_state_pub = this->create_publisher("takeoff_state", 1); - landing_state_pub = this->create_publisher("landing_state", 1); - - // init services - service_callback_group = - this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - takeoff_client = this->create_client("mavros/cmd/takeoff", rmw_qos_profile_services_default, - service_callback_group); - command_server = this->create_service( - "set_takeoff_landing_command", - std::bind(&TakeoffLandingPlanner::set_takeoff_landing_command, this, std::placeholders::_1, - std::placeholders::_2)); - ardupilot_takeoff_server = this->create_service("ardupilot_takeoff", - std::bind(&TakeoffLandingPlanner::ardupilot_takeoff, this, - std::placeholders::_1, std::placeholders::_2)); - - // init variables - got_completion_percentage = false; - is_tracking_point_received = false; - - // TODO set this back to false and remove robot_odom initialization - got_robot_odom = false; - - // track_mode_srv.request.mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK; - high_takeoff = false; - takeoff_traj_gen = - new TakeoffTrajectory(takeoff_height, takeoff_velocity, takeoff_path_roll, - takeoff_path_pitch, takeoff_path_relative_to_orientation); - high_takeoff_traj_gen = - new TakeoffTrajectory(high_takeoff_height, takeoff_velocity, takeoff_path_roll, - takeoff_path_pitch, takeoff_path_relative_to_orientation); - // TODO: this landing point is hardcoded. it should be parameterized - landing_traj_gen = new TakeoffTrajectory(-10000., landing_velocity); - current_command = airstack_msgs::srv::TakeoffLandingCommand::Request::NONE; - - completion_percentage = 0.f; - takeoff_is_newly_active = true; - land_is_newly_active = true; - takeoff_distance_check = false; - ekf_active = false; - - - // timers - timer = rclcpp::create_timer(this, this->get_clock(), rclcpp::Duration::from_seconds(1. / 20.), - std::bind(&TakeoffLandingPlanner::timer_callback, this)); +TakeoffLandingPlanner::TakeoffLandingPlanner() +: rclcpp::Node("takeoff_landing_planner") +{ + // init parameters + takeoff_height = airstack::get_param(this, "takeoff_height", 0.5); + high_takeoff_height = airstack::get_param(this, "high_takeoff_height", 1.2); + takeoff_velocity = airstack::get_param(this, "takeoff_velocity", 0.3); + landing_velocity = airstack::get_param(this, "landing_velocity", 0.3); + takeoff_acceptance_distance = airstack::get_param(this, "takeoff_acceptance_distance", 0.1); + takeoff_acceptance_time = airstack::get_param(this, "takeoff_acceptance_time", 2.0); + landing_stationary_distance = airstack::get_param(this, "landing_stationary_distance", 0.02); + landing_acceptance_time = airstack::get_param(this, "landing_acceptance_time", 5.0); + landing_tracking_point_ahead_time = + airstack::get_param(this, "landing_tracking_point_ahead_time", 5.0); + takeoff_path_roll = airstack::get_param(this, "takeoff_path_roll", 0.) * M_PI / 180.; + takeoff_path_pitch = airstack::get_param(this, "takeoff_path_pitch", 0.) * M_PI / 180.; + takeoff_path_relative_to_orientation = + airstack::get_param(this, "takeoff_path_relative_to_orientation", false); + + // init subscribers + tf_buffer = new tf2_ros::Buffer(this->get_clock()); + tf_listener = new tf2_ros::TransformListener(*tf_buffer); + completion_percentage_sub = this->create_subscription( + "trajectory_completion_percentage", 1, + std::bind( + &TakeoffLandingPlanner::completion_percentage_callback, this, + std::placeholders::_1)); + tracking_point_sub = this->create_subscription( + "tracking_point", 1, + std::bind(&TakeoffLandingPlanner::tracking_point_callback, this, std::placeholders::_1)); + robot_odom_sub = this->create_subscription( + "odometry", 1, + std::bind(&TakeoffLandingPlanner::robot_odom_callback, this, std::placeholders::_1)); + ekf_active_sub = this->create_subscription( + "ekf_active", 1, + std::bind(&TakeoffLandingPlanner::ekf_active_callback, this, std::placeholders::_1)); + high_takeoff_sub = this->create_subscription( + "high_takeoff", 1, + std::bind(&TakeoffLandingPlanner::high_takeoff_callback, this, std::placeholders::_1)); + + // init publishers + traj_override_pub = + this->create_publisher("trajectory_override", 1); + takeoff_state_pub = this->create_publisher("takeoff_state", 1); + landing_state_pub = this->create_publisher("landing_state", 1); + + // init services + service_callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + takeoff_client = this->create_client( + "mavros/cmd/takeoff", rmw_qos_profile_services_default, + service_callback_group); + command_server = this->create_service( + "set_takeoff_landing_command", + std::bind( + &TakeoffLandingPlanner::set_takeoff_landing_command, this, std::placeholders::_1, + std::placeholders::_2)); + ardupilot_takeoff_server = this->create_service( + "ardupilot_takeoff", + std::bind( + &TakeoffLandingPlanner::ardupilot_takeoff, this, + std::placeholders::_1, std::placeholders::_2)); + + // init variables + got_completion_percentage = false; + is_tracking_point_received = false; + + // TODO set this back to false and remove robot_odom initialization + got_robot_odom = false; + + // track_mode_srv.request.mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK; + high_takeoff = false; + takeoff_traj_gen = + new TakeoffTrajectory( + takeoff_height, takeoff_velocity, takeoff_path_roll, + takeoff_path_pitch, takeoff_path_relative_to_orientation); + high_takeoff_traj_gen = + new TakeoffTrajectory( + high_takeoff_height, takeoff_velocity, takeoff_path_roll, + takeoff_path_pitch, takeoff_path_relative_to_orientation); + // TODO: this landing point is hardcoded. it should be parameterized + landing_traj_gen = new TakeoffTrajectory(-10000., landing_velocity); + current_command = airstack_msgs::srv::TakeoffLandingCommand::Request::NONE; + + completion_percentage = 0.f; + takeoff_is_newly_active = true; + land_is_newly_active = true; + takeoff_distance_check = false; + ekf_active = false; + + + // timers + timer = rclcpp::create_timer( + this, this->get_clock(), rclcpp::Duration::from_seconds(1. / 20.), + std::bind(&TakeoffLandingPlanner::timer_callback, this)); } -void TakeoffLandingPlanner::timer_callback() { +void TakeoffLandingPlanner::timer_callback() +{ // RCLCPP_INFO_STREAM(this->get_logger(), -// "conditions: " << got_completion_percentage << " " << is_tracking_point_received << " " << got_robot_odom); - if (!got_completion_percentage || !is_tracking_point_received || !got_robot_odom) return; - - std_msgs::msg::String takeoff_state, landing_state; - takeoff_state.data = landing_state.data = "NONE"; - - // takeoff - if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::TAKEOFF) { - takeoff_state.data = "TAKING_OFF"; - - if (completion_percentage >= 100.f) { - // get distance between tracking point and robot odom - float distance = std::numeric_limits::max(); - try { - tf2::Stamped transform; - geometry_msgs::msg::TransformStamped t = tf_buffer->lookupTransform( - tracking_point_odom.header.frame_id, robot_odom.header.frame_id, - robot_odom.header.stamp, rclcpp::Duration::from_seconds(0.1)); - tf2::fromMsg(t, transform); - - tf2::Vector3 tracking_point_position = - tflib::to_tf(tracking_point_odom.pose.position); - tf2::Vector3 robot_odom_position = - transform * tflib::to_tf(robot_odom.pose.pose.position); - - distance = tracking_point_position.distance(robot_odom_position); - } catch (const tf2::TransformException& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), - "TransformException in TakeoffMonitor: " << ex.what()); - } - - // ROS_INFO_STREAM("distance: " << distance); - - // check if distance meets the takeoff acceptance threshold - if (distance <= takeoff_acceptance_distance) { - if (!takeoff_distance_check) { - takeoff_distance_check = true; - takeoff_acceptance_start = robot_odom.header.stamp; - } - - // check if the distance threshold has been met for the required amount of time - // ROS_INFO_STREAM("duration: " << (robot_odom.header.stamp - - // takeoff_acceptance_start).toSec() << " / " << takeoff_acceptance_time); - if ((rclcpp::Time(robot_odom.header.stamp) - takeoff_acceptance_start).seconds() >= - takeoff_acceptance_time || - ekf_active) { - takeoff_state.data = "COMPLETE"; - } - } else - takeoff_distance_check = false; +// "conditions: " << got_completion_percentage << " " << is_tracking_point_received << " " << got_robot_odom); + if (!got_completion_percentage || !is_tracking_point_received || !got_robot_odom) {return;} + + std_msgs::msg::String takeoff_state, landing_state; + takeoff_state.data = landing_state.data = "NONE"; + + // takeoff + if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::TAKEOFF) { + takeoff_state.data = "TAKING_OFF"; + + if (completion_percentage >= 100.f) { + // get distance between tracking point and robot odom + float distance = std::numeric_limits::max(); + try { + tf2::Stamped transform; + geometry_msgs::msg::TransformStamped t = tf_buffer->lookupTransform( + tracking_point_odom.header.frame_id, robot_odom.header.frame_id, + robot_odom.header.stamp, rclcpp::Duration::from_seconds(0.1)); + tf2::fromMsg(t, transform); + + tf2::Vector3 tracking_point_position = + tflib::to_tf(tracking_point_odom.pose.position); + tf2::Vector3 robot_odom_position = + transform * tflib::to_tf(robot_odom.pose.pose.position); + + distance = tracking_point_position.distance(robot_odom_position); + } catch (const tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "TransformException in TakeoffMonitor: " << ex.what()); + } + + // ROS_INFO_STREAM("distance: " << distance); + + // check if distance meets the takeoff acceptance threshold + if (distance <= takeoff_acceptance_distance) { + if (!takeoff_distance_check) { + takeoff_distance_check = true; + takeoff_acceptance_start = robot_odom.header.stamp; } - } - // land - if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::LAND) { - landing_state.data = "LANDING"; - - // check if the robot has been moving - if (robot_odoms.size() > 2) { - nav_msgs::msg::Odometry initial_odom = robot_odoms.front(); - float time_diff = - (rclcpp::Time(robot_odom.header.stamp) - rclcpp::Time(initial_odom.header.stamp)) - .seconds(); - - // landing_detected = time_diff > landing_acceptance_time; - bool time_check = time_diff > landing_acceptance_time; - while (!robot_odoms.empty() && (rclcpp::Time(robot_odom.header.stamp) - - rclcpp::Time(robot_odoms.front().header.stamp)) - .seconds() > landing_acceptance_time) { - initial_odom = robot_odoms.front(); - robot_odoms.pop_front(); - } - robot_odoms.push_front(initial_odom); - - bool distance_check = false; - for (auto it = robot_odoms.begin(); time_check && it != robot_odoms.end(); it++) { - float distance = tflib::to_tf(robot_odoms.begin()->pose.pose.position) - .distance(tflib::to_tf(it->pose.pose.position)); - distance_check = distance <= landing_stationary_distance; - if (!distance_check) { - // ROS_INFO_STREAM("distance: " << distance); - break; - } - } - - float z_distance = 100000.f; - try { - tf2::Stamped transform; - geometry_msgs::msg::TransformStamped t = tf_buffer->lookupTransform( - tracking_point_odom.header.frame_id, robot_odom.header.frame_id, - robot_odom.header.stamp, rclcpp::Duration::from_seconds(0.1)); - tf2::fromMsg(t, transform); - tf2::Vector3 tracking_point_position = - tflib::to_tf(tracking_point_odom.pose.position); - tf2::Vector3 robot_odom_position = - transform * tflib::to_tf(robot_odom.pose.pose.position); - - z_distance = robot_odom_position.z() - tracking_point_position.z(); - } catch (const tf2::TransformException& ex) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "TransformException in TakeoffMonitor landing tf lookup: " << ex.what()); - } - bool tracking_point_check = - (z_distance / landing_velocity) > landing_tracking_point_ahead_time; - RCLCPP_INFO_STREAM( - this->get_logger(), - "landing: " << z_distance << " " << (z_distance / landing_velocity) << " " - << landing_tracking_point_ahead_time << " " << tracking_point_check); - - // ROS_INFO_STREAM("LANDING CHECK: " << time_diff << " " << time_check << " " << - // distance_check); - - if (time_check && (distance_check || tracking_point_check)) - landing_state.data = "COMPLETE"; + // check if the distance threshold has been met for the required amount of time + // ROS_INFO_STREAM("duration: " << (robot_odom.header.stamp - + // takeoff_acceptance_start).toSec() << " / " << takeoff_acceptance_time); + if ((rclcpp::Time(robot_odom.header.stamp) - takeoff_acceptance_start).seconds() >= + takeoff_acceptance_time || + ekf_active) + { + takeoff_state.data = "COMPLETE"; + } + } else { + takeoff_distance_check = false; + } + } + } + + // land + if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::LAND) { + landing_state.data = "LANDING"; + + // check if the robot has been moving + if (robot_odoms.size() > 2) { + nav_msgs::msg::Odometry initial_odom = robot_odoms.front(); + float time_diff = + (rclcpp::Time(robot_odom.header.stamp) - rclcpp::Time(initial_odom.header.stamp)) + .seconds(); + + // landing_detected = time_diff > landing_acceptance_time; + bool time_check = time_diff > landing_acceptance_time; + while (!robot_odoms.empty() && (rclcpp::Time(robot_odom.header.stamp) - + rclcpp::Time(robot_odoms.front().header.stamp)) + .seconds() > landing_acceptance_time) + { + initial_odom = robot_odoms.front(); + robot_odoms.pop_front(); + } + robot_odoms.push_front(initial_odom); + + bool distance_check = false; + for (auto it = robot_odoms.begin(); time_check && it != robot_odoms.end(); it++) { + float distance = tflib::to_tf(robot_odoms.begin()->pose.pose.position) + .distance(tflib::to_tf(it->pose.pose.position)); + distance_check = distance <= landing_stationary_distance; + if (!distance_check) { + // ROS_INFO_STREAM("distance: " << distance); + break; } + } + + float z_distance = 100000.f; + try { + tf2::Stamped transform; + geometry_msgs::msg::TransformStamped t = tf_buffer->lookupTransform( + tracking_point_odom.header.frame_id, robot_odom.header.frame_id, + robot_odom.header.stamp, rclcpp::Duration::from_seconds(0.1)); + tf2::fromMsg(t, transform); + tf2::Vector3 tracking_point_position = + tflib::to_tf(tracking_point_odom.pose.position); + tf2::Vector3 robot_odom_position = + transform * tflib::to_tf(robot_odom.pose.pose.position); + + z_distance = robot_odom_position.z() - tracking_point_position.z(); + } catch (const tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "TransformException in TakeoffMonitor landing tf lookup: " << ex.what()); + } + bool tracking_point_check = + (z_distance / landing_velocity) > landing_tracking_point_ahead_time; + RCLCPP_INFO_STREAM( + this->get_logger(), + "landing: " << z_distance << " " << (z_distance / landing_velocity) << " " + << landing_tracking_point_ahead_time << " " << tracking_point_check); + + // ROS_INFO_STREAM("LANDING CHECK: " << time_diff << " " << time_check << " " << + // distance_check); + + if (time_check && (distance_check || tracking_point_check)) { + landing_state.data = "COMPLETE"; + } } + } - takeoff_state_pub->publish(takeoff_state); - landing_state_pub->publish(landing_state); + takeoff_state_pub->publish(takeoff_state); + landing_state_pub->publish(landing_state); } // callbacks -void TakeoffLandingPlanner::completion_percentage_callback(std_msgs::msg::Float32::SharedPtr msg) { - got_completion_percentage = true; - completion_percentage = msg->data; +void TakeoffLandingPlanner::completion_percentage_callback(std_msgs::msg::Float32::SharedPtr msg) +{ + got_completion_percentage = true; + completion_percentage = msg->data; } -void TakeoffLandingPlanner::tracking_point_callback(airstack_msgs::msg::Odometry::SharedPtr msg) { - is_tracking_point_received = true; - tracking_point_odom = *msg; +void TakeoffLandingPlanner::tracking_point_callback(airstack_msgs::msg::Odometry::SharedPtr msg) +{ + is_tracking_point_received = true; + tracking_point_odom = *msg; } -void TakeoffLandingPlanner::robot_odom_callback(nav_msgs::msg::Odometry::SharedPtr msg) { - got_robot_odom = true; - robot_odom = *msg; +void TakeoffLandingPlanner::robot_odom_callback(nav_msgs::msg::Odometry::SharedPtr msg) +{ + got_robot_odom = true; + robot_odom = *msg; - if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::LAND) - robot_odoms.push_back(*msg); + if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::LAND) { + robot_odoms.push_back(*msg); + } } -void TakeoffLandingPlanner::ekf_active_callback(std_msgs::msg::Bool::SharedPtr msg) { - ekf_active = msg->data; +void TakeoffLandingPlanner::ekf_active_callback(std_msgs::msg::Bool::SharedPtr msg) +{ + ekf_active = msg->data; } -void TakeoffLandingPlanner::high_takeoff_callback(std_msgs::msg::Bool::SharedPtr msg) { - high_takeoff = msg->data; +void TakeoffLandingPlanner::high_takeoff_callback(std_msgs::msg::Bool::SharedPtr msg) +{ + high_takeoff = msg->data; } void TakeoffLandingPlanner::set_takeoff_landing_command( - const airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr request, - airstack_msgs::srv::TakeoffLandingCommand::Response::SharedPtr response) { - current_command = request->command; - - if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::NONE) { - } else if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::TAKEOFF) { - // put the trajectory controller into track mode - // traj_mode_client.call(track_mode_srv); - // publish a takeoff trajectory - RCLCPP_INFO_STREAM(get_logger(), "takeofflanding 1"); - - airstack_msgs::msg::Odometry takeoff_starting_point = tracking_point_odom; - if (got_robot_odom && takeoff_path_relative_to_orientation) - takeoff_starting_point.pose.orientation = robot_odom.pose.pose.orientation; - - if (high_takeoff) { - RCLCPP_INFO_STREAM(get_logger(), "takeofflanding hightakeoff"); - traj_override_pub->publish( - high_takeoff_traj_gen->get_trajectory(takeoff_starting_point)); - } else { - RCLCPP_INFO_STREAM(get_logger(), "takeofflanding lowtakeoff"); - traj_override_pub->publish(takeoff_traj_gen->get_trajectory(takeoff_starting_point)); - } - } else if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::LAND) { - robot_odoms.clear(); - // put the trajectory controller into track mode - // traj_mode_client.call(track_mode_srv); - // publish a landing trajectory - traj_override_pub->publish(landing_traj_gen->get_trajectory(tracking_point_odom)); + const airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr request, + airstack_msgs::srv::TakeoffLandingCommand::Response::SharedPtr response) +{ + current_command = request->command; + + if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::NONE) { + } else if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::TAKEOFF) { + // put the trajectory controller into track mode + // traj_mode_client.call(track_mode_srv); + // publish a takeoff trajectory + RCLCPP_INFO_STREAM(get_logger(), "takeofflanding 1"); + + airstack_msgs::msg::Odometry takeoff_starting_point = tracking_point_odom; + if (got_robot_odom && takeoff_path_relative_to_orientation) { + takeoff_starting_point.pose.orientation = robot_odom.pose.pose.orientation; } - RCLCPP_INFO_STREAM(get_logger(), "takeofflanding end"); - response->accepted = true; + if (high_takeoff) { + RCLCPP_INFO_STREAM(get_logger(), "takeofflanding hightakeoff"); + traj_override_pub->publish( + high_takeoff_traj_gen->get_trajectory(takeoff_starting_point)); + } else { + RCLCPP_INFO_STREAM(get_logger(), "takeofflanding lowtakeoff"); + traj_override_pub->publish(takeoff_traj_gen->get_trajectory(takeoff_starting_point)); + } + } else if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::LAND) { + robot_odoms.clear(); + // put the trajectory controller into track mode + // traj_mode_client.call(track_mode_srv); + // publish a landing trajectory + traj_override_pub->publish(landing_traj_gen->get_trajectory(tracking_point_odom)); + } + RCLCPP_INFO_STREAM(get_logger(), "takeofflanding end"); + + response->accepted = true; } -void TakeoffLandingPlanner::ardupilot_takeoff(const std::shared_ptr request, - std::shared_ptr response){ +void TakeoffLandingPlanner::ardupilot_takeoff( + const std::shared_ptr request, + std::shared_ptr response) +{ mavros_msgs::srv::CommandTOL::Request::SharedPtr takeoff_request = std::make_shared(); takeoff_request->altitude = takeoff_height; @@ -288,12 +334,13 @@ void TakeoffLandingPlanner::ardupilot_takeoff(const std::shared_ptrsuccess = takeoff_result.get()->success; } -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - std::shared_ptr node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - rclcpp::shutdown(); - return 0; -} +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + std::shared_ptr node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/test/README.md b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/test/README.md new file mode 100644 index 000000000..4b95a1887 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/test/README.md @@ -0,0 +1,171 @@ +# TakeoffLandingPlanner Launch Tests + +This file contains integration tests for the TakeoffLandingPlanner ROS2 node using the ROS2 launch testing framework. The tests verify that the node correctly handles takeoff and landing commands, generates appropriate trajectories, and tracks completion states. + +## Overview + +The test harness launches the TakeoffLandingPlanner node with test-specific parameters and performs a series of tests to verify its behavior. It simulates sensor data, sends commands, and checks the responses to ensure the node operates as expected. + +## Test Configuration + +The test uses a specific YAML configuration file: + +``` +test/config/test_takeoff_landing_planner.yaml +``` + +This configuration file contains test-specific parameters for the TakeoffLandingPlanner node, separate from the default parameters used in production. This allows for controlled testing without modifying the production configuration. + +The parameter file is passed to both the target TakeoffLandingPlanner node via the `ParameterFile` class in the launch description, and is also parsed using PyYAML within the test class itself: + +```python +# Send parameters to the node being tested +takeoff_landing_planner = Node( + package="takeoff_landing_planner", + executable="takeoff_landing_planner", + parameters=[ + ParameterFile( + param_file=param_file, + allow_substs=True, + ) + ], + # ... +) + +# Later in the test class, parse the same file for test verification +with open(param_file, "r") as file: + param_file = yaml.safe_load(file) +cls.params = param_file["/**"]["ros__parameters"] +``` + +This dual use ensures that the test logic operates with the same parameter values as the node being tested. + +## Test Execution Process + +The test follows the ROS2 launch testing pattern as described in the [ROS2 Launch Testing Documentation](https://github.com/ros2/launch/tree/master/launch_testing/doc): + +1. A test description is generated using `generate_test_description()` +2. The TakeoffLandingPlanner node is launched with test-specific parameters +3. A timer ensures a 1-second delay before starting tests ([recommended practice](https://github.com/ros2/launch/blob/master/launch_testing/doc/nodes.md#allowing-nodes-time-to-start)) to allow the node to initialize +4. Tests are run against the live node + +## Unit Tests + +The test suite includes the following test cases: + +### `test_takeoff_parameters` + +Verifies that all required parameters are properly loaded from the configuration file: + +- `takeoff_height` +- `high_takeoff_height` +- `takeoff_landing_velocity` +- `takeoff_acceptance_distance` +- And others... + +### `test_node_active` + +Ensures that the node is active and its services are available by checking that the service client can connect within a timeout period. + +### `test_service_response` + +Tests the service interface by: + +1. Sending takeoff, landing, and reset commands +2. Verifying that responses indicate acceptance +3. Verifying correct response types + +### `test_takeoff_and_landing` + +A comprehensive integration test that: + +1. Simulates the full takeoff sequence: + - Publishes initial ground position + - Sends takeoff command + - Checks generated trajectory parameters + - Simulates in-progress flight + - Verifies "TAKING_OFF" state + - Simulates completion conditions + - Verifies "COMPLETE" state +2. Simulates the full landing sequence: + - Sends landing command + - Checks generated trajectory + - Simulates in-progress landing + - Verifies "LANDING" state + - Simulates landed condition + - Verifies "COMPLETE" state + +## Helper Methods + +The test includes several helper methods: + +- `call_service(command)`: Sends commands to the node and verifies response +- `reset_states()`: Resets internal test state between test cases +- `yaw_to_quat(yaw)`: Converts yaw angle to quaternion +- `get_fake_odom(kind, yaw, curr_odom)`: Generates simulated odometry messages +- `get_fake_ref_point(kind)`: Generates simulated reference points +- `check_takeoff_trajectory()`: Verifies trajectory parameters +- `wait_for(secs)`: Waits for specified time while processing ROS callbacks + +## Simulated Message Types + +The test creates fake data of different types to simulate various conditions: + +- `TYPE_ODOM_GROUNDED`: Robot on the ground +- `TYPE_ODOM_TAKEOFF_PENDING`: Robot in process of taking off +- `TYPE_ODOM_TAKEOFF_COMPLETE`: Robot successfully taken off +- `TYPE_ODOM_LAND_PENDING`: Robot in process of landing +- `TYPE_ODOM_LAND_COMPLETE`: Robot successfully landed + +## Further Reading + +For more information on ROS2 launch testing: + +- [Launch Testing Nodes Documentation](https://github.com/ros2/launch/blob/master/launch_testing/doc/nodes.md) +- [Launch Testing Examples](https://github.com/ros2/launch/tree/master/launch_testing/examples) +- [Launch Testing API Reference](https://github.com/ros2/launch/blob/master/launch_testing/doc/api.md) + +## CMake Integration + +The tests are integrated into the CMake build system using isolated launch testing, which provides better isolation between test runs. The CMakeLists.txt contains the following configuration: + +```cmake +find_package(ament_cmake_ros REQUIRED) +find_package(launch_testing_ament_cmake REQUIRED) + +# Isolated Integration Testing from (https://arnebaeyens.com/blog/2024/ros2-integration-testing/) +function(add_ros_isolated_launch_test path) + set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py") + add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) +endfunction() + +if(BUILD_TESTING) + # Linter configuration is skipped for brevity + + add_ros_isolated_launch_test( + test/scripts/test_takeoff_landing_planner.py + ) +endif() +``` + +This setup: + +1. Requires the necessary ROS2 testing packages +2. Defines a custom function `add_ros_isolated_launch_test` that uses a special runner script for better test isolation +3. Registers our test script with this isolated runner + +The isolated testing approach helps prevent interference between test runs and improves test reliability, as described in [this reference article](https://arnebaeyens.com/blog/2024/ros2-integration-testing/). + +## Running the Tests + +To run these tests: + +```bash +colcon test --packages-select takeoff_landing_planner --event-handlers=console_direct+ +``` + +For more verbose output: + +```bash +colcon test --packages-select takeoff_landing_planner --event-handlers=console_direct+ --pytest-args -v +``` diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/test/config/test_takeoff_landing_planner.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/test/config/test_takeoff_landing_planner.yaml new file mode 100644 index 000000000..38f909e79 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/test/config/test_takeoff_landing_planner.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + takeoff_height: 2.0 + high_takeoff_height: 10.0 + takeoff_landing_velocity: 0.3 + takeoff_acceptance_distance: 0.3 + takeoff_acceptance_time: 1.0 + landing_stationary_distance: 0.02 + landing_acceptance_time: 5.0 + landing_tracking_point_ahead_time: 5.0 + + takeoff_path_roll: 20. # in degrees + takeoff_path_pitch: 20. # in degrees + takeoff_path_relative_to_orientation: true diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/test/scripts/test_takeoff_landing_planner.py b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/test/scripts/test_takeoff_landing_planner.py new file mode 100644 index 000000000..5dceec5d7 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/test/scripts/test_takeoff_landing_planner.py @@ -0,0 +1,390 @@ +# Standard library imports +import os +import math +import unittest +from copy import deepcopy + +# Third-party imports +import pytest +import yaml + +# ROS2 imports +import rclpy +import rclpy.time +from ament_index_python.packages import get_package_share_directory +from geometry_msgs.msg import Quaternion +from nav_msgs.msg import Odometry +from std_msgs.msg import Float32, String + +# Airstack-specific imports +from airstack_msgs.msg import TrajectoryXYZVYaw +from airstack_msgs.msg import Odometry as TrackingPoint +from airstack_msgs.srv import TakeoffLandingCommand + +# Launch-related imports +import launch +import launch_testing +import launch_testing.actions +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile + + +@pytest.mark.launch_test +def generate_test_description(): + # pkg_name = "takeoff_landing_planner" + pkg_path = get_package_share_directory("takeoff_landing_planner") + param_file = os.path.join( + pkg_path, "test", "config", "test_takeoff_landing_planner.yaml" + ) + + # Launch the TakeoffLandingPlanner node + takeoff_landing_planner = Node( + package="takeoff_landing_planner", + executable="takeoff_landing_planner", + name="takeoff_landing_planner", + parameters=[ + ParameterFile( + param_file=param_file, + allow_substs=True, + ) + ], + arguments=["--ros-args", "--log-level", "fatal"], + ) + + return ( + launch.LaunchDescription( + [ + takeoff_landing_planner, + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + {"param_file": param_file}, + ) + + +class TestTakeoff(unittest.TestCase): + @classmethod + def setUpClass(cls, param_file): + # Initialize ROS2 + rclpy.init() + + # Create a ROS2 node for testing + cls.node = rclpy.create_node("test_takeoff_node") + + # Load the YAML file + with open(param_file, "r") as file: + param_file = yaml.safe_load(file) + + # Parameter dictionary + cls.params = param_file["/**"]["ros__parameters"] + + # Different commands + cls.CMD_TAKEOFF = TakeoffLandingCommand.Request.TAKEOFF + cls.CMD_LAND = TakeoffLandingCommand.Request.LAND + cls.CMD_RESET = TakeoffLandingCommand.Request.NONE + + # Different fake odoms for testing different scenarios + cls.TYPE_ODOM_GROUNDED = 0 + cls.TYPE_ODOM_TAKEOFF_PENDING = 1 + cls.TYPE_ODOM_TAKEOFF_COMPLETE = 2 + cls.TYPE_ODOM_LAND_PENDING = 3 + cls.TYPE_ODOM_LAND_COMPLETE = 4 + + # Service Client + cls.client = cls.node.create_client( + TakeoffLandingCommand, "set_takeoff_landing_command" + ) + + # Publishers + cls.odom_pub = cls.node.create_publisher(Odometry, "odometry", 1) + cls.ref_point_pub = cls.node.create_publisher( + TrackingPoint, "tracking_point", 1 + ) + cls.progress_pub = cls.node.create_publisher( + Float32, "trajectory_completion_percentage", 1 + ) + + # Other vars + cls.flag_recvd_traj = False + cls.latest_traj = None + cls.takeoff_state = String(data="NONE") + cls.landing_state = String(data="NONE") + + @classmethod + def tearDownClass(cls): + # Clean up the node + cls.node.destroy_node() + # Shutdown ROS2 + rclpy.shutdown() + + def setUp(cls): + pass + + def tearDown(cls): + pass + + def test_takeoff_parameters(cls): + # Test that the parameters are loaded correctly + cls.assertIn("takeoff_height", cls.params) + cls.assertIn("high_takeoff_height", cls.params) + cls.assertIn("takeoff_landing_velocity", cls.params) + cls.assertIn("takeoff_acceptance_distance", cls.params) + cls.assertIn("takeoff_acceptance_time", cls.params) + cls.assertIn("landing_stationary_distance", cls.params) + cls.assertIn("landing_acceptance_time", cls.params) + cls.assertIn("landing_tracking_point_ahead_time", cls.params) + cls.assertIn("takeoff_path_roll", cls.params) + cls.assertIn("takeoff_path_pitch", cls.params) + cls.assertIn("takeoff_path_relative_to_orientation", cls.params) + + def test_node_active(cls): + # Test that the takeoff service is available + cls.assertTrue(cls.client.wait_for_service(timeout_sec=5.0)) + + # @classmethod + def call_service(cls, command): + request = TakeoffLandingCommand.Request() + request.command = command + future = cls.client.call_async(request) + + rclpy.spin_until_future_complete(cls.node, future, timeout_sec=5.0) + + cls.assertTrue(future.done(), "Service call timed out") + resp = future.result() + cls.assertIsNotNone(resp) + return resp + + # @classmethod + def reset_states(cls): + cls.call_service(cls.CMD_RESET) + cls.flag_recvd_traj = False + cls.latest_traj = None + cls.progress_pub.publish(Float32(data=0.0)) + + def test_service_response(cls): + # Takeoff Service Response Check + resp = cls.call_service(cls.CMD_TAKEOFF) + cls.assertIsNotNone(resp) + cls.assertTrue(resp.accepted) + + # # Reset internal states + cls.reset_states() + + # Landing Service Response Check + resp = cls.call_service(cls.CMD_LAND) + cls.assertIsNotNone(resp) + cls.assertTrue(resp.accepted) + + # Reset internal states + cls.reset_states() + + def yaw_to_quat(cls, yaw): + q = Quaternion() + q.x = q.y = 0.0 + q.z = math.sin(yaw / 2) + q.w = math.cos(yaw / 2) + + return q + + def get_fake_odom(cls, kind, yaw=0, curr_odom=Odometry()): + fake_odom = Odometry() + fake_odom.header.stamp = cls.node.get_clock().now().to_msg() + fake_odom.header.frame_id = "map" + fake_odom.child_frame_id = "robot" + if kind == cls.TYPE_ODOM_GROUNDED: + fake_odom.pose.pose.position.x = 0.0 + fake_odom.pose.pose.position.y = 0.0 + fake_odom.pose.pose.position.z = 0.0 + fake_odom.pose.pose.orientation = cls.yaw_to_quat(yaw) + elif kind == cls.TYPE_ODOM_TAKEOFF_PENDING: + if cls.flag_recvd_traj: + fake_odom.pose.pose.position = deepcopy( + cls.latest_traj.waypoints[-1].position + ) + fake_odom.pose.pose.position.z -= ( + 1.5 * cls.params["takeoff_acceptance_distance"] + ) + fake_odom.pose.pose.orientation.w = 1.0 + elif kind == cls.TYPE_ODOM_TAKEOFF_COMPLETE: + if cls.flag_recvd_traj: + fake_odom.pose.pose.position = cls.latest_traj.waypoints[-1].position + fake_odom.pose.pose.orientation.w = 1.0 + elif kind == cls.TYPE_ODOM_LAND_PENDING: + if cls.flag_recvd_traj and curr_odom is not None: + fake_odom.pose.pose.position = deepcopy(curr_odom.pose.pose.position) + fake_odom.pose.pose.position.z -= 0.5 * cls.params["takeoff_height"] + # This one is for tracking point + elif kind == cls.TYPE_ODOM_LAND_COMPLETE: + if cls.flag_recvd_traj: + fake_odom.pose.pose.position = deepcopy(curr_odom.pose.pose.position) + fake_odom.pose.pose.position.z = -1e5 + + return fake_odom + + # Convert Odometry to TrackingPoint + def get_fake_ref_point(cls, kind): + odom = cls.get_fake_odom(kind) + ref_point = TrackingPoint() + ref_point.header = odom.header + ref_point.child_frame_id = odom.child_frame_id + ref_point.pose = odom.pose.pose + + return ref_point + + # TODO (@kavin-cmu): Split this into smaller unit tests + def check_takeoff_trajectory(cls): + # Check if we have a trajectory + cls.assertIsNotNone(cls.latest_traj, "Dont have a valid trajectory!") + + # Get first and last waypoints + beg_wp = cls.latest_traj.waypoints[0] + end_wp = cls.latest_traj.waypoints[-1] + + # Check frame + cls.assertEqual(cls.latest_traj.header.frame_id, "map") + + # Check if first waypoint is same as the odom that we sent + cls.assertEqual( + beg_wp.position, + cls.get_fake_odom(cls.TYPE_ODOM_GROUNDED).pose.pose.position, + ) + + # Check if the velocity at first waypoint is as expected + cls.assertAlmostEqual( + beg_wp.velocity, cls.params["takeoff_landing_velocity"], delta=1e-7 + ) + + dx = end_wp.position.x - beg_wp.position.x + dy = end_wp.position.y - beg_wp.position.y + dz = end_wp.position.z - beg_wp.position.z + + # Check if final waypoint is at the correct height + cls.assertEqual(dz, cls.params["takeoff_height"]) + + # Check path pitch angle + cls.assertAlmostEqual( + math.degrees(math.asin(dx / dz)), + cls.params["takeoff_path_pitch"], + delta=1e-5, + ) + + # Check path roll angle + cls.assertAlmostEqual( + math.degrees(math.asin(dy / dz)), + cls.params["takeoff_path_roll"], + delta=1e-5, + ) + + def wait_for(cls, secs): + end_time = cls.node.get_clock().now() + rclpy.duration.Duration(seconds=secs) + while cls.node.get_clock().now() < end_time: + rclpy.spin_once(cls.node, timeout_sec=0.01) + + def test_takeoff_and_landing(cls): + # Test that a trajectory override message is published after takeoff + cls.reset_states() + + cls.odom_pub.publish(cls.get_fake_odom(cls.TYPE_ODOM_GROUNDED)) + cls.ref_point_pub.publish(cls.get_fake_ref_point(cls.TYPE_ODOM_GROUNDED)) + + def trajectory_callback(msg): + cls.latest_traj = msg + cls.flag_recvd_traj = True + + def takeoff_state_callback(msg): + cls.takeoff_state = msg + # print(msg.data) + + def landing_state_callback(msg): + cls.landing_state = msg + + takeoff_state_sub = cls.node.create_subscription( + String, + "takeoff_state", + takeoff_state_callback, + 1, + ) + + landing_state_sub = cls.node.create_subscription( + String, + "landing_state", + landing_state_callback, + 1, + ) + + traj_sub = cls.node.create_subscription( + TrajectoryXYZVYaw, + "trajectory_override", + trajectory_callback, + 1, + ) + + # Send takeoff request + cls.call_service(cls.CMD_TAKEOFF) + + # Wait for trajectory override message + cls.wait_for(1.0) + + cls.assertTrue( + cls.flag_recvd_traj, "Did not receive generated trajectory in time!" + ) + + cls.check_takeoff_trajectory() + + # Emulate traj controller setpoint already at end + cls.ref_point_pub.publish( + cls.get_fake_ref_point(cls.TYPE_ODOM_TAKEOFF_COMPLETE) + ) + + # Emulate robot Odometry in progress + cls.odom_pub.publish(cls.get_fake_odom(cls.TYPE_ODOM_TAKEOFF_PENDING)) + cls.progress_pub.publish(Float32(data=75.0)) + cls.wait_for(1.0) + cls.assertEqual(cls.takeoff_state.data, "TAKING_OFF") + + # Emulate robot Odometry complete but time check will fail + odom_end = cls.get_fake_odom(cls.TYPE_ODOM_TAKEOFF_COMPLETE) + cls.odom_pub.publish(odom_end) + cls.progress_pub.publish(Float32(data=100.0)) + cls.wait_for(1.0) + cls.assertEqual(cls.takeoff_state.data, "TAKING_OFF") + + # Send same odometry but with passing timestamp + odom_end.header.stamp.sec += int(cls.params["takeoff_acceptance_time"]) + cls.odom_pub.publish(odom_end) + cls.wait_for(1.0) + cls.assertEqual(cls.takeoff_state.data, "COMPLETE") + + # Landing Checks + cls.reset_states() + + # Send landing request + cls.call_service(cls.CMD_LAND) + cls.wait_for(1.0) + cls.assertTrue( + cls.flag_recvd_traj, "Did not receive generated trajectory in time!" + ) + + # Emulate pending landing + cls.odom_pub.publish(cls.get_fake_odom(cls.TYPE_ODOM_LAND_PENDING)) + cls.ref_point_pub.publish(cls.get_fake_ref_point(cls.TYPE_ODOM_LAND_PENDING)) + cls.wait_for(1.0) + cls.assertEqual(cls.landing_state.data, "LANDING") + + # Emulate completed landing + gnd_odom = cls.get_fake_odom(cls.TYPE_ODOM_GROUNDED) + gnd_odom.header.stamp.sec += int(cls.params["landing_acceptance_time"]) + + # need to publish multiple messages to pass the odom_list count check + for i in range(10): + cls.odom_pub.publish(gnd_odom) + + cls.ref_point_pub.publish(cls.get_fake_ref_point(cls.TYPE_ODOM_LAND_COMPLETE)) + cls.wait_for(1.0) + cls.assertEqual(cls.landing_state.data, "COMPLETE") + + traj_sub.destroy() + takeoff_state_sub.destroy() + landing_state_sub.destroy() diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp index 1b88cc6f3..50626062a 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + #pragma once #include @@ -307,4 +327,4 @@ class TrajectoryLibrary { return n["key"].as(); } -}; +}; diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp index fcf2ef24a..c8e3359f1 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 Waypoint::Waypoint(double x, double y, double z, double yaw, double vx, double vy, double vz, @@ -1286,4 +1306,4 @@ std::vector TrajectoryLibrary::get_dynamic_trajectories( } return trajectories; -} +} diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/common.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/common.hpp index aa818a9c1..a0d4c0c44 100644 --- a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/common.hpp +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/common.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + /* * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland @@ -364,4 +384,4 @@ inline int64_t secondsToNanoseconds(double seconds) { } // namespace mav_msgs -#endif // MAV_MSGS_COMMON_H +#endif // MAV_MSGS_COMMON_H diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/conversions.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/conversions.hpp index f0962586f..ef0f1d6e7 100644 --- a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/conversions.hpp +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/conversions.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + /* * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland @@ -595,4 +615,4 @@ inline void msgMultiDofJointTrajectoryFromEigen( } // end namespace mav_msgs -#endif // MAV_MSGS_CONVERSIONS_H +#endif // MAV_MSGS_CONVERSIONS_H diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_topics.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_topics.hpp index b90b169c9..9d1954548 100644 --- a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_topics.hpp +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_topics.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + /* * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland @@ -68,4 +88,4 @@ static constexpr char FLIGHT_CONTROLLER_BATTERY[] = "mavros/battery"; } // end namespace default_topics } // end namespace mav_msgs -#endif /* DEFAULT_TOPICS_H_ */ +#endif /* DEFAULT_TOPICS_H_ */ diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_values.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_values.hpp index aa24b5a07..5128ece95 100644 --- a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_values.hpp +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_values.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + /* * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland @@ -30,4 +50,4 @@ const double kZurichHeight = 405.94; const double kGravity = MagnitudeOfGravity(kZurichHeight, kZurichLatitude); } // namespace mav_msgs -#endif /* MAV_MSGS_DEFAULT_VALUES_H_ */ +#endif /* MAV_MSGS_DEFAULT_VALUES_H_ */ diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/eigen_mav_msgs.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/eigen_mav_msgs.hpp index 6df1ac477..fde11f818 100644 --- a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/eigen_mav_msgs.hpp +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/eigen_mav_msgs.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + /* * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland @@ -346,4 +366,4 @@ MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenRollPitchYawrateThrust) MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenOdometry) } // namespace mav_msgs -#endif // MAV_MSGS_EIGEN_MAV_MSGS_H +#endif // MAV_MSGS_EIGEN_MAV_MSGS_H diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions.hpp index 820b25245..73ed119ff 100644 --- a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions.hpp +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + /* * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland @@ -114,4 +134,4 @@ inline void polynomialTrajectoryMsgFromEigen( } // namespace mav_planning_msgs -#endif // MAV_PLANNING_MSGS_CONVERSIONS_H +#endif // MAV_PLANNING_MSGS_CONVERSIONS_H diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions_deprecated.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions_deprecated.hpp index ff6f5d9da..58408ddef 100644 --- a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions_deprecated.hpp +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions_deprecated.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + /* * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland @@ -98,4 +118,4 @@ inline void polynomialTrajectoryMsgFromEigen( } // namespace mav_planning_msgs -#endif // MAV_PLANNING_MSGS_CONVERSIONS_DEPRECATED_H +#endif // MAV_PLANNING_MSGS_CONVERSIONS_DEPRECATED_H diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/eigen_planning_msgs.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/eigen_planning_msgs.hpp index 8d6e64bcd..2de85ee9d 100644 --- a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/eigen_planning_msgs.hpp +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/eigen_planning_msgs.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + /* * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland @@ -42,4 +62,4 @@ typedef std::vector EigenPolynomialTrajectory; } // namespace mav_planning_msgs -#endif // MAV_PLANNING_MSGS_EIGEN_MAV_MSGS_H +#endif // MAV_PLANNING_MSGS_EIGEN_MAV_MSGS_H diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp index 4b4ff0da2..f41d3bdab 100644 --- a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 @@ -567,4 +587,4 @@ int main(int argc, char** argv) { rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} +} diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/include/ensemble_global_planner_node.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/include/ensemble_global_planner_node.hpp index ddcff559f..f989e86cf 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/include/ensemble_global_planner_node.hpp +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/include/ensemble_global_planner_node.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + #pragma once @@ -51,4 +71,4 @@ class EnsembleGlobalPlannerNode : public rclcpp::Node { // ROS timers rclcpp::TimerBase::SharedPtr timer; -}; +}; diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/src/ensemble_global_planner_node.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/src/ensemble_global_planner_node.cpp index bf8b10f8a..254bf2498 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/src/ensemble_global_planner_node.cpp +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/src/ensemble_global_planner_node.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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/ensemble_global_planner_node.hpp" @@ -38,4 +58,4 @@ int main(int argc, char *argv[]) { rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp index d87efa07d..faa9bd401 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp @@ -1,3 +1,22 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. #pragma once diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp index 21e70d54d..22b149d8e 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp @@ -1,3 +1,22 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. #pragma once diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp index 685e3f1ea..9b82c96ae 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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/random_walk_logic.hpp" @@ -192,4 +212,4 @@ double get_point_distance(const std::tuple& point1, double deg2rad(double deg) { return deg * M_PI / 180.0; } -double rad2deg(double rad) { return rad * 180.0 / M_PI; } +double rad2deg(double rad) { return rad * 180.0 / M_PI; } diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp index df6bbd5dd..fcdcaecd6 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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/random_walk_node.hpp" #include "../include/random_walk_logic.hpp" @@ -82,7 +102,7 @@ std::optional RandomWalkNode::readParameters() { } this->declare_parameter("position_change_threshold"); if (!this->get_parameter("position_change_threshold", this->position_change_threshold)) { - + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: position_change_threshold"); return std::optional{}; } @@ -312,10 +332,10 @@ void RandomWalkNode::timerCallback() { } else { // Check if position has changed significantly std::tuple last_point = std::make_tuple( - this->last_location.translation.x, + this->last_location.translation.x, this->last_location.translation.y, this->last_location.translation.z); - + if (get_point_distance(current_point, last_point) > this->position_change_threshold) { this->last_location = this->current_location; this->last_position_change = this->now(); @@ -323,7 +343,7 @@ void RandomWalkNode::timerCallback() { // Check if we've been stationary for too long rclcpp::Duration stall_duration = this->now() - this->last_position_change; if (stall_duration.seconds() > this->stall_timeout_seconds) { - RCLCPP_INFO(this->get_logger(), "Robot stationary for %f seconds, clearing plan", + RCLCPP_INFO(this->get_logger(), "Robot stationary for %f seconds, clearing plan", stall_duration.seconds()); this->is_path_executing = false; this->generated_paths.clear(); diff --git a/robot/ros_ws/src/autonomy/4_global/waypoint_interface/src/waypoint_interface_node.cpp b/robot/ros_ws/src/autonomy/4_global/waypoint_interface/src/waypoint_interface_node.cpp index 02544ecf0..2a60087a7 100644 --- a/robot/ros_ws/src/autonomy/4_global/waypoint_interface/src/waypoint_interface_node.cpp +++ b/robot/ros_ws/src/autonomy/4_global/waypoint_interface/src/waypoint_interface_node.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 @@ -116,4 +136,4 @@ int main(int argc, char **argv) { rclcpp::spin(node); rclcpp::shutdown(); return 0; -} +} diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp index e5be9b099..0037e8f46 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 @@ -84,4 +104,4 @@ class BehaviorExecutive : public rclcpp::Node { public: BehaviorExecutive(); -}; +}; diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp index e90f4db68..88aa861d6 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive") { @@ -365,4 +385,4 @@ int main(int argc, char** argv) { rclcpp::shutdown(); return 0; -} +} diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree.hpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree.hpp index 14db3fb95..2c1b9a6ed 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree.hpp +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + #pragma once #include @@ -116,4 +136,4 @@ class Action { void publish(); }; -}; // namespace bt +}; // namespace bt diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.hpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.hpp index 2dd8e30b7..f9adcbc99 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.hpp +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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. + #pragma once #include @@ -169,4 +189,4 @@ class BehaviorTree { // public: BehaviorTree(std::string config_filename, rclcpp::Node* node); bool tick(); -}; +}; diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree.cpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree.cpp index b53ca0a0a..0661b90ae 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree.cpp +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 namespace bt { @@ -113,4 +133,4 @@ void Action::publish() { status_pub->publish(status); } -}; // namespace bt +}; // namespace bt diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree_implementation.cpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree_implementation.cpp index 1dd912861..5807085f7 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree_implementation.cpp +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree_implementation.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 @@ -729,4 +749,4 @@ int main(int argc, char** argv) { rclcpp::shutdown(); return 0; -} +} diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/behavior_tree_example.cpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/behavior_tree_example.cpp index ba481455c..9f7ff0607 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/behavior_tree_example.cpp +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/behavior_tree_example.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 the behavior tree library containing the Action and Condition classes. #include #include @@ -289,4 +309,4 @@ int main(int argc, char* argv[]) { rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} +} diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/robot_node.cpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/robot_node.cpp index 5aae3be6e..4d711bc2a 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/robot_node.cpp +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/robot_node.cpp @@ -1,3 +1,23 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// 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 @@ -206,4 +226,4 @@ int main(int argc, char * argv[]) rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} +}