#include "amr_control/amr_control.h"

#include <robot/robot.h>
#include <tf3/buffer_core.h>
#include <geometry_msgs/Vector3.h>
#include <xmlrpcpp/XmlRpcValue.h>
#include <robot_geometry_msgs/Point.h>

namespace amr_control
{
  AmrController::AmrController(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer)
      : initalized_(false),
        vda_5050_client_api_ptr_(nullptr),
        opc_ua_server_api_ptr_(nullptr),
        move_base_initialized_(false),
        loc_base_initialized_(false),
        monitor_ptr_(nullptr),
        loc_base_ptr_(nullptr),
        move_base_ptr_(nullptr),
        amr_safety_ptr_(nullptr),
        tf_core_ptr_(nullptr),
        loc_base_loader_("loc_core", "loc_core::BaseLocalization")
  {
    this->init(nh, buffer);
  }

  AmrController::AmrController()
      : initalized_(false),
        move_base_initialized_(false),
        loc_base_initialized_(false),
        vda_5050_client_api_ptr_(nullptr),
        opc_ua_server_api_ptr_(nullptr),
        monitor_ptr_(nullptr),
        loc_base_ptr_(nullptr),
        move_base_ptr_(nullptr),
        amr_safety_ptr_(nullptr),
        tf_converter_ptr_(nullptr),
        tf_core_ptr_(nullptr),
        loc_base_loader_("loc_core", "loc_core::BaseLocalization")
  {
  }

  AmrController::~AmrController()
  {
    // Wait for both threads to complete
    if (move_base_thr_)
    {
      move_base_thr_->join();
      move_base_thr_.reset();
    }

    if (loc_base_thr_)
    {
      loc_base_thr_->join();
      loc_base_thr_.reset();
    }

    if (server_thr_)
    {
      server_thr_->join();
      server_thr_.reset();
    }

    if (monitor_thr_)
    {
      monitor_thr_->join();
      monitor_thr_.reset();
    }

    if (opc_ua_server_api_ptr_)
    {
      opc_ua_server_api_ptr_.reset();
    }

    if (vda_5050_client_api_ptr_)
    {
      vda_5050_client_api_ptr_.reset();
    }

    // Stop costmap, cmd_vel and plan publishing
    if (amr_publisher_ptr_)
    {
      amr_publisher_ptr_->stopPublishing();
      amr_publisher_ptr_->stopCmdVelPublishing();
      amr_publisher_ptr_->stopPlanPublishing();
      amr_publisher_ptr_.reset();
    }
  }

  void AmrController::init(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer)
  {
    if (!initalized_)
    {
      nh_ = nh;
      tf_ = buffer;

      monitor_thr_ = std::make_shared<std::thread>(
          [this]()
          {
            this->initalizingMonitorHandle(nh_);
            this->threadHandle();
          });

      move_base_initialized_ = false;
      // Setup base localization
      loc_base_thr_ = std::make_shared<std::thread>(
          [this]()
          {
            this->initalizingLocalizationHandle(nh_);
            {
              std::unique_lock<std::mutex> lock(init_loc_base_mutex_);
              loc_base_initialized_ = true;
            }
            init_loc_base_cv_.notify_one();
          });

      // Setup base navigation
      move_base_thr_ = std::make_shared<std::thread>(
          [this]()
          {
            std::unique_lock<std::mutex> lock(init_loc_base_mutex_);
            init_loc_base_cv_.wait(lock, [this]()
                                   { return loc_base_initialized_; });

            this->initalizingMoveBaseHandle(nh_);
            {
              std::lock_guard<std::mutex> lock(init_move_base_mutex_);
              move_base_initialized_ = true;
            }
            init_move_base_cv_.notify_one();

            // this->controllerDotuff();
          });

      // Setup server
      server_thr_ = std::make_shared<std::thread>(
          [this]()
          {
            std::unique_lock<std::mutex> lock(init_move_base_mutex_);
            init_move_base_cv_.wait(lock, [this]()
                                    { return move_base_initialized_; });
            this->initalizingComunicationHandle(nh_);
          });

      ros::NodeHandle nh_core;
      is_detected_maker_sub_ = nh_core.subscribe("/is_detected_maker", 1, &AmrController::isDetectedMakerCallback, this);
      initalized_ = true;
    }
  }

