From 9534a7cd00122ba21d00d68d75d85e7d83f5ee6e Mon Sep 17 00:00:00 2001
From: bpwilcox <bpwilcox@eng.ucsd.edu>
Date: Fri, 28 Jun 2019 16:02:12 -0700
Subject: [PATCH] fix rebase/merge conflicts

deleting robot package
---
 .../goal_reached_condition.hpp                |   2 +-
 .../include/dwb_controller/dwb_controller.hpp |   3 -
 .../dwb_controller/src/dwb_controller.cpp     |   6 +-
 .../motion_primitive.hpp                      |   3 +-
 nav2_navfn_planner/src/navfn_planner.cpp      |   4 -
 nav2_robot/CMakeLists.txt                     |  55 ----
 nav2_robot/README.md                          |  14 -
 nav2_robot/include/nav2_robot/robot.hpp       |  91 ------
 nav2_robot/package.xml                        |  39 ---
 nav2_robot/src/robot.cpp                      | 193 -------------
 nav2_robot/test/CMakeLists.txt                |  12 -
 nav2_robot/test/test_robot_class.cpp          | 267 ------------------
 nav2_util/CMakeLists.txt                      |  16 --
 nav2_util/include/nav2_util/robot_utils.hpp   |  16 +-
 nav2_util/src/CMakeLists.txt                  |  10 -
 nav2_util/src/map/CMakeLists.txt              |   3 -
 nav2_util/src/map_loader/CMakeLists.txt       |   3 -
 nav2_util/src/motion_model/CMakeLists.txt     |   5 -
 nav2_util/src/pf/CMakeLists.txt               |   3 -
 nav2_util/src/robot_utils.cpp                 |  48 ++--
 nav2_util/src/sensors/CMakeLists.txt          |   3 -
 21 files changed, 47 insertions(+), 749 deletions(-)
 delete mode 100644 nav2_robot/CMakeLists.txt
 delete mode 100644 nav2_robot/README.md
 delete mode 100644 nav2_robot/include/nav2_robot/robot.hpp
 delete mode 100644 nav2_robot/package.xml
 delete mode 100644 nav2_robot/src/robot.cpp
 delete mode 100644 nav2_robot/test/CMakeLists.txt
 delete mode 100644 nav2_robot/test/test_robot_class.cpp

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/goal_reached_condition.hpp
index faaae213..b8942890 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/goal_reached_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/goal_reached_condition.hpp
@@ -86,7 +86,7 @@ public:
 protected:
   void cleanup()
   {
-    robot_.reset();
+    robot_state_.reset();
   }
 
 private:
diff --git a/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp b/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp
index 58ac075f..f2b50f2e 100644
--- a/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp
+++ b/nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp
@@ -23,13 +23,10 @@
 #include "dwb_core/dwb_local_planner.hpp"
 #include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
 #include "nav_2d_utils/odom_subscriber.hpp"
-<<<<<<< HEAD
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_msgs/action/follow_path.hpp"
 #include "nav2_util/simple_action_server.hpp"
-=======
 #include "nav2_util/robot_utils.hpp"
->>>>>>> changing the controller to use the nav2_utils velocity publisher wrapper as well
 
 namespace dwb_controller
 {
diff --git a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
index 1784a7f3..4b0456b7 100644
--- a/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
+++ b/nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
@@ -74,7 +74,7 @@ DwbController::on_configure(const rclcpp_lifecycle::State & state)
   planner_->on_configure(state);
 
   odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(*this);
-  vel_publisher_ = std::make_shared<nav2_util::VelocityPublisher>(temp_node);
+  vel_publisher_ = std::make_shared<nav2_util::VelocityPublisher>(rclcpp_node_);
 
   // Create the action server that we implement with our followPath method
   action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "FollowPath",
@@ -90,7 +90,6 @@ DwbController::on_activate(const rclcpp_lifecycle::State & state)
 
   planner_->on_activate(state);
   costmap_ros_->on_activate(state);
-  vel_pub_->on_activate();
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -102,7 +101,6 @@ DwbController::on_deactivate(const rclcpp_lifecycle::State & state)
 
   planner_->on_deactivate(state);
   costmap_ros_->on_deactivate(state);
-  vel_pub_->on_deactivate();
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -119,7 +117,7 @@ DwbController::on_cleanup(const rclcpp_lifecycle::State & state)
   // Release any allocated resources
   planner_.reset();
   odom_sub_.reset();
-  vel_pub_.reset();
+  vel_publisher_.reset();
   action_server_.reset();
 
   return nav2_util::CallbackReturn::SUCCESS;
diff --git a/nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp b/nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp
index b78a888d..b6f47f58 100644
--- a/nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp
+++ b/nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp
@@ -94,6 +94,7 @@ protected:
   {
     robot_state_.reset();
     action_server_.reset();
+    vel_publisher_.reset();
   }
 
   void execute()
@@ -156,7 +157,7 @@ protected:
     cmd_vel.linear.y = 0.0;
     cmd_vel.angular.z = 0.0;
 
-    vel_publisher_->sendVelocity(cmd_vel);
+    vel_publisher_->publishCommand(cmd_vel);
   }
 };
 
diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp
index 13196008..a5b88f40 100644
--- a/nav2_navfn_planner/src/navfn_planner.cpp
+++ b/nav2_navfn_planner/src/navfn_planner.cpp
@@ -84,11 +84,7 @@ NavfnPlanner::on_configure(const rclcpp_lifecycle::State & /*state*/)
   plan_marker_publisher_ = create_publisher<visualization_msgs::msg::Marker>(
     "endpoints", 1);
 
-<<<<<<< HEAD
   auto node = shared_from_this();
-=======
-  robot_state_ = std::make_unique<nav2_util::RobotStateHelper>(temp_node);
->>>>>>> changing all users of robot_ to their appropriate eq.
 
   // Create the action server that we implement with our navigateToPose method
   action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "ComputePathToPose",
diff --git a/nav2_robot/CMakeLists.txt b/nav2_robot/CMakeLists.txt
deleted file mode 100644
index b46ade46..00000000
--- a/nav2_robot/CMakeLists.txt
+++ /dev/null
@@ -1,55 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(nav2_robot)
-
-find_package(ament_cmake REQUIRED)
-find_package(nav2_common REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(urdf REQUIRED)
-find_package(nav_msgs REQUIRED)
-find_package(nav2_util REQUIRED)
-
-nav2_package()
-
-include_directories(
-  include
-)
-
-add_library(nav2_robot SHARED
-  src/robot.cpp
-)
-
-ament_target_dependencies(nav2_robot
-  rclcpp
-  rclcpp_lifecycle
-  urdf
-  geometry_msgs
-  nav_msgs
-  nav2_util
-)
-
-install(TARGETS ${PROJECT_NAME}
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
-
-install(DIRECTORY include/
-  DESTINATION include/
-)
-
-if(BUILD_TESTING)
-  find_package(ament_cmake_pytest REQUIRED)
-  find_package(ament_cmake_gtest REQUIRED)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-  add_subdirectory(test)
-endif()
-
-ament_export_include_directories(include)
-ament_export_libraries(${PROJECT_NAME})
-ament_export_dependencies(urdf)
-ament_export_dependencies(nav_msgs)
-
-ament_package()
diff --git a/nav2_robot/README.md b/nav2_robot/README.md
deleted file mode 100644
index 38489f77..00000000
--- a/nav2_robot/README.md
+++ /dev/null
@@ -1,14 +0,0 @@
-# Nav2 Robot
-
-`nav2_robot` is a package containing an implementation class `Robot` containing information about robot specific parameters, controls, and information. 
-
-Today `Robot` contains basics for interacting with the robot hardware abstraction. It is the entry point for getting information from the odometry measurements and publishing velocity commands.
-
-## ROS1 Comparison
-
-This package does not have a direct counter-part in Navigation. Navigation contains a set of parameter files and reads information from each as needed. This package's intention is to act as a consolidator of robot-specific needs and information for each package across the meta-package to use. An example of another addition to meet that is to have an abstract implementation of a motion model for use in AMCL rather than hard-coding motion models options in the package itself. 
-
-## Future
-
-* This package may soon be deprecated in favor of some robot-specific utilities
-* Adding motion model, footprint, base parameters, etc 
diff --git a/nav2_robot/include/nav2_robot/robot.hpp b/nav2_robot/include/nav2_robot/robot.hpp
deleted file mode 100644
index bfce4f15..00000000
--- a/nav2_robot/include/nav2_robot/robot.hpp
+++ /dev/null
@@ -1,91 +0,0 @@
-// 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_ROBOT__ROBOT_HPP_
-#define NAV2_ROBOT__ROBOT_HPP_
-
-#include <string>
-
-#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
-#include "nav2_util/lifecycle_node.hpp"
-#include "nav_msgs/msg/odometry.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "urdf/model.h"
-
-namespace nav2_robot
-{
-
-class Robot : public nav2_util::LifecycleHelperInterface
-{
-public:
-  explicit Robot(nav2_util::LifecycleNode::SharedPtr node);
-  explicit Robot(
-    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
-    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
-    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
-    bool auto_start = false);
-  Robot() = delete;
-  ~Robot();
-
-  bool getGlobalLocalizerPose(
-    geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose);
-  bool getCurrentPose(
-    geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose);
-  bool getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom);
-  std::string getName();
-  void sendVelocity(geometry_msgs::msg::Twist twist);
-
-  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
-
-protected:
-  // Interfaces used for logging and creating publishers and subscribers
-  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
-  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
-  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
-
-  // Publishers and subscribers
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
-  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
-
-  // Subscription callbacks
-  void onPoseReceived(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
-  void onOdomReceived(const nav_msgs::msg::Odometry::SharedPtr msg);
-
-  // The current pose as received from the Pose subscription
-  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose_;
-
-  // The odometry as received from the Odometry subscription
-  nav_msgs::msg::Odometry::SharedPtr current_odom_;
-
-  // Whether the subscriptions have been received
-  bool initial_pose_received_{false};
-  bool initial_odom_received_{false};
-
-  // Information about the robot is contained in the URDF file
-  std::string urdf_file_;
-  urdf::Model model_;
-
-  // Auto-start feature for non-lifecycle nodes
-  bool auto_start_;
-  void configure();
-  void cleanup();
-};
-
-}  // namespace nav2_robot
-
-#endif  // NAV2_ROBOT__ROBOT_HPP_
diff --git a/nav2_robot/package.xml b/nav2_robot/package.xml
deleted file mode 100644
index 516e1512..00000000
--- a/nav2_robot/package.xml
+++ /dev/null
@@ -1,39 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>nav2_robot</name>
-  <version>0.1.5</version>
-  <description>TODO</description>
-  <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
-  <maintainer email="mohammad.haghighipanah@intel.com">Mohammad Haghighipanah</maintainer>
-  <license>Apache License 2.0</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <build_depend>rclcpp</build_depend>
-  <build_depend>rclcpp_lifecycle</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>urdf</build_depend>
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>nav2_util</build_depend>
-  <build_depend>nav2_common</build_depend>
-
-  <exec_depend>rclcpp</exec_depend>
-  <exec_depend>rclcpp_lifecycle</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>urdf</exec_depend>
-  <exec_depend>nav_msgs</exec_depend>
-  <exec_depend>nav2_util</exec_depend>
-
-  <test_depend>ament_cmake_gtest</test_depend>
-  <test_depend>ament_cmake_pytest</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>launch</test_depend>
-  <test_depend>launch_testing</test_depend>
-  <test_depend>nav2_util</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
diff --git a/nav2_robot/src/robot.cpp b/nav2_robot/src/robot.cpp
deleted file mode 100644
index 61a23c7d..00000000
--- a/nav2_robot/src/robot.cpp
+++ /dev/null
@@ -1,193 +0,0 @@
-// 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_robot/robot.hpp"
-
-#include <string>
-#include <exception>
-
-#include "rclcpp/create_subscription.hpp"
-#include "rclcpp/create_publisher.hpp"
-#include "rclcpp/qos.hpp"
-#include "urdf/model.h"
-
-namespace nav2_robot
-{
-
-Robot::Robot(nav2_util::LifecycleNode::SharedPtr node)
-: Robot(node->get_node_base_interface(),
-    node->get_node_topics_interface(),
-    node->get_node_logging_interface(),
-    false)
-{
-}
-
-Robot::Robot(
-  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
-  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
-  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
-  bool auto_start)
-: node_base_(node_base),
-  node_topics_(node_topics),
-  node_logging_(node_logging),
-  auto_start_(auto_start)
-{
-  RCLCPP_INFO(node_logging_->get_logger(), "Robot: Creating");
-
-  if (auto_start_) {
-    configure();
-  }
-}
-
-Robot::~Robot()
-{
-  RCLCPP_INFO(node_logging_->get_logger(), "Robot: Destroying");
-
-  if (auto_start_) {
-    cleanup();
-  }
-}
-
-nav2_util::CallbackReturn
-Robot::on_configure(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_INFO(node_logging_->get_logger(), "Robot: Configuring");
-
-  configure();
-
-  return nav2_util::CallbackReturn::SUCCESS;
-}
-
-nav2_util::CallbackReturn
-Robot::on_activate(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_INFO(node_logging_->get_logger(), "Robot: Activating");
-
-  return nav2_util::CallbackReturn::SUCCESS;
-}
-
-nav2_util::CallbackReturn
-Robot::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_INFO(node_logging_->get_logger(), "Robot: Deactivating");
-
-  return nav2_util::CallbackReturn::SUCCESS;
-}
-
-nav2_util::CallbackReturn
-Robot::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_INFO(node_logging_->get_logger(), "Robot: Cleaning up");
-
-  cleanup();
-
-  return nav2_util::CallbackReturn::SUCCESS;
-}
-
-void
-Robot::configure()
-{
-  // This class may be used from a module that comes up after AMCL has output
-  // its initial pose, so that pose message uses durability TRANSIENT_LOCAL
-
-  pose_sub_ = rclcpp::create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
-    node_topics_, "amcl_pose",
-    rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
-    std::bind(&Robot::onPoseReceived, this, std::placeholders::_1));
-
-  odom_sub_ = rclcpp::create_subscription<nav_msgs::msg::Odometry>(
-    node_topics_, "odom", rclcpp::SensorDataQoS(),
-    std::bind(&Robot::onOdomReceived, this, std::placeholders::_1));
-
-  vel_pub_ = rclcpp::create_publisher<geometry_msgs::msg::Twist>(
-    node_topics_, "/cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)));
-}
-
-void
-Robot::cleanup()
-{
-  pose_sub_.reset();
-  odom_sub_.reset();
-  vel_pub_.reset();
-}
-
-void
-Robot::onPoseReceived(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
-{
-  // TODO(mjeronimo): serialize access
-  current_pose_ = msg;
-  if (!initial_pose_received_) {
-    initial_pose_received_ = true;
-  }
-}
-
-void
-Robot::onOdomReceived(const nav_msgs::msg::Odometry::SharedPtr msg)
-{
-  current_odom_ = msg;
-  if (!initial_odom_received_) {
-    initial_odom_received_ = true;
-  }
-}
-
-bool
-Robot::getGlobalLocalizerPose(
-  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose)
-{
-  if (!initial_pose_received_) {
-    RCLCPP_DEBUG(node_logging_->get_logger(),
-      "Robot: Can't return current pose: Initial pose not yet received.");
-    return false;
-  }
-
-  robot_pose = current_pose_;
-  return true;
-}
-
-// TODO(mhpanah): We should get current pose from transforms.
-bool
-Robot::getCurrentPose(
-  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose)
-{
-  return getGlobalLocalizerPose(robot_pose);
-}
-
-bool
-Robot::getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom)
-{
-  if (!initial_odom_received_) {
-    RCLCPP_DEBUG(node_logging_->get_logger(),
-      "Robot: Can't return current velocity: Initial odometry not yet received.");
-    return false;
-  }
-
-  robot_odom = current_odom_;
-  return true;
-}
-
-// TODO(mhpanah): modify this method name and implementation to include robot types and Serial #(ID)
-std::string
-Robot::getName()
-{
-  // Temporarily just returning a string until we enable parsing URDF file.
-  return "turtlebot";
-}
-
-void
-Robot::sendVelocity(geometry_msgs::msg::Twist twist)
-{
-  vel_pub_->publish(twist);
-}
-
-}  // namespace nav2_robot
diff --git a/nav2_robot/test/CMakeLists.txt b/nav2_robot/test/CMakeLists.txt
deleted file mode 100644
index 939b63cb..00000000
--- a/nav2_robot/test/CMakeLists.txt
+++ /dev/null
@@ -1,12 +0,0 @@
-include_directories( ${PROJECT_SOURCE_DIR}/src )
-
-find_package(nav2_util REQUIRED)
-
-ament_add_gtest(test_robot_class
-  test_robot_class.cpp
-)
-ament_target_dependencies(test_robot_class
-  ${dependencies} nav2_util
-)
-target_link_libraries(test_robot_class nav2_robot)
-
diff --git a/nav2_robot/test/test_robot_class.cpp b/nav2_robot/test/test_robot_class.cpp
deleted file mode 100644
index 79f392f9..00000000
--- a/nav2_robot/test/test_robot_class.cpp
+++ /dev/null
@@ -1,267 +0,0 @@
-// 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 <gtest/gtest.h>
-#include <string>
-#include <memory>
-
-#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
-#include "nav2_robot/robot.hpp"
-#include "nav2_util/lifecycle_service_client.hpp"
-#include "nav_msgs/msg/odometry.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-using std::placeholders::_1;
-using lifecycle_msgs::msg::Transition;
-
-class RclCppFixture
-{
-public:
-  RclCppFixture() {rclcpp::init(0, nullptr);}
-  ~RclCppFixture() {rclcpp::shutdown();}
-};
-
-RclCppFixture g_rclcppfixture;
-
-class TestLifecycleNode : public nav2_util::LifecycleNode
-{
-public:
-  TestLifecycleNode()
-  : nav2_util::LifecycleNode("TestLifecycleNode")
-  {
-  }
-
-  nav2_util::CallbackReturn
-  on_configure(const rclcpp_lifecycle::State & state) override
-  {
-    robot_ = std::make_unique<nav2_robot::Robot>(shared_from_this());
-    robot_->on_configure(state);
-    return nav2_util::CallbackReturn::SUCCESS;
-  }
-
-  nav2_util::CallbackReturn
-  on_activate(const rclcpp_lifecycle::State & state) override
-  {
-    robot_->on_activate(state);
-    return nav2_util::CallbackReturn::SUCCESS;
-  }
-
-  nav2_util::CallbackReturn
-  on_deactivate(const rclcpp_lifecycle::State & state) override
-  {
-    robot_->on_deactivate(state);
-    return nav2_util::CallbackReturn::SUCCESS;
-  }
-
-  nav2_util::CallbackReturn
-  on_cleanup(const rclcpp_lifecycle::State & state) override
-  {
-    robot_->on_cleanup(state);
-    robot_.reset();
-    return nav2_util::CallbackReturn::SUCCESS;
-  }
-
-  bool getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom)
-  {
-    return robot_->getOdometry(robot_odom);
-  }
-
-  std::string getName()
-  {
-    return robot_->getName();
-  }
-
-  bool getCurrentPose(
-    geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose)
-  {
-    return robot_->getCurrentPose(robot_pose);
-  }
-
-  void sendVelocity(geometry_msgs::msg::Twist twist)
-  {
-    robot_->sendVelocity(twist);
-  }
-
-protected:
-  std::unique_ptr<nav2_robot::Robot> robot_;
-};
-
-class TestRobotClass : public ::testing::Test
-{
-public:
-  TestRobotClass()
-  {
-    // Create a lifecycle node that uses a robot and start a thread to handle its messages
-    lifecycle_node_ = std::make_shared<TestLifecycleNode>();
-
-    lifecycle_thread_ = std::make_unique<std::thread>(
-      [this](nav2_util::LifecycleNode::SharedPtr node) {
-        for (;; ) {
-          rclcpp::spin_some(node->get_node_base_interface());
-          if (shut_down_threads_) {return;}
-        }
-      }, lifecycle_node_
-    );
-
-    client_ = std::make_shared<rclcpp::Node>("test_robot_client_node");
-
-    lifecycle_client_ =
-      std::make_shared<nav2_util::LifecycleServiceClient>("TestLifecycleNode", client_);
-
-    lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE);
-    lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE);
-
-    // Initializing Pose and Twist messages
-    initTestPose();
-    initTestTwist();
-
-    // Creating fake publishers
-    pose_pub_ = client_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
-      "amcl_pose", rclcpp::SystemDefaultsQoS().transient_local());
-
-    odom_pub_ = client_->create_publisher<nav_msgs::msg::Odometry>(
-      "odom", rclcpp::SystemDefaultsQoS());
-
-    // Subscribing to cmdVelocity topic to make sure Robot class is publishing velocity
-    vel_sub_ = client_->create_subscription<geometry_msgs::msg::Twist>("cmd_vel",
-        rclcpp::SystemDefaultsQoS(),
-        std::bind(&TestRobotClass::velocityReceived, this, std::placeholders::_1));
-
-    velocityCmdReceived_ = false;
-  }
-
-  ~TestRobotClass()
-  {
-    lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
-    lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
-
-    shut_down_threads_ = true;
-    lifecycle_thread_->join();
-  }
-
-  void velocityReceived(const geometry_msgs::msg::Twist::SharedPtr msg);
-
-protected:
-  std::shared_ptr<rclcpp::Node> client_;
-
-  std::shared_ptr<TestLifecycleNode> lifecycle_node_;
-  std::unique_ptr<std::thread> lifecycle_thread_;
-
-  std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
-
-  geometry_msgs::msg::PoseWithCovarianceStamped testPose_;
-  geometry_msgs::msg::Twist testTwist_;
-  nav_msgs::msg::Odometry testOdom_;
-  geometry_msgs::msg::Twist velocityReceived_;
-
-  bool velocityCmdReceived_;
-  bool shut_down_threads_{false};
-
-  void initTestPose();
-  void initTestTwist();
-  void publishPose();
-  void publishOdom();
-
-private:
-  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
-  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::ConstSharedPtr vel_sub_;
-};
-
-void
-TestRobotClass::publishPose()
-{
-  pose_pub_->publish(testPose_);
-}
-
-void
-TestRobotClass::publishOdom()
-{
-  testOdom_.pose = testPose_.pose;
-  testOdom_.twist.twist = testTwist_;
-  testOdom_.header.frame_id = "base_footprint";
-  testOdom_.header.stamp = rclcpp::Time();
-  odom_pub_->publish(testOdom_);
-}
-
-void
-TestRobotClass::velocityReceived(const geometry_msgs::msg::Twist::SharedPtr msg)
-{
-  velocityReceived_ = *msg;
-  velocityCmdReceived_ = true;
-}
-
-void TestRobotClass::initTestPose()
-{
-  testPose_.header.frame_id = "testPose";
-  testPose_.header.stamp = rclcpp::Time();
-  testPose_.pose.pose.position.x = 0.0;
-  testPose_.pose.pose.position.y = 0.0;
-  testPose_.pose.pose.position.z = 0.0;
-  testPose_.pose.pose.orientation.x = 0.0;
-  testPose_.pose.pose.orientation.y = 0.0;
-  testPose_.pose.pose.orientation.z = 0.0;
-  testPose_.pose.pose.orientation.w = 1.0;
-  for (int i = 0; i < 12; i++) {
-    testPose_.pose.covariance[i] = 0.0;
-  }
-}
-
-void TestRobotClass::initTestTwist()
-{
-  testTwist_.linear.x = 0.0;
-  testTwist_.linear.y = 0.0;
-  testTwist_.linear.z = 0.0;
-  testTwist_.angular.x = 0.0;
-  testTwist_.angular.y = 0.0;
-  testTwist_.angular.z = 0.0;
-}
-
-TEST_F(TestRobotClass, getNameTest)
-{
-  std::string robotName = lifecycle_node_->getName();
-  EXPECT_EQ(robotName, "turtlebot");
-}
-
-TEST_F(TestRobotClass, getPoseTest)
-{
-  auto currentPose = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
-  while (!(lifecycle_node_->getCurrentPose(currentPose))) {
-    publishPose();
-    rclcpp::spin_some(client_);
-  }
-  EXPECT_EQ(*currentPose, testPose_);
-}
-
-TEST_F(TestRobotClass, getOdometryTest)
-{
-  auto currentOdom = std::make_shared<nav_msgs::msg::Odometry>();
-  rclcpp::Rate r(10);
-  while (!(lifecycle_node_->getOdometry(currentOdom))) {
-    publishOdom();
-    r.sleep();
-  }
-  EXPECT_EQ(*currentOdom, testOdom_);
-}
-
-TEST_F(TestRobotClass, sendVelocityTest)
-{
-  rclcpp::Rate r(10);
-  while (!velocityCmdReceived_) {
-    lifecycle_node_->sendVelocity(testTwist_);
-    rclcpp::spin_some(client_);
-    r.sleep();
-  }
-  EXPECT_EQ(testTwist_, velocityReceived_);
-}
diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt
index 438c357d..24e3feab 100644
--- a/nav2_util/CMakeLists.txt
+++ b/nav2_util/CMakeLists.txt
@@ -22,22 +22,6 @@ include_directories(include)
 
 set(library_name ${PROJECT_NAME}_core)
 add_subdirectory(src)
