Merge pull request #235 from crdelsey/costmapintegration

Integrate costmap with DWB- Make robot move

nav2_bringup/launch/core.launch.py

@@ -13,8 +13,8 @@ def generate_launch_description():
                 ], period = 1.0),
         launch.actions.TimerAction(
             actions = [
-                #launch_ros.actions.Node( package='nav2_controller_dwb', node_executable='nav2_controller_dwb', output='screen')
-                launch_ros.actions.Node( package='nav2_controller_example', node_executable='dwa_controller', output='screen')
+                launch_ros.actions.Node( package='nav2_controller_dwb', node_executable='nav2_controller_dwb', output='screen')
+                #launch_ros.actions.Node( package='nav2_controller_example', node_executable='dwa_controller', output='screen')
                 ], period = 5.0),
         launch.actions.TimerAction(
             actions = [

nav2_controller/dwb_core/src/dwb_core.cpp

@@ -100,7 +100,7 @@ void DWBLocalPlanner::initialize(
   // Plugins
   std::string traj_generator_name;
   nh_->get_parameter_or("trajectory_generator_name", traj_generator_name,
-    std::string("dwb_plugins::LimitedAccelGenerator"));
+    std::string("dwb_plugins::StandardTrajectoryGenerator"));
   traj_generator_ = std::move(traj_gen_loader_.createUniqueInstance(traj_generator_name));
   traj_generator_->initialize(nh_);
 

nav2_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.h

@@ -103,7 +103,7 @@ protected:
   // Cached square values of min_speed_xy and max_speed_xy
   double min_speed_xy_sq_, max_speed_xy_sq_;
 
-  // void reconfigureCB(KinematicParamsConfig &config, uint32_t level);
+  void reconfigureCB();
   // std::shared_ptr<dynamic_reconfigure::Server<KinematicParamsConfig> > dsrv_;
 };
 

nav2_controller/dwb_plugins/src/kinematic_parameters.cpp

@@ -85,35 +85,47 @@ void KinematicParameters::initialize(const std::shared_ptr<rclcpp::Node> & nh)
   setDecelerationAsNeeded(nh, "y");
   setDecelerationAsNeeded(nh, "theta");
 
+  // TODO(crdelsey): Handle dynamic params
   // the rest of the initial values are loaded through the dynamic reconfigure mechanisms
   // dsrv_ = std::make_shared<dynamic_reconfigure::Server<KinematicParamsConfig> >(nh);
   // dynamic_reconfigure::Server<KinematicParamsConfig>::CallbackType cb =
   //   boost::bind(&KinematicParameters::reconfigureCB, this, _1, _2);
-  // dsrv_->setCallback(cb);
+  reconfigureCB();
 }
 
