int main( int argc, char** argv ) { // initialize ROS ros::init(argc, argv, "iiwa_hw", ros::init_options::NoSigintHandler); // ros spinner ros::AsyncSpinner spinner(1); spinner.start(); // custom signal handlers signal(SIGTERM, quitRequested); signal(SIGINT, quitRequested); signal(SIGHUP, quitRequested); // construct the lbr iiwa ros::NodeHandle iiwa_nh; IIWA_HW iiwa_robot(iiwa_nh); // configuration routines iiwa_robot.start(); ros::Time last(ros::Time::now()); ros::Time now; ros::Duration period(1.0); //the controller manager controller_manager::ControllerManager manager(&iiwa_robot, iiwa_nh); // run as fast as possible while( !g_quit ) { // get the time / period now = ros::Time::now(); period = now - last; last = now; // read current robot position iiwa_robot.read(period); // update the controllers manager.update(now, period); // send command position to the robot iiwa_robot.write(period); // wait for some milliseconds defined in controlFrequency iiwa_robot.getRate()->sleep(); } std::cerr << "Stopping spinner..." << std::endl; spinner.stop(); std::cerr << "Bye!" << std::endl; return 0; }
int main( int argc, char** argv ) { // initialize ROS ros::init(argc, argv, "iiwa_hw", ros::init_options::NoSigintHandler); // ros spinner ros::AsyncSpinner spinner(1); spinner.start(); // custom signal handlers signal(SIGTERM, quitRequested); signal(SIGINT, quitRequested); signal(SIGHUP, quitRequested); // construct the lbr iiwa ros::NodeHandle iiwa_nh; IIWA_HW iiwa_robot(iiwa_nh); // configuration routines iiwa_robot.start(); // timer variables struct timespec ts = {0, 0}; ros::Time last(ts.tv_sec, ts.tv_nsec), now(ts.tv_sec, ts.tv_nsec); ros::Duration period(1.0); //the controller manager controller_manager::ControllerManager manager(&iiwa_robot, iiwa_nh); // run as fast as possible while( !g_quit ) { // get the time / period if (!clock_gettime(CLOCK_REALTIME, &ts)) { now.sec = ts.tv_sec; now.nsec = ts.tv_nsec; period = now - last; last = now; } else { ROS_FATAL("Failed to poll realtime clock!"); break; } // read current robot position iiwa_robot.read(period); // update the controllers manager.update(now, period); // send command position to the robot iiwa_robot.write(period); // wait for some milliseconds defined in controlFrequency iiwa_robot.getRate()->sleep(); } std::cerr << "Stopping spinner..." << std::endl; spinner.stop(); std::cerr << "Bye!" << std::endl; return 0; }