int main(int argc, char** argv)
{
  // Init the ROS node
  ros::init(argc, argv, "omnirob_robin_lwa_interface");

  ros::NodeHandle n;
  ros::Rate loop_rate(100);	// 100 Hz => consistent to sampleTime
  ros::AsyncSpinner spinner(4); // Use 4 threads

  //MyRobot omniRob;
  controller_manager::ControllerManager cm(&omniRob);
  ros::Duration sampleTime(0.01);
  ros::Publisher trajectoryPoint_pub = n.advertise<std_msgs::Float64MultiArray>("lwa/control/commanded_joint_state", 1000);
  ros::Subscriber jointState_sub = n.subscribe("lwa/state/joint_state", 1000, jointState_callback);

  std_msgs::Float64MultiArray trajPoint;

  spinner.start();

  while (ros::ok())
  {
     //omniRob.read();  // automated subscription
     cm.update(ros::Time::now(), sampleTime);
     omniRob.write(trajectoryPoint_pub, trajPoint);
     ROS_INFO("omnirob LWA3 OK...");

     //ros::spinOnce();
     //ros::waitForShutdown();
     loop_rate.sleep();
     //spinner.stop();
  }
  return 0;
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, APPLICATION_NAME);

    // Override the default ros sigint handler.
    // This must be set after the first NodeHandle is created.
    signal(SIGINT, mySigintHandler);

    ros::NodeHandle nh;
//     ros::CallbackQueue queue;
//     nh.setCallbackQueue(&queue);

    // Allow the action server to recieve and send ros messages
    ros::AsyncSpinner spinner(4);
    spinner.start();

    // To create publisher and subscriber... do we really need this node here??
//     ros::NodeHandle nodeHandle;

    std::cout << "Before MyRobot"<< std::endl;

    MyRobot robot;
    if(!robot.init() )
    {
        std::cout << "Error initializing robot model, quitting" << std::endl;
        return -1;
    }

    controller_manager::ControllerManager cm(&robot, nh);

    ros::Duration elapsed_time_(1,0);
    while (ros::ok())
    {

        std::cout << "Updating stuff" << std::endl;
        robot.read();
        cm.update(ros::Time::now(), elapsed_time_);
        robot.write();
        sleep(1);
        ros::spinOnce();
    }
    return 0;
}