void Node::jointStateCB (sm::JointState::ConstPtr m)
{
// The first time round, figure out which joints to ignore
if (joint_ignored_.size()==0)
{
updateIgnoredJoints(*m);
writeJointNames(*m);
}
// Only save every so often
if (ros::Time::now() <= last_processed_ + processing_interval_)
return;
RobotState rs(m, getBasePose(m->header.stamp));
Diffs diffs = getDiffs(last_saved_state_, rs);
saveToDB(diffs);
last_saved_state_ = rs;
}
::Ice::DispatchStatus
RoboCompGenericBase::GenericBase::___getBasePose(::IceInternal::Incoming& __inS, const ::Ice::Current& __current)
{
__checkMode(::Ice::Normal, __current.mode);
__inS.readEmptyParams();
::Ice::Int x;
::Ice::Int z;
::Ice::Float alpha;
try
{
getBasePose(x, z, alpha, __current);
::IceInternal::BasicStream* __os = __inS.__startWriteParams(::Ice::DefaultFormat);
__os->write(x);
__os->write(z);
__os->write(alpha);
__inS.__endWriteParams(true);
return ::Ice::DispatchOK;
}
catch(const ::RoboCompGenericBase::HardwareFailedException& __ex)
{
__inS.__writeUserException(__ex, ::Ice::DefaultFormat);
}
return ::Ice::DispatchUserException;
}
