#include "ros_kinematics/ros_diff_drive.h"
#include <std_msgs/UInt16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>


ros_kinematics::RosDiffDrive::RosDiffDrive(ros::NodeHandle& nh, const std::string& name)
  : nh_(nh),
  name_(name),
  motor_loader_("models", "models::BaseSteerDrive"),
  encoder_loader_("models", "models::BaseAbsoluteEncoder"),
  last_odom_state_publish_time_(ros::Time(0)),
  left_wheel_curr_pos_(0),
  right_wheel_curr_pos_(0),
  publish_time_(ros::Time(0)),
  initialized_(false)
{
  if(!initialized_)
  {
    nh_priv_ = ros::NodeHandle(nh_, name_);
    ROS_INFO("RosDiffDrive get node name is %s", name_.c_str());

    bool init_odom_enc;
    nh_priv_.param("initialize_odom_enc", init_odom_enc, true);
    odom_enc_initialized_ = !init_odom_enc;

    nh_priv_.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_);
    ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by " << wheel_separation_multiplier_ << ".");

    if (nh_priv_.hasParam("wheel_radius_multiplier"))
    {
      double wheel_radius_multiplier;
      nh_priv_.getParam("wheel_radius_multiplier", wheel_radius_multiplier);

      left_wheel_radius_multiplier_ = wheel_radius_multiplier;
      right_wheel_radius_multiplier_ = wheel_radius_multiplier;
    }
    else
    {
      nh_priv_.param("left_wheel_radius_multiplier", left_wheel_radius_multiplier_, left_wheel_radius_multiplier_);
      nh_priv_.param("right_wheel_radius_multiplier", right_wheel_radius_multiplier_, right_wheel_radius_multiplier_);
    }

    ROS_INFO_STREAM_NAMED(name_, "Left wheel radius will be multiplied by " << left_wheel_radius_multiplier_ << ".");
    ROS_INFO_STREAM_NAMED(name_, "Right wheel radius will be multiplied by " << right_wheel_radius_multiplier_ << ".");

    int velocity_rolling_window_size = 10;
    nh_priv_.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
    ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of " << velocity_rolling_window_size << ".");

    odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);

    // Twist command related:
    nh_priv_.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
    ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than " << cmd_vel_timeout_ << "s.");

    nh_priv_.param("allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_, allow_multiple_cmd_vel_publishers_);
    ROS_INFO_STREAM_NAMED(name_, "Allow mutiple cmd_vel publishers is " << (allow_multiple_cmd_vel_publishers_ ? "enabled" : "disabled"));

    nh_priv_.param("base_frame_id", base_frame_id_, base_frame_id_);
    ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);

    nh_priv_.param("odom_frame_id", odom_frame_id_, odom_frame_id_);
    ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_);

    nh_priv_.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
    ROS_INFO_STREAM_NAMED(name_, "Publishing base frame to tf is " << (enable_odom_tf_ ? "enabled" : "disabled"));

    nh_priv_.param("enable_wheel_tf", enable_wheel_tf_, enable_wheel_tf_);
    ROS_INFO_STREAM_NAMED(name_, "Publishing wheel frame to tf is " << (enable_wheel_tf_ ? "enabled" : "disabled"));

    nh_priv_.param("commandTopic", command_topic_, std::string("cmd_vel"));
    // nh_priv_.param("publishOdomTF", publishOdomTF_, false);
    nh_priv_.param("odometryTopic", odometry_topic_, std::string("odom"));
    // nh_priv_.param("odometryFrame", odometry_frame_, std::string("odom"));
    nh_priv_.param("odometryEncTopic", odometry_enc_topic_, std::string("odom_enc"));
    nh_priv_.param("odometryEncChildFrame", odometry_enc_child_frame_, std::string("odom_enc"));
    // nh_priv_.param("robotBaseFrame", robot_base_frame_, std::string("base_link"));
    nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1);
    nh_priv_.param("publish_rate", update_rate_, 100.0);
    publish_period_ = ros::Duration(1.0 / update_rate_);
    nh_priv_.param("open_loop", open_loop_, false);

    // Velocity and acceleration limits:
    nh_priv_.param("linear/x/has_velocity_limits", limiter_lin_.has_velocity_limits, limiter_lin_.has_velocity_limits);
    nh_priv_.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
    nh_priv_.param("linear/x/has_jerk_limits", limiter_lin_.has_jerk_limits, limiter_lin_.has_jerk_limits);
    nh_priv_.param("linear/x/max_velocity", limiter_lin_.max_velocity, limiter_lin_.max_velocity);
    nh_priv_.param("linear/x/min_velocity", limiter_lin_.min_velocity, -limiter_lin_.max_velocity);
    nh_priv_.param("linear/x/max_acceleration", limiter_lin_.max_acceleration, limiter_lin_.max_acceleration);
    nh_priv_.param("linear/x/min_acceleration", limiter_lin_.min_acceleration, -limiter_lin_.max_acceleration);
    nh_priv_.param("linear/x/max_jerk", limiter_lin_.max_jerk, limiter_lin_.max_jerk);
    nh_priv_.param("linear/x/min_jerk", limiter_lin_.min_jerk, -limiter_lin_.max_jerk);

    nh_priv_.param("angular/z/has_velocity_limits", limiter_ang_.has_velocity_limits, limiter_ang_.has_velocity_limits);
    nh_priv_.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
    nh_priv_.param("angular/z/has_jerk_limits", limiter_ang_.has_jerk_limits, limiter_ang_.has_jerk_limits);
    nh_priv_.param("angular/z/max_velocity", limiter_ang_.max_velocity, limiter_ang_.max_velocity);
    nh_priv_.param("angular/z/min_velocity", limiter_ang_.min_velocity, -limiter_ang_.max_velocity);
    nh_priv_.param("angular/z/max_acceleration", limiter_ang_.max_acceleration, limiter_ang_.max_acceleration);
    nh_priv_.param("angular/z/min_acceleration", limiter_ang_.min_acceleration, -limiter_ang_.max_acceleration);
    nh_priv_.param("angular/z/max_jerk", limiter_ang_.max_jerk, limiter_ang_.max_jerk);
    nh_priv_.param("angular/z/min_jerk", limiter_ang_.min_jerk, -limiter_ang_.max_jerk);
    // Publish limited velocity:
    nh_priv_.param("publish_cmd", publish_cmd_, publish_cmd_);
    // Use encoder
    nh_.param(name_ + "/use_encoder", use_abs_encoder, false);
    // If either parameter is not available, we need to look up the value in the URDF
    bool lookup_wheel_separation = !nh_priv_.getParam("wheel_separation", wheel_separation_);
    bool lookup_wheel_radius = !nh_priv_.getParam("wheel_radius", wheel_radius_);

    if(lookup_wheel_separation || lookup_wheel_radius) 
      ROS_INFO("URDF if not specified as a parameter. We don't set the value here \n wheel_separation: %f,  wheel_radius %f",
              wheel_separation_, wheel_radius_);
    
    // Regardless of how we got the separation and radius, use them
    // to set the odometry parameters
    const double ws = wheel_separation_multiplier_ * wheel_separation_;
    const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
    const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;

    odometry_.setWheelParams(ws, lwr, rwr);
    ROS_INFO_STREAM_NAMED(name_,
      "Odometry params : wheel separation " << ws
      << ", left wheel radius " << lwr
      << ", right wheel radius " << rwr);

    if (publish_cmd_)
    {
      cmd_vel_pub_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped>(nh_priv_, "cmd_vel_out", 100));
    }

    // Get joint names from the parameter server
    std::vector<std::string> left_wheel_names, right_wheel_names;
    if (!getWheelNames(nh_priv_, "left_wheel", left_wheel_names) ||
      !getWheelNames(nh_priv_, "right_wheel", right_wheel_names))
    {
      exit(1);
    }

    if (!getWheelParams(nh_priv_, left_wheel_names, left_drive_param_map_) ||
        !getWheelParams(nh_priv_, right_wheel_names, right_drive_param_map_))
    {
      exit(1);
    }

    if (left_wheel_names.size() != right_wheel_names.size()     || 
        left_drive_param_map_.size() != left_wheel_names.size() || 
        right_drive_param_map_.size() != right_wheel_names.size()
       )
    {
      ROS_ERROR_STREAM_NAMED(name_,
        "#left wheels (" << left_wheel_names.size() << ") != " <<
        "#right wheels (" << right_wheel_names.size() << ").");
      exit(1);
    }
    else
    {
      wheel_joints_size_ = left_wheel_names.size();

      left_drive_.resize(wheel_joints_size_);
      right_drive_.resize(wheel_joints_size_);
      if (use_abs_encoder)
      {
        left_encoder_.resize(wheel_joints_size_);
        right_encoder_.resize(wheel_joints_size_);
      }
    }

    // plugin models
    ROS_INFO("RosDiffDrive is plugining models...");
    for (size_t i = 0; i < wheel_joints_size_; ++i)
    {
      std::string left_drive;
      nh_priv_.param(left_wheel_names[i] + "/lookup_name", left_drive, std::string("motor_wheel::LeftMotor"));
      try
      {
        left_drive_[i] = motor_loader_.createInstance(left_drive);
        // left_drive_[i]->initialize(nh_, name_ + "/" + motor_loader_.getName(left_drive));
        left_drive_[i]->initialize(nh_, name_ + "/" + left_wheel_names[i]);
        if (use_abs_encoder)
        {
          std::string right_encoder;
          nh_priv_.param(right_wheel_names[i] + "/encoder/lookup_name", right_encoder, std::string("absolute_encoder::Measurements"));
          try
          {
            right_encoder_[i] = encoder_loader_.createInstance(right_encoder);
            right_encoder_[i]->initialize(nh_, name_ + "/" + right_wheel_names[i] + "/encoder");
          }
          catch (const pluginlib::PluginlibException& ex)
          {
            ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
              right_encoder.c_str(), ex.what());
            exit(1);
          }
        }

      }
      catch (const pluginlib::PluginlibException& ex)
      {
        ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
          left_drive.c_str(), ex.what());
        exit(1);
      }


      std::string right_drive;
      nh_priv_.param(right_wheel_names[i] + "/lookup_name", right_drive, std::string("motor_wheel::RightMotor"));
      try
      {
        right_drive_[i] = motor_loader_.createInstance(right_drive);
        // right_drive_[i]->initialize(nh_, name_ + "/" + motor_loader_.getName(right_drive));
        right_drive_[i]->initialize(nh_, name_ + "/" + right_wheel_names[i]);

        if (use_abs_encoder)
        {
          std::string left_encoder;
          nh_priv_.param(left_wheel_names[i] + "/encoder/lookup_name", left_encoder, std::string("absolute_encoder::Measurements"));
          try
          {
            left_encoder_[i] = encoder_loader_.createInstance(left_encoder);
            left_encoder_[i]->initialize(nh_, name_ + "/" + left_wheel_names[i] + "/encoder");
          }
          catch (const pluginlib::PluginlibException& ex)
          {
            ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
              left_encoder.c_str(), ex.what());
            exit(1);
          }
        }
      }
      catch (const pluginlib::PluginlibException& ex)
      {
        ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
          right_drive.c_str(), ex.what());
        exit(1);
      }
    }

    if(enable_wheel_tf_) 
    {
      left_wheel_tf_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(nh_, "/tf", 100));
      for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator itr = left_drive_param_map_.begin(); itr != left_drive_param_map_.end(); ++itr)
      {
        geometry_msgs::TransformStamped tfs;
        tfs.child_frame_id = static_cast<std::string>(itr->second["frame_id"]);
        tfs.header.frame_id = "base_link";
        tfs.transform.translation.x = static_cast<double>(itr->second["origin"][0]);
        tfs.transform.translation.y = static_cast<double>(itr->second["origin"][1]);
        tfs.transform.translation.z = static_cast<double>(itr->second["origin"][2]);
        double roll = static_cast<double>(itr->second["origin"][3]);
        double pitch = static_cast<double>(itr->second["origin"][4]);
        double yaw = static_cast<double>(itr->second["origin"][5]);
        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
        tfs.transform.rotation = q;
        left_wheel_tf_pub_->msg_.transforms.push_back(tfs);
      }

      right_wheel_tf_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(nh_, "/tf", 100));
      for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator itr = right_drive_param_map_.begin(); itr != right_drive_param_map_.end(); ++itr)
      {
        geometry_msgs::TransformStamped tfs;
        tfs.child_frame_id = static_cast<std::string>(itr->second["frame_id"]);
        tfs.header.frame_id = "base_link";
        tfs.transform.translation.x = static_cast<double>(itr->second["origin"][0]);
        tfs.transform.translation.y = static_cast<double>(itr->second["origin"][1]);
        tfs.transform.translation.z = static_cast<double>(itr->second["origin"][2]);
        double roll = static_cast<double>(itr->second["origin"][3]);
        double pitch = static_cast<double>(itr->second["origin"][4]);
        double yaw = static_cast<double>(itr->second["origin"][5]);
        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
        tfs.transform.rotation = q;
        right_wheel_tf_pub_->msg_.transforms.push_back(tfs);
      }
    }

    schedule_delay_ = ros::Duration(1/update_rate_);
    schedule_lastest_delay_ = ros::Duration(0.5);

    this->setOdomPubFields(nh_, nh_priv_);
    transform_broadcaster_  = boost::shared_ptr<tf2_ros::TransformBroadcaster>(new tf2_ros::TransformBroadcaster());
    cmd_vel_subscriber_     = nh_.subscribe(command_topic_, m_subscribe_queue_size_, &ros_kinematics::RosDiffDrive::CmdVelCallback, this);
    
    // Kinematics
    kinematics_param_ptr_ = boost::make_shared<ros_kinematics::KinematicDiffDriveParameters>();
    kinematics_param_ptr_->initialize(nh_);

    // Thread
    callback_thread_ =  boost::make_shared<boost::thread>(&ros_kinematics::RosDiffDrive::updateChild, this);
    
    ROS_INFO_NAMED("RosDiffDrive","Initializing on %s is successed", name_.c_str());
    initialized_ = true;
  }
}

