C++ ProductManager类代码示例

int main(int argc, char** argv)
{
  ros::init(argc, argv, "bhand_node");

  ProductManager pm;
  if (!pm.foundHand())
  {
    printf("ERROR: No Hand found on bus!\n");
    return 1;
  }

  Hand* hand = pm.getHand();

  BHandNode bhand_node(hand);

  bhand_node.init();

  ros::Rate pub_rate(PUBLISH_FREQ);

  while (bhand_node.n_.ok())
  {
    ros::spinOnce();
    bhand_node.publish();
    pub_rate.sleep();
  }

  hand->idle();

  return 0;
}

int main() {
	ProductManager pm;
	if ( !pm.foundGimbalsHandController() ) {
		printf("ERROR: No Gimbals Hand Controller found!\n");
		return 1;
	}
	GimbalsHandController& ghc = *pm.getGimbalsHandController();


	int loopCount = 0;
	while (true) {
		usleep(10000);

		ghc.update();

		loopCount = (loopCount+1) % 25;
		if (loopCount == 0) {
			printf("%d,%d  %d,%d  %d,%d  %d,%d  %f\n",
					ghc.getThumbOpen(), ghc.getThumbClose(),
					ghc.getPointerOpen(), ghc.getPointerClose(),
					ghc.getMiddleOpen(), ghc.getMiddleClose(),
					ghc.getRockerUp(), ghc.getRockerDown(),
					ghc.getKnob());
		}
	}
}

int main(int argc, char** argv) {
	if (argc != 2) {
		printf("Usage: %s <calibrationFile>\n", argv[0]);
		printf("    <calibrationFile>    File containing the gain-matrix for the attached Force-Torque Sensor\n");
		return 1;
	}


	ProductManager pm;
	if ( !pm.foundForceTorqueSensor() ) {
		printf("ERROR: No Force-Torque Sensor found!\n");
		return 1;
	}
	ForceTorqueSensor& fts = *pm.getForceTorqueSensor();


	int calValue = 0;
	ifstream cal(argv[1]);
	for (int i = 0; i < GM_SIZE; ++i) {
		if (cal.good()) {
			cal >> calValue;
			if (calValue < -32768  ||  calValue > 32767) {
				printf("Calibration file is poorly formated: value out of range: %d.\n", calValue);
				return 1;
			}

			// TODO(dc): Fix once F/T has working ROLE
			//fts.setProperty(Puck::GM, calValue);
			Puck::setProperty(pm.getBus(), fts.getPuck()->getId(), Puck::getPropertyId(Puck::GM, Puck::PT_ForceTorque, 152), calValue);

			printf(".");
			fflush(stdout);
			usleep(1000000);
		} else {

boost::thread startWam(ProductManager& pm,
		boost::function<void (ProductManager&, systems::Wam<4>&)> wt4,
		boost::function<void (ProductManager&, systems::Wam<7>&)> wt7)
{
	pm.waitForWam();
	pm.wakeAllPucks();

	if (pm.foundWam4()) {
		return boost::thread(wt4, boost::ref(pm), boost::ref(*pm.getWam4()));
	} else {
		return boost::thread(wt7, boost::ref(pm), boost::ref(*pm.getWam7()));
	}
}

int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
	BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

	const char* path = pm.getWamDefaultConfigPath();
	std::cout<<"got configuration "<<std::endl;
	size_t i=0;
	while(path[i]!='\0'){
	  std::cout<<path[i];
	  i++;
	}
	std::cout<<"configuration should be printed"<<std::endl;
    pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
    return 0;
}

int wam_main(int argc, char** argv, ProductManager& pm,
		systems::Wam<DOF>& wam) {
	BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

	std::string filename(argv[1]);

	printf("\nMoving to start configuration \n");

	jp_type jp(0.0);
	jp[1] = -1.967;
	jp[3] = 2.5;
	jp[5] = -0.5;
	wam.moveTo(jp);

	printf("Opening hands\n");

	// Open hands.
	barrett::Hand& hand = *pm.getHand();
	hand.initialize();

	// wam.idle();

	Teach<DOF> teach(wam, pm, filename);

	teach.init();

	printf("\nPress [Enter] to start teaching.\n");
	waitForEnter();
	teach.record();
	//boost::thread t(&Teach<DOF>::display, &teach);

	printf("Press [Enter] to stop teaching.\n");
	waitForEnter();
	teach.createSpline();

	// Move to start and close hands.
	wam.moveTo(jp);
	hand.close();
	hand.idle();

	wam.idle();

	pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);

	return 0;
}

int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
	BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

	/* Read configuration file */
	libconfig::Config config;
	try {
		config.readFile(CAL_CONFIG_FILE.c_str());
	} catch (const libconfig::FileIOException &fioex) {
		printf("EXITING: I/O error while reading %s\n", CAL_CONFIG_FILE.c_str());
		btsleep(5.0);
		return (false);
	} catch (const libconfig::ParseException &pex) {
		printf("EXITING: Parse error at %s: %d - %s\n", pex.getFile(), pex.getLine(), pex.getError());
		btsleep(5.0);
		return (false);
	}
	const libconfig::Setting& setting = config.lookup("autotension")[pm.getWamDefaultConfigPath()];

	std::vector<int> arg_list = validate_args<DOF>(argc, argv);
	if (arg_list.size() == 0) {
		pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
		return 1;
	}

	printf("Press <Enter> to begin autotensioning.\n");
	detail::waitForEnter();

	AutoTension<DOF> autotension(wam, setting);
	autotension.init(pm, arg_list);

	/* Autotension Specified Joints*/
	while (arg_list.size() != 0)
		arg_list = autotension.tensionJoint(arg_list);

	/* Re-fold and exit */
	if (autotension.hand != 0) {
		autotension.hand->open(Hand::GRASP);
		autotension.hand->close(Hand::SPREAD);
		autotension.hand->trapezoidalMove(Hand::jp_type(M_PI / 2.0), Hand::GRASP);
	}
	wam.moveHome();
	printf("\n**************************\n");
	printf("Autotensioning Routine Complete\n");
	pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
	return 0;
}