-<<<<<<< HEAD
-=======
-
-install(TARGETS
-  ${library_name}
-  map_lib
-  pf_lib
-  sensors_lib
-  motions_lib
-  map_loader
-  lifecycle_bringup
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
->>>>>>> resolve merge conflicts
 
 install(DIRECTORY include/
   DESTINATION include/
diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp
index 442bbc26..0431c633 100644
--- a/nav2_util/include/nav2_util/robot_utils.hpp
+++ b/nav2_util/include/nav2_util/robot_utils.hpp
@@ -30,31 +30,35 @@ namespace nav2_util
 class VelocityPublisher
 {
 public:
-  VelocityPublisher(rclcpp::Node::SharedPtr & node,
+  VelocityPublisher(
+    rclcpp::Node::SharedPtr & node,
     const std::string topic = std::string("/cmd_vel"));
 
-  void publishCommand(const geometry_msgs::msg::Twist& cmd_vel);
+  void publishCommand(const geometry_msgs::msg::Twist & cmd_vel);
 
 private:
-    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
 };
 
 class RobotStateHelper
 {
 public:
-  RobotStateHelper(rclcpp::Node::SharedPtr & node,
+  RobotStateHelper(
+    rclcpp::Node::SharedPtr & node,
     const std::string odom_topic = std::string("/odom"));
 
   bool getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom);
 
-  bool getCurrentPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose,
+  bool getCurrentPose(
+    geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose,
     const bool use_topic = true);
 
 private:
   void onPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr robot_pose);
   void onOdomReceived(nav_msgs::msg::Odometry::SharedPtr msg);
 
-  bool getGlobalLocalizerPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose);
+  bool getGlobalLocalizerPose(
+    geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose);
   bool getTfPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose);
 
 private:
diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt
index 5fca88bd..59fe6583 100644
--- a/nav2_util/src/CMakeLists.txt
+++ b/nav2_util/src/CMakeLists.txt
@@ -4,12 +4,8 @@ add_library(${library_name} SHARED
   lifecycle_service_client.cpp
   string_utils.cpp
   lifecycle_utils.cpp
-<<<<<<< HEAD
   lifecycle_node.cpp
-  src/robot_utils.cpp
-=======
   robot_utils.cpp
->>>>>>> resolve merge conflicts
 )
 
 ament_target_dependencies(${library_name}
@@ -20,10 +16,7 @@ ament_target_dependencies(${library_name}
   nav_msgs
   geometry_msgs
   lifecycle_msgs
-<<<<<<< HEAD
   rclcpp_lifecycle
-=======
->>>>>>> resolve merge conflicts
 )
 
 add_subdirectory(pf)
@@ -36,7 +29,6 @@ add_executable(lifecycle_bringup
   lifecycle_bringup_commandline.cpp
 )
 target_link_libraries(lifecycle_bringup ${library_name})
-<<<<<<< HEAD
 
 install(TARGETS
   ${library_name}
@@ -45,5 +37,3 @@ install(TARGETS
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
-=======
->>>>>>> resolve merge conflicts
diff --git a/nav2_util/src/map/CMakeLists.txt b/nav2_util/src/map/CMakeLists.txt
index c69128c3..7017eb49 100644
--- a/nav2_util/src/map/CMakeLists.txt
+++ b/nav2_util/src/map/CMakeLists.txt
@@ -5,7 +5,6 @@ add_library(map_lib SHARED
   map_draw.c
   map_cspace.cpp
 )
-<<<<<<< HEAD
 
 install(TARGETS
   map_lib
@@ -13,5 +12,3 @@ install(TARGETS
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
-=======
->>>>>>> resolve merge conflicts
diff --git a/nav2_util/src/map_loader/CMakeLists.txt b/nav2_util/src/map_loader/CMakeLists.txt
index a1b2bbac..ece55ee9 100644
--- a/nav2_util/src/map_loader/CMakeLists.txt
+++ b/nav2_util/src/map_loader/CMakeLists.txt
@@ -15,7 +15,6 @@ target_link_libraries(map_loader
     ${SDL_LIBRARY}
     ${SDL_IMAGE_LIBRARIES}
 )
-<<<<<<< HEAD
 
 install(TARGETS
   map_loader
@@ -23,5 +22,3 @@ install(TARGETS
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
-=======
->>>>>>> resolve merge conflicts
diff --git a/nav2_util/src/motion_model/CMakeLists.txt b/nav2_util/src/motion_model/CMakeLists.txt
index 031fd1d6..c9c80e5c 100644
--- a/nav2_util/src/motion_model/CMakeLists.txt
+++ b/nav2_util/src/motion_model/CMakeLists.txt
@@ -1,7 +1,6 @@
 add_library(motions_lib SHARED
   omni_motion_model.cpp
   differential_motion_model.cpp
-<<<<<<< HEAD
   motion_model.cpp
 )
 target_link_libraries(motions_lib pf_lib)
@@ -12,7 +11,3 @@ install(TARGETS
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
-=======
-)
-target_link_libraries(motions_lib pf_lib)
->>>>>>> resolve merge conflicts
diff --git a/nav2_util/src/pf/CMakeLists.txt b/nav2_util/src/pf/CMakeLists.txt
index cbc252b3..b277cc93 100644
--- a/nav2_util/src/pf/CMakeLists.txt
+++ b/nav2_util/src/pf/CMakeLists.txt
@@ -10,7 +10,6 @@ add_library(pf_lib SHARED
   eig3.c
   pf_draw.c
 )
-<<<<<<< HEAD
 
 install(TARGETS
   pf_lib
@@ -18,5 +17,3 @@ install(TARGETS
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
-=======
->>>>>>> resolve merge conflicts
diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp
index 4a38d18d..99f09f91 100644
--- a/nav2_util/src/robot_utils.cpp
+++ b/nav2_util/src/robot_utils.cpp
@@ -18,18 +18,24 @@
 namespace nav2_util
 {
 
-VelocityPublisher::VelocityPublisher(rclcpp::Node::SharedPtr & node,
-    const std::string topic) {
-    vel_pub_ = node->create_publisher<geometry_msgs::msg::Twist>(topic, 1);
-  }
+VelocityPublisher::VelocityPublisher(
+  rclcpp::Node::SharedPtr & node,
+  const std::string topic)
+{
+  vel_pub_ = node->create_publisher<geometry_msgs::msg::Twist>(topic, 1);
+}
 
-void 
-VelocityPublisher::publishCommand(const geometry_msgs::msg::Twist& cmd_vel) {
-    vel_pub_->publish(cmd_vel);
+void
+VelocityPublisher::publishCommand(const geometry_msgs::msg::Twist & cmd_vel)
+{
+  vel_pub_->publish(cmd_vel);
 }
 
-RobotStateHelper::RobotStateHelper(rclcpp::Node::SharedPtr & node,
-  const std::string odom_topic) : node_(node) {
+RobotStateHelper::RobotStateHelper(
+  rclcpp::Node::SharedPtr & node,
+  const std::string odom_topic)
+: node_(node)
+{
   pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "amcl_pose", std::bind(&RobotStateHelper::onPoseReceived, this, std::placeholders::_1));
 
@@ -41,7 +47,8 @@ RobotStateHelper::RobotStateHelper(rclcpp::Node::SharedPtr & node,
 }
 
 bool
-RobotStateHelper::getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom) {
+RobotStateHelper::getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom)
+{
   std::shared_lock<std::shared_timed_mutex> lock(state_mutex_);
   if (!initial_odom_received_) {
     RCLCPP_DEBUG(node_->get_logger(),
@@ -54,8 +61,10 @@ RobotStateHelper::getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom) {
 }
 
 bool
-RobotStateHelper::getCurrentPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose,
-  const bool use_topic) {
+RobotStateHelper::getCurrentPose(
+  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose,
+  const bool use_topic)
+{
   if (use_topic) {
     return getGlobalLocalizerPose(robot_pose);
   }
@@ -64,7 +73,9 @@ RobotStateHelper::getCurrentPose(geometry_msgs::msg::PoseWithCovarianceStamped::
 }
 
 void
-RobotStateHelper::onPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr robot_pose) {
+RobotStateHelper::onPoseReceived(
+  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr robot_pose)
+{
   std::unique_lock<std::shared_timed_mutex> lock(state_mutex_);
   current_pose_ = robot_pose;
   if (!initial_pose_received_) {
@@ -73,7 +84,8 @@ RobotStateHelper::onPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::
 }
 
 void
-RobotStateHelper::onOdomReceived(nav_msgs::msg::Odometry::SharedPtr msg) {
+RobotStateHelper::onOdomReceived(nav_msgs::msg::Odometry::SharedPtr msg)
+{
   std::unique_lock<std::shared_timed_mutex> lock(state_mutex_);
   current_odom_ = msg;
   if (!initial_odom_received_) {
@@ -82,7 +94,9 @@ RobotStateHelper::onOdomReceived(nav_msgs::msg::Odometry::SharedPtr msg) {
 }
 
 bool
-RobotStateHelper::getGlobalLocalizerPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose) {
+RobotStateHelper::getGlobalLocalizerPose(
+  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose)
+{
   std::shared_lock<std::shared_timed_mutex> lock(state_mutex_);
   if (!initial_pose_received_) {
     RCLCPP_DEBUG(node_->get_logger(),
@@ -95,7 +109,9 @@ RobotStateHelper::getGlobalLocalizerPose(geometry_msgs::msg::PoseWithCovarianceS
 }
 
 bool
-RobotStateHelper::getTfPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & /*robot_pose*/) {
+RobotStateHelper::getTfPose(
+  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & /*robot_pose*/)
+{
   std::shared_lock<std::shared_timed_mutex> lock(state_mutex_);
   RCLCPP_DEBUG(node_->get_logger(), "getTfPose is not yet implemented.");
   return false;
diff --git a/nav2_util/src/sensors/CMakeLists.txt b/nav2_util/src/sensors/CMakeLists.txt
index bd63f710..56c3a382 100644
--- a/nav2_util/src/sensors/CMakeLists.txt
+++ b/nav2_util/src/sensors/CMakeLists.txt
@@ -5,7 +5,6 @@ add_library(sensors_lib SHARED
   laser/likelihood_field_model_prob.cpp
 )
 target_link_libraries(sensors_lib pf_lib)
-<<<<<<< HEAD
 
 install(TARGETS
   sensors_lib
@@ -13,5 +12,3 @@ install(TARGETS
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
-=======
->>>>>>> resolve merge conflicts
-- 
GitLab