ros_kinematics::RosDiffDrive::~RosDiffDrive()
{
  callback_thread_ = nullptr;
  kinematics_param_ptr_ = nullptr;
}

void ros_kinematics::RosDiffDrive::CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg)
{
  // check that we don't have multiple publishers on the command topic
  if (!allow_multiple_cmd_vel_publishers_ && cmd_vel_subscriber_.getNumPublishers() > 1)
  {
    ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << cmd_vel_subscriber_.getNumPublishers()
      << " publishers " << command_topic_ << ". Only 1 publisher is allowed. Going to brake.");
    brake();
    return;
  }

  if (!std::isfinite(cmd_msg->angular.z) || !std::isfinite(cmd_msg->linear.x))
  {
    ROS_WARN_THROTTLE(1.0, "Received NaN in velocity command. Ignoring.");
    return;
  }

  command_struct_.angular.z = cmd_msg->angular.z;
  command_struct_.linear.x = cmd_msg->linear.x;
  command_struct_.stamp = ros::Time::now();
  command_.writeFromNonRT(command_struct_);
  this->scheduleMotorController(true);
  ROS_DEBUG_STREAM_NAMED(name_,
    "Added values to command. "
    << "Ang: " << command_struct_.angular.z << ", "
    << "Lin: " << command_struct_.linear.x << ", "
    << "Stamp: " << command_struct_.stamp);
}