int main() {
	ProductManager pm;
	SafetyModule& sm = *pm.getSafetyModule();
	SafetyModule::PendantState ps;

	// Optional: Instantiate a Wam object and start the realtime control loop
	if (pm.foundWam4()) {
		pm.getWam4(false);
	} else {
		pm.getWam7(false);
	}

	while (true) {
		sm.getPendantState(&ps);
		std::cout << ps.toString() << " " << ps.allSafe() << " " << ps.hasFaults() << "\n";
		usleep(1000000);
	}

	return 0;
}

int main(int argc, char** argv) {

	// Message to user
	printf("\n"
			"                  *** Barrett WAM Autotensioning Utility ***\n"
			"\n"
			"This utility will autotension the specified cables of your WAM Arm.\n"
			"Cable tensioning is necessary after signs of the WAM cables becoming\n"
			"loose after extended use, or after replacing any cables on your WAM Arm.\n"
			"After completion of this routine, you must zero calibrate and gravity\n"
			"calibrate the WAM, as pulling tension from the cables will introduce\n"
			"offsets to your previous calibrations.\n"
			"\n"
			"WAMs with serial numbers < 5 and WAM wrists with serial numbers < 9 are not\n"
			"eligible for autotensioning.\n"
			"\n"
			"This program assumes the WAM is mounted such that the base is horizontal.\n"
			"\n"
			"\n");

	// For clean stack traces
	barrett::installExceptionHandler();

	// Create our product manager
	ProductManager pm;
	pm.waitForWam();

	if (pm.foundWam4()) {
		return wam_main<4>(argc, argv, pm, *pm.getWam4(true, NULL));
	} else if (pm.foundWam7()) {
		return wam_main<7>(argc, argv, pm, *pm.getWam7(true, NULL));
	}
}

template <size_t DOF> void wamThread0(ProductManager& pm0, systems::Wam<DOF>& wam0) {
	BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

	wam0.gravityCompensate();

	jp_type jp(0.0);
	while (pm0.getSafetyModule()->getMode() == SafetyModule::ACTIVE) {
		wam0.moveTo(jp);
		sleep(1);
		wam0.moveHome();
		sleep(1);
	}
}

