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