Change nav2_dwb_controller to dwb_controller

nav2_dwb_controller/dwb_controller/CMakeLists.txt

+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()

nav2_dwb_controller/dwb_controller/include/nav2_controller_dwb/dwb_controller.hpp

+// 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_

nav2_dwb_controller/dwb_controller/package.xml

+<?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>

nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp

+// 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

nav2_dwb_controller/dwb_controller/src/main.cpp

+// 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;
+}