Beispiel #1
0
void CPTG_RobotShape_Polygonal::setRobotShape(
	const mrpt::math::CPolygon& robotShape)
{
	ASSERT_ABOVEEQ_(robotShape.size(), 3u);
	m_robotShape = robotShape;

	m_robotMaxRadius = .0;  // Default minimum
	for (const auto& v : m_robotShape)
		mrpt::utils::keep_max(m_robotMaxRadius, v.norm());

	internal_processNewRobotShape();
}
/*---------------------------------------------------------------
						loadFromFile
  ---------------------------------------------------------------*/
bool CParameterizedTrajectoryGenerator::CColisionGrid::loadFromFile( CStream *f, const mrpt::math::CPolygon & current_robotShape  )
{
	try
	{
		if (!f) return false;

		// Return false if the file contents doesn't match what we expected:
		uint32_t file_magic;
		*f >> file_magic;

		// It doesn't seem to be a valid file or was in an old format, just recompute the grid:
		if (COLGRID_FILE_MAGIC!=file_magic)
			return false;

		uint8_t serialized_version;
		*f >> serialized_version;

		switch (serialized_version)
		{
		case 2:
			{
				mrpt::math::CPolygon stored_shape;
				*f >> stored_shape;

				const bool shapes_match =
					( stored_shape.size()==current_robotShape.size() &&
					  std::equal(stored_shape.begin(),stored_shape.end(), current_robotShape.begin() ) );

				if (!shapes_match) return false; // Must recompute if the robot shape changed.
			}
			break;

		case 1:
		default:
			// Unknown version: Maybe we are loading a file from a more recent version of MRPT? Whatever, we can't read it: It's safer just to re-generate the PTG data
			return false;
		};

		// Standard PTG data:
		const std::string expected_desc = m_parent->getDescription();
		std::string desc;
		*f >> desc;
		if (desc!=expected_desc) return false;

		// and standard PTG data:
		float	  ff;
		uint16_t  nAlphaStored;
		*f >> nAlphaStored; if (nAlphaStored!=m_parent->getAlfaValuesCount()) return false;
		*f >> ff; if (ff!=m_parent->getMax_V()) return false;
		*f >> ff; if (ff!=m_parent->getMax_W()) return false;

		// Cell dimensions:
		*f >> ff; if(ff!=m_x_min) return false;
		*f >> ff; if(ff!=m_x_max) return false;
		*f >> ff; if(ff!=m_y_min) return false;
		*f >> ff; if(ff!=m_y_max) return false;
		*f >> ff; if(ff!=m_resolution) return false;

		// OK, all parameters seem to be exactly the same than when we precomputed the table: load it.
		//v1 was:  *f >> m_map;
		uint32_t N;
		*f >> N;
		 m_map.resize(N);
		for (uint32_t i=0;i<N;i++)
		{
			uint32_t M;
			*f >> M;
			 m_map[i].resize(M);
			for (uint32_t k=0;k<M;k++)
				*f >> m_map[i][k].first >> m_map[i][k].second;
		}

		return true;
	}
	catch(std::exception &e)
	{
		std::cerr << "[CColisionGrid::loadFromFile] " << e.what();
		return false;
	}
	catch(...)
	{
		return false;
	}
}