#include <ros/ros.h>
#include <boost/shared_ptr.hpp>
#include "ros_kinematics/base_drive.h"
#include "ros_kinematics/ros_steer_drive.h"
#include "ros_kinematics/ros_diff_drive.h"

int main(int argc, char **argv)
{
  /* Khoi tao Node */
  ros::init(argc, argv, "ros_kinematics");
  ros::NodeHandle nh;
  int type;
  nh.param(ros::this_node::getName()+"/type", type, 1);
  /* Constructors */
  boost::shared_ptr<ros_kinematics::BaseDrive> ros_drive = nullptr;
  switch (type)
  {
    case 1:
      ros_drive = boost::make_shared<ros_kinematics::RosSteerDrive>(nh, ros::this_node::getName());
      break;

    case 2:
      ros_drive = boost::make_shared<ros_kinematics::RosDiffDrive>(nh, ros::this_node::getName());
      break;

    default:
      ros_drive = boost::make_shared<ros_kinematics::RosSteerDrive>(nh, ros::this_node::getName());
      break;
  }
  ros::spin();
  ros_drive = nullptr;
  return 0;
}
