add red to green color scale

ros-industrial · Aug 16, 2023 · 557a88d · 557a88d
1 parent 7830f5d
commit 557a88d
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 4 deletions.
diff --git a/include/reach_ros/display/ros_display.h b/include/reach_ros/display/ros_display.h
@@ -30,7 +30,7 @@ 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, bool use_red_to_green);
 
   void showEnvironment() const override;
   void updateRobotPose(const std::map<std::string, double>& pose) const override;
@@ -43,6 +43,7 @@ class ROSDisplay : public reach::Display
   const std::string kinematic_base_frame_;
   const double marker_scale_;
   const bool use_full_color_range_;
+  const bool use_red_to_green_;
   visualization_msgs::msg::Marker collision_marker_;
 
   // ROS comoponents

diff --git a/src/display/ros_display.cpp b/src/display/ros_display.cpp
@@ -29,10 +29,12 @@ 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,
+                       bool use_red_to_green)
   : kinematic_base_frame_(std::move(kinematic_base_frame))
   , marker_scale_(marker_scale)
   , use_full_color_range_(use_full_color_range)
+  , use_red_to_green_(use_red_to_green)
 {
   // utils::initROS();
   server_ = std::make_shared<interactive_markers::InteractiveMarkerServer>(INTERACTIVE_MARKER_TOPIC,
@@ -71,7 +73,7 @@ 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_, use_red_to_green_);
 
   for (std::size_t i = 0; i < db.size(); ++i)
   {
@@ -138,8 +140,11 @@ 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");
+  bool use_red_to_green = false;
+  if (config["use_red_to_green"])
+    use_red_to_green = reach::get<bool>(config, "use_red_to_green");
 
-  auto display = std::make_shared<ROSDisplay>(kinematic_base_frame, marker_scale, use_fcr);
+  auto display = std::make_shared<ROSDisplay>(kinematic_base_frame, marker_scale, use_fcr, use_red_to_green);
 
   // Optionally add a collision mesh
   const std::string collision_mesh_filename_key = "collision_mesh_filename";