bool ros_kinematics::RosDiffDrive::isCmdVelTriggered(void)
{
  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
  return !publish_time_.isZero() && ros::Time::now() > publish_time_;
}


bool ros_kinematics::RosDiffDrive::isCmdVelLastestTriggered(void)
{
  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
  return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_;
}


void ros_kinematics::RosDiffDrive::scheduleMotorController(bool schedule)
{
  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
  if(schedule && publish_time_.isZero())
  {
    publish_time_ = ros::Time::now() + schedule_delay_;
    publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_;
  }
  if(!schedule && !publish_time_.isZero())
  {
    publish_time_ = ros::Time(0);
    publish_lastest_time_ = ros::Time(0);
  }
}

void ros_kinematics::RosDiffDrive::updateOdometryEncoder(const ros::Time& current_time)
{
  // ros::Time current_time = ros::Time::now();
  double step_time = ros::Duration(current_time - last_odom_update_).toSec();
  last_odom_update_ = current_time;

  // Apply (possibly new) multipliers:
  const double ws = wheel_separation_multiplier_ * wheel_separation_;
  const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
  const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
  odometry_.setWheelParams(ws, lwr, rwr);
  // COMPUTE AND PUBLISH ODOMETRY
  if (open_loop_)
  {
    odometry_.updateOpenLoop(last0_cmd_.linear.x, last0_cmd_.angular.z, current_time);
  }
  else
  {
    double left_pos = 0.0;
    double right_pos = 0.0;
    for (size_t i = 0; i < wheel_joints_size_; ++i)
    {
      double left_speed, right_speed;
      if (use_abs_encoder)
      {
        left_speed = left_encoder_[i]->Velocity();
        right_speed = right_encoder_[i]->Velocity();
      }
      else
      {
        left_speed = left_drive_[i]->GetVelocity();
        right_speed = right_drive_[i]->GetVelocity();
      }

      const double lp = left_speed  * step_time;  // rad/s -> rad
      const double rp = right_speed * step_time;
  
      if (std::isnan(lp) || std::isnan(rp))
        return;

      left_pos += lp;
      right_pos += rp;
    }
    left_pos /= wheel_joints_size_;
    right_pos /= wheel_joints_size_;
    left_wheel_curr_pos_ += left_pos;
    right_wheel_curr_pos_ += right_pos;
    // Estimate linear and angular velocity using joint information
    odometry_.update(left_wheel_curr_pos_, right_wheel_curr_pos_, current_time);

    if(enable_wheel_tf_)
    {
      if(left_wheel_tf_pub_->trylock())
      {
        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(0, left_wheel_curr_pos_, 0);
        for(auto &msg_tf : left_wheel_tf_pub_->msg_.transforms)
        {
          msg_tf.header.stamp = current_time;
          msg_tf.transform.rotation = q;
        }
        left_wheel_tf_pub_->unlockAndPublish();
      }

      if(right_wheel_tf_pub_->trylock())
      {
        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(0, right_wheel_curr_pos_, 0);
        for(auto &msg_tf : right_wheel_tf_pub_->msg_.transforms)
        {
          msg_tf.header.stamp = current_time;
          msg_tf.transform.rotation = q;
        }
        right_wheel_tf_pub_->unlockAndPublish();
      }
    }

  }
}

