Set motor power via switch input

rt-net · Feb 7, 2024 · 85217a0 · 85217a0
1 parent 29ad351
commit 85217a0
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 11 deletions.
diff --git a/include/raspimouse_ros2_examples/camera_line_follower_component.hpp b/include/raspimouse_ros2_examples/camera_line_follower_component.hpp
@@ -56,6 +56,7 @@ class Camera_Follower : public rclcpp_lifecycle::LifecycleNode
   rclcpp::Subscription<raspimouse_msgs::msg::Switches>::SharedPtr switches_sub_;
   rclcpp::TimerBase::SharedPtr cmd_vel_timer_;
 
+  void set_motor_power(const bool motor_on);
   std::string mat_type2encoding(const int mat_type) const;
   void convert_frame_to_message(
     const cv::Mat & frame,

diff --git a/src/camera_line_follower_component.cpp b/src/camera_line_follower_component.cpp
@@ -64,9 +64,11 @@ void Camera_Follower::callback_switches(const raspimouse_msgs::msg::Switches::Sh
 {
   if (msg->switch0) {
     RCLCPP_INFO(this->get_logger(), "Stop following.");
+    set_motor_power(false);
     enable_following_ = false;
   } else if (msg->switch2) {
     RCLCPP_INFO(this->get_logger(), "Start following.");
+    set_motor_power(true);
     enable_following_ = true;
   }
 }
@@ -95,6 +97,19 @@ void Camera_Follower::on_cmd_vel_timer()
   cmd_vel_pub_->publish(std::move(msg));
 }
 
+void Camera_Follower::set_motor_power(const bool motor_on)
+{
+  if (motor_power_client_ == nullptr) {
+    RCLCPP_ERROR(
+      this->get_logger(),
+      "Service motor_power is not avaliable.");
+    return;
+  }
+  auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
+  request->data = motor_on;
+  auto future_result = motor_power_client_->async_send_request(request);
+}
+
 // Ref: https://github.com/ros2/demos/blob/dashing/image_tools/src/cam2image.cpp
 std::string Camera_Follower::mat_type2encoding(const int mat_type) const
 {
@@ -195,6 +210,14 @@ CallbackReturn Camera_Follower::on_configure(const rclcpp_lifecycle::State &)
   // Don't actually start publishing data until activated
   cmd_vel_timer_->cancel();
 
+  motor_power_client_ = create_client<std_srvs::srv::SetBool>("motor_power");
+  if (!motor_power_client_->wait_for_service(5s)) {
+    RCLCPP_ERROR(
+      this->get_logger(),
+      "Service motor_power is not avaliable.");
+    return CallbackReturn::FAILURE;
+  }
+
   result_image_pub_ = create_publisher<sensor_msgs::msg::Image>("result_image", 1);
   cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
   image_sub_ = create_subscription<sensor_msgs::msg::Image>(
@@ -216,17 +239,6 @@ CallbackReturn Camera_Follower::on_activate(const rclcpp_lifecycle::State &)
 {
   RCLCPP_INFO(this->get_logger(), "on_activate() is called.");
 
-  motor_power_client_ = create_client<std_srvs::srv::SetBool>("motor_power");
-  if (!motor_power_client_->wait_for_service(5s)) {
-    RCLCPP_ERROR(
-      this->get_logger(),
-      "Service motor_power is not avaliable.");
-    return CallbackReturn::FAILURE;
-  }
-  auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
-  request->data = true;
-  auto future_result = motor_power_client_->async_send_request(request);
-
   result_image_pub_->on_activate();
   cmd_vel_pub_->on_activate();
   cmd_vel_timer_->reset();
@@ -237,6 +249,9 @@ CallbackReturn Camera_Follower::on_activate(const rclcpp_lifecycle::State &)
 CallbackReturn Camera_Follower::on_deactivate(const rclcpp_lifecycle::State &)
 {
   RCLCPP_INFO(this->get_logger(), "on_deactivate() is called.");
+
+  set_motor_power(false);
+
   result_image_pub_->on_deactivate();
   cmd_vel_pub_->on_deactivate();
   cmd_vel_timer_->cancel();
@@ -250,6 +265,8 @@ CallbackReturn Camera_Follower::on_cleanup(const rclcpp_lifecycle::State &)
 {
   RCLCPP_INFO(this->get_logger(), "on_cleanup() is called.");
 
+  set_motor_power(false);
+
   result_image_pub_.reset();
   cmd_vel_pub_.reset();
   cmd_vel_timer_.reset();
@@ -263,6 +280,8 @@ CallbackReturn Camera_Follower::on_shutdown(const rclcpp_lifecycle::State &)
 {
   RCLCPP_INFO(this->get_logger(), "on_shutdown() is called.");
 
+  set_motor_power(false);
+
   result_image_pub_.reset();
   cmd_vel_pub_.reset();
   cmd_vel_timer_.reset();