int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
	BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
	typedef boost::tuple<double, jp_type> jp_sample_type;


	char tmpFile[] = "/tmp/btXXXXXX";
	if (mkstemp(tmpFile) == -1) {
		printf("ERROR: Couldn't create temporary file!\n");
		return 1;
	}

	const double T_s = pm.getExecutionManager()->getPeriod();


	//wam.gravityCompensate();
	boost::thread displayThread(displayEntryPoint, pm.getHand());



	std::remove(tmpFile);
	pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);

	return 0;
}

void takeAccelSample(ProductManager& pm, int duration_us, const char* fileName)
{
	BARRETT_UNITS_FIXED_SIZE_TYPEDEFS;

	if ( !pm.foundForceTorqueSensor() ) {
		throw std::runtime_error("Couldn't find an FTS!");
	}
	pm.startExecutionManager();

	char tmpFile[] = "/tmp/btXXXXXX";
	if (mkstemp(tmpFile) == -1) {
		throw std::runtime_error("Couldn't create temporary file!");
	}


	systems::Ramp time(pm.getExecutionManager(), 1.0);
	FTSAccel ftsa(pm.getForceTorqueSensor());

	systems::TupleGrouper<double, ca_type> tg;
	connect(time.output, tg.getInput<0>());
	connect(ftsa.output, tg.getInput<1>());

	typedef boost::tuple<double, ca_type> tuple_type;
	const size_t PERIOD_MULTIPLIER = 1;
	systems::PeriodicDataLogger<tuple_type> logger(
			pm.getExecutionManager(),
			new log::RealTimeWriter<tuple_type>(tmpFile, PERIOD_MULTIPLIER * pm.getExecutionManager()->getPeriod()),
			PERIOD_MULTIPLIER);

	time.start();
	connect(tg.output, logger.input);
	printf("Logging started.\n");


	usleep(duration_us);


	logger.closeLog();
	printf("Logging stopped.\n");

	log::Reader<tuple_type> lr(tmpFile);
	lr.exportCSV(fileName);
	printf("Output written to %s.\n", fileName);
	std::remove(tmpFile);
}

int wam_main(int argc, char** argv, ProductManager& pm,
        systems::Wam<DOF>& wam) {
    BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

    std::string filename(argv[1]);

    Teach<DOF> teach(wam, pm, filename);

    teach.init();

    printf("\nPress [Enter] to start teaching.\n");
    waitForEnter();
    teach.record();
    //boost::thread t(&Teach<DOF>::display, &teach);

    printf("Press [Enter] to stop teaching.\n");
    waitForEnter();
    teach.createSpline();

    pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);

    return 0;
}

int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
	BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
	typedef boost::tuple<double, jp_type> jp_sample_type;

	char tmpFile[] = "/tmp/btXXXXXX";
	if (mkstemp(tmpFile) == -1) {
		printf("ERROR: Couldn't create temporary file!\n");
		return 1;
	}

	const double T_s = pm.getExecutionManager()->getPeriod();


	wam.gravityCompensate();

	systems::Ramp time(pm.getExecutionManager());

	systems::TupleGrouper<double, jp_type> jpLogTg;

	// Record at 1/10th of the loop rate
	systems::PeriodicDataLogger<jp_sample_type> jpLogger(pm.getExecutionManager(),
			new barrett::log::RealTimeWriter<jp_sample_type>(tmpFile, 10*T_s), 10);


	printf("Press [Enter] to start teaching.\n");
	waitForEnter();
	{
		// Make sure the Systems are connected on the same execution cycle
		// that the time is started. Otherwise we might record a bunch of
		// samples all having t=0; this is bad because the Spline requires time
		// to be monotonic.
		BARRETT_SCOPED_LOCK(pm.getExecutionManager()->getMutex());

		connect(time.output, jpLogTg.template getInput<0>());
		connect(wam.jpOutput, jpLogTg.template getInput<1>());
		connect(jpLogTg.output, jpLogger.input);
		time.start();
	}

	printf("Press [Enter] to stop teaching.\n");
	waitForEnter();
	jpLogger.closeLog();
	disconnect(jpLogger.input);


	// Build spline between recorded points
	log::Reader<jp_sample_type> lr(tmpFile);
	std::vector<jp_sample_type> vec;
	for (size_t i = 0; i < lr.numRecords(); ++i) {
		vec.push_back(lr.getRecord());
	}
	math::Spline<jp_type> spline(vec);

	printf("Press [Enter] to play back the recorded trajectory.\n");
	waitForEnter();

	// First, move to the starting position
	wam.moveTo(spline.eval(spline.initialS()));

	// Then play back the recorded motion
	time.stop();
	time.setOutput(spline.initialS());

	systems::Callback<double, jp_type> trajectory(boost::ref(spline));
	connect(time.output, trajectory.input);
	wam.trackReferenceSignal(trajectory.output);

	time.start();

	while (trajectory.input.getValue() < spline.finalS()) {
		usleep(100000);
	}


	printf("Press [Enter] to idle the WAM.\n");
	waitForEnter();
	wam.idle();


	std::remove(tmpFile);
	pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);

	return 0;
}

