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); } } }