本文整理汇总了C++中scoped_ptr::getState方法的典型用法代码示例。如果您正苦于以下问题：C++ scoped_ptr::getState方法的具体用法？C++ scoped_ptr::getState怎么用？C++ scoped_ptr::getState使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类scoped_ptr的用法示例。
在下文中一共展示了scoped_ptr::getState方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: main
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();
}示例2: main
//.........这里部分代码省略.........
servo.cleanup();
return 0;
}
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);
//ros::Rate loop_rate(10);
ros::Time begin(ros::Time::now());
while (ros::ok()) {
//UDP camera in and out
if (cam) {
bytes = cameraUDP.recvPacket((char*)rawCamData, sizeof(rawCamData));
if (bytes <1) {
fprintf(stderr,"Error: no camera data recieved");
servo.cleanup();
return 0;
}
for (size_t ii=0; ii < bytes/sizeof(Position3d); ++ii){
camData(ii,0) = rawCamData[ii].x*1e-3;
camData(ii,1) = rawCamData[ii].y*1e-3;
camData(ii,2) = rawCamData[ii].z*1e-3;
}
Matrix temp(R * camData.transpose() + d*Matrix::Ones(1,bytes/sizeof(Position3d)));
state.camData_ = temp.transpose();
}
/*
//Vis out
for (size_t ii(0); ii<6 ; ++ii) {
info.q[ii] = state.position_[ii];
}
Skill::task_table_t const * tasks(servo.skill->getTaskTable());
info.R[0] = 0.1;
info.T[0] = 0.4;//No longer automatic-> should not be manual
if (cam) {
for (size_t ii(0); ii < state.camData_.rows(); ++ii) {
for (size_t jj(0); jj < state.camData_.cols(); ++jj) {
info.sp[ii][jj] = state.camData_(ii,jj);
}
}
}
info.eedes[0] = 0.0;
info.eedes[1] = 0.0;
info.eedes[2] = 0.0;
info.cp[0] = 0.0;
info.cp[1] = 0.0;
info.cp[2] = 0.0;
bytes = visUDP.sendPacket((char*)&info, sizeof(info));
*/
status = servo.update(state, command);
if (0 != status) {
fprintf(stderr, "update callback returned %d\n", status);
servo.cleanup();
return 0;
}
for (size_t ii(0); ii<7; ++ii) {
if (isnan(command[ii])) {
torque_msg.efforts[ii] = 0;
}
else {
torque_msg.efforts[ii] = command[ii];
}
}
torque_pub.publish(torque_msg);
jointimp_pub.publish(jointimp_msg);
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(controller->getCommand(), cout, "gamma", " ");
jspace::pretty_print(model->getState().camData_, cout, "cam", " ");
}
}
if (t1 - dump_t0 > dump_dt) {
dump_t0 = t1;
controller->qhlog(*servo.skill, ros::Duration(t1-begin).toSec()*1000);
}
ros::spinOnce();
//loop_rate.sleep();
}
warnx("shutting down");
ros::shutdown();
servo.cleanup();
//cameraUDP.~UdpOSI();
}