Skip to content

Commit

Permalink
Add customizable color scale for visualization (#18)
Browse files Browse the repository at this point in the history
* add red to green color scale

* generalize color scale

* Updated REACH dependency

---------

Co-authored-by: Michael Ripperger <[email protected]>
  • Loading branch information
SammyRamone and marip8 authored Aug 16, 2023
1 parent 7830f5d commit 4e792dc
Show file tree
Hide file tree
Showing 4 changed files with 23 additions and 6 deletions.
4 changes: 4 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -213,6 +213,10 @@ Parameters:
- The length (in meters) of the arrow markers representing the target Cartesian points
- **`use_full_color_range`** (optional, default: False)
- Colorize the heat map using the full range of colors (such that the target with the lowest score is the deepest hue of blue, and the target with the highest score is the deepest hue of red)
- **`hue_low_score`** (optional, default: 270.0)
- Allows changing the default heatmap style coloring to a different color scale in HSV space (e.g. `hue_low_score: 0.0, hue_high_score: 180.0` will lead to a red to green color scale).
- **`hue_high_score`** (optional, default: 0.0)
- See `hue_low_score` for an explanation.
## Target Pose Generator Plugins
Expand Down
2 changes: 1 addition & 1 deletion dependencies.repos
Original file line number Diff line number Diff line change
Expand Up @@ -9,4 +9,4 @@
- git:
local-name: reach
uri: https://github.com/ros-industrial/reach.git
version: 1.4.0
version: 1.5.1
5 changes: 4 additions & 1 deletion include/reach_ros/display/ros_display.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,8 @@ namespace display
class ROSDisplay : public reach::Display
{
public:
ROSDisplay(std::string kinematic_base_frame, double marker_scale, bool use_full_color_range);
ROSDisplay(std::string kinematic_base_frame, double marker_scale, bool use_full_color_range, float hue_low_score,
float hue_high_score);

void showEnvironment() const override;
void updateRobotPose(const std::map<std::string, double>& pose) const override;
Expand All @@ -43,6 +44,8 @@ class ROSDisplay : public reach::Display
const std::string kinematic_base_frame_;
const double marker_scale_;
const bool use_full_color_range_;
const float hue_low_score_;
const float hue_high_score_;
visualization_msgs::msg::Marker collision_marker_;

// ROS comoponents
Expand Down
18 changes: 14 additions & 4 deletions src/display/ros_display.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,10 +29,13 @@ namespace reach_ros
{
namespace display
{
ROSDisplay::ROSDisplay(std::string kinematic_base_frame, double marker_scale, bool use_full_color_range)
ROSDisplay::ROSDisplay(std::string kinematic_base_frame, double marker_scale, bool use_full_color_range,
float hue_low_score, float hue_high_score)
: kinematic_base_frame_(std::move(kinematic_base_frame))
, marker_scale_(marker_scale)
, use_full_color_range_(use_full_color_range)
, hue_low_score_(hue_low_score)
, hue_high_score_(hue_high_score)
{
// utils::initROS();
server_ = std::make_shared<interactive_markers::InteractiveMarkerServer>(INTERACTIVE_MARKER_TOPIC,
Expand Down Expand Up @@ -71,7 +74,8 @@ void ROSDisplay::showResults(const reach::ReachResult& db) const
updateRobotPose(db.at(idx).goal_state);
};

Eigen::MatrixX3f heatmap_colors = reach::computeHeatMapColors(db, use_full_color_range_);
Eigen::MatrixX3f heatmap_colors =
reach::computeHeatMapColors(db, use_full_color_range_, hue_low_score_, hue_high_score_);

for (std::size_t i = 0; i < db.size(); ++i)
{
Expand Down Expand Up @@ -138,8 +142,14 @@ reach::Display::ConstPtr ROSDisplayFactory::create(const YAML::Node& config) con
bool use_fcr = false;
if (config["use_full_color_range"])
use_fcr = reach::get<bool>(config, "use_full_color_range");

auto display = std::make_shared<ROSDisplay>(kinematic_base_frame, marker_scale, use_fcr);
float hue_low_score = 270.0;
if (config["hue_low_score"])
hue_low_score = reach::get<float>(config, "hue_low_score");
float hue_high_score = 0.0;
if (config["hue_high_score"])
hue_high_score = reach::get<float>(config, "hue_high_score");
auto display =
std::make_shared<ROSDisplay>(kinematic_base_frame, marker_scale, use_fcr, hue_low_score, hue_high_score);

// Optionally add a collision mesh
const std::string collision_mesh_filename_key = "collision_mesh_filename";
Expand Down

0 comments on commit 4e792dc

Please sign in to comment.