From 24b60d784f8073071dbc8540ee6192c7106c78a5 Mon Sep 17 00:00:00 2001 From: Josh Gadeken Date: Wed, 16 Feb 2022 21:30:25 -0700 Subject: [PATCH 1/2] Add parameter to enable the OI mode reporting bug workaround https://github.com/AutonomyLab/create_robot/issues/64 --- README.md | 1 + create_driver/include/create_driver/create_driver.h | 1 + create_driver/src/create_driver.cpp | 5 +++++ 3 files changed, 7 insertions(+) diff --git a/README.md b/README.md index e3feff4dc..cc4e84e24 100644 --- 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 diff --git a/create_driver/include/create_driver/create_driver.h b/create_driver/include/create_driver/create_driver.h index 0f1502561..8247b35bf 100644 --- a/create_driver/include/create_driver/create_driver.h +++ b/create_driver/include/create_driver/create_driver.h @@ -123,6 +123,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 index 545070ab0..aab6f5e6c 100644 --- a/create_driver/src/create_driver.cpp +++ b/create_driver/src/create_driver.cpp @@ -47,6 +47,7 @@ CreateDriver::CreateDriver() latch_duration_ = declare_parameter("latch_cmd_duration", 0.2); loop_hz_ = declare_parameter("loop_hz", 10.0); publish_tf_ = declare_parameter("publish_tf", true); + oi_mode_workaround_ = declare_parameter("oi_mode_workaround", false); auto robot_model_name = declare_parameter("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."); From 9de2ee6ff61ec8fa880e45aef6270964deab8dd6 Mon Sep 17 00:00:00 2001 From: Josh Gadeken Date: Tue, 5 Apr 2022 20:08:49 -0600 Subject: [PATCH 2/2] Add myself to contributors list --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index cc4e84e24..25b9dc7a2 100644 --- a/README.md +++ b/README.md @@ -243,6 +243,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