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