From 92dd70a1d23018207e5d926ab4f775138d451e48 Mon Sep 17 00:00:00 2001
From: Dirk Thomas <dirk-thomas@users.noreply.github.com>
Date: Fri, 18 Oct 2019 14:06:32 -0700
Subject: [PATCH] separate background color from drawn paths, trigger repaint
 on parameter changes

---
 turtlesim/include/turtlesim/turtle_frame.h |  4 +++
 turtlesim/src/turtle_frame.cpp             | 34 ++++++++++++++++------
 2 files changed, 29 insertions(+), 9 deletions(-)

diff --git a/turtlesim/include/turtlesim/turtle_frame.h b/turtlesim/include/turtlesim/turtle_frame.h
index a8051774..362856a0 100644
--- a/turtlesim/include/turtlesim/turtle_frame.h
+++ b/turtlesim/include/turtlesim/turtle_frame.h
@@ -39,6 +39,7 @@
 # include <rclcpp/rclcpp.hpp>
 # include <ament_index_cpp/get_package_share_directory.hpp>
 
+# include <rcl_interfaces/msg/parameter_event.hpp>
 # include <std_srvs/srv/empty.hpp>
 # include <turtlesim/srv/spawn.hpp>
 # include <turtlesim/srv/kill.hpp>
@@ -76,6 +77,8 @@ private slots:
   bool spawnCallback(const turtlesim::srv::Spawn::Request::SharedPtr, turtlesim::srv::Spawn::Response::SharedPtr);
   bool killCallback(const turtlesim::srv::Kill::Request::SharedPtr, turtlesim::srv::Kill::Response::SharedPtr);
 
+  void parameterEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr);
+
   rclcpp::Node::SharedPtr nh_;
 
   QTimer* update_timer_;
@@ -90,6 +93,7 @@ private slots:
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_srv_;
   rclcpp::Service<turtlesim::srv::Spawn>::SharedPtr spawn_srv_;
   rclcpp::Service<turtlesim::srv::Kill>::SharedPtr kill_srv_;
+  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub_;
 
   typedef std::map<std::string, TurtlePtr> M_Turtle;
   M_Turtle turtles_;
diff --git a/turtlesim/src/turtle_frame.cpp b/turtlesim/src/turtle_frame.cpp
index aedfa328..f30c7d6b 100644
--- a/turtlesim/src/turtle_frame.cpp
+++ b/turtlesim/src/turtle_frame.cpp
@@ -99,6 +99,10 @@ TurtleFrame::TurtleFrame(rclcpp::Node::SharedPtr& node_handle, QWidget* parent,
   spawn_srv_ = nh_->create_service<turtlesim::srv::Spawn>("spawn", std::bind(&TurtleFrame::spawnCallback, this, std::placeholders::_1, std::placeholders::_2));
   kill_srv_ = nh_->create_service<turtlesim::srv::Kill>("kill", std::bind(&TurtleFrame::killCallback, this, std::placeholders::_1, std::placeholders::_2));
 
+  rclcpp::QoS qos(rclcpp::KeepLast(100), rmw_qos_profile_sensor_data);
+  parameter_event_sub_ = nh_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
+    "/parameter_events", qos, std::bind(&TurtleFrame::parameterEventCallback, this, std::placeholders::_1));
+
   RCLCPP_INFO(nh_->get_logger(), "Starting turtlesim with node name %s", nh_->get_node_names()[0].c_str());
 
   width_in_meters_ = (width() - 1) / meter_;
@@ -152,6 +156,16 @@ bool TurtleFrame::killCallback(const turtlesim::srv::Kill::Request::SharedPtr re
   return true;
 }
 
+void TurtleFrame::parameterEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
+{
+  // only consider events from this node
+  if (event->node == nh_->get_fully_qualified_name())
+  {
+    // since parameter events for this even aren't expected frequently just always call update()
+    update();
+  }
+}
+
 bool TurtleFrame::hasTurtle(const std::string& name)
 {
   return turtles_.find(name) != turtles_.end();
@@ -193,15 +207,8 @@ std::string TurtleFrame::spawnTurtle(const std::string& name, float x, float y,
 
 void TurtleFrame::clear()
 {
-  int r = DEFAULT_BG_R;
-  int g = DEFAULT_BG_G;
-  int b = DEFAULT_BG_B;
-
-  nh_->get_parameter("background_r", r);
-  nh_->get_parameter("background_g", g);
-  nh_->get_parameter("background_b", b);
-
-  path_image_.fill(qRgb(r, g, b));
+  // make all pixels fully transparent
+  path_image_.fill(qRgba(255, 255, 255, 0));
   update();
 }
 
@@ -222,6 +229,15 @@ void TurtleFrame::paintEvent(QPaintEvent*)
 {
   QPainter painter(this);
 
+  int r = DEFAULT_BG_R;
+  int g = DEFAULT_BG_G;
+  int b = DEFAULT_BG_B;
+  nh_->get_parameter("background_r", r);
+  nh_->get_parameter("background_g", g);
+  nh_->get_parameter("background_b", b);
+  QRgb background_color = qRgb(r, g, b);
+  painter.fillRect(0, 0, width(), height(), background_color);
+
   painter.drawImage(QPoint(0, 0), path_image_);
 
   M_Turtle::iterator it = turtles_.begin();