TEST(sensor_msgs, PointCloud2Iterator)
{
// Create a dummy PointCloud2
size_t n_points = 4;
sensor_msgs::msg::PointCloud2 cloud_msg_1, cloud_msg_2;
cloud_msg_1.height = static_cast<uint32_t>(n_points);
cloud_msg_1.width = 1;
sensor_msgs::PointCloud2Modifier modifier(cloud_msg_1);
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
cloud_msg_2 = cloud_msg_1;
// Fill 1 by hand
float point_data_raw[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0};
std::vector<float> point_data(point_data_raw, point_data_raw + 3 * n_points);
// colors in RGB order
uint8_t color_data_raw[] = {40, 80, 120, 160, 200, 240, 20, 40, 60, 80, 100, 120};
std::vector<uint8_t> color_data(color_data_raw, color_data_raw + 3 * n_points);
float * data = reinterpret_cast<float *>(&cloud_msg_1.data.front());
for (size_t n = 0, i = 0; n < n_points; ++n) {
for (; i < 3 * (n + 1); ++i) {
*(data++) = point_data[i];
}
// Add an extra float of padding
++data;
uint8_t * bgr = reinterpret_cast<uint8_t *>(data++);
// add the colors in order BGRA like PCL
size_t j_max = 2;
for (size_t j = 0; j <= j_max; ++j) {
*(bgr++) = color_data[3 * n + (j_max - j)];
}
// Add 3 extra floats of padding
data += 3;
}
// Fill 2 using an iterator
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg_2, "x");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(cloud_msg_2, "r");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(cloud_msg_2, "g");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(cloud_msg_2, "b");
for (size_t i = 0; i < n_points; ++i, ++iter_x, ++iter_r, ++iter_g, ++iter_b) {
for (size_t j = 0; j < 3; ++j) {
iter_x[j] = point_data[j + 3 * i];
}
*iter_r = color_data[3 * i];
*iter_g = color_data[3 * i + 1];
*iter_b = color_data[3 * i + 2];
}
// Check the values using an iterator
sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_x(cloud_msg_1, "x"), iter_const_2_x(
cloud_msg_2, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_y(cloud_msg_1, "y"), iter_const_2_y(
cloud_msg_2, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_z(cloud_msg_1, "z"), iter_const_2_z(
cloud_msg_2, "z");
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_r(cloud_msg_1, "r"), iter_const_2_r(
cloud_msg_2, "r");
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_g(cloud_msg_1, "g"), iter_const_2_g(
cloud_msg_2, "g");
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_b(cloud_msg_1, "b"), iter_const_2_b(
cloud_msg_2, "b");
size_t i = 0;
for (; iter_const_1_x != iter_const_1_x.end();
++i, ++iter_const_1_x, ++iter_const_2_x, ++iter_const_1_y, ++iter_const_2_y,
++iter_const_1_z, ++iter_const_2_z, ++iter_const_1_r, ++iter_const_1_g, ++iter_const_1_b)
{
EXPECT_EQ(*iter_const_1_x, *iter_const_2_x);
EXPECT_EQ(*iter_const_1_x, point_data[0 + 3 * i]);
EXPECT_EQ(*iter_const_1_y, *iter_const_2_y);
EXPECT_EQ(*iter_const_1_y, point_data[1 + 3 * i]);
EXPECT_EQ(*iter_const_1_z, *iter_const_2_z);
EXPECT_EQ(*iter_const_1_z, point_data[2 + 3 * i]);
EXPECT_EQ(*iter_const_1_r, *iter_const_2_r);
EXPECT_EQ(*iter_const_1_r, color_data[3 * i + 0]);
EXPECT_EQ(*iter_const_1_g, *iter_const_2_g);
EXPECT_EQ(*iter_const_1_g, color_data[3 * i + 1]);
EXPECT_EQ(*iter_const_1_b, *iter_const_2_b);
EXPECT_EQ(*iter_const_1_b, color_data[3 * i + 2]);
// This is to test the different operators
++iter_const_2_r;
iter_const_2_g += 1;
iter_const_2_b = iter_const_2_b + 1;
}
EXPECT_EQ(i, n_points);
}
/** fill the point cloud with the 3D points */
void LinemodPointcloud::fill(const std::vector<cv::Vec3f> & pts, const cv::Vec3b &color)
{
int size_old = modifier->size();
modifier->resize(size_old + pts.size());
sensor_msgs::PointCloud2Iterator<float> iter_x(pc_msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(pc_msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(pc_msg, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(pc_msg, "r");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(pc_msg, "g");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(pc_msg, "b");
iter_x += size_old;
iter_y += size_old;
iter_z += size_old;
iter_r += size_old;
iter_g += size_old;
iter_b += size_old;
std::vector<cv::Vec3f>::const_iterator it_data = pts.begin();
for(; it_data != pts.end(); ++it_data, ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b)
{
*iter_x = (*it_data)(0);
*iter_y = (*it_data)(1);
*iter_z = (*it_data)(2);
*iter_r = color(0);
*iter_g = color(1);
*iter_b = color(2);
}
}
// ////////////////////////////////////////////////////////////////////////////
void SocketSpecLocal::ParseSocketSpecList(const std::string &spec_string,
SocketSpecLocalList &out_datum)
{
std::vector<std::string> tmp_list;
MLB::Utility::SplitString(spec_string, ",", tmp_list, 0, false);
SocketSpecLocalList tmp_datum;
std::set<SocketSpecLocal> mc_spec_set;
std::vector<std::string>::const_iterator iter_b(tmp_list.begin());
std::vector<std::string>::const_iterator iter_e(tmp_list.end());
try {
for ( ; iter_b != iter_e; ++iter_b) {
SocketSpecLocal tmp_spec;
tmp_spec.FromString(*iter_b);
if (mc_spec_set.find(tmp_spec) != mc_spec_set.end())
MLB::Utility::ThrowInvalidArgument("The local socket "
"specification '" + (*iter_b) + "' occurs multiple times.");
tmp_datum.push_back(tmp_spec);
mc_spec_set.insert(tmp_spec);
}
}
catch (const std::exception &except) {
MLB::Utility::Rethrow(except, "Failed to parse the list of local socket "
"specificatons: " + std::string(except.what()));
}
out_datum.swap(tmp_datum);
}
// ////////////////////////////////////////////////////////////////////////////
bool DetermineFileInfo(const std::string &file_name,
const std::string &file_info_name, WORD lang_code, WORD code_page,
std::string &file_info_value, bool throw_if_not_found)
{
VS_FIXEDFILEINFO version_info;
OS_VersionInfoSet string_map;
OS_GetFileVersionInfo(file_name, lang_code, code_page, version_info,
string_map);
OS_VersionInfoSetIter iter_b(string_map.begin());
OS_VersionInfoSetIter iter_e(string_map.end());
for ( ; iter_b != iter_e; ++iter_b) {
if (((!lang_code) || (lang_code == iter_b->lang_code_)) &&
((!code_page) || (code_page == iter_b->code_page_)) &&
(!stricmp(file_info_name.c_str(), iter_b->info_name_.c_str()))) {
file_info_value.swap(
const_cast<OS_VersionInfoKey *>(&(*iter_b))->info_value_);
return(true);
}
}
if (throw_if_not_found)
MLB::Utility::ThrowException("Unable to locate file information name '" +
file_info_name + "' in the file '" + file_name + "'.");
return(false);
}
// ////////////////////////////////////////////////////////////////////////////
MLB::Utility::StringVector GetFileInfoStringList(const std::string &file_name,
WORD lang_code, WORD code_page)
{
VS_FIXEDFILEINFO version_info;
OS_VersionInfoSet string_map;
OS_GetFileVersionInfo(file_name, lang_code, code_page, version_info,
string_map);
MLB::Utility::StringVector out_list;
OS_VersionInfoSetIterC iter_b(string_map.begin());
OS_VersionInfoSetIterC iter_e(string_map.end());
out_list.reserve(string_map.size());
for ( ; iter_b != iter_e; ++iter_b) {
std::ostringstream o_str;
o_str
<< std::hex << std::setfill('0')
<< "0x" << std::setw(sizeof(iter_b->lang_code_) * 2)
<< iter_b->lang_code_
<< " "
<< "0x" << std::setw(sizeof(iter_b->code_page_) * 2)
<< iter_b->code_page_
<< " "
<< std::dec << std::setfill(' ')
<< std::left << std::setw(OS_PredefinedVersionInfoNameMax)
<< iter_b->info_name_
<< " " << std::right
<< iter_b->info_value_;
out_list.push_back(o_str.str());
}
return(out_list);
}
// ////////////////////////////////////////////////////////////////////////////
void InsContext::DumpTemplateMapFull()
{
InsItemMapIterC iter_b(template_map_.begin());
InsItemMapIterC iter_e(template_map_.end());
for ( ; iter_b != iter_e; ++iter_b) {
InsItem::DumpItemListFull(iter_b->second.second,
&(ins_item_list_[iter_b->second.first]), dict_value_list_);
std::cout << std::endl;
}
}
// ////////////////////////////////////////////////////////////////////////////
bool LocateDll(const MLB::Utility::StringVector &name_list,
LocateDllPredicate &dll_predicate, std::string &out_dll_name)
{
MLB::Utility::StringVectorIterC iter_b(name_list.begin());
MLB::Utility::StringVectorIterC iter_e(name_list.end());
for ( ; iter_b != iter_e; ++iter_b) {
if (LocateDll(*iter_b, dll_predicate, out_dll_name))
return(true);
}
return(false);
}
// ////////////////////////////////////////////////////////////////////////////
// CODE NOTE: To be moved into ProcessAffinity.cpp
void SetAffinityProcessRandom(const std::string &process_name,
ProcessorAffinity desired_affinity_mask)
{
std::vector<ProcessId> process_id_list;
ProcessNameToProcessId(process_name, process_id_list);
std::vector<ProcessId>::const_iterator iter_b(process_id_list.begin());
std::vector<ProcessId>::const_iterator iter_e(process_id_list.end());
while (iter_b != iter_e) {
SetAffinityProcess(*iter_b,
INTERNAL_FixUpAffinityRandom(desired_affinity_mask));
++iter_b;
}
}
// ////////////////////////////////////////////////////////////////////////////
MLB::Utility::StringVector &LocateDllInList(const std::string &dll_name,
const MLB::Utility::StringVector &path_list,
MLB::Utility::StringVector &out_dll_list)
{
MLB::Utility::StringVector tmp_dll_list;
MLB::Utility::StringVectorIterC iter_b(path_list.begin());
MLB::Utility::StringVectorIterC iter_e(path_list.end());
for ( ; iter_b != iter_e; ++iter_b) {
std::string out_dll_name;
if (LocateDllInternal(dll_name, *iter_b, out_dll_name))
tmp_dll_list.push_back(out_dll_name);
}
out_dll_list.swap(tmp_dll_list);
return(out_dll_list);
}
// ////////////////////////////////////////////////////////////////////////////
unsigned int InsContext::DetermineTemplateIndex(
const InsItemNameMapI &template_name_map, const InsItemList &ins_item_list,
const char *template_name)
{
if ((template_name == NULL) || (!(*template_name)))
MLB::Utility::ThrowInvalidArgument("The template name to be located "
"is " + std::string((template_name == NULL) ? "NULL." : "empty."));
InsItemNameMapIterC iter_b(template_name_map.begin());
InsItemNameMapIterC iter_e(template_name_map.end());
for ( ; iter_b != iter_e; ++iter_b) {
if (!MLB::Utility::Utility_stricmp(template_name,
ins_item_list[iter_b->second.first].field_name_.c_str()))
return(iter_b->second.first);
}
return(InvalidTemplateIndex);
}
// ////////////////////////////////////////////////////////////////////////////
// CODE NOTE: To be moved into ProcessAffinity.cpp
void SetAffinityProcessProgressive(const std::string &process_name,
ProcessorAffinity ¤t_cpu, ProcessorAffinity desired_affinity_mask,
unsigned int max_count)
{
std::vector<ProcessId> process_id_list;
ProcessNameToProcessId(process_name, process_id_list, max_count);
std::vector<ProcessId>::const_iterator iter_b(process_id_list.begin());
std::vector<ProcessId>::const_iterator iter_e(process_id_list.end());
while (iter_b != iter_e) {
ProcessorAffinity tmp_current_cpu = current_cpu;
SetAffinityProcess(*iter_b, INTERNAL_FixUpAffinityProgressive(tmp_current_cpu,
desired_affinity_mask));
current_cpu = tmp_current_cpu;
++iter_b;
}
}
// ////////////////////////////////////////////////////////////////////////////
MLB::Utility::StringVector &LocateDllInPathString(const std::string &dll_name,
const std::string &path_string, MLB::Utility::StringVector &out_dll_list)
{
MLB::Utility::StringVector path_list_1;
MLB::Utility::StringVector path_list_2;
MLB::Utility::StringVector dll_list;
MLB::Utility::SplitString(MLB::Utility::Trim(path_string), ";",
path_list_1, 0, true);
MLB::Utility::StringVectorIter iter_b(path_list_1.begin());
MLB::Utility::StringVectorIter iter_e(path_list_1.end());
for ( ; iter_b != iter_e; ++iter_b) {
if (!MLB::Utility::Trim(*iter_b).empty())
path_list_2.push_back(*iter_b);
}
return(LocateDllInList(dll_name, path_list_2, out_dll_list));
}