void ros_kinematics::RosDiffDrive::publishOdometry(const ros::Time& current_time)
{
  // Publish odometry message
  if (last_odom_state_publish_time_ + publish_period_ < current_time)
  {
    last_odom_state_publish_time_ += publish_period_;
    // Compute and store orientation info
    const geometry_msgs::Quaternion orientation(
      tf::createQuaternionMsgFromYaw(odometry_.getHeading()));

    // Populate odom message and publish
    if (odom_pub_ && odom_pub_->trylock())
    {
      odom_pub_->msg_.header.stamp = current_time;
      odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
      odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
      odom_pub_->msg_.pose.pose.orientation = orientation;
      odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear();
      odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
      odom_pub_->unlockAndPublish();
    }

    // Publish tf /odom frame
    if (enable_odom_tf_ && tf_odom_pub_->trylock())
    {
      geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
      odom_frame.header.stamp = current_time;
      odom_frame.transform.translation.x = odometry_.getX();
      odom_frame.transform.translation.y = odometry_.getY();
      odom_frame.transform.rotation = orientation;
      tf_odom_pub_->unlockAndPublish();
    }
  }
}

void ros_kinematics::RosDiffDrive::updateChild(void)
{
  ros::Rate rate(update_rate_);
  last_odom_state_publish_time_ = ros::Time::now();
  last_odom_update_ = ros::Time::now();
  odometry_.init(ros::Time::now());

  while (ros::ok())
  {
    ros::Time current_time = ros::Time::now();
    // update parameter from dynamic reconf
    this->updateDynamicParams();

    const double ws  = wheel_separation_multiplier_ * wheel_separation_;
    const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
    const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
    
    if (this->allReady())
    {
      this->updateOdometryEncoder(current_time);
      this->publishOdometry(current_time);
    }

    // MOVE ROBOT
    DriveCmd curr_cmd = *(command_.readFromRT());
    const double dt = (current_time - curr_cmd.stamp).toSec();

    // Brake if cmd_vel has timeout:
    if (dt > cmd_vel_timeout_)
    {
      curr_cmd.linear.x = 0.0;
      curr_cmd.angular.z = 0.0;
    }
    
    // Limit velocities and accelerations:
    const double cmd_dt(dt);

    limiter_lin_.limit(curr_cmd.linear.x, last0_cmd_.linear.x, last1_cmd_.linear.x, cmd_dt);
    limiter_ang_.limit(curr_cmd.angular.z, last0_cmd_.angular.z, last1_cmd_.angular.z, cmd_dt);

    last1_cmd_ = last0_cmd_;
    last0_cmd_ = curr_cmd;

    // Compute wheels velocities:
    const double vel_left = (curr_cmd.linear.x - curr_cmd.angular.z * ws / 2.0) / lwr;
    const double vel_right = (curr_cmd.linear.x + curr_cmd.angular.z * ws / 2.0) / rwr;
    
    if(this->isCmdVelTriggered())
    {
      // Publish limited velocity:
      if (publish_cmd_ && cmd_vel_pub_ && cmd_vel_pub_->trylock())
      {
        cmd_vel_pub_->msg_.header.stamp = current_time;
        cmd_vel_pub_->msg_.twist.linear.x = curr_cmd.linear.x;
        cmd_vel_pub_->msg_.twist.angular.z = curr_cmd.angular.z;
        cmd_vel_pub_->unlockAndPublish();
      }

      for (size_t i = 0; i < wheel_joints_size_; ++i)
      {
        left_drive_[i]->SetVelocity(vel_left);
        right_drive_[i]->SetVelocity(vel_right);
      }
      scheduleMotorController(false);
    }
    
    if(this->isCmdVelLastestTriggered())
    {
      int counter = 3;
      do
      {
        for (size_t i = 0; i < wheel_joints_size_; ++i)
        {
          left_drive_[i]->SetVelocity(0.0);
          right_drive_[i]->SetVelocity(0.0);
        } 
        counter--;
        rate.sleep();
        ros::spinOnce();
      }while(ros::ok() && counter > 0);
      scheduleMotorController(false);
    }

    rate.sleep();
    ros::spinOnce();
  }

}

