diff --git a/create_driver/include/create_driver/create_driver.h b/create_driver/include/create_driver/create_driver.h index c8757791..d9e9379e 100644 --- a/create_driver/include/create_driver/create_driver.h +++ b/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::SharedPtr cmd_vel_sub_; rclcpp::Subscription::SharedPtr debris_led_sub_; rclcpp::Subscription::SharedPtr spot_led_sub_; @@ -82,6 +85,10 @@ class CreateDriver : public rclcpp::Node rclcpp::Subscription::SharedPtr main_brush_motor_sub_; rclcpp::Subscription::SharedPtr vacuum_motor_sub_; + + rclcpp::Subscription::SharedPtr clean_mode_sub_; + + rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Publisher::SharedPtr clean_btn_pub_; rclcpp::Publisher::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); diff --git a/create_driver/src/create_driver.cpp b/create_driver/src/create_driver.cpp index 3da8ed99..b4f6acc6 100644 --- a/create_driver/src/create_driver.cpp +++ b/create_driver/src/create_driver.cpp @@ -143,6 +143,12 @@ CreateDriver::CreateDriver() vacuum_motor_sub_ = create_subscription( "vacuum_motor", 10, std::bind(&CreateDriver::vacuumBrushMotor, this, std::placeholders::_1)); + + + clean_mode_sub_ = create_subscription( + "set_clean_mode", 10, std::bind(&CreateDriver::cleanModeCallback, this, std::placeholders::_1)); + + // Setup publishers odom_pub_ = create_publisher("odom", 30); clean_btn_pub_ = create_publisher("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(); diff --git a/create_msgs/CMakeLists.txt b/create_msgs/CMakeLists.txt index 9288e234..ee5ec769 100644 --- a/create_msgs/CMakeLists.txt +++ b/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( diff --git a/create_msgs/msg/CleanMode.msg b/create_msgs/msg/CleanMode.msg new file mode 100644 index 00000000..db97d789 --- /dev/null +++ b/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 \ No newline at end of file