Humble Sync 1: Aug 24 (#3148)

* Fix size_t format specifier (#3003) * clear up params file (#3004) Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * Bt test fix (#2999) * fixed tests * undo * linting fix (#3007) Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * Add nav2_constrained_smoother to metapackage * adding humble release to table (#3008) * Fix for costmap nodes lifecycle status (#3009) Lifecycle status for global and local cost nodes not correct. ros2 lifecycle/service commands shows unconfigured for these two. This is due to directly calling on_configure/on_activate/on_cleanup calls in parent node. This PR to replace on_xxxxxx() to configure()/activate()/cleanup() calls of lifecycle base. Signed-off-by: Arshad <arshad.mehmood@intel.com> * Get parameters on configure transition addressing #2985 (#3000) * Get parameters on configure transition Signed-off-by: MartiBolet <mboletboixeda@gmail.com> * Remove past setting of parameters Signed-off-by: MartiBolet <mboletboixeda@gmail.com> * Expose transition functions to public for test Signed-off-by: MartiBolet <mboletboixeda@gmail.com> * fix: wrong input type in navigate_to_pose_action.hpp and navigate_to_… (#2994) * fix: wrong input type in navigate_to_pose_action.hpp and navigate_to_pose_action.hpp * Update navigate_through_poses_action.hpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Nav2 Velocity Smoother (#2964) * WIP velocity smoother with ruckig * a few comments * vel smoother prototype * updating defaults * adding defaults to readme * removing note from readme * updates to velocity smoother TODO items * adding unit tests * finishing system tests * adding failure to change parameters tests * fix last bits * fixing negative sign bug * lint * update tests * setting defaults * Adding warning * Update velocity_smoother.cpp * Fixing rebase issue * adding plugin type for static in local + removing unused print (#3021) * removed extra includes (#3026) * remove extra sub (#3025) * Fix missing dependency on nav2_velocity_smoother (#3031) * adding timeout for action client initialization (#3029) * adding timeout for action client initialization Signed-off-by: Vinny Ruia <vinny.ruia@fireflyautomatix.com> * adding constant 1s timeout, catching exception Signed-off-by: Vinny Ruia <vinny.ruia@fireflyautomatix.com> * cleanup warnings (#3028) * cleanup warnings * removed referenc * installing plugins folder of nav2_behaviors package (#3051) Co-authored-by: Srijanee Biswas <srijanee.biswas@toyotatmh.com> * Completed PR 2929 (#3038) * accepting empty yaml_filename if no initial map is available * invalid load_map-request does not invalidate existing map, added Testcase * style * finish PR 2929 * finish linting * removing change * removing change * Update test_map_server_node.cpp * Update test_map_server_node.cpp Co-authored-by: Nikolas Engelhard <nikolas.engelhard@gmail.com> * zero z values in rpp transformed plan (#3066) * removing get node options default for nav2 utils (#3073) * adding looping timeout for lifecycle service clients + adding string name of action for BT action nodes (#3071) * adding looping timeout for lifecycle service clients + adding string name of action for BT action nodes * fix linting * remove inline comment * adding goal updated controller node to test * Smac Planner 2D changes (#3083) * removing 4-connected option from smac; fixing 2D heuristic and traversal function from other planner's changes * fix name of variable since no longer a neighborhood * partial test updates * ported unit tests fully * revert to no costmap downsampling * Collision monitor (#2982) * Add Collision Monitor node * Meet review items * Fix next review items * Code cleanup * Support dynamic footprint. More optimizations. * Switch to multiple footprints. Move variables. Remove odom subscriber. Add 0-velocity optimization * Update nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_collision_monitor/params/collision_monitor_params.yaml Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_collision_monitor/params/collision_monitor_params.yaml Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_collision_monitor/params/collision_monitor_params.yaml Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Meet smaller review items * Add fixes found during unit test development * Fix uncrustify issues * Add unit tests * Fix number of polygons points * Move tests * Add kinematics unit test * Minor tests fixes * Remove commented line * Add edge case checking testcase and references * Update comment * Add README.md * Fixed table * Minor changes in README.md * Fix README.md for documentation pages * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Meet review items * Meet review items (part 2) * Update polygons picture for README * Change simulation_time_step to 0.1 * Fix bounding boxes to fit the demo from README.md * Terminology fixes Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * removing the extra argument in class dec (#3084) * Fix for #3078, fix image path in YAML (#3082) * Fix for #3078, fix image path in YAML * Update map_io.cpp * Update map_io.cpp * Update map_io.cpp * Update map_io.cpp * do not depend on velocity for approach scaling (#3047) * do not depend on velocity for approach scaling * add approach_velocity_scaling_dist to README * do not pass references to shared ptr * fixup! do not pass references to shared ptr * fix approach velocity scaling compile issues * default approach_velocity_scaling_dist based on costmap size * change default approach_velocity_scaling_dist to 0.6 to match previous behavior * update tests for approach velocity scaling * remove use_approach_linear_velocity_scaling from readme * smooth approach velocity scaling * Use correct timeout when checking future (#3087) * Adds missing commas so default plugin names are not stuck together (#3093) Signed-off-by: Aaron Chong <aaronchongth@gmail.com> Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Fix Costmap Filters system tests (#3120) * Fix Costmap Filters system tests * Update map_io.cpp Co-authored-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com> * Finding distance H if obstacle H is <= 0 (#3122) * adding checks on goal IDs in results for waypoint follower (#3130) * ComputePathToPose Sets empty path on blackboard if action is aborted or cancelled (#3114) * set empty path on black on failure * docs * switched to correct message type * set empty path for compute_path_through_poses * Ignore feedback from old goals in waypoint follower (#3139) * apply joinchar in pathify (#3141) Co-authored-by: kevin <kevin@floatic.io> * Log level (#3110) * added log level for navigation launch * localization log level * slam log level * revert use_comp * added log level to components * linting and uncrusitfy fixes for CI (#3144) * start is now added to the path (#3140) * start is now added to the path * lint fix * Update README.md (#3147) Current example doesn't work with the updates. * fixing linting problem * Setting map map's yaml path to empty string, since overridden in launch (#3123) * Update nav2_params.yaml * Update nav2_params.yaml * Update nav2_params.yaml * bumping to 1.1.1 for release Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> Signed-off-by: Arshad <arshad.mehmood@intel.com> Signed-off-by: MartiBolet <mboletboixeda@gmail.com> Signed-off-by: Vinny Ruia <vinny.ruia@fireflyautomatix.com> Signed-off-by: Aaron Chong <aaronchongth@gmail.com> Co-authored-by: M. Mostafa Farzan <m2_farzan@yahoo.com> Co-authored-by: Zhenpeng Ge <zhenpeng.ge@qq.com> Co-authored-by: Joshua Wallace <47819219+jwallace42@users.noreply.github.com> Co-authored-by: Arshad Mehmood <arshad.mehmood@intel.com> Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com> Co-authored-by: shoufei <907575489@qq.com> Co-authored-by: hodnajit <jitkahodna67@gmail.com> Co-authored-by: Vinny Ruia <vinny.ruia@fireflyautomatix.com> Co-authored-by: SrijaneeBiswas <30804865+SrijaneeBiswas@users.noreply.github.com> Co-authored-by: Srijanee Biswas <srijanee.biswas@toyotatmh.com> Co-authored-by: Nikolas Engelhard <nikolas.engelhard@gmail.com> Co-authored-by: Adam Aposhian <aposhian.dev@gmail.com> Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com> Co-authored-by: nakai-omer <108797279+nakai-omer@users.noreply.github.com> Co-authored-by: Samuel Lindgren <samuel@dynorobotics.se> Co-authored-by: Aaron Chong <aaronchongth@gmail.com> Co-authored-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: 정찬희 <60467877+ladianchad@users.noreply.github.com> Co-authored-by: kevin <kevin@floatic.io> Co-authored-by: Austin Greisman <92941098+austin-InDro@users.noreply.github.com>
ros-navigation · Aug 24, 2022 · 74fae42 · 74fae42
1 parent 0bae341
commit 74fae42
Show file tree

Hide file tree

Showing 147 changed files with 6,452 additions and 509 deletions.
diff --git a/README.md b/README.md
diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.1.0</version>
+  <version>1.1.1</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "behaviortree_cpp_v3/action_node.h"
 #include "nav2_util/node_utils.hpp"
@@ -26,6 +27,8 @@
 namespace nav2_behavior_tree
 {
 
+using namespace std::chrono_literals;  // NOLINT
+
 /**
  * @brief Abstract class representing an action based BT node
  * @tparam ActionT Type of action
@@ -90,7 +93,12 @@ class BtActionNode : public BT::ActionNodeBase
 
     // Make sure the server is actually there before continuing
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
-    action_client_->wait_for_action_server();
+    if (!action_client_->wait_for_action_server(1s)) {
+      RCLCPP_ERROR(
+        node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
+        action_name.c_str());
+      throw std::runtime_error(std::string("Action server %s not available", action_name.c_str()));
+    }
   }
 
   /**

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -80,7 +80,8 @@ bool BtActionServer<ActionT>::on_configure()
   auto options = rclcpp::NodeOptions().arguments(
     {"--ros-args",
       "-r",
-      std::string("__node:=") + std::string(node->get_name()) + client_node_name + "_rclcpp_node",
+      std::string("__node:=") +
+      std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node",
       "--"});
 
   // Support for handling the topic-based goal pose from rviz
@@ -171,7 +172,13 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
     std::istreambuf_iterator<char>());
 
   // Create the Behavior Tree from the XML input
-  tree_ = bt_->createTreeFromText(xml_string, blackboard_);
+  try {
+    tree_ = bt_->createTreeFromText(xml_string, blackboard_);
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());
+    return false;
+  }
+
   topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
 
   current_bt_xml_filename_ = filename;

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
@@ -166,7 +166,7 @@ class BtServiceNode : public BT::ActionNodeBase
       auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
 
       rclcpp::FutureReturnCode rc;
-      rc = callback_group_executor_.spin_until_future_complete(future_result_, server_timeout_);
+      rc = callback_group_executor_.spin_until_future_complete(future_result_, timeout);
       if (rc == rclcpp::FutureReturnCode::SUCCESS) {
         request_sent_ = false;
         BT::NodeStatus status = on_completion(future_result_.get());

diff --git a/...vior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/...vior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp
@@ -54,6 +54,16 @@ class ComputePathThroughPosesAction
    */
   BT::NodeStatus on_success() override;
 
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancelation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp
@@ -51,6 +51,16 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
    */
   BT::NodeStatus on_success() override;
 
+  /**
+   * @brief Function to perform some user-defined operation upon abortion of the action
+   */
+  BT::NodeStatus on_aborted() override;
+
+  /**
+   * @brief Function to perform some user-defined operation upon cancelation of the action
+   */
+  BT::NodeStatus on_cancelled() override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports

diff --git a/...behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/...behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp
@@ -56,7 +56,7 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
     return providedBasicPorts(
       {
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goals", "Destinations to plan through"),
-        BT::InputPort<geometry_msgs::msg::PoseStamped>("behavior_tree", "Behavior tree to run"),
+        BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
       });
   }
 };

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp
@@ -56,7 +56,7 @@ class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPo
     return providedBasicPorts(
       {
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
-        BT::InputPort<geometry_msgs::msg::PoseStamped>("behavior_tree", "Behavior tree to run"),
+        BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
       });
   }
 };

diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.1.0</version>
+  <version>1.1.1</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp
@@ -44,6 +44,20 @@ BT::NodeStatus ComputePathThroughPosesAction::on_success()
   return BT::NodeStatus::SUCCESS;
 }
 
+BT::NodeStatus ComputePathThroughPosesAction::on_aborted()
+{
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus ComputePathThroughPosesAction::on_cancelled()
+{
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"

diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
@@ -43,6 +43,20 @@ BT::NodeStatus ComputePathToPoseAction::on_success()
   return BT::NodeStatus::SUCCESS;
 }
 
+BT::NodeStatus ComputePathToPoseAction::on_aborted()
+{
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus ComputePathToPoseAction::on_cancelled()
+{
+  nav_msgs::msg::Path empty_path;
+  setOutput("path", empty_path);
+  return BT::NodeStatus::SUCCESS;
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp_v3/bt_factory.h"

diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp
@@ -83,6 +83,7 @@ class CancelBackUpActionTestFixture : public ::testing::Test
     config_ = nullptr;
     node_.reset();
     action_server_.reset();
+    client_.reset();
     factory_.reset();
   }
 

diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp
@@ -83,6 +83,7 @@ class CancelControllerActionTestFixture : public ::testing::Test
     config_ = nullptr;
     node_.reset();
     action_server_.reset();
+    client_.reset();
     factory_.reset();
   }
 

diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp
@@ -86,6 +86,7 @@ class CancelDriveOnHeadingTestFixture : public ::testing::Test
     config_ = nullptr;
     node_.reset();
     action_server_.reset();
+    client_.reset();
     factory_.reset();
   }
 

diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp
@@ -83,6 +83,7 @@ class CancelSpinActionTestFixture : public ::testing::Test
     config_ = nullptr;
     node_.reset();
     action_server_.reset();
+    client_.reset();
     factory_.reset();
   }
 

diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp
@@ -83,6 +83,7 @@ class CancelWaitActionTestFixture : public ::testing::Test
     config_ = nullptr;
     node_.reset();
     action_server_.reset();
+    client_.reset();
     factory_.reset();
   }
 

diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp
@@ -67,9 +67,10 @@ class IsPathValidTestFixture : public ::testing::Test
   {
     delete config_;
     config_ = nullptr;
+    server_.reset();
     node_.reset();
     factory_.reset();
-    server_.reset();
+    tree_.reset();
   }
 
   static std::shared_ptr<IsPathValidService> server_;

diff --git a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp
@@ -43,6 +43,7 @@ class PathExpiringTimerConditionTestFixture : public nav2_behavior_tree::Behavio
   {
     delete config_;
     config_ = nullptr;
+    node_.reset();
     bt_node_.reset();
   }
 

diff --git a/nav2_behaviors/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt
@@ -125,6 +125,10 @@ install(FILES behavior_plugin.xml
   DESTINATION share/${PROJECT_NAME}
 )
 
+install(DIRECTORY plugins/
+  DESTINATION share/${PROJECT_NAME}/plugins/
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
@@ -63,13 +63,12 @@ class TimedBehavior : public nav2_core::Behavior
   TimedBehavior()
   : action_server_(nullptr),
     cycle_frequency_(10.0),
-    enabled_(false)
+    enabled_(false),
+    transform_tolerance_(0.0)
   {
   }
 
-  virtual ~TimedBehavior()
-  {
-  }
+  virtual ~TimedBehavior() = default;
 
   // Derived classes can override this method to catch the command and perform some checks
   // before getting into the main loop. The method will only be called
@@ -196,19 +195,6 @@ class TimedBehavior : public nav2_core::Behavior
       return;
     }
 