void ros_kinematics::RosDiffDrive::brake()
{
  const double vel = 0.0;
  for (size_t i = 0; i < wheel_joints_size_; ++i)
  {
    left_drive_[i]->SetVelocity(vel);
    right_drive_[i]->SetVelocity(vel);
  }
}

void ros_kinematics::RosDiffDrive::updateDynamicParams()
{
  // Retreive dynamic params:
  if (kinematics_param_ptr_ != nullptr)
  {
    left_wheel_radius_multiplier_ = kinematics_param_ptr_->getLeftWheelRadiusMultiplier();
    right_wheel_radius_multiplier_ = kinematics_param_ptr_->getRightWheelRadiusMultiplier();
    wheel_separation_multiplier_ = kinematics_param_ptr_->getWheelSeparationMultiplier();
    publish_period_ = ros::Duration(1.0 / kinematics_param_ptr_->getPublishRate());
    // enable_odom_tf_ = kinematics_param_ptr_->getEnableOdomTf();
  }
}

void ros_kinematics::RosDiffDrive::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
{
  // Get and check params for covariances
  XmlRpc::XmlRpcValue pose_cov_list;
  controller_nh.getParam("pose_covariance_diagonal", pose_cov_list);
  ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
  ROS_ASSERT(pose_cov_list.size() == 6);
  for (int i = 0; i < pose_cov_list.size(); ++i)
    ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);

  XmlRpc::XmlRpcValue twist_cov_list;
  controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
  ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
  ROS_ASSERT(twist_cov_list.size() == 6);
  for (int i = 0; i < twist_cov_list.size(); ++i)
    ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);

  // Setup odometry realtime publisher + odom message constant fields
  odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
  odom_pub_->msg_.header.frame_id = odom_frame_id_;
  odom_pub_->msg_.child_frame_id = base_frame_id_;
  odom_pub_->msg_.pose.pose.position.z = 0;
  odom_pub_->msg_.pose.covariance = {
      static_cast<double>(pose_cov_list[0]), 0., 0., 0., 0., 0.,
      0., static_cast<double>(pose_cov_list[1]), 0., 0., 0., 0.,
      0., 0., static_cast<double>(pose_cov_list[2]), 0., 0., 0.,
      0., 0., 0., static_cast<double>(pose_cov_list[3]), 0., 0.,
      0., 0., 0., 0., static_cast<double>(pose_cov_list[4]), 0.,
      0., 0., 0., 0., 0., static_cast<double>(pose_cov_list[5]) };
  odom_pub_->msg_.twist.twist.linear.y  = 0;
  odom_pub_->msg_.twist.twist.linear.z  = 0;
  odom_pub_->msg_.twist.twist.angular.x = 0;
  odom_pub_->msg_.twist.twist.angular.y = 0;
  odom_pub_->msg_.twist.covariance = {
      static_cast<double>(twist_cov_list[0]), 0., 0., 0., 0., 0.,
      0., static_cast<double>(twist_cov_list[1]), 0., 0., 0., 0.,
      0., 0., static_cast<double>(twist_cov_list[2]), 0., 0., 0.,
      0., 0., 0., static_cast<double>(twist_cov_list[3]), 0., 0.,
      0., 0., 0., 0., static_cast<double>(twist_cov_list[4]), 0.,
      0., 0., 0., 0., 0., static_cast<double>(twist_cov_list[5]) };
  tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
  tf_odom_pub_->msg_.transforms.resize(1);
  tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
  tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
  tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
}


