int main(int argc, char ** argv) { struct sigaction sa; bzero(&sa, sizeof(sa)); sa.sa_handler = handle; if (0 != sigaction(SIGINT, &sa, 0)) { err(EXIT_FAILURE, "sigaction"); } keep_running = true; parse_options(argc, argv); Servo servo; try { actual_servo_rate = servo_rate; servo.start(servo_rate); } catch (std::runtime_error const & ee) { errx(EXIT_FAILURE, "failed to start servo: %s", ee.what()); } warnx("started servo RT thread"); while (keep_running) { usleep(300000); cout << "**************************************************\n"; pretty_print(position, cout, "position", " "); pretty_print(desired_torque, cout, "desired_torque", " "); pretty_print(actual_torque, cout, "actual_torque", " "); } warnx("shutting down"); servo.shutdown(); }
int main(int argc, char ** argv) { struct sigaction sa; bzero(&sa, sizeof(sa)); sa.sa_handler = handle; if (0 != sigaction(SIGINT, &sa, 0)) { err(EXIT_FAILURE, "sigaction"); } ros::init(argc, argv, "m3_base_ctrl_servo", ros::init_options::NoSigintHandler); ros::NodeHandle node("~"); Servo servo; try { if (verbose) { warnx("starting servo with %lld Hz", servo_rate); } actual_servo_rate = servo_rate; servo.start(servo_rate); } catch (std::runtime_error const & ee) { errx(EXIT_FAILURE, "failed to start servo: %s", ee.what()); } warnx("started servo RT thread"); ros::Time dbg_t0(ros::Time::now()); ros::Time dump_t0(ros::Time::now()); ros::Duration dbg_dt(0.1); ros::Duration dump_dt(0.05); while (ros::ok()) { ros::Time t1(ros::Time::now()); if (verbose) { if (t1 - dbg_t0 > dbg_dt) { dbg_t0 = t1; cout << "--------------------------------------------------\n"; jspace::pretty_print(base_controller->getState().position_, cout, "jpos", " "); jspace::pretty_print(base_controller->getState().velocity_, cout, "jvel", " "); jspace::pretty_print(base_controller->getCommand(), cout, "gamma", " "); cout << "servo rate: " << actual_servo_rate << "\n"; } } if (t1 - dump_t0 > dump_dt) { dump_t0 = t1; } ros::spinOnce(); usleep(10000); // 100Hz-ish } warnx("shutting down"); servo.shutdown(); }
int main(int argc, char ** argv) { struct sigaction sa; bzero(&sa, sizeof(sa)); sa.sa_handler = handle; if (0 != sigaction(SIGINT, &sa, 0)) { err(EXIT_FAILURE, "sigaction"); } // Before we attempt to read any tasks and skills from the YAML // file, we need to inform the static type registry about custom // additions such as the HelloGoodbyeSkill. Factory::addSkillType<uta_opspace::HelloGoodbyeSkill>("uta_opspace::HelloGoodbyeSkill"); ros::init(argc, argv, "wbc_m3_ctrl_servo", ros::init_options::NoSigintHandler); parse_options(argc, argv); ros::NodeHandle node("~"); controller.reset(new ControllerNG("wbc_m3_ctrl::servo")); param_cbs.reset(new ParamCallbacks()); Servo servo; try { if (verbose) { warnx("initializing param callbacks"); } registry.reset(factory->createRegistry()); registry->add(controller); param_cbs->init(node, registry, 1, 100); if (verbose) { warnx("starting servo with %lld Hz", servo_rate); } actual_servo_rate = servo_rate; servo.start(servo_rate); } catch (std::runtime_error const & ee) { errx(EXIT_FAILURE, "failed to start servo: %s", ee.what()); } warnx("started servo RT thread"); ros::Time dbg_t0(ros::Time::now()); ros::Time dump_t0(ros::Time::now()); ros::Duration dbg_dt(0.1); ros::Duration dump_dt(0.05); while (ros::ok()) { ros::Time t1(ros::Time::now()); if (verbose) { if (t1 - dbg_t0 > dbg_dt) { dbg_t0 = t1; servo.skill->dbg(cout, "\n\n**************************************************", ""); controller->dbg(cout, "--------------------------------------------------", ""); cout << "--------------------------------------------------\n"; jspace::pretty_print(model->getState().position_, cout, "jpos", " "); jspace::pretty_print(model->getState().velocity_, cout, "jvel", " "); jspace::pretty_print(model->getState().force_, cout, "jforce", " "); jspace::pretty_print(controller->getCommand(), cout, "gamma", " "); Vector gravity; model->getGravity(gravity); jspace::pretty_print(gravity, cout, "gravity", " "); cout << "servo rate: " << actual_servo_rate << "\n"; } } if (t1 - dump_t0 > dump_dt) { dump_t0 = t1; controller->qhlog(*servo.skill, rt_get_cpu_time_ns() / 1000); } ros::spinOnce(); usleep(10000); // 100Hz-ish } warnx("shutting down"); servo.shutdown(); }