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