From ed8ffea6843ada32afea7f3ec8e9c7df111306f5 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Sun, 31 Dec 2023 13:58:31 +0100 Subject: [PATCH] feat(joy_controller): allow to control without odometry (#6000) * feat(joy_controller): allow to control without odometry Signed-off-by: amadeuszsz * style(pre-commit): autofix --------- Signed-off-by: amadeuszsz Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- control/joy_controller/README.md | 1 + .../config/joy_controller.param.yaml | 1 + .../include/joy_controller/joy_controller.hpp | 1 + .../schema/joy_controller.schema.json | 6 ++++++ .../src/joy_controller/joy_controller_node.cpp | 15 ++++++++++----- 5 files changed, 19 insertions(+), 5 deletions(-) diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index d1215481188c7..b8ee8ad79a33d 100644 --- 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 index 8d48948a2d133..73a5d028985c5 100644 --- 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 index c6de42afc38d9..22064f9cefaad 100644 --- 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 index 803997ced6dae..d4c6351324935 100644 --- 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 index d82967324aef7..5eec438032410 100644 --- 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("steering_angle_velocity"); accel_sensitivity_ = declare_parameter("accel_sensitivity"); brake_sensitivity_ = declare_parameter("brake_sensitivity"); + raw_control_ = declare_parameter("control_command.raw_control"); velocity_gain_ = declare_parameter("control_command.velocity_gain"); max_forward_velocity_ = declare_parameter("control_command.max_forward_velocity"); max_backward_velocity_ = declare_parameter("control_command.max_backward_velocity"); @@ -480,10 +481,14 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & sub_joy_ = this->create_subscription( "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), subscriber_option); - sub_odom_ = this->create_subscription( - "input/odometry", 1, - std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), - subscriber_option); + if (!raw_control_) { + sub_odom_ = this->create_subscription( + "input/odometry", 1, + std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), + subscriber_option); + } else { + twist_ = std::make_shared(); + } // Publisher pub_control_command_ =