C++ CSerializablePtr类代码示例

void navlog_viewer_GUI_designDialog::loadLogfile(const std::string &filName)
{
	WX_START_TRY

	this->edLogFile->SetValue(_U(filName.c_str()));

	CFileInputStream f(filName);

	m_logdata.clear();

	wxBusyCursor busy;

	set<string>  validClasses;
	validClasses.insert("CLogFileRecord");

	m_log_first_tim = INVALID_TIMESTAMP;
	m_log_last_tim  = INVALID_TIMESTAMP;

	for (;;)
	{
		try
		{
			CSerializablePtr obj = f.ReadObject();
			if (validClasses.find(string(obj->GetRuntimeClass()->className))==validClasses.end())
			{
				wxMessageBox(_U(format("Unexpected class found: %s",obj->GetRuntimeClass()->className).c_str()),_("Error loading log:"));
				break;
			}
			m_logdata.push_back(obj);

			// generate time stats:
			if (IS_CLASS(obj,CLogFileRecord))
			{
                const CLogFileRecordPtr logptr = CLogFileRecordPtr(obj);
                if (logptr->timestamp!=INVALID_TIMESTAMP)
                    m_log_last_tim = logptr->timestamp;
			}

			if (m_log_first_tim == INVALID_TIMESTAMP && m_log_last_tim!=INVALID_TIMESTAMP)
                m_log_first_tim = m_log_last_tim;
		}
		catch (CExceptionEOF &)
		{
			break;
		}
		catch (std::exception &)
		{
			// EOF in the middle of an object... It may be usual if the logger is shut down not cleanly.
			break;
		}
	}

	// Update stats, etc...
	UpdateInfoFromLoadedLog();

	WX_END_TRY
}

/* -----------------------------------------------------------------------
	Used to pass CORBA-like object into a MRPT object.
		See doc about "Integration with BABEL".
   ----------------------------------------------------------------------- */
void utils::StringToObject(const std::string &str, CSerializablePtr &obj)
{
	MRPT_START

	obj.clear_unique();
	if (str.empty()) return;

	CMemoryStream	tmp;
	size_t			n;
	size_t			i,lastIdx;

	obj.clear_unique();

	n = str.size();

	// Scan the string to decode it:
	// ----------------------------------
	lastIdx = 0;
	const char *data = str.c_str();
	unsigned char c;
	for (i=0;i<n && (c=data[i])!=0;i++)
	{
		// Search for first "0x01" byte:
		if ( c == 0x01 )
		{
			// Copy all till now:
			tmp.WriteBuffer( &data[lastIdx], i - lastIdx + 1 );
			i+=1; // +1 from "for" loop
			lastIdx = i+1;

			// And decode:
			//   0x01 0x01 --> 0x01
			//   0x01 0x02 --> 0x00
			if (data[i]==0x01)
					((unsigned char*)tmp.getRawBufferData())[tmp.getTotalBytesCount()-1] = (unsigned char)0x01;
			else 	((unsigned char*)tmp.getRawBufferData())[tmp.getTotalBytesCount()-1] = (unsigned char)0x00;
		}
	} // end for i

	// Copy the rest:
	if ( (n-lastIdx) > 0)
		tmp.WriteBuffer( &data[lastIdx], n - lastIdx );

	// And the '\0' char:
	char dummy = '\0';
	tmp.WriteBuffer( &dummy, sizeof(char) );

	tmp.Seek(0,CStream::sFromBeginning);
	obj = tmp.ReadObject();

	MRPT_END

}

/*---------------------------------------------------------------
				deserializeIntoNewObject
 ---------------------------------------------------------------*/
void  CMessage::deserializeIntoNewObject( CSerializablePtr &obj )
{
	MRPT_START
	CMemoryStream		auxStream;

	// Copy data into the stream:
	if (!content.empty())
	{
		auxStream.WriteBuffer( &content[0], content.size() );
		auxStream.Seek(0);

		// Try to parse data into a new object:
		obj = auxStream.ReadObject( );
	}
	else obj.clear_unique();


	MRPT_END
}

/* -----------------------------------------------------------------------
				RawStringToObject
   ----------------------------------------------------------------------- */
void utils::RawStringToObject(const std::string & in_data, CSerializablePtr &obj)
{
	try
	{
		obj.clear_unique();

		if (in_data.empty()) return;

		CMemoryStream	tmp( &in_data[0], in_data.size());
		obj = tmp.ReadObject();
	}
	catch (std::bad_alloc &e)
	{
		throw e;
	}
	catch(std::exception &e)
	{
		std::cerr << "[OctetVectorToObject] Exception: " << e.what() << std::endl;
	}
	catch(...)
	{
		std::cerr << "[OctetVectorToObject] Unknown exception" << std::endl;
	}
}

