スイッチ0を押してcmd_velの配信をオンオフできるように変更

CMakeLists.txt

@@ -64,7 +64,8 @@ ament_target_dependencies(camera_line_follower_component
   sensor_msgs
   geometry_msgs
   OpenCV
-  cv_bridge)
+  cv_bridge
+  raspimouse_msgs)
 rclcpp_components_register_nodes(camera_line_follower_component "camera_line_follower::Camera_Follower")
 
 add_library(line_follower_component SHARED

include/raspimouse_ros2_examples/camera_line_follower_component.hpp

@@ -18,6 +18,7 @@
 #include <memory>
 #include <string>
 
+#include "raspimouse_msgs/msg/switches.hpp"
 #include "raspimouse_ros2_examples/visibility_control.h"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
@@ -39,18 +40,22 @@ class Camera_Follower : public rclcpp_lifecycle::LifecycleNode
 
 protected:
   void image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image);
+  void callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg);
   void on_cmd_vel_timer();
 
 private:
   cv::VideoCapture cap_;
   bool object_is_detected_;
+  bool can_publish_cmdvel_;
   cv::Point2d object_normalized_point_;
   double object_normalized_area_;
   geometry_msgs::msg::Twist cmd_vel_;
+  raspimouse_msgs::msg::Switches switches_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Image>> result_image_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>> cmd_vel_pub_;
   std::shared_ptr<rclcpp::Client<std_srvs::srv::SetBool>> motor_power_client_;
   rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+  rclcpp::Subscription<raspimouse_msgs::msg::Switches>::SharedPtr switches_sub_;
   rclcpp::TimerBase::SharedPtr cmd_vel_timer_;
 
   std::string mat_type2encoding(int mat_type);

src/camera_line_follower_component.cpp

@@ -57,6 +57,11 @@ void Camera_Follower::image_callback(const sensor_msgs::msg::Image::SharedPtr ms
   }
 }
 
+void Camera_Follower::callback_switches(const raspimouse_msgs::msg::Switches::SharedPtr msg)
+{
+  switches_ = *msg;
+}
+
 void Camera_Follower::on_cmd_vel_timer()
 {
   const double LINEAR_VEL = 0.2;  // unit: m/s
@@ -73,6 +78,23 @@ void Camera_Follower::on_cmd_vel_timer()
     cmd_vel_.linear.x = 0.0;
     cmd_vel_.angular.z = 0.0;
   }
+
+  if (switches_.switch0) {
+    if (can_publish_cmdvel_) {
+      RCLCPP_INFO(this->get_logger(), "Start following.");
+      can_publish_cmdvel_ = false;
+    } else {
+      RCLCPP_INFO(this->get_logger(), "Stop following.");
+      can_publish_cmdvel_ = true;
+    }
+  }
+  switches_ = raspimouse_msgs::msg::Switches();  // Reset switch values
+
+  if (!can_publish_cmdvel_) {
+    cmd_vel_.linear.x = 0.0;
+    cmd_vel_.angular.z = 0.0;
+  }
+
   auto msg = std::make_unique<geometry_msgs::msg::Twist>(cmd_vel_);
   cmd_vel_pub_->publish(std::move(msg));
 }
@@ -178,6 +200,8 @@ CallbackReturn Camera_Follower::on_configure(const rclcpp_lifecycle::State &)
   image_sub_ = create_subscription<sensor_msgs::msg::Image>(
     "camera/color/image_raw", rclcpp::SensorDataQoS(),
     std::bind(&Camera_Follower::image_callback, this, std::placeholders::_1));
+  switches_sub_ = create_subscription<raspimouse_msgs::msg::Switches>(
+    "switches", 1, std::bind(&Camera_Follower::callback_switches, this, std::placeholders::_1));
 
   return CallbackReturn::SUCCESS;
 }
@@ -225,6 +249,7 @@ CallbackReturn Camera_Follower::on_cleanup(const rclcpp_lifecycle::State &)
   cmd_vel_pub_.reset();
   cmd_vel_timer_.reset();
   image_sub_.reset();
+  switches_sub_.reset();
 
   return CallbackReturn::SUCCESS;
 }
@@ -237,6 +262,7 @@ CallbackReturn Camera_Follower::on_shutdown(const rclcpp_lifecycle::State &)
   cmd_vel_pub_.reset();
   cmd_vel_timer_.reset();
   image_sub_.reset();
+  switches_sub_.reset();
 
   return CallbackReturn::SUCCESS;
 }