-    // Log a message every second
-    {
-      auto node = node_.lock();
-      if (!node) {
-        throw std::runtime_error{"Failed to lock node"};
-      }
-
-      auto timer = node->create_wall_timer(
-        1s,
-        [&]()
-        {RCLCPP_INFO(logger_, "%s running...", behavior_name_.c_str());});
-    }
-
     auto start_time = steady_clock_.now();
 
     // Initialize the ActionT result

diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behaviors</name>
-  <version>1.1.0</version>
+  <version>1.1.1</version>
   <description>TODO</description>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git a/nav2_behaviors/plugins/drive_on_heading.cpp b/nav2_behaviors/plugins/drive_on_heading.cpp
@@ -13,14 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <chrono>
-#include <ctime>
 #include <memory>
-#include <utility>
-
 #include "drive_on_heading.hpp"
-#include "nav2_util/node_utils.hpp"
-#include "nav2_msgs/action/back_up.hpp"
 
 #include "pluginlib/class_list_macros.hpp"
 PLUGINLIB_EXPORT_CLASS(nav2_behaviors::DriveOnHeading<>, nav2_core::Behavior)
diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/plugins/drive_on_heading.hpp
@@ -41,13 +41,14 @@ class DriveOnHeading : public TimedBehavior<ActionT>
    */
   DriveOnHeading()
   : TimedBehavior<ActionT>(),
-    feedback_(std::make_shared<typename ActionT::Feedback>())
+    feedback_(std::make_shared<typename ActionT::Feedback>()),
+    command_x_(0.0),
+    command_speed_(0.0),
+    simulate_ahead_time_(0.0)
   {
   }
 
-
-  ~DriveOnHeading()
-  {}
+  ~DriveOnHeading() = default;
 
   /**
    * @brief Initialization to run behavior
@@ -122,7 +123,6 @@ class DriveOnHeading : public TimedBehavior<ActionT>
       return Status::SUCCEEDED;
     }
 
-    // TODO(mhpanah): cmd_vel value should be passed as a parameter
     auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
     cmd_vel->linear.y = 0.0;
     cmd_vel->angular.z = 0.0;
@@ -199,18 +199,14 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     node->get_parameter("simulate_ahead_time", simulate_ahead_time_);
   }
 
-  double min_linear_vel_;
-  double max_linear_vel_;
-  double linear_acc_lim_;
+  typename ActionT::Feedback::SharedPtr feedback_;
 
   geometry_msgs::msg::PoseStamped initial_pose_;
   double command_x_;
   double command_speed_;
   rclcpp::Duration command_time_allowance_{0, 0};
   rclcpp::Time end_time_;
   double simulate_ahead_time_;
-
-  typename ActionT::Feedback::SharedPtr feedback_;
 };
 
 }  // namespace nav2_behaviors