void hmt_slam_guiFrame::updateLocalMapView()
{
    WX_START_TRY

	m_glLocalArea->m_openGLScene->clear();

	// Get the hypothesis ID:
	THypothesisID	hypID = (THypothesisID	)atoi( cbHypos->GetStringSelection().mb_str() );
	if ( m_hmtslam->m_LMHs.find(hypID)==m_hmtslam->m_LMHs.end() )
	{
		wxMessageBox(_U( format("No LMH has hypothesis ID %i!", (int)hypID).c_str() ), _("Error with topological hypotesis"));
		return;
	}

	// Get the selected area or LMH in the tree view:
	wxArrayTreeItemIds	lstSelect;
	size_t	nSel = treeView->GetSelections(lstSelect);
	if (!nSel) return;

	CItemData	*data1 = static_cast<CItemData*>( treeView->GetItemData( lstSelect.Item(0) ) );
	if (!data1) return;
	if (!data1->m_ptr) return;

	CSerializablePtr obj = data1->m_ptr;
	if (obj->GetRuntimeClass()==CLASS_ID(CHMHMapNode))
	{
		// The 3D view:
		opengl::CSetOfObjectsPtr objs = opengl::CSetOfObjects::Create();

		// -------------------------------------------
		// Draw a grid on the ground:
		// -------------------------------------------
		{
			opengl::CGridPlaneXYPtr obj = opengl::CGridPlaneXY::Create(-100,100,-100,100,0,5);
			obj->setColor(0.4,0.4,0.4);
			objs->insert(obj);  // it will free the memory
		}


		// Two passes: 1st draw the map on the ground, then the rest.
		for (int nRound=0;nRound<2;nRound++)
		{
			CHMHMapNodePtr firstArea;
			CPose3DPDFGaussian		refPoseThisArea;

			for (size_t  nSelItem = 0; nSelItem<nSel;nSelItem++)
			{
				CItemData	*data1 = static_cast<CItemData*>( treeView->GetItemData( lstSelect.Item(nSelItem) ) );
				if (!data1) continue;
				if (!data1->m_ptr) continue;

				CHMHMapNodePtr area= CHMHMapNodePtr(data1->m_ptr);
				if (!area) continue;

				// Is this the first rendered area??
				if ( !firstArea )
				{
					firstArea = area;
				}
				else
				{
					// Compute the translation btw. ref. and current area:
					CPose3DPDFParticles		pdf;

					m_hmtslam->m_map.computeCoordinatesTransformationBetweenNodes(
						firstArea->getID(),
						area->getID(),
						pdf,
						hypID,
						200 );
						/*0.15f,
						DEG2RAD(5.0f) );*/

					refPoseThisArea.copyFrom( pdf );
					cout << "Pose " << firstArea->getID() << " - " << area->getID() << refPoseThisArea << endl;
				}

				CMultiMetricMapPtr obj_mmap = area->m_annotations.getAs<CMultiMetricMap>( NODE_ANNOTATION_METRIC_MAPS, hypID, false );

				CRobotPosesGraphPtr obj_robposes = area->m_annotations.getAs<CRobotPosesGraph>( NODE_ANNOTATION_POSES_GRAPH, hypID, false );

				TPoseID	refPoseID;
				area->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, refPoseID, hypID, true);

				// ---------------------------------------------------------
				// The metric map:
				// ---------------------------------------------------------
				if (nRound==0)
				{
					opengl::CSetOfObjectsPtr objMap= opengl::CSetOfObjects::Create();
					obj_mmap->getAs3DObject(objMap);
					objMap->setPose( refPoseThisArea.mean );
					objs->insert(objMap);
				}

				if (nRound==1)
				{
					// ---------------------------------------------------------
					// Bounding boxes for grid maps:
					// ---------------------------------------------------------
//.........这里部分代码省略.........

/** This is the common function for all operations over a rawlog file ("filter" a rawlog file into a new one) or over the loaded rawlog (depending on the user selection in the GUI).
  */
void CFormChangeSensorPositions::executeOperationOnRawlog( TRawlogFilter operation, const char *endMsg )
{
    WX_START_TRY

    int   			processMax;
    bool			isInMemory;
    CStream 		*in_fil=NULL,*out_fil=NULL;


	sensorPoseReadOK = false;
	camReadIsOk = false;

    if (rbLoaded->GetValue())
    {
        // APPLY TO rawlog in memory:
        isInMemory = true;

        processMax = (int)rawlog.size();
    }
    else
    {
        // APPLY TO rawlog files:
        isInMemory = false;

        if ( !txtInputFile->GetValue().size() )
            THROW_EXCEPTION("An input rawlog file must be selected")
            if ( !txtOutputFile->GetValue().size() )
                THROW_EXCEPTION("An output rawlog file must be selected")

                string   fileName_IN( txtInputFile->GetValue().mbc_str() );
        if (!mrpt::system::fileExists(fileName_IN) )
            THROW_EXCEPTION("Input file does not exist!")

            string   fileName_OUT( txtOutputFile->GetValue().mbc_str() );

        if (!fileName_OUT.compare(fileName_IN))
            THROW_EXCEPTION("Input and output files must be different!")

		in_fil = new CFileGZInputStream(fileName_IN);
		out_fil = new CFileGZOutputStream(fileName_OUT);

        processMax = (int)in_fil->getTotalBytesCount();
    }

    wxProgressDialog    progDia(
        wxT("Modifying rawlog"),
        wxT("Processing..."),
        processMax, // range
        this, // parent
        wxPD_CAN_ABORT |
        wxPD_APP_MODAL |
        wxPD_SMOOTH |
        wxPD_AUTO_HIDE |
        wxPD_ELAPSED_TIME |
        wxPD_ESTIMATED_TIME |
        wxPD_REMAINING_TIME);

    wxTheApp->Yield();  // Let the app. process messages

    unsigned int		countLoop = 0;
    bool                keepLoading=true;
    string              errorMsg;
    wxString			auxStr;

    // Apply changes:
    int 	changes = 0;
    wxBusyCursor    cursor;

    while ((( !isInMemory && keepLoading ) ||
            (  isInMemory && countLoop < rawlog.size() ))&& !sensorPoseReadOK && !camReadIsOk )
    {
        CSerializablePtr newObj;
        try
        {
            if (isInMemory)
            {
                newObj = rawlog.getAsGeneric(countLoop);
            }
            else
            {
                (*in_fil) >> newObj;
            }

            // Check type:
            if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
            {
                // A sensory frame:
                CSensoryFramePtr sf(newObj);

                // Process & save:
				operation(NULL,sf.pointer(),changes );

				if (!isInMemory)  (*out_fil) << *sf.pointer();
            }
            else if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
            {
                // This is an action:
                CActionCollectionPtr acts =CActionCollectionPtr( newObj );
//.........这里部分代码省略.........

void CStream_WriteObject(CStream& self, CSerializablePtr& obj)
{
    self.WriteObject(obj.pointer());
}

// CStream
void CStream_ReadObject(CStream& self, CSerializablePtr& obj)
{
    self.ReadObject(obj.pointer());
}

void xRawLogViewerFrame::OnMenuConvertObservationOnly(wxCommandEvent& event)
{
	WX_START_TRY

	wxMessageBox( _("Select the rawlog file to convert...") );

	string 	str;
	if ( !AskForOpenRawlog( str ) ) return;

	wxMessageBox( _("Select the target file where to save the new rawlog.") );
	string 	filToSave;
	if ( !AskForSaveRawlog( filToSave ) ) return;

	wxBusyCursor        waitCursor;
	CFileGZInputStream	fil(str);
	unsigned int        filSize = (unsigned int)fil.getTotalBytesCount();

	CFileGZOutputStream	f_out(filToSave);

	wxString            auxStr;
	wxProgressDialog    progDia(
		wxT("Progress"),
		wxT("Parsing file..."),
		filSize, // range
		this, // parent
		wxPD_CAN_ABORT |
		wxPD_APP_MODAL |
		wxPD_SMOOTH |
		wxPD_AUTO_HIDE |
		wxPD_ELAPSED_TIME |
		wxPD_ESTIMATED_TIME |
		wxPD_REMAINING_TIME);

	wxTheApp->Yield();  // Let the app. process messages

	unsigned int        countLoop = 0;
	bool                keepLoading=true;
	string              errorMsg;

	CPose2D				odometry_accum;

	// We'll save here all the individual observations ordered in time:
	TListTimeAndObservations	time_ordered_list_observation;

	mrpt::system::TTimeStamp	lastValidObsTime = INVALID_TIMESTAMP;

	while (keepLoading)
	{
		if (countLoop++ % 5 == 0)
		{
			auxStr.sprintf(wxT("Parsing file... %u objects"),countLoop );
			if (!progDia.Update( (int)fil.getPosition(), auxStr ))
				keepLoading = false;
			wxTheApp->Yield();  // Let the app. process messages
		}

		try
		{
			CSerializablePtr newObj;
			fil >> newObj;

			// Check type:
			if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame) )
			{
				CSensoryFramePtr SF(newObj);
				for (CSensoryFrame::iterator it=SF->begin();it!=SF->end();++it)
				{
					time_ordered_list_observation.insert( TTimeObservationPair( (*it)->timestamp, (*it) ));
					lastValidObsTime = (*it)->timestamp;
				}
			}
			else
				if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection) )
				{
					// Replace "odometry action" with "odometry observation":
					CActionCollectionPtr	acts = CActionCollectionPtr( newObj );
					// Get odometry:
					CActionRobotMovement2DPtr actOdom = acts->getBestMovementEstimation();
					if (actOdom)
					{
						odometry_accum = odometry_accum + actOdom->poseChange->getMeanVal();

						// Generate "odometry obs":
						CObservationOdometryPtr  newO = CObservationOdometry::Create();
						newO->sensorLabel = "odometry";
						newO->timestamp   = actOdom->timestamp!=INVALID_TIMESTAMP ?  actOdom->timestamp : lastValidObsTime;
						newO->odometry    = odometry_accum;

						time_ordered_list_observation.insert( TTimeObservationPair( newO->timestamp, newO ));
					}
				}
				else
				if ( newObj->GetRuntimeClass()->derivedFrom( CLASS_ID(CObservation) ) )
				{
					CObservationPtr o = CObservationPtr( newObj );
					time_ordered_list_observation.insert( TTimeObservationPair( o->timestamp, o ));
				}


			// Dump to the new file: Only the oldest one:
//.........这里部分代码省略.........

void xRawLogViewerFrame::OnMenuLossLessDecFILE(wxCommandEvent& event)
{
	WX_START_TRY

	string 	filToOpen;
	if ( !AskForOpenRawlog( filToOpen ) ) return;

	wxString strDecimation = wxGetTextFromUser(
								 _("The number of observations will be decimated (only 1 out of M will be kept). Enter the decimation ratio M:"),
								 _("Decimation"),
								 _("1") );
	long	DECIMATE_RATIO;
	strDecimation.ToLong( &DECIMATE_RATIO );

	ASSERT_(DECIMATE_RATIO>=1)

	string filToSave;
	AskForSaveRawlog(filToSave);

	CFileGZInputStream	fil(filToOpen);
	CFileGZOutputStream	f_out(filToSave);

	wxBusyCursor        waitCursor;
	unsigned int        filSize = (unsigned int)fil.getTotalBytesCount();

	wxString            auxStr;
	wxProgressDialog    progDia(
		wxT("Progress"),
		wxT("Parsing file..."),
		filSize, // range
		this, // parent
		wxPD_CAN_ABORT |
		wxPD_APP_MODAL |
		wxPD_SMOOTH |
		wxPD_AUTO_HIDE |
		wxPD_ELAPSED_TIME |
		wxPD_ESTIMATED_TIME |
		wxPD_REMAINING_TIME);

	wxTheApp->Yield();  // Let the app. process messages

	unsigned int        countLoop = 0;
	int					entryIndex = 0;
	bool                keepLoading=true;
	string              errorMsg;

	// ------------------------------------------------------------------------------
	// METHOD TO BE MEMORY EFFICIENT:
	//  To free the memory of the current rawlog entries as we create the new one,
	//  then call "clearWithoutDelete" at the end.
	// ------------------------------------------------------------------------------
	CSensoryFramePtr		accum_sf;
	CActionRobotMovement2D::TMotionModelOptions	odometryOptions;
	bool					cummMovementInit = false;
	long 					SF_counter = 0;

	// Reset cummulative pose change:
	CPose2D		accumMovement(0,0,0);

	TTimeStamp	timestamp_lastAction = INVALID_TIMESTAMP;


	while (keepLoading)
	{
		if (countLoop++ % 100 == 0)
		{
			auxStr.sprintf(wxT("Parsing file... %u objects"),entryIndex );
			if (!progDia.Update( (int)fil.getPosition(), auxStr ))
				keepLoading = false;
			wxTheApp->Yield();  // Let the app. process messages
		}

		CSerializablePtr newObj;
		try
		{
			fil >> newObj;
			entryIndex++;

			// Check type:
			if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame) )
			{
				// Decimate Observations
				// ---------------------------

				// Add observations to the accum. SF:
				if (!accum_sf)
					accum_sf = CSensoryFrame::Create();

				// Copy pointers to observations only (fast):
				accum_sf->moveFrom( *CSensoryFramePtr(newObj) );

				if ( ++SF_counter >= DECIMATE_RATIO )
				{
					SF_counter = 0;

					// INSERT OBSERVATIONS:
					f_out << *accum_sf;
					accum_sf.clear_unique();

					// INSERT ACTIONS:
//.........这里部分代码省略.........