Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Path tracer colors #599

Merged
merged 7 commits into from
Sep 21, 2020
Merged

Path tracer colors #599

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
41dc846
Modify path tracer to use an RTF value
Sep 8, 2020
adb1f29
Start of path tracer color yaml configuration
Sep 9, 2020
b2804cd
Support yaml configuration for colors and rtf
Sep 9, 2020
d3af590
merged with citadel
Sep 16, 2020
46a6489
Update to use DRY suggestion
Sep 21, 2020
2811db9
Merge branch 'path_tracer_colors' of github.com:osrf/subt into path_t…
Sep 21, 2020
d79dccc
Update subt_ign/src/path_tracer.cc
nkoenig Sep 21, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
223 changes: 184 additions & 39 deletions subt_ign/src/path_tracer.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,168 @@
#include "path_tracer.hh"

//////////////////////////////////////////////////
Processor::Processor(const std::string &_path, double _rtf)
: rtf(_rtf)
// Load a color from YAML helper function.
// \param[in] _node YAML node that contains color information
// \param[in] _def Default color value that is used if a color property is
// missing.
ignition::math::Color loadColor(const YAML::Node &_node, double _def = 1.0)
{
ignition::math::Color clr(_def, _def, _def, _def);

if (_node["r"])
clr.R(_node["r"].as<double>());

if (_node["g"])
clr.G(_node["g"].as<double>());

if (_node["b"])
clr.B(_node["b"].as<double>());

if (_node["a"])
clr.A(_node["a"].as<double>());

return clr;
}

//////////////////////////////////////////////////
MarkerColor::MarkerColor(const YAML::Node &_node)
{
if (_node["ambient"])
this->ambient = loadColor(_node["ambient"]);

if (_node["diffuse"])
this->diffuse = loadColor(_node["diffuse"]);

if (_node["emissive"])
this->emissive = loadColor(_node["emissive"]);
}

//////////////////////////////////////////////////
MarkerColor::MarkerColor(const MarkerColor &_clr)
:ambient(_clr.ambient), diffuse(_clr.diffuse), emissive(_clr.emissive)
{
}

//////////////////////////////////////////////////
MarkerColor::MarkerColor(const ignition::math::Color &_ambient,
const ignition::math::Color &_diffuse,
const ignition::math::Color &_emissive)
: ambient(_ambient), diffuse(_diffuse), emissive(_emissive)
{
}

