Skip to content

Publish battery state message #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
HansRobo wants to merge 3 commits into
base: ros2
Choose a base branch
from
Open

Publish battery state message #4

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
dad5a06
Publish battery state message
HansRobo Apr 5, 2025
fefe2a8
Fix battery publisher type
HansRobo Apr 5, 2025
24e27c2
Delete unused import
HansRobo Apr 5, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
59 changes: 58 additions & 1 deletion scripts/dualsense_driver.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,8 @@
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):
Expand All @@ -12,6 +13,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(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
Expand Down Expand Up @@ -70,7 +72,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()

Expand Down