cmnDataFormat::cmnDataFormat(void) { // determine size of pointer if (sizeof(void *) == 4) { this->WordSizeMember = CMN_DATA_32_BITS; } else if (sizeof(void *) == 8) { this->WordSizeMember = CMN_DATA_64_BITS; } else { CMN_ASSERT(false); } // determine endianness int one = 1; char * part = reinterpret_cast<char *>(&one); if (part[0] == 1) { this->EndiannessMember = CMN_DATA_LITTLE_ENDIAN; } else { this->EndiannessMember = CMN_DATA_BIG_ENDIAN; } // determine size of size_t if (sizeof(size_t) == 4) {\ this->SizeTSizeMember = CMN_DATA_SIZE_T_SIZE_32; } else if (sizeof(size_t) == 8) { this->SizeTSizeMember = CMN_DATA_SIZE_T_SIZE_64; } else { CMN_ASSERT(false); } }
void clientTask<_dataType>::Startup(void) { // check argument prototype for event handler mtsInterfaceRequired * required = GetInterfaceRequired("Required"); CMN_ASSERT(required); mtsCommandWriteBase * eventHandler = required->GetEventHandlerWrite("EventWrite"); CMN_ASSERT(eventHandler); }
void mtsIntuitiveResearchKitPSM::Init(void) { // main initialization from base type mtsIntuitiveResearchKitArm::Init(); // initialize trajectory data JointTrajectory.Velocity.SetAll(90.0 * cmnPI_180); // degrees per second JointTrajectory.Velocity.Element(2) = 0.2; // m per second JointTrajectory.Acceleration.SetAll(90.0 * cmnPI_180); JointTrajectory.Acceleration.Element(2) = 0.2; // m per second JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees // high values for engage adapter/tool until these use a proper trajectory generator PotsToEncodersTolerance.SetAll(100.0 * cmnPI_180); // 100 degrees for rotations PotsToEncodersTolerance.Element(2) = 20.0 * cmn_mm; // 20 mm // for tool/adapter engage procedure EngagingJointSet.SetSize(NumberOfJoints()); mtsInterfaceRequired * interfaceRequired; // Main interface should have been created by base class init CMN_ASSERT(RobotInterface); RobotInterface->AddEventWrite(ClutchEvents.ManipClutch, "ManipClutch", prmEventButton()); RobotInterface->AddEventWrite(ClutchEvents.SUJClutch, "SUJClutch", prmEventButton()); // Event Adapter engage: digital input button event from PSM interfaceRequired = AddInterfaceRequired("Adapter"); if (interfaceRequired) { Adapter.IsPresent = false; interfaceRequired->AddFunction("GetButton", Adapter.GetButton); interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerAdapter, this, "Button"); } // Event Tool engage: digital input button event from PSM interfaceRequired = AddInterfaceRequired("Tool"); if (interfaceRequired) { Tool.IsPresent = false; interfaceRequired->AddFunction("GetButton", Tool.GetButton); interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerTool, this, "Button"); } // ManipClutch: digital input button event from PSM interfaceRequired = AddInterfaceRequired("ManipClutch"); if (interfaceRequired) { interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerManipClutch, this, "Button"); } // Main interface should have been created by base class init CMN_ASSERT(RobotInterface); RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitPSM::SetJawPosition, this, "SetJawPosition"); // Initialize the optimizer Optimizer = new mtsIntuitiveResearchKitOptimizer(6); Optimizer->InitializeFollowVF(6, "FollowVFSlave", "CurrentSlaveKinematics", "DesiredSlaveKinematics"); }
cdgTypedef::cdgTypedef(size_t lineNumber): cdgScope("typedef", lineNumber) { cdgField * field; field = this->AddField("name", "", true, "name of the new type defined"); CMN_ASSERT(field); field = this->AddField("type", "", true, "C/C++ type used to define the new type"); CMN_ASSERT(field); this->AddKnownScope(*this); }
cdgEnum::cdgEnum(size_t lineNumber): cdgScope("enum", lineNumber) { cdgField * field; field = this->AddField("name", "", true, "name of the enum, will be added to the scope"); CMN_ASSERT(field); field = this->AddField("description", "", false, "description of the enum"); CMN_ASSERT(field); this->AddKnownScope(*this); cdgEnumValue newEnumValue(0); this->AddSubScope(newEnumValue); }
void mtsIntuitiveResearchKitECM::Init(void) { // main initialization from base type mtsIntuitiveResearchKitArm::Init(); // initialize trajectory data JointTrajectory.Velocity.Assign(30.0 * cmnPI_180, // degrees per second 30.0 * cmnPI_180, 0.05, // m per second 20.0 * cmnPI_180); JointTrajectory.Acceleration.Assign(30.0 * cmnPI_180, 30.0 * cmnPI_180, 0.05, 30.0 * cmnPI_180); JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees PotsToEncodersTolerance.SetAll(10.0 * cmnPI_180); // 10 degrees for rotations PotsToEncodersTolerance.Element(2) = 20.0 * cmn_mm; // 20 mm mtsInterfaceRequired * interfaceRequired; // Main interface should have been created by base class init CMN_ASSERT(RobotInterface); RobotInterface->AddEventWrite(ClutchEvents.ManipClutch, "ManipClutch", prmEventButton()); // ManipClutch: digital input button event from ECM interfaceRequired = AddInterfaceRequired("ManipClutch"); if (interfaceRequired) { interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitECM::EventHandlerManipClutch, this, "Button"); } }
void mtsIntuitiveResearchKitMTM::Init(void) { mtsIntuitiveResearchKitArm::Init(); RobotType = MTM_NULL; SetMTMType(); // initialize gripper state GripperClosed = false; JointTrajectory.Velocity.SetAll(720.0 * cmnPI_180); // degrees per second JointTrajectory.Acceleration.SetAll(720.0 * cmnPI_180); JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees // IO level treats the gripper as joint :-) PotsToEncodersTolerance.SetAll(10.0 * cmnPI_180); // 10 degrees for rotations // pots on gripper rotation are not directly mapped to encoders PotsToEncodersTolerance.Element(6) = cmnTypeTraits<double>::PlusInfinityOrMax(); // last joint is gripper, encoders can be anything PotsToEncodersTolerance.Element(7) = cmnTypeTraits<double>::PlusInfinityOrMax(); this->StateTable.AddData(GripperPosition, "GripperAngle"); // Main interface should have been created by base class init CMN_ASSERT(RobotInterface); RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitMTM::SetWrench, this, "SetWrench"); // Gripper RobotInterface->AddCommandReadState(this->StateTable, GripperPosition, "GetGripperPosition"); RobotInterface->AddEventVoid(GripperEvents.GripperPinch, "GripperPinchEvent"); RobotInterface->AddEventWrite(GripperEvents.GripperClosed, "GripperClosedEvent", true); }
int32_t COutputHandleTask::add_io_obj(const io_obj_ptr_t &pIoObj) { CMN_ASSERT(NULL != m_pIoEvtNotifier); pIoObj->set_task_id(EIT_OUTPUT_TYPE, m_i32TaskId); return m_pIoEvtNotifier->add_ioobj(pIoObj); }
bool mtsMulticastCommandWriteBase::AddCommand(BaseType * command) { if (command) { VectorType::iterator it = std::find(Commands.begin(), Commands.end(), command); if (it != Commands.end()) { CMN_LOG_INIT_WARNING << "Class mtsMulticastCommandWriteBase: AddCommand: command " << command->GetName() << " already added" << std::endl; return false; } // check if the command already has an argument prototype if (command->GetArgumentPrototype()) { CMN_ASSERT(this->GetArgumentPrototype()); if (command->GetArgumentPrototype()->Services() != this->GetArgumentPrototype()->Services()) { CMN_LOG_INIT_ERROR << "Class mtsMulticastCommandWriteBase: AddCommand: command argument types don't match, this multicast command uses " << this->GetArgumentPrototype()->Services()->GetName() << " but the command added (event handler potentially) uses " << command->GetArgumentPrototype()->Services()->GetName() << std::endl; return false; } else { // copy the multicast command prototype to each added command using in place new this->GetArgumentPrototype()->Services()->Create(const_cast<mtsGenericObject *>(command->GetArgumentPrototype()), *(this->GetArgumentPrototype())); // Add the command to the list this->Commands.push_back(command); return true; } } else { // create a new object command->SetArgumentPrototype(reinterpret_cast<const mtsGenericObject *>(this->GetArgumentPrototype()->Services()->Create(*(this->GetArgumentPrototype())))); // Add the command to the list this->Commands.push_back(command); return true; } } return false; }
inline bool CreateVTKObjects(void) { this->Source = vtkSphereSource::New(); CMN_ASSERT(this->Source); this->Source->SetRadius(1.0); this->Mapper = vtkPolyDataMapper::New(); CMN_ASSERT(this->Mapper); this->Mapper->SetInputConnection(this->Source->GetOutputPort()); this->Mapper->ImmediateModeRenderingOn(); this->Actor = vtkActor::New(); CMN_ASSERT(this->Actor); this->Actor->SetMapper(this->Mapper); this->AddPart(this->Actor); return true; }
RegistrationBehavior::RegistrationBehavior(const std::string & name): ui3BehaviorBase(std::string("RegistrationBehavior::") + name, 0), Widget3D(0), VisibleObject1(0), ModelFiducials(0) { this->Widget3D = new ui3Widget3D("RegistrationBehavior"); CMN_ASSERT(this->Widget3D); AddWidget3D(this->Widget3D); this->VisibleObject1 = new RegistrationModel(); CMN_ASSERT(this->VisibleObject1); this->Widget3D->Add(this->VisibleObject1); this->ModelFiducials = new ui3VisibleList("RegistrationModelFiducials"); CMN_ASSERT(this->ModelFiducials); this->Widget3D->Add(this->ModelFiducials); }
int32_t CLiteFile::get(cmn_byte_t *pBuf, u_int32_t ui32Size) { CMN_ASSERT(0 <= m_i32FileDesc /*&& SSIZE_MAX >= ui32Size*/); ///On success, the number of bytes read is returned (zero indicates end of file), and the file position is advanced by this number. It is not an ///error if this number is smaller than the number of bytes requested; this may happen for example because fewer bytes are actually available right ///now (maybe because we were close to end-of-file, or because we are reading from a pipe, or from a terminal), or because read() was interrupted by a signal return read(m_i32FileDesc, static_cast<void*>(pBuf), ui32Size); }
int32_t CLiteFile::open(const cmn_string_t &strFullName, int32_t i32Mode) { CMN_ASSERT(0 > m_i32FileDesc); if ("" == strFullName) { return RET_ERR; } return (m_i32FileDesc = ::open(strFullName.c_str(), O_RDWR|i32Mode)) < 0 ? -1 : RET_SUC; }
void CInputHandleTask::exec() { CMN_ASSERT(NULL != m_pIoEvtNotifier); ///main logic circle while (!is_stopped()) { /// if (m_pIoEvtNotifier->exec() < 0) { break; } } }
inline bool CreateVTKObjects(void) { std::cout << "Marker set up: "<< endl; mCylinder = vtkCylinderSource::New(); CMN_ASSERT(mCylinder); mCylinder->SetHeight( 4 ); mCylinder->SetRadius( 1 ); mCylinder->SetResolution( 25 ); markerMapper = vtkPolyDataMapper::New(); CMN_ASSERT(markerMapper); markerMapper->SetInputConnection( mCylinder->GetOutputPort() ); markerMapper->ImmediateModeRenderingOn(); marker = vtkActor::New(); CMN_ASSERT(marker); marker->SetMapper( markerMapper); marker->RotateX(90); this->AddPart(this->marker); return true; }
cdgClass::cdgClass(size_t lineNumber): cdgScope("class", lineNumber) { CMN_ASSERT(this->AddField("name", "", true, "name of the generated C++ class")); CMN_ASSERT(this->AddField("attribute", "", false, "string placed between 'class' and the class name (e.g. CISST_EXPORT)")); cdgField * field; field = this->AddField("ctor-all-members", "false", false, "adds a constructor requiring an initial value for each member"); CMN_ASSERT(field); field->AddPossibleValue("true"); field->AddPossibleValue("false"); field = this->AddField("virtual-dtor", "false", false, "make the destructor virtual"); CMN_ASSERT(field); field->AddPossibleValue("true"); field->AddPossibleValue("false"); field = this->AddField("generate-human-readable", "true", false, "generate the code for std::string _type.HumanReadable(void), set this to false to provide own implementation"); CMN_ASSERT(field); field->AddPossibleValue("true"); field->AddPossibleValue("false"); CMN_ASSERT(this->AddField("namespace", "", false, "namespace for the class")); field = this->AddField("mts-proxy", "true", false, "generate the code to create a cisstMultiTask proxy, set this to false to avoid proxy generation or \"declaration-only\" to manually instantiate the proxy in a different source file (.cpp)"); CMN_ASSERT(field); field->AddPossibleValue("true"); field->AddPossibleValue("declaration-only"); field->AddPossibleValue("false"); this->AddKnownScope(*this); cdgBaseClass newBaseClass(0); this->AddSubScope(newBaseClass); cdgTypedef newTypedef(0); this->AddSubScope(newTypedef); cdgMember newMember(0); this->AddSubScope(newMember); cdgEnum newEnum(0); this->AddSubScope(newEnum); cdgInline newInline(0, cdgInline::CDG_INLINE_HEADER); this->AddSubScope(newInline); cdgInline newCode(0, cdgInline::CDG_INLINE_CODE); this->AddSubScope(newCode); }
bool mtsStateTable::Write(mtsStateDataId id, const mtsGenericObject & object) { bool result; CMN_ASSERT(id != -1); if (id == -1) { CMN_LOG_CLASS_INIT_ERROR << "Write: object must be created using NewElement " << std::endl; return false; } if (!StateVector[id]) { CMN_LOG_CLASS_INIT_ERROR << "Write: no state data array corresponding to given id: " << id << std::endl; return false; } result = StateVector[id]->Set(IndexWriter, object); if (!result) { CMN_LOG_CLASS_INIT_ERROR << "Write: error setting data array value in id: " << id << std::endl; } return result; }
bool ui3VisibleAxes::CreateVTKObjects(void) { this->AxesActor = vtkAxesActor::New(); CMN_ASSERT(this->AxesActor); this->AxesActor->SetShaftTypeToCylinder(); this->AddPart(this->AxesActor); this->SetSize(10.0); this->ShowLabels(); this->Matrix->SetElement(0, 3, 0.0); this->Matrix->SetElement(1, 3, 0.0); this->Matrix->SetElement(2, 3, -200.0); return true; }
mtsExecutionResult mtsFunctionVoidReturn::ExecuteGeneric(mtsGenericObject & result) const { if (!Command) return mtsExecutionResult::FUNCTION_NOT_BOUND; #if CISST_MTS_HAS_ICE mtsExecutionResult executionResult = Command->Execute(result); if (executionResult.GetResult() == mtsExecutionResult::COMMAND_QUEUED && !this->IsProxy) { this->ThreadSignalWait(); executionResult = mtsExecutionResult::COMMAND_SUCCEEDED; } #else // If Command is valid (not NULL), then CompletionCommand should also be valid CMN_ASSERT(CompletionCommand); mtsExecutionResult executionResult = Command->Execute(result, CompletionCommand->GetCommand()); if (executionResult.GetResult() == mtsExecutionResult::COMMAND_QUEUED) executionResult = WaitForResult(result); #endif return executionResult; }
osaOSGMono::osaOSGMono( osaOSGWorld* world, int x, int y, int width, int height, double fovy, double aspectRatio, double zNear, double zFar, bool trackball, const vctFrame4x4<double>& Rtoffset, bool quadbufferstereo, bool offscreenrendering ) : osaOSGCamera( world, trackball, Rtoffset, offscreenrendering ), x( x ), // x position y( y ), // y position width( width ), // width of images height( height ), quadbufferstereo( quadbufferstereo ){ // Set the intrinsic paramters getCamera()->setProjectionMatrixAsPerspective(fovy, aspectRatio, zNear, zFar); // Create the view port first since the FinalDrawCallback needs it and we need // to create the final callback in the constructor to initialize the SVL stuff // right away getCamera()->setViewport( new osg::Viewport( x, y, width, height ) ); // Setup the OpenCV stuff #ifdef SAW_OPENSCENEGRAPH_SUPPORTS_OPENCV // Create a drawing callback. This callback is set to capture depth+color // buffer (true, true) osg::ref_ptr<osaOSGCamera::FinalDrawCallback> finaldrawcallback; try{ finaldrawcallback = new FinalDrawCallback( getCamera() ); } catch( std::bad_alloc& ){ CMN_LOG_RUN_ERROR << "Failed to allocate FinalDrawCallback" << std::endl; } CMN_ASSERT( finaldrawcallback ); getCamera()->setFinalDrawCallback( finaldrawcallback ); #endif // SAW_OPENSCENEGRAPH_SUPPORTS_OPENCV }
void CRupEndpointTester::handle_pkg(nm_pkg::CPkgTest &pkg, u_int32_t uiTag) { //std::cout<<"recv: "<<pkg.i32<<std::endl; static int32_t i = 0; CMN_ASSERT(i++ == pkg.i32); if (i % 10000 == 0) { std::cout<<i<<std::endl; } // nm_pkg::CArchive<nm_pkg::CPkgHdr, nm_pkg::CPkgTest> ar; // ar.get_next_body()->i32 = pkg.i32 + 1; // send_data(ar.serialize()); // // static u_int64_t ui64 = 0; // if (((++ui64) % 100000) == 0) // { // std::cout<< "recved : " << ui64 << pkg.get_id() << std::endl; // } //send_data(); //std::cout<< "handle_pkg reg pkg: " << pkg.get_opcode() << ", id: "<< pkg.get_id()<< std::endl; }
int32_t COutputHandleTask::del_io_obj(const io_obj_ptr_t &pioobj) { CMN_ASSERT(NULL != m_pIoEvtNotifier); return m_pIoEvtNotifier->del_ioobj(pioobj); }
inline mtsExecutionResult Execute(void) { CMN_ASSERT(this->PlaceHolder); return this->Function(*(this->PlaceHolder)); }
int32_t CLiteFile::set(cmn_byte_t *pBytes, u_int32_t ui32Len) { CMN_ASSERT(0 <= m_i32FileDesc); return write(m_i32FileDesc, static_cast<void*>(pBytes), ui32Len); }
osaOSGStereo::osaOSGStereo( osaOSGWorld* world, int x, int y, int width, int height, double fovy, double aspectRatio, double zNear, double zFar, double baseline, bool trackball, const vctFrame4x4<double>& Rtoffset ) : osaOSGCamera( world, trackball, Rtoffset ), x( x ), // x position y( y ), // y position width( width ), // width of images height( height ), baseline( baseline ){ double K00 = 533.401729541061854; double K11 = 532.351481757753163; double K02 = 327.972320586762351/1000.0; double K12 = 247.901576976705911/1000.0; double x0 = 0; double y0 = 0; osg::Matrixd K; K( 0, 0 ) = 2*K00/width; K( 1, 0 ) = 0.0; K( 2, 0 ) = (2*K02 + 2*x0)/width - 1.0; K( 3, 0 ) = 0; K( 0, 1 ) = 0.0; K( 1, 1 ) = 2*K11/height; K( 2, 1 ) = (2*K12 + 2*y0)/height -1.0; K( 3, 1 ) = 0; K( 0, 2 ) = 0.0; K( 1, 2 ) = 0.0; K( 2, 2 ) = -(zFar + zNear)/(zFar - zNear); K( 3, 2 ) = -2*zFar*zNear/(zFar - zNear); K( 0, 3 ) = 0.0; K( 1, 3 ) = 0.0; K( 2, 3 ) = -1; K( 3, 3 ) = 0; // Set the intrinsic paramters getCamera()->setProjectionMatrixAsPerspective(fovy,aspectRatio,zNear,zFar); K = getCamera()->getProjectionMatrix( ); // Setup the left camera { // Create a new (slave) camera osg::ref_ptr<osg::Camera> camera = new osg::Camera; // Create the view port. Again, the reason why the viewport is created // here is because the SVL stuff in the final draw callback needs to be // created during the constructor camera->setViewport( new osg::Viewport( 0, 0, width, height) ); // add this slave camera to the viewer, with a shift left of the // projection matrix addSlave( camera.get(), osg::Matrixd(), osg::Matrixd() ); // Only do drawing callback if OpenCV is enabled #ifdef SAW_OPENSCENEGRAPH_SUPPORTS_OPENCV // Create a drawing callback. This callback is set to capture // depth+color buffer (true, true) osg::ref_ptr<osaOSGCamera::FinalDrawCallback> finaldrawcallback; try{ finaldrawcallback = new FinalDrawCallback( camera ); } catch( std::bad_alloc& ){ CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS << "Failed to allocate FinalDrawCallback." << std::endl; } CMN_ASSERT( finaldrawcallback ); camera->setFinalDrawCallback( finaldrawcallback ); #endif //SAW_OPENSCENEGRAPH_SUPPORTS_OPENCV } // setup the right camera { // Create a new (slave) camera osg::ref_ptr<osg::Camera> camera = new osg::Camera; // Create the view port. Again, the reason why the viewport is created // here is because the SVL stuff in the final draw callback needs to be // created during the constructor camera->setViewport( new osg::Viewport( 0, 0, width, height) ); // add this slave camera to the viewer, with a shift right of the // projection matrix addSlave( camera.get(), osg::Matrixd(), osg::Matrixd() ); // Only do drawing callback if OpenCV is enabled #ifdef SAW_OPENSCENEGRAPH_SUPPORTS_OPENCV // Create a drawing callback. This callback is set to capture // depth+color buffer (true, true) osg::ref_ptr<osaOSGCamera::FinalDrawCallback> finaldrawcallback; try{ finaldrawcallback = new FinalDrawCallback( camera ); } catch( std::bad_alloc& ){ CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS << "Failed to allocate FinalDrawCallback." << std::endl; } CMN_ASSERT( finaldrawcallback ); camera->setFinalDrawCallback( finaldrawcallback ); #endif // SAW_OPENSCENEGRAPH_SUPPORTS_OPENCV } }
int32_t CLiteFile::flush() { CMN_ASSERT(0 <= m_i32FileDesc); return fsync(m_i32FileDesc); }