void control_LEDs(const std::vector<uint16_t>& led_list) { // first set up the random-number-generators: std::default_random_engine generator( static_cast<unsigned long>(std::chrono::system_clock::now().time_since_epoch().count()) ); std::uniform_int_distribution<useconds_t> sleep_time_distribution( settings::min_sleep_time, settings::max_sleep_time); std::uniform_int_distribution<useconds_t> fade_time_distribution( settings::min_fade_time, settings::max_fade_time); std::uniform_int_distribution<size_t> color_distribution(0, settings::colorset.size() - 1); // and now start the actual work: vlpp::rgba_color last_color; while(true){ if(settings::thread_return_flag){ return; } vlpp::rgba_color tmp = settings::colorset.at(color_distribution(generator)); if(tmp == last_color){ continue; } fade_to(led_list, fade_time_distribution(generator), last_color, tmp); last_color = tmp; usleep (sleep_time_distribution(generator)); } }
void CloudViewer::on_cloud_xyzrgb() { CLOG(LTRACE) << "CloudViewer::on_cloud_xyzrgb\n"; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = in_cloud_xyzrgb.read(); // Filter the NaN points. std::vector<int> indices; cloud->is_dense = false; pcl::removeNaNFromPointCloud(*cloud, *cloud, indices); pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color_distribution(cloud); viewer->removePointCloud("viewcloud") ; viewer->addPointCloud<pcl::PointXYZRGB>(cloud, color_distribution, "viewcloud") ; }
void ObstaclePointCloud::broadcast() { if (q_obstacles_.size() < 1) { return; } const auto sim_obstacles = q_obstacles_.front(); using Cell = std::pair<float, float>; std::vector<Cell> all_cells; for (const auto& line : sim_obstacles->obstacles) { const auto cells = pedsim::LineObstacleToCells(line.start.x, line.start.y, line.end.x, line.end.y); std::copy(cells.begin(), cells.end(), std::back_inserter(all_cells)); } constexpr int point_density = 100; const int num_points = all_cells.size() * point_density; std::default_random_engine generator; // \todo - Read params from config file. std::uniform_int_distribution<int> color_distribution(1, 255); std::uniform_real_distribution<float> height_distribution(0, 1); std::uniform_real_distribution<float> width_distribution(-0.5, 0.5); sensor_msgs::PointCloud pcd_global; pcd_global.header.stamp = ros::Time::now(); pcd_global.header.frame_id = sim_obstacles->header.frame_id; pcd_global.points.resize(num_points); pcd_global.channels.resize(1); pcd_global.channels[0].name = "intensities"; pcd_global.channels[0].values.resize(num_points); sensor_msgs::PointCloud pcd_local; pcd_local.header.stamp = ros::Time::now(); pcd_local.header.frame_id = robot_odom_.header.frame_id; pcd_local.points.resize(num_points); pcd_local.channels.resize(1); pcd_local.channels[0].name = "intensities"; pcd_local.channels[0].values.resize(num_points); // prepare the transform to robot odom frame. tf::StampedTransform robot_transform; try { transform_listener_->lookupTransform(robot_odom_.header.frame_id, sim_obstacles->header.frame_id, ros::Time(0), robot_transform); } catch (tf::TransformException& e) { ROS_WARN_STREAM_THROTTLE(5.0, "TFP lookup from [" << sim_obstacles->header.frame_id << "] to [" << robot_odom_.header.frame_id << "] failed. Reason: " << e.what()); return; } size_t index = 0; for (const auto& cell : all_cells) { const int cell_color = color_distribution(generator); for (size_t j = 0; j < point_density; ++j) { if (fov_->inside(cell.first, cell.second)) { const tf::Vector3 point(cell.first + width_distribution(generator), cell.second + width_distribution(generator), 0.); const auto transformed_point = transformPoint(robot_transform, point); pcd_local.points[index].x = transformed_point.getOrigin().x(); pcd_local.points[index].y = transformed_point.getOrigin().y(); pcd_local.points[index].z = height_distribution(generator); pcd_local.channels[0].values[index] = cell_color; // Global observations. pcd_global.points[index].x = cell.first + width_distribution(generator); pcd_global.points[index].y = cell.second + width_distribution(generator); pcd_global.points[index].z = height_distribution(generator); pcd_global.channels[0].values[index] = cell_color; } index++; } } if (pcd_local.channels[0].values.size() > 1) { pub_signals_local_.publish(pcd_local); } if (pcd_global.channels[0].values.size() > 1) { pub_signals_global_.publish(pcd_global); } q_obstacles_.pop(); };