bool ros_kinematics::RosDiffDrive::getWheelNames(ros::NodeHandle& _nh,
  const std::string& wheel_param,
  std::vector<std::string>& wheel_names)
{
  XmlRpc::XmlRpcValue wheel_list;
  if (!nh_priv_.getParam(wheel_param, wheel_list))
  {
    ROS_ERROR_STREAM_NAMED(name_,
      "Couldn't retrieve wheel param '" << wheel_param << "'.");
    return false;
  }

  if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
  {
    if (wheel_list.size() == 0)
    {
      ROS_ERROR_STREAM_NAMED(name_,
        "Wheel param '" << wheel_param << "' is an empty list");
      return false;
    }

    for (int i = 0; i < wheel_list.size(); ++i)
    {
      if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
      {
        ROS_ERROR_STREAM_NAMED(name_,
          "Wheel param '" << wheel_param << "' #" << i <<
          " isn't a string.");
        return false;
      }
    }

    wheel_names.resize(wheel_list.size());
    for (int i = 0; i < wheel_list.size(); ++i)
    {
      wheel_names[i] = static_cast<std::string>(wheel_list[i]);
    }
  }
  else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
  {
    wheel_names.push_back(wheel_list);
  }
  else
  {
    ROS_ERROR_STREAM_NAMED(name_,
      "Wheel param '" << wheel_param <<
      "' is neither a list of strings nor a string.");
    return false;
  }

  return true;
}


