diff --git a/nav2_dwb_controller/dwb_controller/CMakeLists.txt b/nav2_dwb_controller/dwb_controller/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..b4b3cfefdde8e36768cf8f68812bd69afba131a8 --- /dev/null +++ b/nav2_dwb_controller/dwb_controller/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_controller_dwb) + +# 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 -Werror) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav2_tasks REQUIRED) +find_package(nav2_robot REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(dwb_core REQUIRED) +find_package(Boost REQUIRED COMPONENTS system thread) + +include_directories( + include + ${Boost_INCLUDE_DIRS} +) + +set(executable_name nav2_controller_dwb) + +add_executable(${executable_name} + src/main.cpp +) + +set(library_name ${executable_name}_core) + +add_library(${library_name} + src/dwb_controller.cpp +) + +set(dependencies + rclcpp + std_msgs + nav2_tasks + nav2_robot + nav2_msgs + dwb_core +) + +ament_target_dependencies(${library_name} + ${dependencies} +) +target_link_libraries(${library_name} ${Boost_LIBRARIES}) + +ament_target_dependencies(${executable_name} + ${dependencies} +) +target_link_libraries(${executable_name} ${library_name} ${Boost_LIBRARIES}) + +install(TARGETS ${executable_name} ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) + +ament_package() diff --git a/nav2_dwb_controller/dwb_controller/include/nav2_controller_dwb/dwb_controller.hpp b/nav2_dwb_controller/dwb_controller/include/nav2_controller_dwb/dwb_controller.hpp new file mode 100644 index 0000000000000000000000000000000000000000..3fbc3739d1a204f6b1570830b947e0727efc0247 --- /dev/null +++ b/nav2_dwb_controller/dwb_controller/include/nav2_controller_dwb/dwb_controller.hpp @@ -0,0 +1,52 @@ +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_CONTROLLER_DWB__DWB_CONTROLLER_HPP_ +#define NAV2_CONTROLLER_DWB__DWB_CONTROLLER_HPP_ + +#include <memory> +#include <string> +#include "nav2_tasks/follow_path_task.hpp" +#include "dwb_core/dwb_core.h" +#include "dwb_core/common_types.h" +#include "nav_2d_msgs/msg/pose2_d_stamped.hpp" +#include "nav_2d_utils/odom_subscriber.h" + +namespace nav2_controller_dwb +{ + +class DwbController : public nav2_tasks::FollowPathTaskServer +{ +public: + DwbController(); + ~DwbController(); + + nav2_tasks::TaskStatus execute(const nav2_tasks::FollowPathCommand::SharedPtr path) override; + +protected: + bool isGoalReached(const nav_2d_msgs::msg::Pose2DStamped & pose2d); + void publishVelocity(const nav_2d_msgs::msg::Twist2DStamped & velocity); + void publishZeroVelocity(); + bool getRobotPose(nav_2d_msgs::msg::Pose2DStamped & pose2d); + dwb_core::CostmapROSPtr cm_; + dwb_core::DWBLocalPlanner planner_; + std::shared_ptr<nav_2d_utils::OdomSubscriber> odom_sub_; + std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::Twist>> vel_pub_; + tf2_ros::Buffer tfBuffer_; + tf2_ros::TransformListener tfListener_; +}; + +} // namespace nav2_controller_dwb + +#endif // NAV2_CONTROLLER_DWB__DWB_CONTROLLER_HPP_ diff --git a/nav2_dwb_controller/dwb_controller/package.xml b/nav2_dwb_controller/dwb_controller/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..cedb1e9dcb0d37202fba8edee02179e91f9a6fd7 --- /dev/null +++ b/nav2_dwb_controller/dwb_controller/package.xml @@ -0,0 +1,29 @@ +<?xml version="1.0"?> +<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> +<package format="3"> + <name>nav2_controller_dwb</name> + <version>0.1.0</version> + <description>TODO</description> + <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer> + <license>Apache License 2.0</license> + + <buildtool_depend>ament_cmake</buildtool_depend> + + <build_depend>rclcpp</build_depend> + <build_depend>std_msgs</build_depend> + <build_depend>nav2_tasks</build_depend> + <build_depend>nav2_robot</build_depend> + <build_depend>nav2_msgs</build_depend> + <build_depend>dwb_core</build_depend> + + <exec_depend>rclcpp</exec_depend> + <exec_depend>std_msgs</exec_depend> + <exec_depend>nav2_msgs</exec_depend> + + <test_depend>ament_lint_common</test_depend> + <test_depend>ament_lint_auto</test_depend> + + <export> + <build_type>ament_cmake</build_type> + </export> +</package> diff --git a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp new file mode 100644 index 0000000000000000000000000000000000000000..988d0d788f1a001efd3a49f54cfa2c1b30b9dc4d --- /dev/null +++ b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp @@ -0,0 +1,119 @@ +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_controller_dwb/dwb_controller.hpp" +#include <string> +#include <chrono> +#include <memory> +#include "dwb_core/exceptions.h" +#include "nav_2d_utils/conversions.h" + +using namespace std::chrono_literals; +using std::shared_ptr; +using nav2_tasks::TaskStatus; +using dwb_core::DWBLocalPlanner; +using dwb_core::CostmapROSPtr; + +namespace nav2_controller_dwb +{ + +// TODO(cdelsey): provide the correct clock to tfBuffer_ +DwbController::DwbController() +: nav2_tasks::FollowPathTaskServer("FollowPathNode"), tfBuffer_(std::make_shared<rclcpp::Clock>()), tfListener_(tfBuffer_) +{ +} + +DwbController::~DwbController() +{ +} + +TaskStatus +DwbController::execute(const nav2_tasks::FollowPathCommand::SharedPtr command) +{ + RCLCPP_INFO(get_logger(), "Starting controller"); + try { + auto path = nav_2d_utils::pathToPath2D(*command); + cm_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("local_costmap", tfBuffer_); + auto nh = shared_from_this(); + odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(*this); + vel_pub_ = + this->create_publisher<geometry_msgs::msg::Twist>("/mobile_base/commands/velocity", 1); + planner_.initialize(nh, shared_ptr<tf2_ros::Buffer>(&tfBuffer_), cm_); + planner_.setPlan(path); + RCLCPP_INFO(get_logger(), "Initialized"); + while (true) { + nav_2d_msgs::msg::Pose2DStamped pose2d; + if (!getRobotPose(pose2d)) { + RCLCPP_INFO(get_logger(), "No pose. Stopping robot"); + publishZeroVelocity(); + } else { + if (isGoalReached(pose2d)) { + break; + } + auto velocity = odom_sub_->getTwist(); + auto cmd_vel_2d = planner_.computeVelocityCommands(pose2d, velocity); + publishVelocity(cmd_vel_2d); + RCLCPP_INFO(get_logger(), "Publishing velocity"); + if (cancelRequested()) { + RCLCPP_INFO(this->get_logger(), "execute: task has been canceled"); + setCanceled(); + return TaskStatus::CANCELED; + } + } + std::this_thread::sleep_for(100ms); + } + } catch (nav_core2::PlannerException & e) { + RCLCPP_INFO(this->get_logger(), e.what()); + return TaskStatus::FAILED; + } + + nav2_tasks::FollowPathResult result; + setResult(result); + + return TaskStatus::SUCCEEDED; +} + +void DwbController::publishVelocity(const nav_2d_msgs::msg::Twist2DStamped & velocity) +{ + auto cmd_vel = nav_2d_utils::twist2Dto3D(velocity.velocity); + vel_pub_->publish(cmd_vel); +} + +void DwbController::publishZeroVelocity() +{ + nav_2d_msgs::msg::Twist2DStamped velocity; + velocity.velocity.x = 0; + velocity.velocity.y = 0; + velocity.velocity.theta = 0; + publishVelocity(velocity); +} + +bool DwbController::isGoalReached(const nav_2d_msgs::msg::Pose2DStamped & pose2d) +{ + nav_2d_msgs::msg::Twist2D velocity = odom_sub_->getTwist(); + return planner_.isGoalReached(pose2d, velocity); +} + +bool DwbController::getRobotPose(nav_2d_msgs::msg::Pose2DStamped & pose2d) +{ + geometry_msgs::msg::PoseStamped current_pose; + if (!cm_->getRobotPose(current_pose)) { + RCLCPP_ERROR(this->get_logger(), "Could not get robot pose"); + return false; + } + pose2d = nav_2d_utils::poseStampedToPose2D(current_pose); + return true; +} + +} // namespace nav2_controller_dwb diff --git a/nav2_dwb_controller/dwb_controller/src/main.cpp b/nav2_dwb_controller/dwb_controller/src/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ee5ca394c409065c685d663ced27ee6ba2d30b2f --- /dev/null +++ b/nav2_dwb_controller/dwb_controller/src/main.cpp @@ -0,0 +1,26 @@ +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include <memory> +#include "rclcpp/rclcpp.hpp" +#include "nav2_controller_dwb/dwb_controller.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared<nav2_controller_dwb::DwbController>()); + rclcpp::shutdown(); + + return 0; +}