-// TODO(crdelsey): Handle params
-// void KinematicParameters::reconfigureCB(KinematicParamsConfig &config, uint32_t level)
-// {
-//   min_vel_x_ = config.min_vel_x;
-//   min_vel_y_ = config.min_vel_y;
-//   max_vel_x_ = config.max_vel_x;
-//   max_vel_y_ = config.max_vel_y;
-//   max_vel_theta_ = config.max_vel_theta;
-//
-//   min_speed_xy_ = config.min_speed_xy;
-//   max_speed_xy_ = config.max_speed_xy;
-//   min_speed_xy_sq_ = min_speed_xy_ * min_speed_xy_;
-//   max_speed_xy_sq_ = max_speed_xy_ * max_speed_xy_;
-//   min_speed_theta_ = config.min_speed_theta;
-//
-//   acc_lim_x_ = config.acc_lim_x;
-//   acc_lim_y_ = config.acc_lim_y;
-//   acc_lim_theta_ = config.acc_lim_theta;
-//   decel_lim_x_ = config.decel_lim_x;
-//   decel_lim_y_ = config.decel_lim_y;
-//   decel_lim_theta_ = config.decel_lim_theta;
-// }
+void KinematicParameters::reconfigureCB()
+{
+  // TODO(crdelsey): Remove hard coded parameters when dynamic_reconfigure is in
+  min_vel_x_ = -0.26;
+  max_vel_x_ = 0.26;
+  max_vel_theta_ = 1.0;
+
+  max_speed_xy_ = max_vel_x_;
+
+  acc_lim_x_ = 2.5;
+  acc_lim_theta_ = 3.2;
+  decel_lim_x_ = -acc_lim_x_;
+  decel_lim_theta_ = -acc_lim_theta_;
+
+  // min_vel_x_ = config.min_vel_x;
+  // min_vel_y_ = config.min_vel_y;
+  // max_vel_x_ = config.max_vel_x;
+  // max_vel_y_ = config.max_vel_y;
+  // max_vel_theta_ = config.max_vel_theta;
+  //
+  // min_speed_xy_ = config.min_speed_xy;
+  // max_speed_xy_ = config.max_speed_xy;
+  min_speed_xy_sq_ = min_speed_xy_ * min_speed_xy_;
+  max_speed_xy_sq_ = max_speed_xy_ * max_speed_xy_;
+  // min_speed_theta_ = config.min_speed_theta;
+  //
+  // acc_lim_x_ = config.acc_lim_x;
+  // acc_lim_y_ = config.acc_lim_y;
+  // acc_lim_theta_ = config.acc_lim_theta;
+  // decel_lim_x_ = config.decel_lim_x;
+  // decel_lim_y_ = config.decel_lim_y;
+  // decel_lim_theta_ = config.decel_lim_theta;
+}
 
 bool KinematicParameters::isValidSpeed(double x, double y, double theta)
 {

nav2_controller/nav2_controller_dwb/include/nav2_controller_dwb/dwb_controller.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include "rclcpp/rclcpp.hpp"
 #include "nav2_tasks/follow_path_task.hpp"
 #include "dwb_core/dwb_core.h"
 #include "dwb_core/common_types.h"
@@ -29,7 +30,7 @@ namespace nav2_controller_dwb
 class DwbController : public nav2_tasks::FollowPathTaskServer
 {
 public:
-  DwbController();
+  DwbController(rclcpp::executor::Executor & executor);
   ~DwbController();
 
   nav2_tasks::TaskStatus execute(const nav2_tasks::FollowPathCommand::SharedPtr path) override;

nav2_controller/nav2_controller_dwb/src/dwb_controller.cpp

@@ -25,13 +25,19 @@ using nav2_tasks::TaskStatus;
 using dwb_core::DWBLocalPlanner;
 using dwb_core::CostmapROSPtr;
 
+#define NO_OP_DELETER [](auto){}
+
 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(rclcpp::executor::Executor & executor)
+: nav2_tasks::FollowPathTaskServer("FollowPathNode"), tfBuffer_(get_clock()), tfListener_(tfBuffer_)
 {
+  cm_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("local_costmap", tfBuffer_);
+  executor.add_node(cm_);
+  odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(*this);
+  vel_pub_ = 
+    this->create_publisher<geometry_msgs::msg::Twist>("/mobile_base/commands/velocity", 1);
 }
 
 DwbController::~DwbController()
@@ -44,12 +50,8 @@ 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_.initialize(nh, shared_ptr<tf2_ros::Buffer>(&tfBuffer_, NO_OP_DELETER), cm_);
     planner_.setPlan(path);
     RCLCPP_INFO(get_logger(), "Initialized");
     while (true) {

nav2_controller/nav2_controller_dwb/src/main.cpp

@@ -19,7 +19,10 @@
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<nav2_controller_dwb::DwbController>());
+  rclcpp::executors::SingleThreadedExecutor exec;
+  auto controller_node = std::make_shared<nav2_controller_dwb::DwbController>(exec);
+  exec.add_node(controller_node);
+  exec.spin();
   rclcpp::shutdown();
 
   return 0;

nav2_costmap_2d/plugins/static_layer.cpp

@@ -100,8 +100,9 @@ void StaticLayer::onInitialize()
   has_updated_data_ = false;
 
   rclcpp::Rate r(10);
+  rclcpp::executors::SingleThreadedExecutor exec;
   while (!map_received_ && rclcpp::ok()) {
-    rclcpp::spin_some(node_);
+    exec.spin_node_once(node_->get_node_base_interface(), std::chrono::milliseconds(100));
     r.sleep();
   }
 

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -124,17 +124,25 @@ Costmap2DROS::Costmap2DROS(const std::string & name, tf2_ros::Buffer & tf)
   setPluginParams(node_);
   }
 
-  if (parameters_client_->has_parameter("plugin_names") &&
-    parameters_client_->has_parameter("plugin_types")) {
-    auto param = get_parameters({"plugin_names", "plugin_types"});
-    for (int32_t i = 0; i < param[0].get_value<std::vector<std::string>>().size(); ++i) {
-      std::string pname = (param[0].get_value<std::vector<std::string>>())[i];
-      std::string type = (param[1].get_value<std::vector<std::string>>())[i];
-      RCLCPP_INFO(get_logger(), "Using plugin \"%s\"", pname.c_str());
-      std::shared_ptr<Layer> plugin = plugin_loader_.createSharedInstance(type);
-      layered_costmap_->addPlugin(plugin);
-      plugin->initialize(layered_costmap_, name + "_" + pname, &tf_, node_);
-    }
+  // if (parameters_client_->has_parameter("plugin_names") &&
+  //   parameters_client_->has_parameter("plugin_types")) {
+  //   auto param = get_parameters({"plugin_names", "plugin_types"});
+  //   for (int32_t i = 0; i < param[0].get_value<std::vector<std::string>>().size(); ++i) {
+  //     std::string pname = (param[0].get_value<std::vector<std::string>>())[i];
+  //     std::string type = (param[1].get_value<std::vector<std::string>>())[i];
+  //     RCLCPP_INFO(get_logger(), "Using plugin \"%s\"", pname.c_str());
+  //     std::shared_ptr<Layer> plugin = plugin_loader_.createSharedInstance(type);
+  //     layered_costmap_->addPlugin(plugin);
+  //     plugin->initialize(layered_costmap_, name + "_" + pname, &tf_, node_);
+  //   }
+  // }
+
+  std::vector<std::string> plugin_names = {"static_layer","inflation_layer"};
+  std::vector<std::string> plugin_types = {"nav2_costmap_2d::StaticLayer","nav2_costmap_2d::InflationLayer"};
+  for (int i = 0; i < 2; i++) {
+    std::shared_ptr<Layer> plugin = plugin_loader_.createSharedInstance(plugin_types[i]);
+    layered_costmap_->addPlugin(plugin);
+    plugin->initialize(layered_costmap_, name + "_" + plugin_names[i], &tf_, node_);
   }
 
   // subscribe to the footprint topic