diff --git a/README.md b/README.md index c348667..9f81f8f 100644 --- a/README.md +++ b/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 diff --git a/dependencies.repos b/dependencies.repos index 4dbcc73..c91a180 100644 --- a/dependencies.repos +++ b/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 diff --git a/include/reach_ros/display/ros_display.h b/include/reach_ros/display/ros_display.h index 885c303..833807e 100644 --- a/include/reach_ros/display/ros_display.h +++ b/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& 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 diff --git a/src/display/ros_display.cpp b/src/display/ros_display.cpp index 6323191..138bbc4 100644 --- a/src/display/ros_display.cpp +++ b/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_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(config, "use_full_color_range"); - - auto display = std::make_shared(kinematic_base_frame, marker_scale, use_fcr); + float hue_low_score = 270.0; + if (config["hue_low_score"]) + hue_low_score = reach::get(config, "hue_low_score"); + float hue_high_score = 0.0; + if (config["hue_high_score"]) + hue_high_score = reach::get(config, "hue_high_score"); + auto display = + std::make_shared(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";