Add customizable color scale for visualization

README.md

@@ -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
 

dependencies.repos

@@ -9,4 +9,4 @@
 - git:
    local-name: reach
    uri: https://github.com/ros-industrial/reach.git
-   version: 1.4.0
+   version: 1.5.1

include/reach_ros/display/ros_display.h

@@ -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;
@@ -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

src/display/ros_display.cpp

@@ -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,
@@ -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)
   {
@@ -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";