diff --git a/src/control/CMakeLists.txt b/src/control/CMakeLists.txt new file mode 100644 index 00000000000..f958d02bb9f --- /dev/null +++ b/src/control/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.5) +project(control) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(task REQUIRED) + +include_directories( + include + ../task/include + ../robot/include + ) + +link_directories(${CMAKE_BINARY_DIR}/../libs) + +add_executable(dwa_controller + src/ControlTask.cpp + src/DwaController.cpp + src/main.cpp + ) + +target_link_libraries(dwa_controller + robot + task + ) + +ament_target_dependencies(dwa_controller + rclcpp + std_msgs + task + robot + ) + +install(TARGETS dwa_controller + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/src/task/include/task/ControlTask.hpp b/src/control/include/control/ControlTask.hpp similarity index 63% rename from src/task/include/task/ControlTask.hpp rename to src/control/include/control/ControlTask.hpp index 7db4d7a9ebc..d4f9ebdd123 100644 --- a/src/task/include/task/ControlTask.hpp +++ b/src/control/include/control/ControlTask.hpp @@ -1,19 +1,19 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright 2018 Intel Corporation. All Rights Reserved. -#ifndef TASK__CONTROLTASK_HPP_ -#define TASK__CONTROLTASK_HPP_ +#ifndef CONTROL__CONTROLTASK_HPP_ +#define CONTROL__CONTROLTASK_HPP_ #include "task/RobotTask.hpp" class ControlTask : public RobotTask { public: - explicit ControlTask(const std::string & name, Robot * robot); + ControlTask(const std::string & name, Robot * robot); ControlTask() = delete; ~ControlTask(); virtual void executePlan() = 0; }; -#endif // TASK__CONTROLTASK_HPP_ +#endif // CONTROL__CONTROLTASK_HPP_ diff --git a/src/control/include/control/DwaController.hpp b/src/control/include/control/DwaController.hpp new file mode 100644 index 00000000000..3dbe5ea5bca --- /dev/null +++ b/src/control/include/control/DwaController.hpp @@ -0,0 +1,22 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright 2018 Intel Corporation. All Rights Reserved. + +#ifndef CONTROL__DWACONTROLLER_HPP_ +#define CONTROL__DWACONTROLLER_HPP_ + +#include "control/ControlTask.hpp" + +class DwaController : public ControlTask +{ +public: + DwaController(const std::string & name, Robot * robot); + DwaController() = delete; + ~DwaController(); + + void executePlan() override; + +protected: + void workerThread() override; +}; + +#endif // CONTROL__DWACONTROLLER_HPP_ diff --git a/src/control/package.xml b/src/control/package.xml new file mode 100644 index 00000000000..3b9c979e9c9 --- /dev/null +++ b/src/control/package.xml @@ -0,0 +1,19 @@ + + + + control + 0.1.0 + TODO + Oregon Robotics Team + Oregon Robotics Team + Apache License 2.0 + ament_cmake + rclcpp + task + robot + rclcpp + + ament_cmake + + + diff --git a/src/task/src/ControlTask.cpp b/src/control/src/ControlTask.cpp similarity index 91% rename from src/task/src/ControlTask.cpp rename to src/control/src/ControlTask.cpp index 49dc3adce3b..cccea0de391 100644 --- a/src/task/src/ControlTask.cpp +++ b/src/control/src/ControlTask.cpp @@ -1,7 +1,7 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright 2018 Intel Corporation. All Rights Reserved. -#include "task/ControlTask.hpp" +#include "control/ControlTask.hpp" ControlTask::ControlTask(const std::string & name, Robot * robot) : RobotTask(name, robot) diff --git a/src/control/src/DwaController.cpp b/src/control/src/DwaController.cpp new file mode 100644 index 00000000000..1ea21774384 --- /dev/null +++ b/src/control/src/DwaController.cpp @@ -0,0 +1,27 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright 2018 Intel Corporation. All Rights Reserved. + +#include "control/DwaController.hpp" + +DwaController::DwaController(const std::string & name, Robot * robot) +: ControlTask(name, robot) +{ + RCLCPP_INFO(get_logger(), "DwaController::DwaController"); +} + +DwaController::~DwaController() +{ + RCLCPP_INFO(get_logger(), "DwaController::~DwaController"); +} + +void +DwaController::executePlan() +{ + RCLCPP_INFO(get_logger(), "DwaController::executePlan"); +} + +void +DwaController::workerThread() +{ + RCLCPP_INFO(get_logger(), "DwaController::workerThread"); +} diff --git a/src/control/src/main.cpp b/src/control/src/main.cpp new file mode 100644 index 00000000000..e86ab39c20e --- /dev/null +++ b/src/control/src/main.cpp @@ -0,0 +1,20 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright 2018 Intel Corporation. All Rights Reserved. + +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "control/DwaController.hpp" +#include "robot/RosRobot.hpp" + +int main(int argc, char ** argv) +{ + std::string urdf_filename; + RosRobot robot(urdf_filename); + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared("DwaController", &robot)); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/mission_execution/CMakeLists.txt b/src/mission_execution/CMakeLists.txt index 2685654f475..478ad0fb828 100644 --- a/src/mission_execution/CMakeLists.txt +++ b/src/mission_execution/CMakeLists.txt @@ -31,10 +31,6 @@ ament_target_dependencies(mission_execution std_msgs ) -target_link_libraries(mission_execution - yasmine - ) - install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/src/navigation/CMakeLists.txt b/src/navigation/CMakeLists.txt new file mode 100644 index 00000000000..1184efe20b3 --- /dev/null +++ b/src/navigation/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.5) +project(navigation) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(task REQUIRED) + +include_directories( + include + ../task/include + ../robot/include + ../planning/include + ../control/include + ) + +link_directories(${CMAKE_BINARY_DIR}/../libs) + +add_executable(simple_navigator + src/NavigateToPoseTask.cpp + src/SimpleNavigator.cpp + src/main.cpp + ) + +target_link_libraries(simple_navigator + robot + task + ) + +ament_target_dependencies(simple_navigator + rclcpp + std_msgs + planning + control + task + robot + ) + +install(TARGETS simple_navigator + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/src/navigation/include/navigation/NavigateToPoseTask.hpp b/src/navigation/include/navigation/NavigateToPoseTask.hpp new file mode 100644 index 00000000000..2701ee5ef79 --- /dev/null +++ b/src/navigation/include/navigation/NavigateToPoseTask.hpp @@ -0,0 +1,19 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright 2018 Intel Corporation. All Rights Reserved. + +#ifndef NAVIGATION__NAVIGATETOPOSETASK_HPP_ +#define NAVIGATION__NAVIGATETOPOSETASK_HPP_ + +#include "task/RobotTask.hpp" + +class NavigateToPoseTask : public RobotTask +{ +public: + NavigateToPoseTask(const std::string & name, Robot * robot); + NavigateToPoseTask() = delete; + ~NavigateToPoseTask(); + + virtual void navigateTo(/*pose*/) = 0; +}; + +#endif // NAVIGATION__NAVIGATETOPOSETASK_HPP_ diff --git a/src/navigation/include/navigation/SimpleNavigator.hpp b/src/navigation/include/navigation/SimpleNavigator.hpp new file mode 100644 index 00000000000..3803391c051 --- /dev/null +++ b/src/navigation/include/navigation/SimpleNavigator.hpp @@ -0,0 +1,28 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright 2018 Intel Corporation. All Rights Reserved. + +#ifndef NAVIGATION__SIMPLENAVIGATOR_HPP_ +#define NAVIGATION__SIMPLENAVIGATOR_HPP_ + +#include "navigation/NavigateToPoseTask.hpp" +#include "planning/AStarPlanner.hpp" +#include "control/DwaController.hpp" + +class SimpleNavigator : public NavigateToPoseTask +{ +public: + SimpleNavigator(const std::string & name, Robot * robot); + SimpleNavigator() = delete; + ~SimpleNavigator(); + + void navigateTo(/*pose*/) override; + +protected: + void workerThread(); + + // TODO: These will be the client-side proxies (like SimpleActionClient): + AStarPlanner * planner_; + DwaController * controller_; +}; + +#endif // NAVIGATION__SIMPLENAVIGATOR_HPP_ diff --git a/src/navigation/package.xml b/src/navigation/package.xml new file mode 100644 index 00000000000..37b250d980b --- /dev/null +++ b/src/navigation/package.xml @@ -0,0 +1,19 @@ + + + + navigation + 0.1.0 + TODO + Oregon Robotics Team + Oregon Robotics Team + Apache License 2.0 + ament_cmake + rclcpp + task + robot + rclcpp + + ament_cmake + + + diff --git a/src/navigation/src/NavigateToPoseTask.cpp b/src/navigation/src/NavigateToPoseTask.cpp new file mode 100644 index 00000000000..eee69d49ec1 --- /dev/null +++ b/src/navigation/src/NavigateToPoseTask.cpp @@ -0,0 +1,15 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright 2018 Intel Corporation. All Rights Reserved. + +#include "navigation/NavigateToPoseTask.hpp" + +NavigateToPoseTask::NavigateToPoseTask(const std::string & name, Robot * robot) +: RobotTask(name, robot) +{ + RCLCPP_INFO(get_logger(), "NavigateToPoseTask::NavigateToPoseTask"); +} + +NavigateToPoseTask::~NavigateToPoseTask() +{ + RCLCPP_INFO(get_logger(), "NavigateToPoseTask::~NavigateToPoseTask"); +} diff --git a/src/navigation/src/SimpleNavigator.cpp b/src/navigation/src/SimpleNavigator.cpp new file mode 100644 index 00000000000..7a1b5dd8300 --- /dev/null +++ b/src/navigation/src/SimpleNavigator.cpp @@ -0,0 +1,27 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright 2018 Intel Corporation. All Rights Reserved. + +#include "navigation/SimpleNavigator.hpp" + +SimpleNavigator::SimpleNavigator(const std::string & name, Robot * robot) +: NavigateToPoseTask(name, robot), planner_(nullptr), controller_(nullptr) +{ + RCLCPP_INFO(get_logger(), "SimpleNavigator::SimpleNavigator"); +} + +SimpleNavigator::~SimpleNavigator() +{ + RCLCPP_INFO(get_logger(), "SimpleNavigator::~SimpleNavigator"); +} + +void +SimpleNavigator::navigateTo() +{ + RCLCPP_INFO(get_logger(), "SimpleNavigator::executePlan"); +} + +void +SimpleNavigator::workerThread() +{ + RCLCPP_INFO(get_logger(), "SimpleNavigator::workerThread"); +} diff --git a/src/navigation/src/main.cpp b/src/navigation/src/main.cpp new file mode 100644 index 00000000000..f3cb2262ff5 --- /dev/null +++ b/src/navigation/src/main.cpp @@ -0,0 +1,20 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright 2018 Intel Corporation. All Rights Reserved. + +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "navigation/SimpleNavigator.hpp" +#include "robot/RosRobot.hpp" + +int main(int argc, char ** argv) +{ + std::string urdf_filename; + RosRobot robot(urdf_filename); + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared("SimpleNavigator", &robot)); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/planning/CMakeLists.txt b/src/planning/CMakeLists.txt new file mode 100644 index 00000000000..2b2b1f178aa --- /dev/null +++ b/src/planning/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.5) +project(planning) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(task REQUIRED) + +include_directories( + include + ../task/include + ) + +link_directories(${CMAKE_BINARY_DIR}/../libs) + +add_executable(astar_planner + src/AStarPlanner.cpp + src/PlanningTask.cpp + src/main.cpp + ) + +target_link_libraries(astar_planner + task + ) + +ament_target_dependencies(astar_planner + rclcpp + std_msgs + task + ) + +install(TARGETS astar_planner + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/src/planning/include/planning/AStarPlanner.hpp b/src/planning/include/planning/AStarPlanner.hpp new file mode 100644 index 00000000000..aca1f2eec87 --- /dev/null +++ b/src/planning/include/planning/AStarPlanner.hpp @@ -0,0 +1,23 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright 2018 Intel Corporation. All Rights Reserved. + +#ifndef PLANNING__ASTARPLANNER_HPP_ +#define PLANNING__ASTARPLANNER_HPP_ + +#include "planning/PlanningTask.hpp" + +class AStarPlanner : public PlanningTask +{ +public: + AStarPlanner(const std::string & name); + AStarPlanner() = delete; + ~AStarPlanner(); + + void createPlan(const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal); + +protected: + void workerThread() override; +}; + +#endif // PLANNING__ASTARPLANNER_HPP_ diff --git a/src/task/include/task/PlanningTask.hpp b/src/planning/include/planning/PlanningTask.hpp similarity index 80% rename from src/task/include/task/PlanningTask.hpp rename to src/planning/include/planning/PlanningTask.hpp index 68d894e8f8f..1e3e8319d13 100644 --- a/src/task/include/task/PlanningTask.hpp +++ b/src/planning/include/planning/PlanningTask.hpp @@ -1,8 +1,8 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright 2018 Intel Corporation. All Rights Reserved. -#ifndef TASK__PLANNINGTASK_HPP_ -#define TASK__PLANNINGTASK_HPP_ +#ifndef PLANNING__PLANNINGTASK_HPP_ +#define PLANNING__PLANNINGTASK_HPP_ #include "task/Task.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -18,4 +18,4 @@ class PlanningTask : public Task const geometry_msgs::msg::PoseStamped & goal) = 0; }; -#endif // TASK__PLANNINGTASK_HPP_ +#endif // PLANNING__PLANNINGTASK_HPP_ diff --git a/src/planning/package.xml b/src/planning/package.xml new file mode 100644 index 00000000000..a5937f427b6 --- /dev/null +++ b/src/planning/package.xml @@ -0,0 +1,19 @@ + + + + planning + 0.1.0 + TODO + Oregon Robotics Team + Oregon Robotics Team + Apache License 2.0 + ament_cmake + rclcpp + task + robot + rclcpp + + ament_cmake + + + diff --git a/src/planning/src/AStarPlanner.cpp b/src/planning/src/AStarPlanner.cpp new file mode 100644 index 00000000000..b5463de7315 --- /dev/null +++ b/src/planning/src/AStarPlanner.cpp @@ -0,0 +1,28 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright 2018 Intel Corporation. All Rights Reserved. + +#include "planning/AStarPlanner.hpp" + +AStarPlanner::AStarPlanner(const std::string & name) +: PlanningTask(name) +{ + RCLCPP_INFO(get_logger(), "AStarPlanner::AStarPlanner"); +} + +AStarPlanner::~AStarPlanner() +{ + RCLCPP_INFO(get_logger(), "AStarPlanner::~AStarPlanner"); +} + +void +AStarPlanner::createPlan(const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + RCLCPP_INFO(get_logger(), "AStarPlanner::createPlan"); +} + +void +AStarPlanner::workerThread() +{ + RCLCPP_INFO(get_logger(), "AStarPlanner::workerThread"); +} diff --git a/src/task/src/PlanningTask.cpp b/src/planning/src/PlanningTask.cpp similarity index 90% rename from src/task/src/PlanningTask.cpp rename to src/planning/src/PlanningTask.cpp index 1849b2db045..8982a29547d 100644 --- a/src/task/src/PlanningTask.cpp +++ b/src/planning/src/PlanningTask.cpp @@ -1,7 +1,7 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright 2018 Intel Corporation. All Rights Reserved. -#include "task/PlanningTask.hpp" +#include "planning/PlanningTask.hpp" PlanningTask::PlanningTask(const std::string & name) : Task(name) diff --git a/src/planning/src/main.cpp b/src/planning/src/main.cpp new file mode 100644 index 00000000000..7d718084cb3 --- /dev/null +++ b/src/planning/src/main.cpp @@ -0,0 +1,15 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright 2018 Intel Corporation. All Rights Reserved. + +#include +#include "rclcpp/rclcpp.hpp" +#include "planning/AStarPlanner.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared("AStarPlanner")); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/robot/CMakeLists.txt b/src/robot/CMakeLists.txt index 24c6acdacf9..82d1f745a6b 100644 --- a/src/robot/CMakeLists.txt +++ b/src/robot/CMakeLists.txt @@ -12,6 +12,8 @@ include_directories( set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "../libs/") +link_directories(${CMAKE_BINARY_DIR}/../libs) + add_library(robot STATIC src/RosRobot.cpp ) diff --git a/src/robot/include/robot/RosRobot.hpp b/src/robot/include/robot/RosRobot.hpp index d05b7fb7fba..e8b8d5d4041 100644 --- a/src/robot/include/robot/RosRobot.hpp +++ b/src/robot/include/robot/RosRobot.hpp @@ -15,8 +15,7 @@ class RosRobot : public Robot * * @param[in] filename The filename of the URDF file describing this robot. */ - explicit RosRobot(std::string & filename); - + explicit RosRobot(const std::string & urdf_filename); RosRobot() = delete; ~RosRobot(); diff --git a/src/robot/src/RosRobot.cpp b/src/robot/src/RosRobot.cpp index c6b8e3fe535..f5c72ee6163 100644 --- a/src/robot/src/RosRobot.cpp +++ b/src/robot/src/RosRobot.cpp @@ -1,10 +1,10 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright 2018 Intel Corporation. All Rights Reserved. -#include "robot/RosRobot.hpp" #include +#include "robot/RosRobot.hpp" -RosRobot::RosRobot(std::string & /*filename*/) +RosRobot::RosRobot(const std::string & urdf_filename) { // Open and parser the URDF file } diff --git a/src/task/CMakeLists.txt b/src/task/CMakeLists.txt index e7d4f7f9ce6..d102ac691c0 100644 --- a/src/task/CMakeLists.txt +++ b/src/task/CMakeLists.txt @@ -19,16 +19,11 @@ include_directories( ../robot/include ) -link_directories(${CMAKE_BINARY_DIR}/../libs) +set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "../libs/") add_library(task STATIC - src/ControlTask.cpp - src/NavigationTask.cpp - src/PlanningTask.cpp src/RobotTask.cpp src/Task.cpp - src/AStarPlanner.cpp - src/DwaController.cpp ) ament_target_dependencies(task @@ -36,9 +31,4 @@ ament_target_dependencies(task std_msgs ) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME}) - ament_package() diff --git a/src/task/include/task/NavigationTask.hpp b/src/task/include/task/NavigationTask.hpp deleted file mode 100644 index 4a9bc03df25..00000000000 --- a/src/task/include/task/NavigationTask.hpp +++ /dev/null @@ -1,23 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright 2018 Intel Corporation. All Rights Reserved. - -#ifndef TASK__NAVIGATIONTASK_HPP_ -#define TASK__NAVIGATIONTASK_HPP_ - -#include "task/RobotTask.hpp" -#include "task/AStarPlanner.hpp" -#include "task/DwaController.hpp" - -class NavigationTask : public RobotTask -{ -public: - explicit NavigationTask(const std::string & name, Robot * robot); - NavigationTask() = delete; - ~NavigationTask(); - -protected: - AStarPlanner planner_; - DwaController controller_; -}; - -#endif // TASK__NAVIGATIONTASK_HPP_ diff --git a/src/task/src/NavigationTask.cpp b/src/task/src/NavigationTask.cpp deleted file mode 100644 index 0419fe21059..00000000000 --- a/src/task/src/NavigationTask.cpp +++ /dev/null @@ -1,15 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright 2018 Intel Corporation. All Rights Reserved. - -#include "task/NavigationTask.hpp" - -NavigationTask::NavigationTask(const std::string & name, Robot * robot) -: RobotTask(name, robot), planner_(name + "Planner"), controller_(name + "Controller", robot) -{ - RCLCPP_INFO(get_logger(), "NavigationTask::NavigationTask"); -} - -NavigationTask::~NavigationTask() -{ - RCLCPP_INFO(get_logger(), "NavigationTask::~NavigationTask"); -}