Control frame rate with topic ros2 topic pub /frame_rate std_msgs/Flo…

…at64 "{data: 1.0}" #10
lucasw · Nov 25, 2017 · 60850b3 · 60850b3
1 parent 5e82039
commit 60850b3
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 16 deletions.
diff --git a/screen_grab_ros2/include/screen_grab/screen_grab.h b/screen_grab_ros2/include/screen_grab/screen_grab.h
@@ -34,6 +34,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/region_of_interest.hpp>
 #include <sensor_msgs/msg/image.hpp>
+#include <std_msgs/msg/float64.hpp>
 #include <X11/Xlib.h>
 
 class ScreenGrab : public rclcpp::Node
@@ -43,22 +44,25 @@ class ScreenGrab : public rclcpp::Node
 
   rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr screen_pub_;
 
-  rclcpp::Subscription<sensor_msgs::msg::RegionOfInterest>::SharedPtr roi_sub_;
-
   void onInit();
+
+  int x_offset_;
+  int y_offset_;
+  int width_;
+  int height_;
+  rclcpp::Subscription<sensor_msgs::msg::RegionOfInterest>::SharedPtr roi_sub_;
   void roiCallback(const sensor_msgs::msg::RegionOfInterest::SharedPtr msg);
-  void updateConfig();
+
   double update_rate_;
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr frame_rate_sub_;
+  void frameRateCallback(const std_msgs::msg::Float64::SharedPtr msg);
+
+  void updateConfig();
 
   // TODO(lucasw) need custom message to replace dynamic reconfigure
 
   void checkRoi(int& x_offset, int& y_offset, int& width, int& height);
 
-  int x_offset_;
-  int y_offset_;
-  int width_;
-  int height_;
-
   int screen_w_;
   int screen_h_;
 

diff --git a/screen_grab_ros2/src/screen_grab.cpp b/screen_grab_ros2/src/screen_grab.cpp
@@ -77,11 +77,11 @@ void XImage2RosImage(XImage& ximage, Display& _xDisplay, Screen& _xScreen,
 ScreenGrab::ScreenGrab(rclcpp::node::Node::SharedPtr clock_node) :
   Node("screen_grab"),
   ts_(clock_node),
-  update_rate_(10.0),
   x_offset_(0),
   y_offset_(0),
   width_(640),
   height_(480),
+  update_rate_(10.0),
   first_error_(false)
 {
   onInit();
@@ -97,6 +97,24 @@ void ScreenGrab::roiCallback(const sensor_msgs::msg::RegionOfInterest::SharedPtr
   updateConfig();
 }
 
+void ScreenGrab::frameRateCallback(const std_msgs::msg::Float64::SharedPtr msg)
+{
+  if (msg->data <= 0.0)
+  {
+    RCLCPP_ERROR(this->get_name(), "bad update rate %f", msg->data);
+    // TODO(lucasw) disable any existing timer_?
+    return;
+  }
+  update_rate_ = msg->data;
+  const float period = 1.0 / update_rate_;
+  // TODO(lucasw) does RCLCPP_INFO log, or is RCUTILS_LOG_INFO_NAMED needed?
+  // does it publish to a ros topic?
+  RCLCPP_INFO(this->get_name(), "period %f, update rate %f", period, update_rate_);
+  std::chrono::nanoseconds period_ns(static_cast<int>(period * 1e9));
+  timer_ = this->create_wall_timer(
+      period_ns, std::bind(&ScreenGrab::spinOnce, this));
+}
+
 void ScreenGrab::checkRoi(int& x_offset, int& y_offset, int& width, int& height)
 {
   // TODO(lucasw) with cv::Rect this could be one line rect1 & rect2
@@ -186,13 +204,12 @@ void ScreenGrab::onInit()
   roi_sub_ = this->create_subscription<sensor_msgs::msg::RegionOfInterest>(
         "roi", std::bind(&ScreenGrab::roiCallback, this, _1));
 
-  const float period = 1.0 / update_rate_;
-  // TODO(lucasw) does RCLCPP_INFO log, or is RCUTILS_LOG_INFO_NAMED needed?
-  // does it publish to a ros topic?
-  RCLCPP_INFO(this->get_name(), "period %f", period);
-  std::chrono::nanoseconds period_ns(static_cast<int>(period * 1e9));
-  timer_ = this->create_wall_timer(
-      period_ns, std::bind(&ScreenGrab::spinOnce, this));
+  auto msg = std::make_shared<std_msgs::msg::Float64>();
+  msg->data = update_rate_;
+  frameRateCallback(msg);
+
+  frame_rate_sub_ = this->create_subscription<std_msgs::msg::Float64>(
+        "frame_rate", std::bind(&ScreenGrab::frameRateCallback, this, _1));
 }
 
 void ScreenGrab::spinOnce()