feat(joy_controller): allow to control without odometry (#6000)

* feat(joy_controller): allow to control without odometry Signed-off-by: amadeuszsz <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: amadeuszsz <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Dec 31, 2023 · ed8ffea · ed8ffea
1 parent 3b6dad3
commit ed8ffea
Show file tree

Hide file tree

Showing 5 changed files with 19 additions and 5 deletions.
diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md
@@ -37,6 +37,7 @@
 | `steering_angle_velocity` | double | steering angle velocity for operation                                                                              |
 | `accel_sensitivity`       | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) |
 | `brake_sensitivity`       | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) |
+| `raw_control`             | bool   | skip input odometry if true                                                                                        |
 | `velocity_gain`           | double | ratio to calculate velocity by acceleration                                                                        |
 | `max_forward_velocity`    | double | absolute max velocity to go forward                                                                                |
 | `max_backward_velocity`   | double | absolute max velocity to go backward                                                                               |

diff --git a/control/joy_controller/config/joy_controller.param.yaml b/control/joy_controller/config/joy_controller.param.yaml
@@ -9,6 +9,7 @@
     accel_sensitivity: 1.0
     brake_sensitivity: 1.0
     control_command:
+      raw_control: false
       velocity_gain: 3.0
       max_forward_velocity: 20.0
       max_backward_velocity: 3.0

diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/joy_controller/include/joy_controller/joy_controller.hpp
@@ -59,6 +59,7 @@ class AutowareJoyControllerNode : public rclcpp::Node
   double brake_sensitivity_;
 
   // ControlCommand Parameter
+  bool raw_control_;
   double velocity_gain_;
   double max_forward_velocity_;
   double max_backward_velocity_;

diff --git a/control/joy_controller/schema/joy_controller.schema.json b/control/joy_controller/schema/joy_controller.schema.json
@@ -55,6 +55,11 @@
         "control_command": {
           "type": "object",
           "properties": {
+            "raw_control": {
+              "type": "boolean",
+              "description": "Whether to skip input odometry",
+              "default": false
+            },
             "velocity_gain": {
               "type": "number",
               "description": "Ratio to calculate velocity by acceleration",
@@ -79,6 +84,7 @@
             }
           },
           "required": [
+            "raw_control",
             "velocity_gain",
             "max_forward_velocity",
             "max_backward_velocity",

diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp
@@ -220,7 +220,7 @@ bool AutowareJoyControllerNode::isDataReady()
   }
 
   // Twist
-  {
+  if (!raw_control_) {
     if (!twist_) {
       RCLCPP_WARN_THROTTLE(
         get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(),
@@ -461,6 +461,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions &
   steering_angle_velocity_ = declare_parameter<double>("steering_angle_velocity");
   accel_sensitivity_ = declare_parameter<double>("accel_sensitivity");
   brake_sensitivity_ = declare_parameter<double>("brake_sensitivity");
+  raw_control_ = declare_parameter<bool>("control_command.raw_control");
   velocity_gain_ = declare_parameter<double>("control_command.velocity_gain");
   max_forward_velocity_ = declare_parameter<double>("control_command.max_forward_velocity");
   max_backward_velocity_ = declare_parameter<double>("control_command.max_backward_velocity");
@@ -480,10 +481,14 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions &
   sub_joy_ = this->create_subscription<sensor_msgs::msg::Joy>(
     "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1),
     subscriber_option);
-  sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
-    "input/odometry", 1,
-    std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1),
-    subscriber_option);
+  if (!raw_control_) {
+    sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
+      "input/odometry", 1,
+      std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1),
+      subscriber_option);
+  } else {
+    twist_ = std::make_shared<geometry_msgs::msg::TwistStamped>();
+  }
 
   // Publisher
   pub_control_command_ =