From dad5a06e1d3846d58c4b7f38b34838be7aced44e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 5 Apr 2025 15:28:58 +0900 Subject: [PATCH 1/3] Publish battery state message --- scripts/dualsense_driver.py | 60 ++++++++++++++++++++++++++++++++++++- 1 file changed, 59 insertions(+), 1 deletion(-) diff --git a/scripts/dualsense_driver.py b/scripts/dualsense_driver.py index 1472e9d..455fe1f 100755 --- a/scripts/dualsense_driver.py +++ b/scripts/dualsense_driver.py @@ -1,9 +1,11 @@ #!/usr/bin/env python3 +import math import rclpy from rclpy.node import Node from pydualsense import pydualsense, TriggerModes -from sensor_msgs.msg import Joy +import pydualsense.enums as enums +from sensor_msgs.msg import Joy, BatteryState from rcl_interfaces.msg import SetParametersResult class DualSenseDriver(Node): @@ -12,6 +14,7 @@ def __init__(self): self.dualsense = pydualsense() self.dualsense.init() self.joy_pub = self.create_publisher(Joy, '/joy', 10) + self.battery_pub = self.create_publisher(Joy, '/battery', 10) self.timer = self.create_timer(1.0/60.0, self.update) self.right_motor_power = self.declare_parameter('right_motor', 0).value @@ -70,7 +73,62 @@ def update(self): joy.buttons.append(self.dualsense.state.ps) joy.buttons.append(self.dualsense.state.touchBtn) self.joy_pub.publish(joy) + self.battery_pub.publish(self.get_battery_state_msg()) + def get_battery_state_msg(self): + battery_state_msg = BatteryState() + battery_state = self.dualsense.battery.State + + if battery_state == enums.BatteryState.POWER_SUPPLY_STATUS_CHARGING: + battery_state_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING + elif battery_state == enums.BatteryState.POWER_SUPPLY_STATUS_DISCHARGING: + battery_state_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + elif battery_state == enums.BatteryState.POWER_SUPPLY_STATUS_FULL: + battery_state_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL + elif battery_state == enums.BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING: + battery_state_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING + else: + battery_state_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_UNKNOWN + + battery_state_msg.present = True # controller connected + battery_state_msg.percentage = self.dualsense.battery.Level / 100.0 # pydualsense report range is 0-100 + battery_state_msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LION + battery_state_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD + + battery_state_msg.voltage = float('nan') + if battery_state_msg.percentage <= 0.0: + battery_state_msg.voltage = 3.0 + elif battery_state_msg.percentage < 0.1: + battery_state_msg.voltage = 3.5 + elif battery_state_msg.percentage < 0.2: + battery_state_msg.voltage = 3.7 + elif battery_state_msg.percentage < 0.3: + battery_state_msg.voltage = 3.8 + elif battery_state_msg.percentage < 0.4: + battery_state_msg.voltage = 3.9 + elif battery_state_msg.percentage < 0.5: + battery_state_msg.voltage = 3.95 + elif battery_state_msg.percentage < 0.6: + battery_state_msg.voltage = 4.0 + elif battery_state_msg.percentage < 0.7: + battery_state_msg.voltage = 4.05 + elif battery_state_msg.percentage < 0.8: + battery_state_msg.voltage = 4.1 + elif battery_state_msg.percentage < 0.9: + battery_state_msg.voltage = 4.15 + else: + battery_state_msg.voltage = 4.2 + battery_state_msg.temperature = float('nan') + battery_state_msg.current = float('nan') + battery_state_msg.charge = float('nan') + battery_state_msg.capacity = float('nan') + battery_state_msg.design_capacity = float('nan') + battery_state_msg.location = "" + battery_state_msg.serial_number = "" + + battery_state_msg.header.stamp = self.get_clock().now().to_msg() + battery_state_msg.header.frame_id = "dualsense_controller" + return battery_state_msg def shutdown(self): self.dualsense.close() From fefe2a8a6eecfded0f52e71302280e6c2d011bad Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 5 Apr 2025 15:31:18 +0900 Subject: [PATCH 2/3] Fix battery publisher type Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- scripts/dualsense_driver.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/dualsense_driver.py b/scripts/dualsense_driver.py index 455fe1f..50359a2 100755 --- a/scripts/dualsense_driver.py +++ b/scripts/dualsense_driver.py @@ -14,7 +14,7 @@ def __init__(self): self.dualsense = pydualsense() self.dualsense.init() self.joy_pub = self.create_publisher(Joy, '/joy', 10) - self.battery_pub = self.create_publisher(Joy, '/battery', 10) + self.battery_pub = self.create_publisher(BatteryState, '/battery', 10) self.timer = self.create_timer(1.0/60.0, self.update) self.right_motor_power = self.declare_parameter('right_motor', 0).value From 24e27c268938e7dabf7420c7fae0b19a00279a35 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 5 Apr 2025 15:34:12 +0900 Subject: [PATCH 3/3] Delete unused import Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- scripts/dualsense_driver.py | 1 - 1 file changed, 1 deletion(-) diff --git a/scripts/dualsense_driver.py b/scripts/dualsense_driver.py index 50359a2..d933925 100755 --- a/scripts/dualsense_driver.py +++ b/scripts/dualsense_driver.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 -import math import rclpy from rclpy.node import Node from pydualsense import pydualsense, TriggerModes