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