//////////////////////////////////////////////////
Processor::Processor(const std::string &_path, const std::string &_configPath)
{
YAML::Node cfg;
if (!_configPath.empty())
{
if (ignition::common::exists(_configPath))
{
try
{
cfg = YAML::LoadFile(_configPath);
}
catch (YAML::Exception &ex)
{
std::cerr << "Unable to load configuration file["
<< _configPath << "]: "
<< "error at line " << ex.mark.line + 1 << ", column "
<< ex.mark.column + 1 << ": " << ex.msg << "\n";
}
}
else
{
std::cerr << "Configuration file[" << _configPath << "] doesn't exist\n";
}
}

// Set the RTF value
if (cfg && cfg["rtf"])
this->rtf = cfg["rtf"].as<double>();

// Color of incorrect reports.
if (cfg && cfg["incorrect_report_color"])
{
this->artifactColors["incorrect_report_color"] =
MarkerColor(cfg["incorrect_report_color"]);
}
else
{
this->artifactColors["incorrect_report_color"] =
MarkerColor({1.0, 0.0, 0.0, 0.5},
{1.0, 0.0, 0.0, 0.5},
{0.2, 0.0, 0.0, 0.1});
}

// Color of correct reports.
if (cfg && cfg["correct_report_color"])
{
this->artifactColors["correct_report_color"] =
MarkerColor(cfg["correct_report_color"]);
}
else
{
this->artifactColors["correct_report_color"] =
MarkerColor({0.0, 1.0, 0.0, 1.0},
{0.0, 1.0, 0.0, 1.0},
{0.0, 1.0, 0.0, 1.0});
}

// Color of artifact locations
if (cfg && cfg["artifact_location_color"])
{
this->artifactColors["artifact_location_color"] =
MarkerColor(cfg["artifact_location_color"]);
}
else
{
this->artifactColors["artifact_location_color"] =
MarkerColor({0.0, 1.0, 1.0, 0.5},
{0.0, 1.0, 1.0, 0.5},
{0.0, 0.2, 0.2, 0.5});
}

// Color of robot paths
if (cfg && cfg["robot_colors"])
{
for (std::size_t i = 0; i < cfg["robot_colors"].size(); ++i)
{
this->robotColors.push_back(MarkerColor(cfg["robot_colors"][i]));
}
}
else
{
this->robotColors.push_back(
MarkerColor({0.6, 0.0, 1.0, 1.0},
{0.6, 0.0, 1.0, 1.0},
{0.6, 0.0, 1.0, 1.0}));
this->robotColors.push_back(
MarkerColor({0.678, 0.2, 1.0, 1.0},
{0.678, 0.2, 1.0, 1.0},
{0.678, 0.2, 1.0, 1.0}));
this->robotColors.push_back(
MarkerColor({0.761, 0.4, 1.0, 1.0},
{0.761, 0.4, 1.0, 1.0},
{0.761, 0.4, 1.0, 1.0}));
this->robotColors.push_back(
MarkerColor({0.839, 0.6, 1.0, 1.0},
{0.839, 0.6, 1.0, 1.0},
{0.839, 0.6, 1.0, 1.0}));
this->robotColors.push_back(
MarkerColor({1.0, 0.6, 0.0, 1.0},
{1.0, 0.6, 0.0, 1.0},
{1.0, 0.6, 0.0, 1.0}));
this->robotColors.push_back(
MarkerColor({1.0, 0.678, 0.2, 1.0},
{1.0, 0.678, 0.2, 1.0},
{1.0, 0.678, 0.2, 1.0}));
this->robotColors.push_back(
MarkerColor({1.0, 0.761, 0.4, 1.0},
{1.0, 0.761, 0.4, 1.0},
{1.0, 0.761, 0.4, 1.0}));
}

// Create the transport node with the partition used by simulation
// world.
ignition::transport::NodeOptions opts;
Expand Down Expand Up @@ -266,14 +425,15 @@ void Processor::DisplayArtifacts()
{
for (const auto &artifact : this->artifacts)
{
this->SpawnMarker(this->colors[2], artifact.second.Pos(),
this->SpawnMarker(this->artifactColors["artifact_location_color"],
artifact.second.Pos(),
ignition::msgs::Marker::SPHERE,
ignition::math::Vector3d(8, 8, 8));
}
}

//////////////////////////////////////////////////
void Processor::SpawnMarker(ignition::math::Color &_color,
void Processor::SpawnMarker(MarkerColor &_color,
const ignition::math::Vector3d &_pos,
ignition::msgs::Marker::Type _type,
const ignition::math::Vector3d &_scale)
Expand All @@ -288,18 +448,18 @@ void Processor::SpawnMarker(ignition::math::Color &_color,
markerMsg.set_visibility(ignition::msgs::Marker::GUI);

// Set color
markerMsg.mutable_material()->mutable_ambient()->set_r(_color.R());
markerMsg.mutable_material()->mutable_ambient()->set_g(_color.G());
markerMsg.mutable_material()->mutable_ambient()->set_b(_color.B());
markerMsg.mutable_material()->mutable_ambient()->set_a(_color.A());
markerMsg.mutable_material()->mutable_diffuse()->set_r(_color.R());
markerMsg.mutable_material()->mutable_diffuse()->set_g(_color.G());
markerMsg.mutable_material()->mutable_diffuse()->set_b(_color.B());
markerMsg.mutable_material()->mutable_diffuse()->set_a(_color.A());
markerMsg.mutable_material()->mutable_emissive()->set_r(_color.R());
markerMsg.mutable_material()->mutable_emissive()->set_g(_color.G());
markerMsg.mutable_material()->mutable_emissive()->set_b(_color.B());
markerMsg.mutable_material()->mutable_emissive()->set_a(_color.A());
markerMsg.mutable_material()->mutable_ambient()->set_r(_color.ambient.R());
markerMsg.mutable_material()->mutable_ambient()->set_g(_color.ambient.G());
markerMsg.mutable_material()->mutable_ambient()->set_b(_color.ambient.B());
markerMsg.mutable_material()->mutable_ambient()->set_a(_color.ambient.A());
markerMsg.mutable_material()->mutable_diffuse()->set_r(_color.diffuse.R());
markerMsg.mutable_material()->mutable_diffuse()->set_g(_color.diffuse.G());
markerMsg.mutable_material()->mutable_diffuse()->set_b(_color.diffuse.B());
markerMsg.mutable_material()->mutable_diffuse()->set_a(_color.diffuse.A());
markerMsg.mutable_material()->mutable_emissive()->set_r(_color.emissive.R());
markerMsg.mutable_material()->mutable_emissive()->set_g(_color.emissive.G());
markerMsg.mutable_material()->mutable_emissive()->set_b(_color.emissive.B());
markerMsg.mutable_material()->mutable_emissive()->set_a(_color.emissive.A());

ignition::msgs::Set(markerMsg.mutable_scale(), _scale);

Expand Down Expand Up @@ -330,7 +490,8 @@ void Processor::Cb(const ignition::msgs::Pose_V &_msg)

if (this->robots.find(name) == this->robots.end())
{
this->robots[name] = this->colors[this->robots.size()+3];
this->robots[name] = this->robotColors[
this->robots.size() % this->robotColors.size()];
this->prevPose[name] = pose;
}

Expand Down Expand Up @@ -373,13 +534,13 @@ void ReportData::Render(Processor *_p)
// Otherwise render a red box.
if (this->score > 0)
{
_p->SpawnMarker(_p->colors[1], this->pos,
_p->SpawnMarker(_p->artifactColors["correct_report_color"], this->pos,
ignition::msgs::Marker::SPHERE,
ignition::math::Vector3d(4, 4, 4));
}
else
{
_p->SpawnMarker(_p->colors[0], this->pos,
_p->SpawnMarker(_p->artifactColors["incorrect_report_color"], this->pos,
ignition::msgs::Marker::BOX,
ignition::math::Vector3d(4, 4, 4));
}
Expand All @@ -388,27 +549,11 @@ void ReportData::Render(Processor *_p)
/////////////////////////////////////////////////
int main(int _argc, char **_argv)
{
double rtf = 1;
if (_argc > 2)
{
try
{
rtf = std::stod(_argv[2]);
}
catch(...)
{
std::cerr << "Invalid RTF. Defaulting to 1.0\n";
}
}

if (rtf <= 0)
{
std::cerr << "Invalid RTF of [" << rtf << "]. Defaulting to 1.0\n";
rtf = 1.0;
}

// Create and run the processor.
Processor p(_argv[1], rtf);
if (_argc > 2)
Processor p(_argv[1], _argv[2]);
else
Processor p(_argv[1]);
std::cout << "\nPlayback complete.\n";
return 0;
}
Loading