int main() {
	typedef Hand::jp_type hjp_t;

	ProductManager pm;
	if ( !pm.foundHand() ) {
		printf("ERROR: No Hand found on bus!\n");
		return 1;
	}
	Hand& hand = *pm.getHand();
	hand.initialize();


	double O = 0.0;
	double C = 2.4;
	double SC = M_PI;
	hjp_t open(O);
	hjp_t closed(C);
	closed[3] = SC;

	double OR = -0.75;
	double CR = 0.75;
	Hand::jv_type opening(OR);
	Hand::jv_type closing(CR);



	{
		assertPosition(hand, open);
		hand.close();
		assertPosition(hand, closed);
		hand.open();
		assertPosition(hand, open);
		hand.close(Hand::SPREAD);
		assertPosition(hand, hjp_t(O,O,O,SC));
		hand.close(Hand::GRASP);
		assertPosition(hand, closed);
		hand.open(Hand::GRASP, false);
		btsleep(0.5);
		assertPosition(hand, hjp_t(1.6,1.6,1.6,SC));
		hand.open();
		assertPosition(hand, open);
	}

	{
		// Original interface preserved? Should move all 4 motors.
		hand.trapezoidalMove(closed);
		assertPosition(hand, closed);
		hand.trapezoidalMove(open, false);
		hand.waitUntilDoneMoving();
		assertPosition(hand, open);

		// New interface
		hand.trapezoidalMove(closed, Hand::SPREAD);
		assertPosition(hand, hjp_t(O,O,O,SC));
		hand.trapezoidalMove(closed, Hand::F1);
		assertPosition(hand, hjp_t(C,O,O,SC));
		hand.trapezoidalMove(closed, Hand::F2);
		assertPosition(hand, hjp_t(C,C,O,SC));
		hand.trapezoidalMove(closed, Hand::F3);
		assertPosition(hand, closed);
		hand.trapezoidalMove(open, Hand::GRASP);
		assertPosition(hand, hjp_t(O,O,O,SC));
		hand.trapezoidalMove(open, Hand::SPREAD);
		assertPosition(hand, open);
		hand.trapezoidalMove(closed, Hand::F3 | Hand::SPREAD);
		assertPosition(hand, hjp_t(O,O,C,SC));
		hand.trapezoidalMove(open, Hand::WHOLE_HAND);
		assertPosition(hand, open);
	}

	{
		double t = 0.0;

		// Original interface preserved? Should move all 4 motors.
		hand.velocityMove(closing);
		btsleep(1);
		t = 1.0;
		assertPosition(hand, hjp_t(CR*t), 0.2);

		// New interface
		hand.velocityMove(opening, Hand::GRASP);
		btsleep(1);
		t = 2.0;
		assertPosition(hand, hjp_t(O,O,O,CR*t), 0.4);
		hand.velocityMove(opening, Hand::WHOLE_HAND);
		hand.waitUntilDoneMoving();
		assertPosition(hand, open);
	}

	{
		double t = 0.0;

		// Original interface preserved? Should move all 4 motors.
		hand.velocityMove(closing);
		btsleep(1);
		t = 1.0;
		assertPosition(hand, hjp_t(CR*t), 0.2);

		// SPREAD should continue its velocity move
		hand.trapezoidalMove(open, Hand::F1);  // Only blocks for F1
//.........这里部分代码省略.........