bool ros_kinematics::RosDiffDrive::getWheelParams(ros::NodeHandle& controller_nh, 
  const std::vector<std::string>& wheel_names,
  std::map<std::string, XmlRpc::XmlRpcValue>& drive_param_map)
{
  for (int i = 0; i < wheel_names.size(); ++i)
  {
    XmlRpc::XmlRpcValue element_param;
    if (!nh_priv_.getParam(wheel_names[i], element_param))
    {
      ROS_ERROR_STREAM_NAMED(name_,
        "Couldn't retrieve wheel param '" << wheel_names[i] << "'.");
      return false;
    }
    // check type of member
    if(element_param.hasMember("lookup_name"))
    {
      if(element_param["lookup_name"].getType() != XmlRpc::XmlRpcValue::TypeString)
      {
        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->lookup_name" << " isn't a string.");
        return false;
      }
    }

    if(element_param.hasMember("mesurement_topic"))
    {
      if(element_param["mesurement_topic"].getType() != XmlRpc::XmlRpcValue::TypeString)
      {
        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->mesurement_topic" << " isn't a string.");
        return false;
      }
    }

    if(element_param.hasMember("frame_id"))
    {
      if(element_param["frame_id"].getType() != XmlRpc::XmlRpcValue::TypeString)
      {
        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->frame_id" << " isn't a string.");
        return false;
      }
    }

    if(element_param.hasMember("wheel_topic"))
    {
      if(element_param["wheel_topic"].getType() != XmlRpc::XmlRpcValue::TypeString)
      {
        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->wheel_topic" << " isn't a string.");
        return false;
      }
    }

    if(element_param.hasMember("max_publish_rate"))
    {
      if( element_param["max_publish_rate"].getType() != XmlRpc::XmlRpcValue::TypeDouble && 
          element_param["max_publish_rate"].getType() != XmlRpc::XmlRpcValue::TypeInt 
        )
      {
        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->max_publish_rate" << " isn't a double/int.");
        return false;
      }
    }

    if(element_param.hasMember("subscribe_queue_size"))
    {
      if( element_param["subscribe_queue_size"].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
          element_param["subscribe_queue_size"].getType() != XmlRpc::XmlRpcValue::TypeInt
        )
      {
        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->subscribe_queue_size" << " isn't a double/int.");
        return false;
      }
    }

    if(element_param.hasMember("command_timeout"))
    {
      if( element_param["command_timeout"].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
          element_param["command_timeout"].getType() != XmlRpc::XmlRpcValue::TypeInt
        )
      {
        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->command_timeout" << " isn't a double/int.");
        return false;
      }
    }

    if(!element_param.hasMember("origin"))
    {
      ROS_ERROR_STREAM_NAMED(name_,
        "Couldn't retrieve wheel param '" << wheel_names[i] << "'->origin.");
      return false;
    }
    else
    {
      if(element_param["origin"].getType() != XmlRpc::XmlRpcValue::TypeArray)
      {
        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->origin" << " isn't a arrays.");
        return false;
      }
      else
      {
        if(element_param["origin"].size() < 6) // x y z roll pich yaw
        {
          ROS_ERROR_STREAM_NAMED(name_,
          "Wheel param '" << wheel_names[i] << "'->origin is an error array");
          return false;
        }
        else
        {
          for (int j = 0; j < element_param["origin"].size(); ++j)
          {
            if( element_param["origin"][j].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
                element_param["origin"][j].getType() != XmlRpc::XmlRpcValue::TypeInt
              )
            {
              ROS_ERROR_STREAM_NAMED(name_,
                "Wheel param '" << wheel_names[i] << "'->origin #" << j <<
                " isn't a double/int.");
              return false;
            }
          }
        }
      }
    }
    // Done
    drive_param_map[wheel_names[i]] = element_param;
  }
  return true;
}


bool ros_kinematics::RosDiffDrive::allReady()
{
  bool result = true;
  for (size_t i = 0; i < wheel_joints_size_; ++i)
  {
    result = result && left_drive_[i]->Ready() && right_drive_[i]->Ready();
  }
  return result;
}

