RTC::ReturnCode_t GyroRTC::onExecute(RTC::UniqueId ec_id)
{
simInt bufSize;
simChar* pBuffer = simTubeRead(m_tubeHandle, &bufSize);
if (pBuffer == NULL) {
return RTC::RTC_OK;
}
float_byte buffer;
int data_size = bufSize / 4;
if (data_size != 3) {
std::cout << "[GyroRTC] Invalid data size (" << data_size << "!=3)" << std::endl;
return RTC::RTC_OK;
}
float time = simGetSimulationTime();
long sec = floor(time);
long nsec = (time - sec) * 1000*1000*1000;
m_gyro.tm.sec = sec;
m_gyro.tm.nsec = nsec;
float x[3];
for(int i = 0;i < 3;i++) {
for(int j = 0;j < 4;j++) {
buffer.byte_value[j] = pBuffer[i*4+j];
}
x[i] = buffer.float_value;
}
m_gyro.data.avx = x[0];
m_gyro.data.avy = x[1];
m_gyro.data.avz = x[2];
m_gyroOut.write();
simReleaseBuffer((simChar*)pBuffer);
return RTC::RTC_OK;
}
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);
}
}
}
void simulatorLoop()
{ // The main application loop (excluding the GUI part)
static bool wasRunning=false;
int auxValues[4];
int messageID=0;
int dataSize;
if (autoStart)
{
simStartSimulation();
autoStart=false;
}
while (messageID!=-1)
{
simChar* data=simGetSimulatorMessage(&messageID,auxValues,&dataSize);
if (messageID!=-1)
{
if (messageID==sim_message_simulation_start_resume_request)
simStartSimulation();
if (messageID==sim_message_simulation_pause_request)
simPauseSimulation();
if (messageID==sim_message_simulation_stop_request)
simStopSimulation();
if (data!=NULL)
simReleaseBuffer(data);
}
}
// Handle a running simulation:
if ( (simGetSimulationState()&sim_simulation_advancing)!=0 )
{
wasRunning=true;
if ( (simGetRealTimeSimulation()!=1)||(simIsRealTimeSimulationStepNeeded()==1) )
{
if ((simHandleMainScript()&sim_script_main_script_not_called)==0)
simAdvanceSimulationByOneStep();
if ((simStopDelay>0)&&(simGetSimulationTime()>=float(simStopDelay)/1000.0f))
{
simStopDelay=0;
simStopSimulation();
}
}
}
else
{
if (wasRunning&&autoQuit)
{
wasRunning=false;
simQuitSimulator(true); // will post the quit command
}
}
}
RTC::ReturnCode_t ObjectRTC::onExecute(RTC::UniqueId ec_id)
{
simFloat position[3];
simFloat orientation[3];
::simGetObjectPosition(m_objectHandle, -1, position);
::simGetObjectOrientation(m_objectHandle, -1, orientation);
/*
simInt bufSize;
simChar* pBuffer = simTubeRead(m_tubeHandle, &bufSize);
if (pBuffer == NULL) {
return RTC::RTC_OK;
}
float_byte buffer;
int data_size = bufSize / 4;
if (data_size != 3) {
std::cout << "[ObjectRTC] Invalid data size (" << data_size << "!=3)" << std::endl;
return RTC::RTC_OK;
}
float x[3];
for(int i = 0;i < 3;i++) {
for(int j = 0;j < 4;j++) {
buffer.byte_value[j] = pBuffer[i*4+j];
}
x[i] = buffer.float_value;
}
m_object.data.avx = x[0];
m_object.data.avy = x[1];
m_object.data.avz = x[2];
m_objectOut.write();
simReleaseBuffer((simChar*)pBuffer);
*/
float time = simGetSimulationTime();
long sec = floor(time);
long nsec = (time - sec) * 1000 * 1000 * 1000;
m_pose.tm.sec = sec;
m_pose.tm.nsec = nsec;
m_pose.data.position.x = position[0];
m_pose.data.position.y = position[1];
m_pose.data.position.z = position[2];
m_pose.data.orientation.r = orientation[0];
m_pose.data.orientation.p = orientation[1];
m_pose.data.orientation.y = orientation[2];
m_poseOut.write();
return RTC::RTC_OK;
}
void DrawMeshHandler::handleSimulation() {
// called when the main script calls: simHandleModule
if (!_initialized) {
_initialize();
}
std::stringstream ss;
const simFloat currentSimulationTime = simGetSimulationTime();
shape_msgs::MeshConstPtr msg = _lastMsg;
_lastMsg.reset(); //only draw new meshes TODO: we might loose a message if it arrives between this and the previous line!
if ((currentSimulationTime - _lastTime) >= 1.0 / _frequency && msg) {
if (_drawingObject > 0)
simRemoveDrawingObject(_drawingObject);
_drawingObject = simAddDrawingObject(
sim_drawing_triangles + sim_drawing_painttag
+ sim_drawing_followparentvisibility + _transparency,
_width, 0.0, _associatedObjectID, msg->triangles.size(),
_diffuse, NULL, _specular, _emission);
simFloat data[9];
for (shape_msgs::Mesh::_triangles_type::const_iterator it =
msg->triangles.begin(); it != msg->triangles.end(); ++it) {
for (unsigned int i = 0; i < 3; ++i) {
data[3 * i + 0] = msg->vertices[it->vertex_indices[i]].x;
data[3 * i + 1] = msg->vertices[it->vertex_indices[i]].y;
data[3 * i + 2] = msg->vertices[it->vertex_indices[i]].z;
}
simAddDrawingObjectItem(_drawingObject, data);
}
_lastTime = currentSimulationTime;
}
if (ss.rdbuf()->in_avail()) {
ConsoleHandler::printInConsole(ss);
}
}
// This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages):
VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
{ // This is called quite often. Just watch out for messages/events you want to handle
// Keep following 5 lines at the beginning and unchanged:
//static bool refreshDlgFlag=true;
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
void* retVal=NULL;
// Here we can intercept many messages from V-REP (actually callbacks). Only the most important messages are listed here.
// For a complete list of messages that you can intercept/react with, search for "sim_message_eventcallback"-type constants
// in the V-REP user manual.
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{ // The main script is about to be run (only called while a simulation is running (and not paused!))
// main script is called every dynamics calculation.
tickRTCs(simGetSimulationTimeStep());
}
if (message==sim_message_eventcallback_simulationabouttostart)
{ // Simulation is about to start
simulatorClock.setSimulationTimeStep(simGetSimulationTimeStep());
//simulatorClock.setSimulationTime(simGetSimulationTime());
startRTCs();
}
if (message == sim_message_eventcallback_menuitemselected) {
simInt handle = auxiliaryData[0];
simInt state = auxiliaryData[1];
if (handle == robotItemHandle) {
std::cout << "Robot Menu Selected" << std::endl;
mainItemHandle = robotItemHandle;
spawnRTCMethod = spawnRobotRTC;
modelDlgHandle = simDisplayDialog("Input Robot Item", "Input Robot Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
else if (handle == rangeItemHandle) {
std::cout << "Range Menu Selected" << std::endl;
mainItemHandle = rangeItemHandle;
spawnRTCMethod = spawnRangeRTC;
modelDlgHandle = simDisplayDialog("Input Item", "Input Range Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
else if (handle == cameraItemHandle) {
std::cout << "Camera Menu Selected" << std::endl;
mainItemHandle = cameraItemHandle;
spawnRTCMethod = spawnCameraRTC;
modelDlgHandle = simDisplayDialog("Input Item", "Input Vision Sensor Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
else if (handle == accelItemHandle) {
std::cout << "Acceleration Menu Selected" << std::endl;
mainItemHandle = accelItemHandle;
spawnRTCMethod = spawnAccelerometerRTC;
modelDlgHandle = simDisplayDialog("Input Item", "Input Accel Sensor Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
else if (handle == gyroItemHandle) {
std::cout << "Gyro Menu Selected" << std::endl;
mainItemHandle = gyroItemHandle;
spawnRTCMethod = spawnGyroRTC;
modelDlgHandle = simDisplayDialog("Input Item", "Input Gyro Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
else if (handle == depthItemHandle) {
std::cout << "Depth Menu Selected" << std::endl;
mainItemHandle = depthItemHandle;
spawnRTCMethod = spawnDepthRTC;
modelDlgHandle = simDisplayDialog("Input Item", "Input Depth Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
else if (handle == objectItemHandle) {
std::cout << "Object Menu Selected" << std::endl;
mainItemHandle = objectItemHandle;
spawnRTCMethod = spawnObjectRTC;
modelDlgHandle = simDisplayDialog("Input Item", "Input Object Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
}
if (message == sim_message_eventcallback_refreshdialogs) {
if (modelDlgHandle >= 0) {
simInt ret = simGetDialogResult(modelDlgHandle);
if (ret != sim_dlgret_still_open) {
if (ret == sim_dlgret_ok) {
char *buf = simGetDialogInput(modelDlgHandle);
modelName = buf;
std::cout << "Model Name is " << buf << std::endl;
simReleaseBuffer(buf);
argDlgHandle = simDisplayDialog("Input Argument of RTC", "Input Argument of createComponent method", sim_dlgstyle_input, "", NULL, NULL, NULL);
modelDlgHandle = -1;
}
}
} else if (argDlgHandle >= 0) {
simInt ret = simGetDialogResult(argDlgHandle);
if (ret != sim_dlgret_still_open) {
if (ret == sim_dlgret_ok) {
char *buf = simGetDialogInput(argDlgHandle);
argument = buf;
std::cout << "createComponent Argument is " << buf << std::endl;
simReleaseBuffer(buf);
argDlgHandle = -1;
spawnRTCMethod(modelName, argument);
}
}
}
}
if (message==sim_message_eventcallback_simulationended)
{ // Simulation just ended
stopRTCs();
}
simulatorClock.setSimulationTime(simGetSimulationTime());
message_pump();
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
void message_pump() {
// Task Queue Check for unsynchronized task thrown from Service Port of RT-Component.
Task t = taskQueue.popTask();
simInt ret;
switch(t.value){
case Task::START:
std::cout << " - Task::Starting Simulation" << std::endl;
ret = simStartSimulation();
if (ret == 0) {
returnQueue.returnReturn(Return(Return::RET_FAILED));
} else if(ret < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::STOP:
std::cout << " - Task::Stopping Simulation" << std::endl;
ret = simStopSimulation();
if (ret == 0) {
returnQueue.returnReturn(Return(Return::RET_FAILED));
} else if(ret < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::PAUSE:
std::cout << " - Task::Pausing Simulation" << std::endl;
ret = simPauseSimulation();
if (ret == 0) {
returnQueue.returnReturn(Return(Return::RET_FAILED));
} else if(ret < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::LOADPROJECT:
std::cout << " - Task::Loading Project: " << t.key.c_str() << std::endl;
ret = simLoadScene(t.key.c_str());
std::cout << " -- ret = " << ret;
if (ret < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::SPAWNROBOT:
std::cout << " - Task::SpawnRobot" << std::endl;
if (spawnRobotRTC(t.key, t.arg) < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::SPAWNRANGE:
std::cout << " - Task::Spawnrange" << std::endl;
if (spawnRangeRTC(t.key, t.arg) < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::SPAWNDEPTH:
std::cout << " - Task::Spawndepth" << std::endl;
if (spawnDepthRTC(t.key, t.arg) < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::SPAWNACCEL:
std::cout << " - Task::Spawnaccel" << std::endl;
if (spawnAccelerometerRTC(t.key, t.arg) < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::SPAWNGYRO:
std::cout << " - Task::Spawngyro" << std::endl;
if (spawnGyroRTC(t.key, t.arg) < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::SPAWNCAMERA:
std::cout << " - Task::SpawnCamera" << std::endl;
if (spawnCameraRTC(t.key, t.arg) < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::SPAWNOBJECT:
std::cout << " - Task::SpawnObject" << std::endl;
if (spawnObjectRTC(t.key, t.arg) < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
}
else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::KILLRTC:
std::cout << " - Task::KillRTC" << std::endl;
if (killRTC(t.key) < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::KILLALLRTC:
std::cout << " - Task::KillAllRTC" << std::endl;
if (killAllRTC() < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::SYNCRTC:
std::cout << " - Task::SYNCRTC" << std::endl;
if (syncRTC(t.key) < 0) {
returnQueue.returnReturn(Return(Return::RET_ERROR));
} else {
returnQueue.returnReturn(Return(Return::RET_OK));
}
break;
case Task::GETSYNCRTC:
std::cout << " - Task::GETSYNCRTC" << std::endl;
{
Return r(Return::RET_OK);
if (getSyncRTCs(r.stringList) < 0) {
r.value = Return::RET_ERROR;
returnQueue.returnReturn(r);
} else {
for(int i = 0;i < r.stringList.size();i++) {
std::cout << " - Task: " << r.stringList[i] << std::endl;
}
returnQueue.returnReturn(r);
}
}
break;
case Task::GETSIMTIME:
std::cout << " - Task::GETSIMTIME" << std::endl;
{
Return r(Return::RET_OK);
r.floatValue = simGetSimulationTime();
returnQueue.returnReturn(r);
}
break;
case Task::GETSIMSTEP:
std::cout << " - Task::GETSIMSTEP" << std::endl;
{
Return r(Return::RET_OK);
r.floatValue = simGetSimulationTimeStep();
returnQueue.returnReturn(r);
}
break;
case Task::GETOBJPOSE:
returnQueue.returnReturn(Return(Return::RET_OK));
break;
case Task::SETOBJPOSE:
returnQueue.returnReturn(Return(Return::RET_OK));
break;
default:
//returnQueue.returnReturn(Return(Return::RET_ERROR));
break;
}
}
