void GetTwistHandler::handleSimulation(){
// called when the main script calls: simHandleModule
if(!_initialized){
_initialize();
}
ros::Time now = ros::Time::now();
const simFloat currentSimulationTime = simGetSimulationTime();
if ((currentSimulationTime-_lastPublishedObjTwistTime) >= 1.0/_ObjTwistFrequency){
Eigen::Quaternion< simFloat > orientation; //(x,y,z,w)
Eigen::Matrix<simFloat, 3, 1> linVelocity;
Eigen::Matrix<simFloat, 3, 1> angVelocity;
bool error = false;
// Get object velocity. If the object is not static simGetVelocity is more accurate.
if (_isStatic){
error = error || simGetObjectVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
} else {
error = error || simGetVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
}
// Get object orientation
error = error || simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data()) == -1;
if(!error){
linVelocity = orientation.conjugate()*linVelocity; // Express linear velocity in body frame
angVelocity = orientation.conjugate()*angVelocity; // Express angular velocity in body frame
// Fill the status msg
geometry_msgs::TwistStamped msg;
msg.twist.linear.x = linVelocity[0];
msg.twist.linear.y = linVelocity[1];
msg.twist.linear.z = linVelocity[2];
msg.twist.angular.x = angVelocity[0];
msg.twist.angular.y = angVelocity[1];
msg.twist.angular.z = angVelocity[2];
msg.header.stamp = now;
_pub.publish(msg);
_lastPublishedObjTwistTime = currentSimulationTime;
} else {
std::stringstream ss;
ss << "- [" << _associatedObjectName << "] Error getting object velocity and/or orientation." << std::endl;;
ConsoleHandler::printInConsole(ss);
}
}
}