  void AmrController::stop()
  {
  }

  void AmrController::initalizingComunicationHandle(ros::NodeHandle &nh)
  {

    this->count_ng_max_ = this->count_ok_max_ = 1;
    std::shared_ptr opc_ua_server_thr = std::make_shared<std::thread>(
        [this, nh]()
        {
          opc_ua_server_api_ptr_ =
              std::make_shared<OpcUAServerAPI>(nh, move_base_ptr_, loc_base_ptr_, monitor_ptr_);

          OpcUAServerAPI::muted_value_ = &this->muted_;
          OpcUAServerAPI::unLoad_excuted_ = std::bind(&AmrController::unLoadCallBack, this);
          OpcUAServerAPI::load_excuted_ = std::bind(&AmrController::loadCallBack, this);
          OpcUAServerAPI::belt_thread_ptr_ = &this->belt_thread_;
          OpcUAServerAPI::cur_belt_state_en_ = &this->cur_belt_state_en_;
          OpcUAServerAPI::belt_cancel_ = &this->cancel_;

          OpcUAServerAPI::arm_function_ptr_ = std::bind(&AmrController::ArmCallBack, this);
          OpcUAServerAPI::arm_thread_ptr_ = &this->arm_thread_;
          OpcUAServerAPI::arm_cancel_ = &this->enable_;
          OpcUAServerAPI::arm_go_home_ = &this->arm_go_home_;
          OpcUAServerAPI::arm_continue_ = &this->arm_continue_;
          OpcUAServerAPI::arm_power_on_ = &this->arm_power_on_;
          OpcUAServerAPI::count_ng_max_ = &this->count_ng_max_;
          OpcUAServerAPI::count_ok_max_ = &this->count_ok_max_;
          OpcUAServerAPI::status_code_ptr_ = this->status_code_ptr_;
          opc_ua_server_api_ptr_->start();
        });

    std::shared_ptr vda_5050_client_thr = std::make_shared<std::thread>(
        [this, nh]()
        {
          vda_5050_client_api_ptr_ =
              std::make_shared<VDA5050ClientAPI>(nh, move_base_ptr_, loc_base_ptr_, monitor_ptr_);

          vda_5050_client_api_ptr_->muted_value_ = &this->muted_;
          vda_5050_client_api_ptr_->unLoad_excuted_ = std::bind(&AmrController::unLoadCallBack, this);
          vda_5050_client_api_ptr_->load_excuted_ = std::bind(&AmrController::loadCallBack, this);
          vda_5050_client_api_ptr_->cur_belt_state_en_ = &this->cur_belt_state_en_;
          vda_5050_client_api_ptr_->belt_thread_ptr_ = &this->belt_thread_;

          vda_5050_client_api_ptr_->arm_function_ptr_ = std::bind(&AmrController::ArmCallBack, this);
          vda_5050_client_api_ptr_->arm_thread_ptr_ = &this->arm_thread_;
          vda_5050_client_api_ptr_->enable_action_ = &this->enable_;
          vda_5050_client_api_ptr_->cancel_action_ = &this->cancel_;
          vda_5050_client_api_ptr_->pause_action_ = &this->pause_;
          vda_5050_client_api_ptr_->count_ng_max_ = &this->count_ng_max_;
          vda_5050_client_api_ptr_->count_ok_max_ = &this->count_ok_max_;
        });
    ROS_INFO("Initalizing comunication is done");
    opc_ua_server_thr->join();
    vda_5050_client_thr->join();
  }

