From c3bca8e9a5aa2f17ae042c2e130352f43d8960bf Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Fri, 12 Apr 2024 11:44:50 -0500 Subject: [PATCH 1/4] Fix managed_steam and add config for throttle --- .../src/autonav_launch/launch/managed_steam.xml | 15 ++++++++++++++- .../autonav_manual/src/remote_steamcontroller.cpp | 10 ++++++++-- display/scripts/globals.js | 8 ++++---- 3 files changed, 26 insertions(+), 7 deletions(-) diff --git a/autonav_ws/src/autonav_launch/launch/managed_steam.xml b/autonav_ws/src/autonav_launch/launch/managed_steam.xml index 5b1cfed5..fa972043 100644 --- a/autonav_ws/src/autonav_launch/launch/managed_steam.xml +++ b/autonav_ws/src/autonav_launch/launch/managed_steam.xml @@ -1,17 +1,30 @@ + + + + + - + + + + + + + + + \ No newline at end of file diff --git a/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp b/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp index e3c42369..ef671a83 100644 --- a/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp +++ b/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp @@ -19,8 +19,10 @@ struct SteamJoyNodeConfig float turn_speed; float max_turn_speed; float max_forward_speed; + bool invert_throttle; + bool invert_steering; - NLOHMANN_DEFINE_TYPE_INTRUSIVE(SteamJoyNodeConfig, throttle_deadzone, steering_deadzone, forward_speed, turn_speed, max_turn_speed, max_forward_speed) + NLOHMANN_DEFINE_TYPE_INTRUSIVE(SteamJoyNodeConfig, throttle_deadzone, steering_deadzone, forward_speed, turn_speed, max_turn_speed, max_forward_speed, invert_throttle, invert_steering) }; class SteamJoyNode : public SCR::Node @@ -51,6 +53,8 @@ class SteamJoyNode : public SCR::Node defaultConfig.turn_speed = 1.0f; defaultConfig.max_turn_speed = 3.14159265f; defaultConfig.max_forward_speed = 2.2f; + defaultConfig.invert_steering = true; + defaultConfig.invert_throttle = true; return defaultConfig; } @@ -119,7 +123,9 @@ class SteamJoyNode : public SCR::Node // input.forward_velocity = SCR::Utilities::clamp(throttle * config.forward_speed, -config.max_forward_speed, config.max_forward_speed); // input.angular_velocity = -1 * SCR::Utilities::clamp(steering * config.turn_speed, -config.max_turn_speed, config.max_turn_speed); input.forward_velocity = SCR::Utilities::clamp(current_throttle, -config.max_forward_speed, config.max_forward_speed); - input.angular_velocity = -1 * SCR::Utilities::clamp(current_steering * config.turn_speed, -config.max_turn_speed, config.max_turn_speed); + input.angular_velocity = SCR::Utilities::clamp(current_steering * config.turn_speed, -config.max_turn_speed, config.max_turn_speed); + input.forward_velocity *= config.invert_throttle ? -1 : 1; + input.angular_velocity *= config.invert_steering ? -1 : 1; motor_publisher->publish(input); } diff --git a/display/scripts/globals.js b/display/scripts/globals.js index 1052e765..80b23f9b 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -43,9 +43,7 @@ var addressKeys = { "blur_weight": "int", "blur_iterations": "int", "rod_offset": "int", - "map_res": "int", - "image_warp_tl": "float", - "image_warp_tr": "float" + "map_res": "int" }, "autonav_vision_expandifier": { @@ -72,7 +70,9 @@ var addressKeys = { "throttle_deadzone": "float", "turn_speed": "float", "max_forward_speed": "float", - "max_turn_speed": "float" + "max_turn_speed": "float", + "invert_steering": "bool", + "invert_throttle": "bool" }, "autonav_nav_astar": { From 9e4ee49dff28686c777dfabb94a09706f22bb41c Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Fri, 12 Apr 2024 14:01:04 -0500 Subject: [PATCH 2/4] Update node name in SteamJoyNode constructor --- autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp b/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp index ef671a83..fa0a7028 100644 --- a/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp +++ b/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp @@ -28,7 +28,7 @@ struct SteamJoyNodeConfig class SteamJoyNode : public SCR::Node { public: - SteamJoyNode() : SCR::Node("autonav_manual_steam") + SteamJoyNode() : SCR::Node("autonav_manual_steamcontroller") { steam_subscription = create_subscription("/autonav/joy/steam", 20, std::bind(&SteamJoyNode::onSteamDataReceived, this, std::placeholders::_1)); motor_publisher = create_publisher("/autonav/MotorInput", 20); From 1b4d387bde36fbef2ccf526a6cdf93dd23f3f54e Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Fri, 12 Apr 2024 14:18:53 -0500 Subject: [PATCH 3/4] Update throttle and steering rates in SteamJoyNodeConfig and globals.js --- .../autonav_manual/src/remote_steamcontroller.cpp | 12 +++++++----- display/scripts/globals.js | 4 +++- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp b/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp index fa0a7028..c504d4cc 100644 --- a/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp +++ b/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp @@ -21,8 +21,10 @@ struct SteamJoyNodeConfig float max_forward_speed; bool invert_throttle; bool invert_steering; + float throttle_rate; + float steering_rate; - NLOHMANN_DEFINE_TYPE_INTRUSIVE(SteamJoyNodeConfig, throttle_deadzone, steering_deadzone, forward_speed, turn_speed, max_turn_speed, max_forward_speed, invert_throttle, invert_steering) + NLOHMANN_DEFINE_TYPE_INTRUSIVE(SteamJoyNodeConfig, throttle_deadzone, steering_deadzone, forward_speed, turn_speed, max_turn_speed, max_forward_speed, invert_throttle, invert_steering, throttle_rate, steering_rate) }; class SteamJoyNode : public SCR::Node @@ -55,6 +57,8 @@ class SteamJoyNode : public SCR::Node defaultConfig.max_forward_speed = 2.2f; defaultConfig.invert_steering = true; defaultConfig.invert_throttle = true; + defaultConfig.throttle_rate = 0.01f; + defaultConfig.steering_rate = 0.01f; return defaultConfig; } @@ -114,10 +118,8 @@ class SteamJoyNode : public SCR::Node } // Generate a forward/angular velocity command that ramps up/down smoothly - const float throttleRate = 0.03; - const float steeringRate = 0.01; - current_throttle = lerp(current_throttle, target_throttle * config.forward_speed, throttleRate * (is_working ? 1 : 1.8)); - current_steering = lerp(current_steering, target_steering * config.turn_speed, steeringRate * (is_working ? 1 : 1.8)); + current_throttle = lerp(current_throttle, target_throttle * config.forward_speed, config.throttle_rate * (is_working ? 1 : 2.4)); + current_steering = lerp(current_steering, target_steering * config.turn_speed, config.steering_rate * (is_working ? 1 : 1.8)); autonav_msgs::msg::MotorInput input; // input.forward_velocity = SCR::Utilities::clamp(throttle * config.forward_speed, -config.max_forward_speed, config.max_forward_speed); diff --git a/display/scripts/globals.js b/display/scripts/globals.js index 80b23f9b..a5d5cdae 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -72,7 +72,9 @@ var addressKeys = { "max_forward_speed": "float", "max_turn_speed": "float", "invert_steering": "bool", - "invert_throttle": "bool" + "invert_throttle": "bool", + "throttle_rate": "float", + "steering_rate": "float" }, "autonav_nav_astar": { From f7280176acf6cab3bc6e822f584fc5b37a5e5261 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Fri, 12 Apr 2024 14:21:42 -0500 Subject: [PATCH 4/4] Update throttle and steering rates in SteamJoyNodeConfig and remote_steamcontroller.cpp --- autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp b/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp index c504d4cc..918647f5 100644 --- a/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp +++ b/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp @@ -52,13 +52,13 @@ class SteamJoyNode : public SCR::Node defaultConfig.throttle_deadzone = 0.15f; defaultConfig.steering_deadzone = 0.15f; defaultConfig.forward_speed = 1.8f; - defaultConfig.turn_speed = 1.0f; + defaultConfig.turn_speed = 1.3f; defaultConfig.max_turn_speed = 3.14159265f; defaultConfig.max_forward_speed = 2.2f; defaultConfig.invert_steering = true; defaultConfig.invert_throttle = true; defaultConfig.throttle_rate = 0.01f; - defaultConfig.steering_rate = 0.01f; + defaultConfig.steering_rate = 0.02f; return defaultConfig; }