Added set_clean_mode

create_driver/include/create_driver/create_driver.h

@@ -38,6 +38,8 @@ POSSIBILITY OF SUCH DAMAGE.
 #include "create_msgs/msg/play_song.hpp"
 #include "create_msgs/msg/motor_setpoint.hpp"
 
+#include "create_msgs/msg/clean_mode.hpp"
+
 #include "create/create.h"
 
 #include "diagnostic_updater/diagnostic_updater.hpp"
@@ -67,6 +69,7 @@ class CreateDriver : public rclcpp::Node
   create::Create* robot_;
   create::RobotModel model_;
 
+
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
   rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr debris_led_sub_;
   rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr spot_led_sub_;
@@ -82,6 +85,10 @@ class CreateDriver : public rclcpp::Node
   rclcpp::Subscription<create_msgs::msg::MotorSetpoint>::SharedPtr main_brush_motor_sub_;
   rclcpp::Subscription<create_msgs::msg::MotorSetpoint>::SharedPtr vacuum_motor_sub_;
 
+
+  rclcpp::Subscription<create_msgs::msg::CleanMode>::SharedPtr clean_mode_sub_;
+
+
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
   rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr clean_btn_pub_;
   rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr day_btn_pub_;
@@ -148,6 +155,8 @@ class CreateDriver : public rclcpp::Node
   void mainBrushMotor(create_msgs::msg::MotorSetpoint::UniquePtr msg);
   void vacuumBrushMotor(create_msgs::msg::MotorSetpoint::UniquePtr msg);
 
+  void cleanModeCallback(create_msgs::msg::CleanMode::UniquePtr msg);
+
   bool update();
   void updateBatteryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat);
   void updateSafetyDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat);

create_driver/src/create_driver.cpp

@@ -143,6 +143,12 @@ CreateDriver::CreateDriver()
   vacuum_motor_sub_ = create_subscription<create_msgs::msg::MotorSetpoint>(
     "vacuum_motor", 10, std::bind(&CreateDriver::vacuumBrushMotor, this, std::placeholders::_1));
 
+
+
+  clean_mode_sub_ = create_subscription<create_msgs::msg::CleanMode>(
+    "set_clean_mode", 10, std::bind(&CreateDriver::cleanModeCallback, this, std::placeholders::_1));
+
+
   // Setup publishers
   odom_pub_ = create_publisher<nav_msgs::msg::Odometry>("odom", 30);
   clean_btn_pub_ = create_publisher<std_msgs::msg::Empty>("clean_button", 30);
@@ -308,6 +314,47 @@ void CreateDriver::vacuumBrushMotor(create_msgs::msg::MotorSetpoint::UniquePtr m
   }
 }
 
+
+
+void CreateDriver::cleanModeCallback(create_msgs::msg::CleanMode::UniquePtr msg)
+{
+  switch(msg->mode){
+    case 0:
+      if (robot_->setMode(create::CreateMode::MODE_SAFE)){
+        RCLCPP_INFO(this->get_logger(), "[CREATE] Stopping Cleaning Mode");
+      } else {
+        RCLCPP_ERROR(this->get_logger(), "[CREATE] Failed to stop cleaning mode");
+      }
+      break;
+    case 1:
+      if (robot_->clean(create::CleanMode::CLEAN_DEFAULT)){
+        RCLCPP_INFO(this->get_logger(), "[CREATE] Starting Cleaning Mode");
+      } else {
+        RCLCPP_ERROR(this->get_logger(), "[CREATE] Failed to start cleaning mode");
+      }
+      break;
+    case 2:
+      if (robot_->clean(create::CleanMode::CLEAN_MAX)){
+        RCLCPP_INFO(this->get_logger(), "[CREATE] Starting Max Cleaning Mode");
+      } else {
+        RCLCPP_ERROR(this->get_logger(), "[CREATE] Failed to start max cleaning mode");
+      }
+      break;
+    case 3:
+      if (robot_->clean(create::CleanMode::CLEAN_SPOT)){
+        RCLCPP_INFO(this->get_logger(), "[CREATE] Starting Spot Cleaning Mode");
+      } else {
+        RCLCPP_ERROR(this->get_logger(), "[CREATE] Failed to start spot cleaning mode");
+      }
+      break;
+    default:
+      RCLCPP_ERROR(this->get_logger(), "[CREATE] Invalid cleaning mode: %d", msg->mode);
+  }
+}
+
+
+
+
 bool CreateDriver::update()
 {
   publishOdom();

create_msgs/CMakeLists.txt

@@ -13,6 +13,7 @@ set(msg_files
   msg/PlaySong.msg
   msg/MotorSetpoint.msg
   msg/Cliff.msg
+  msg/CleanMode.msg
 )
 
 rosidl_generate_interfaces(

create_msgs/msg/CleanMode.msg

@@ -0,0 +1,6 @@
+#uint8 CLEAN_MODE_NONE=0
+#uint8 CLEAN_MODE_DEFAULT=1
+#uint8 CLEAN_MODE_MAX=2
+#uint8 CLEAN_MODE_SPOT=3
+std_msgs/Header header
+uint8 mode