/* * Methods corresponding to IDL attributes and operations */ void PathServiceSVC_impl::get_path(RTC::Path2D_out path, const RTC::Pose2D& position, const RTC::Pose2D& target) { CSerializablePtr serializable_ptr; // Convert the simple map from string format to serializable object StringToObject(rtc_ptr_->get_map(), serializable_ptr); // The path std::deque<TPoint2D> deque_path; COccupancyGridMap2D grid_map; // Convert serializable simple map object to grid map grid_map.loadFromSimpleMap(*CSimpleMapPtr(serializable_ptr)); CPathPlanningCircularRobot path_planning; path_planning.robotRadius = 0.17f; bool not_found; // Find the path path_planning.computePath(grid_map, CPose2D(position.position.x, position.position.y, position.heading), CPose2D(target.position.x, target.position.y, target.heading), deque_path, not_found, 1000.0f); #ifdef DEBUG CImage img; grid_map.getAsImage(img, false, true); int R = round(path_planning.robotRadius / grid_map.getResolution() ); for (std::deque<TPoint2D>::const_iterator it=deque_path.begin();it!=deque_path.end();++it) img.drawCircle( grid_map.x2idx(it->x),grid_map.getSizeY()-1-grid_map.y2idx(it->y),R, TColor(0,0,255) ); img.cross(grid_map.x2idx(position.position.x),grid_map.getSizeY()-1-grid_map.y2idx(position.position.y),TColor(0x20,0x20,0x20),'+',10); img.cross(grid_map.x2idx(target.position.x),grid_map.getSizeY()-1-grid_map.y2idx(target.position.y),TColor(0x50,0x50,0x50),'x',10); const std::string dest = "path_planning.png"; img.saveToFile(dest); #endif path = new RTC::Path2D(); if(!not_found) { // if found any path // Copy path from deque_path to path path->waypoints.length(deque_path.size()); int i = 0; std::deque<TPoint2D>::const_iterator it = deque_path.begin(); for(;it != deque_path.end(); ++it, i++) { path->waypoints[i].target.position.x = it->x; path->waypoints[i].target.position.y = it->y; path->waypoints[i].target.heading = 0.0f; } } else { // if didn't find the path path->waypoints.length(0); cout<<"Not Found"<<endl; } }
//------------------------------------------------------------------------------------- HRESULT CDXUTEffectMap::SetStandardParameter( const WCHAR* strSemantic, const WCHAR* strObject, DWORD dwObjectIndex, float* pData, DWORD dwDataLen, const WCHAR* strType, const WCHAR* strUnits, const WCHAR* strSpace ) { // Map the semantic to the standard set DXUT_SEMANTIC eSemantic = StringToSemantic( strSemantic ); if( eSemantic == DXUT_UNKNOWN_SEMANTIC ) return E_FAIL; // Map the object to the standard set DXUT_OBJECT eObject = StringToObject( strObject ); if( eObject == DXUT_UNKNOWN_OBJECT ) return E_FAIL; return SetStandardParameter( eSemantic, eObject, dwObjectIndex, pData, dwDataLen, strType, strUnits, strSpace ); }
//------------------------------------------------------------------------------------- DXUT_OBJECT CDXUTEffectMap::StringToObject( const WCHAR* strObject ) { char cstr[MAX_PATH+1] = {0}; WideCharToMultiByte( CP_ACP, 0, strObject, -1, cstr, MAX_PATH, NULL, NULL ); return StringToObject( cstr ); }
//------------------------------------------------------------------------------------- // Investigates all the parameters, looking at semantics and annotations and placing // handles to these parameters within the internal database. //------------------------------------------------------------------------------------- HRESULT CDXUTEffectMap::AddEffect( ID3DXEffect* pEffect ) { HRESULT hr; if( pEffect == NULL ) return E_INVALIDARG; // Get the number of parameters D3DXEFFECT_DESC descEffect; V_RETURN( pEffect->GetDesc( &descEffect ) ); // Enumerate the parameters for( UINT iParam=0; iParam < descEffect.Parameters; iParam++ ) { // Retrieve param D3DXHANDLE hParameter = pEffect->GetParameter( NULL, iParam ); if( NULL == hParameter ) return E_FAIL; // Grab description D3DXPARAMETER_DESC desc; V_RETURN( pEffect->GetParameterDesc( hParameter, &desc ) ); // If this parameter doesn't have a semantic, skip to the next parameter if( desc.Semantic == NULL ) continue; // Map the semantic to the standard set DXUT_SEMANTIC eSemantic = StringToSemantic( desc.Semantic ); if( eSemantic == DXUT_UNKNOWN_SEMANTIC ) continue; // Get the object annotation const char* cstrObject = "Geometry"; D3DXHANDLE hAnnotation = pEffect->GetAnnotationByName( hParameter, "Object" ); if( hAnnotation ) { V_RETURN( pEffect->GetString( hAnnotation, &cstrObject ) ); } // Map the object to the standard set DXUT_OBJECT eObject = StringToObject( cstrObject ); if( eObject == DXUT_UNKNOWN_OBJECT ) continue; // Extract the index from the semantic int index = 0; const char* strIndex = desc.Semantic + strlen(desc.Semantic)-1; // If there is a digit at the end of the semantic, locate the beginning of the index // and convert to an integer if( isdigit( (BYTE) (*strIndex) ) ) { while( isdigit( (BYTE) (*(strIndex-1)) ) ) { --strIndex; } index = atoi( strIndex ); } // Check whether index is out of bounds if( index < 0 || index >= MAX_INDEX ) continue; // Store the handle CGrowableArray<ParamList>* pBindings = &m_Bindings[ eSemantic ][ eObject ][ index ]; bool bBound = false; for( int i=0; i < pBindings->GetSize(); i++ ) { if( pBindings->GetAt(i).pEffect == pEffect ) { // Found the containing effect for this parameter in the list, add the new handle pBindings->GetAt(i).ahParameters.Add( hParameter ); bBound = true; break; } } if( !bBound ) { // This parameter belongs to a new effect ParamList newParamList; newParamList.pEffect = pEffect; pEffect->AddRef(); newParamList.ahParameters.Add( hParameter ); pBindings->Add( newParamList ); } } return S_OK; }