Add parameter to enable the OI mode reporting bug workaround (#95)

* Add parameter to enable the OI mode reporting bug workaround #64 * Add myself to contributors list
AutonomyLab · Apr 26, 2022 · 0f4e957 · 0f4e957
1 parent c8de6c8
commit 0f4e957
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 0 deletions.
diff --git a/README.md b/README.md
@@ -160,6 +160,7 @@ $ ros2 launch create_bringup create_2.launch config:=/abs/path/to/config.yaml de
 `publish_tf`  |  Publish the transform from `odom_frame` to `base_frame` | `true`  
 `robot_model` |  The type of robot being controlled (supported values: `ROOMBA_400`, `CREATE_1` and `CREATE_2`) | `CREATE_2`
 `baud`        |  Serial baud rate | Inferred based on robot model, but is overwritten upon providing a value
+`oi_mode_workaround` | Some Roomba models incorrectly report the current OI mode in their sensor streams. Setting this to `true` will cause `libcreate` to decrement the OI mode received in the sensor stream by `1` | `false`
 
 ### Publishers
 
@@ -243,6 +244,8 @@ Contributing to the development and maintenance of _create\_autonomy_ is encoura
     - Added Create 1 description ([#27](https://github.com/AutonomyLab/create_autonomy/pull/27)).
 * [Pedro Grojsgold](https://github.com/pgold)
     - Ported to ROS 2 ([commit](https://github.com/AutonomyLab/create_robot/commit/198345071aa8a9df154d8490feabf5784b78da16)).
+* [Josh Gadeken](https://github.com/process1183)
+    - Added parameter for [libcreate's OI Mode reporting workaround](https://github.com/AutonomyLab/libcreate/pull/67) ([#95](https://github.com/AutonomyLab/create_robot/pull/95))
 
 [libcreate]:  https://github.com/AutonomyLab/libcreate
 [oi_spec]:  https://www.adafruit.com/datasheets/create_2_Open_Interface_Spec.pdf

diff --git a/create_driver/include/create_driver/create_driver.h b/create_driver/include/create_driver/create_driver.h
@@ -130,6 +130,7 @@ class CreateDriver : public rclcpp::Node
   double loop_hz_;
   bool publish_tf_;
   int baud_;
+  bool oi_mode_workaround_;
 
   void cmdVelCallback(geometry_msgs::msg::Twist::UniquePtr msg);
   void debrisLEDCallback(std_msgs::msg::Bool::UniquePtr msg);

diff --git a/create_driver/src/create_driver.cpp b/create_driver/src/create_driver.cpp
@@ -47,6 +47,7 @@ CreateDriver::CreateDriver()
   latch_duration_ = declare_parameter<double>("latch_cmd_duration", 0.2);
   loop_hz_ = declare_parameter<double>("loop_hz", 10.0);
   publish_tf_ = declare_parameter<bool>("publish_tf", true);
+  oi_mode_workaround_ = declare_parameter<bool>("oi_mode_workaround", false);
 
   auto robot_model_name = declare_parameter<std::string>("robot_model", "CREATE_2");
   if (robot_model_name == "ROOMBA_400")
@@ -75,6 +76,10 @@ CreateDriver::CreateDriver()
   // Disable signal handler; let rclcpp handle them
   robot_ = new create::Create(model_, false);
 
+  // Enable/disable the OI mode reporting workaround in libcreate.
+  // https://github.com/AutonomyLab/create_robot/issues/64
+  robot_->setModeReportWorkaround(oi_mode_workaround_);
+
   if (!robot_->connect(dev_, baud_))
   {
     RCLCPP_FATAL(get_logger(), "[CREATE] Failed to establish serial connection with Create.");