  void AmrController::initalizingLocalizationHandle(ros::NodeHandle &nh)
  {
    std::string obj_name("loc_base::LocBase");
    if (tf_ == nullptr)
      throw std::runtime_error("tf2_ros::Buffer object is null");
    try
    {
      loc_base_ptr_ = loc_base_loader_.createUniqueInstance(obj_name);
      ros::NodeHandle node = ros::NodeHandle(nh, "global_costmap");
      loc_base_ptr_->initialize(node, tf_);
    }
    catch (pluginlib::PluginlibException &ex)
    {
      ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
      exit(1);
    }
    catch (std::exception &e)
    {
      ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
      exit(1);
    }
  }

  void AmrController::initalizingMoveBaseHandle(ros::NodeHandle &nh)
  {
    std::string obj_name("move_base::MoveBase");
    if (tf_ == nullptr)
      throw std::runtime_error("tf2_ros::Buffer object is null");

    tf_converter_ptr_ = std::make_shared<amr_control::TfConverter>(tf_);
    tf_converter_ptr_->TfBuffer(tf_core_ptr_);
    try
    {
      robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__);
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath("MoveBase");
      move_base_loader_ = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
        path_file_so, "MoveBase", boost::dll::load_mode::append_decorations);
      
      robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
      move_base_ptr_ = move_base_loader_();
      sensor_converter_ptr_ = std::make_shared<amr_control::SensorConverter>(nh, move_base_ptr_);
      move_base_ptr_->initialize(tf_core_ptr_);

      // Initialize costmap publisher for RViz visualization
      robot::log_info("[%s:%d]\n Initializing costmap publisher...", __FILE__, __LINE__);
      amr_publisher_ptr_ = std::make_shared<amr_control::AmrPublisher>(nh, move_base_ptr_);
      amr_subscriber_ptr_ = std::make_shared<amr_control::AmrSubscriber>(nh, move_base_ptr_);
      
      ros::Rate r(3);
      do
      {
        r.sleep();
        ros::spinOnce();
      } while (ros::ok() && !move_base_ptr_->getFeedback()->is_ready);

      if (move_base_ptr_ != nullptr &&
          move_base_ptr_->getFeedback() != nullptr &&
          move_base_ptr_->getFeedback()->is_ready)
      {
        // Read footprint from parameter server and set it
        std::vector<robot_geometry_msgs::Point> footprint;
        if (nh.hasParam("footprint"))
        {
          XmlRpc::XmlRpcValue footprint_xmlrpc;
          nh.getParam("footprint", footprint_xmlrpc);
          
          if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_xmlrpc.size() >= 3)
          {
            footprint.reserve(footprint_xmlrpc.size());
            for (int i = 0; i < footprint_xmlrpc.size(); ++i)
            {
              if (footprint_xmlrpc[i].getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_xmlrpc[i].size() == 2)
              {
                robot_geometry_msgs::Point pt;
                pt.x = static_cast<double>(footprint_xmlrpc[i][0]);
                pt.y = static_cast<double>(footprint_xmlrpc[i][1]);
                pt.z = 0.0;
                footprint.push_back(pt);
              }
            }
            
            if (footprint.size() >= 3)
            {
              move_base_ptr_->setRobotFootprint(footprint);
              robot::log_info("[%s:%d] Set robot footprint with %zu points", __FILE__, __LINE__, footprint.size());
              for (size_t i = 0; i < footprint.size(); ++i)
              {
                robot::log_info("[%s:%d] Footprint point[%zu]: (%.3f, %.3f)", 
                              __FILE__, __LINE__, i, footprint[i].x, footprint[i].y);
              }
            }
            else
            {
              robot::log_warning("[%s:%d] Footprint must have at least 3 points, got %zu. Using default footprint.", 
                                __FILE__, __LINE__, footprint.size());
            }
          }
          else
          {
            robot::log_warning("[%s:%d] Invalid footprint parameter format. Expected array of arrays with at least 3 points.", 
                              __FILE__, __LINE__);
          }
        }
        else
        {
          robot::log_info("[%s:%d] No footprint parameter found, using default footprint from move_base config.", 
                        __FILE__, __LINE__);
        }


        robot_geometry_msgs::Vector3 linear;
        linear.x = 0.3;
        move_base_ptr_->setTwistLinear(linear);
        linear.x = -0.3;
        move_base_ptr_->setTwistLinear(linear);

        // Request map từ service hoặc đợi message (vì map server chỉ publish một lần)
        robot::log_info("[%s:%d]\n Requesting map from server...", __FILE__, __LINE__);
        sensor_converter_ptr_->requestMapFromServer(nh);
        
        // Start publishing costmaps to RViz
        // Global costmap: 1 Hz, Local costmap: 5 Hz

        if (amr_publisher_ptr_)
        {
          robot::log_info("[%s:%d]\n Starting costmap publishing to RViz...", __FILE__, __LINE__);
          amr_publisher_ptr_->startPublishing(1.0, 5.0);
          
          // Start publishing cmd_vel using WallTimer
          double cmd_vel_rate = 20.0;  // Default: 20 Hz
          nh.param("cmd_vel_publish_rate", cmd_vel_rate, cmd_vel_rate);
          robot::log_info("[%s:%d]\n Starting cmd_vel publishing at %.2f Hz...", __FILE__, __LINE__, cmd_vel_rate);
          amr_publisher_ptr_->startCmdVelPublishing(cmd_vel_rate);
          
          // Start publishing plans using Timer
          double global_plan_rate = 10.0;  // Default: 10 Hz
          double local_plan_rate = 10.0;    // Default: 10 Hz
          nh.param("global_plan_publish_rate", global_plan_rate, global_plan_rate);
          nh.param("local_plan_publish_rate", local_plan_rate, local_plan_rate);
          robot::log_info("[%s:%d]\n Starting plan publishing - Global: %.2f Hz, Local: %.2f Hz...", 
                         __FILE__, __LINE__, global_plan_rate, local_plan_rate);
          amr_publisher_ptr_->startPlanPublishing(global_plan_rate, local_plan_rate);
        }
      }
    }
    catch (const std::exception &e)
    {
      robot::log_error("[%s:%d]\n Exception: %s", __FILE__, __LINE__, e.what());
      exit(1);
    }
    catch (...)
    {
      robot::log_error("[%s:%d]\n Unknown exception occurred", __FILE__, __LINE__);
      exit(1);
    }
  }

  void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh)
  {
    this->monitor_ptr_ = std::make_shared<AmrMonitor>(nh);
  }

  void AmrController::ArmCallBack()
  {
    ROS_INFO("Arm Calling");
    std::lock_guard<std::mutex> lock(this->arm_mutex_);
    {
      arm_joined_ = true;
      this->arm_thread_ = std::thread(&AmrController::ArmDotuff, this);
    }
  }

  void AmrController::ArmDotuff()
  {
    std::shared_ptr<imr_nova_control> arm_control_ptr;
    arm_control_ptr = std::make_shared<imr_nova_control>();
    arm_control_ptr->enable_ = &this->enable_;
    arm_control_ptr->go_home_flag_ = &this->arm_go_home_;
    arm_control_ptr->continue_flag_ = &this->arm_continue_;
    arm_control_ptr->power_on_flag_ = &this->arm_power_on_;
    OpcUAServerAPI::mode_ptr_ = &arm_control_ptr->mode_;
    this->status_code_ptr_ = reinterpret_cast<unsigned int *>(&arm_control_ptr->statusCode_);

    if (!this->arm_go_home_)
    {
      arm_control_ptr->ok_count_max_ = &this->count_ok_max_;
      arm_control_ptr->ng_count_max_ = &this->count_ng_max_;
      arm_control_ptr->startModeThread();
    }
    else
    {
      arm_control_ptr->startHomeThread();
    }

    arm_control_ptr.reset();
    ROS_INFO("Arm Finished");
    std::lock_guard<std::mutex> lock(this->arm_mutex_);
    this->arm_joined_ = false;
  }

  void AmrController::unLoadCallBack()
  {
    std::lock_guard<std::mutex> lock(this->arm_mutex_);
    {
      ROS_INFO("Shiping call");
      this->belt_joined_ = true;
      this->cancel_ = false;
      this->cur_belt_state_en_ = amr_control::State::WAITING;
      this->belt_thread_ = std::thread(&AmrController::conveyorBeltsShipping, this, std::ref(this->cur_belt_state_en_));
    }
  }

  void AmrController::conveyorBeltsShipping(amr_control::State &state)
  {
    state = amr_control::State::INITIALIZING;
    std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
    plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
    plc_controller_ptr_->connect();

    if (!plc_controller_ptr_->checkConnected())
    {
      state = amr_control::State::FAILED;
      return;
    }

    if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
    {
      int shipping_regsister = 119, receiving_regsister = 116;
      plc_controller_ptr_->setM(shipping_regsister);
      plc_controller_ptr_->resetM(receiving_regsister);
      ros::Duration(0.1).sleep();
      plc_controller_ptr_->resetM(shipping_regsister);
      plc_controller_ptr_->resetM(receiving_regsister);

      ros::Rate r(5);
      while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
      {
        state = amr_control::State::RUNNING;
        bool output_belt[2];
        plc_controller_ptr_->mulGetM(124, 125, output_belt);

        bool enable_shipping = output_belt[0];
        bool enable_receiving = output_belt[1];
        if (!enable_shipping && !enable_receiving)
        {
          state = amr_control::State::FINISHED;
          break;
        }
        r.sleep();
      }

      if (cancel_ || !plc_controller_ptr_->checkConnected())
        state = amr_control::State::FAILED;

      plc_controller_ptr_->close();
    }
    if (plc_controller_ptr_)
      plc_controller_ptr_.reset();

    std::lock_guard<std::mutex> lock(this->arm_mutex_);
    this->belt_joined_ = false;
  }

  void AmrController::loadCallBack()
  {
    std::lock_guard<std::mutex> lock(this->arm_mutex_);
    {
      ROS_INFO("Receiving call");
      this->belt_joined_ = true;
      this->cancel_ = false;
      this->cur_belt_state_en_ = amr_control::State::WAITING;
      this->belt_thread_ = std::thread(&AmrController::conveyorBeltsReceiving, this, std::ref(this->cur_belt_state_en_));
    }
  }

  void AmrController::conveyorBeltsReceiving(amr_control::State &state)
  {
    state = amr_control::State::INITIALIZING;
    std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
    plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
    plc_controller_ptr_->connect();

    if (!plc_controller_ptr_->checkConnected())
    {
      state = amr_control::State::FAILED;
      return;
    }

    if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
    {
      int shipping_regsister = 119, receiving_regsister = 116;
      plc_controller_ptr_->resetM(shipping_regsister);
      plc_controller_ptr_->setM(receiving_regsister);
      ros::Duration(0.1).sleep();
      plc_controller_ptr_->resetM(shipping_regsister);
      plc_controller_ptr_->resetM(receiving_regsister);

      ros::Rate r(5);
      while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
      {
        state = amr_control::State::RUNNING;
        bool output_belt[2];
        plc_controller_ptr_->mulGetM(124, 125, output_belt);

        bool enable_shipping = output_belt[0];
        bool enable_receiving = output_belt[1];
        if (!enable_shipping && !enable_receiving)
        {
          state = amr_control::State::FINISHED;
          break;
        }
        r.sleep();
      }
      if (this->cancel_ || !plc_controller_ptr_->checkConnected())
        state = amr_control::State::FAILED;

      plc_controller_ptr_->close();
    }
    if (plc_controller_ptr_)
      plc_controller_ptr_.reset();

    std::lock_guard<std::mutex> lock(this->arm_mutex_);
    this->belt_joined_ = false;
  }

  void AmrController::controllerDotuff()
  {
    ros::Rate r(10);
    while (ros::ok())
    {
      std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
      plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
      plc_controller_ptr_->connect();
      r.sleep();
      ros::spinOnce();

      if (plc_controller_ptr_ == nullptr)
        continue;
      if (!plc_controller_ptr_->checkConnected())
        continue;

      this->amr_safety_ptr_ = std::make_shared<AmrSafety>();
      this->amr_safety_ptr_->getController(plc_controller_ptr_);

      while (ros::ok() && plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
      {
        if (!this->monitor_ptr_)
          continue;
        nav_2d_msgs::Twist2D velocity;
        if (this->monitor_ptr_->getVelocity(velocity))
        {
          cmd_vel_mtx.lock();
          this->amr_safety_ptr_->safetyHandle(velocity, cmd_vel_max_, cmd_vel_recommended_);
          cmd_vel_mtx.unlock();
        }

        if (move_base_ptr_ != nullptr &&
            move_base_ptr_->getFeedback() != nullptr &&
            move_base_ptr_->getFeedback()->is_ready)
        {
          if (velocity.x <= -0.01)
            this->amr_safety_ptr_->writeMutesSafety(true);
          else
          {
            this->amr_safety_ptr_->writeMutesSafety(this->muted_);
          }
        }

        bool have_goods;
        int have_goods_regsister = 142;
        if (!OpcUAServerAPI::belt_cancel_)
        {
          plc_controller_ptr_->getM(have_goods_regsister, have_goods);
          OpcUAServerAPI::have_goods_ = have_goods;
        }
        else
        {
          plc_controller_ptr_->resetM(have_goods_regsister);
        }

        if (vda_5050_client_api_ptr_)
        {
          amr_control::OperatingMode mode_;
          bool operating_mode[4];
          plc_controller_ptr_->mulGetM(14, 17, operating_mode);
          if (operating_mode[0])
            mode_ = amr_control::OperatingMode::AUTOMATIC;
          else if (operating_mode[2])
            mode_ = amr_control::OperatingMode::SERVICE;
          else if (operating_mode[3])
            mode_ = amr_control::OperatingMode::MANUAL;

          switch (mode_)
          {
          case amr_control::OperatingMode::AUTOMATIC:
            vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC;
            break;
          case amr_control::OperatingMode::MANUAL:
            vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL;
            break;
          case amr_control::OperatingMode::SERVICE:
            vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE;
            break;
          default:
            vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default
            break;
          }
        }
        r.sleep();
        ros::spinOnce();
      }
      if (plc_controller_ptr_)
        plc_controller_ptr_.reset();
      if (amr_safety_ptr_)
        amr_safety_ptr_.reset();
    }
  }

  void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg)
  {
    this->muted_ = msg->data;
  }

  void AmrController::threadHandle()
  {
    ros::Rate r(5);
    while (ros::ok())
    {
      if (this->arm_thread_.joinable())
      {
        std::lock_guard<std::mutex> lock(this->arm_mutex_);
        {
          if (!this->arm_joined_)
          {
            this->arm_thread_.join();
          }
        }
      }

      if (this->belt_thread_.joinable())
      {
        std::lock_guard<std::mutex> lock(this->belt_mutex_);
        {
          if (!this->belt_joined_)
          {
            this->belt_thread_.join();
          }
        }
      }

      if (move_base_ptr_ != nullptr &&
          move_base_ptr_->getFeedback() != nullptr &&
          move_base_ptr_->getFeedback()->is_ready)
      {
        nav_2d_msgs::Twist2D velocity;
        if (this->monitor_ptr_ && this->monitor_ptr_->getVelocity(velocity))
        {
          this->cmd_vel_max_.x = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, std::max(OpcUAServerAPI::cmd_vel_max_.x, vda_5050_client_api_ptr_->cmd_vel_max_.x));
          this->cmd_vel_max_.theta = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, vda_5050_client_api_ptr_->cmd_vel_max_.theta);

          robot_geometry_msgs::Vector3 linear;
          robot_geometry_msgs::Vector3 angular;

          cmd_vel_mtx.lock();
          linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x;
          angular.z = this->cmd_vel_max_.theta;
          cmd_vel_mtx.unlock();

          this->move_base_ptr_->setTwistLinear(linear);
          linear.x *= -1.0;
          this->move_base_ptr_->setTwistLinear(linear);
          this->move_base_ptr_->setTwistAngular(angular);
        }
      }
      r.sleep();
      ros::spinOnce();
    }
  }
}