def __init__(self, status_topic='status', feedback_topic='set_feedback'):
self._min_interval = 0.1
self._last_pub_time = rospy.Time()
self._prev = Status()
self._led = {
'r': 0,
'g': 0,
'b': 0,
'flashing': False,
}
self._pub_feedback = rospy.Publisher(feedback_topic, Feedback, queue_size=1)
rospy.Subscriber(status_topic, Status, self.cb_status, queue_size=1)
def __init__(self, status_topic="status", feedback_topic="set_feedback"):
self._min_interval = 0.1
self._last_pub_time = rospy.Time()
self._prev = Status()
self._led = {
"r": 0,
"g": 0,
"b": 0,
"flashing": False,
}
self._pub_feedback = rospy.Publisher(feedback_topic,
Feedback,
queue_size=1)
rospy.Subscriber(status_topic, Status, self.cb_status, queue_size=1)
def __init__(self,
node,
status_topic="status",
feedback_topic="set_feedback"):
self._node = node
self._min_interval = 0.1
self._last_pub_time = self._node.get_clock().now()
self._prev = Status()
self._led = {
"r": 0,
"g": 0,
"b": 0,
"flashing": False,
}
self._pub_feedback = self._node.create_publisher(
Feedback, feedback_topic, 0)
self._sub_status = self._node.create_subscription(
Status, status_topic, self.cb_status, 0)
def _report_to_status_(report_msg, deadzone=0.05):
status_msg = Status()
status_msg.header = copy.deepcopy(report_msg.header)
# Sticks (signs are flipped for consistency with other joypads)
status_msg.axis_left_x = -ControllerRos._normalize_axis_(
report_msg.left_analog_x, deadzone)
status_msg.axis_left_y = -ControllerRos._normalize_axis_(
report_msg.left_analog_y, deadzone)
status_msg.axis_right_x = -ControllerRos._normalize_axis_(
report_msg.right_analog_x, deadzone)
status_msg.axis_right_y = -ControllerRos._normalize_axis_(
report_msg.right_analog_y, deadzone)
# Shoulder buttons
status_msg.axis_l2 = report_msg.l2_analog / 255.0
status_msg.axis_r2 = report_msg.r2_analog / 255.0
# Buttons
status_msg.button_dpad_up = report_msg.dpad_up
status_msg.button_dpad_down = report_msg.dpad_down
status_msg.button_dpad_left = report_msg.dpad_left
status_msg.button_dpad_right = report_msg.dpad_right
plug_attrs = [
attr for attr in dir(report_msg) if attr.startswith('button_')
]
for attr in plug_attrs:
val = getattr(report_msg, attr)
setattr(status_msg, attr, val)
# IMU (X: right, Y: up, Z: towards user)
status_msg.imu.header = copy.deepcopy(status_msg.header)
# To m/s^2: 0.98 mg/LSB (BMI055 data sheet Chapter 5.2.1)
def to_mpss(v):
return float(v) / (2**13 - 1) * 9.80665 * 0.98
# To rad/s: 32767: 2000 deg/s (BMI055 data sheet Chapter 7.2.1)
def to_radps(v):
return float(v) / (2**15 - 1) * math.pi / 180
# Convert
status_msg.imu.linear_acceleration.x = to_mpss(report_msg.lin_acc_x)
status_msg.imu.linear_acceleration.y = to_mpss(report_msg.lin_acc_y)
status_msg.imu.linear_acceleration.z = to_mpss(report_msg.lin_acc_z)
status_msg.imu.angular_velocity.x = to_radps(report_msg.ang_vel_x)
status_msg.imu.angular_velocity.y = to_radps(report_msg.ang_vel_y)
status_msg.imu.angular_velocity.z = to_radps(report_msg.ang_vel_z)
# No orientation reported
status_msg.imu.orientation_covariance[0] = -1
# Trackpads
status_msg.touch0.id = report_msg.trackpad_touch0_id
status_msg.touch0.active = report_msg.trackpad_touch0_active
status_msg.touch0.x = report_msg.trackpad_touch0_x / float(
Controller.TOUCHPAD_MAX_X)
status_msg.touch0.y = report_msg.trackpad_touch0_y / float(
Controller.TOUCHPAD_MAX_Y)
status_msg.touch1.id = report_msg.trackpad_touch1_id
status_msg.touch1.active = report_msg.trackpad_touch1_active
status_msg.touch1.x = report_msg.trackpad_touch1_x / float(
Controller.TOUCHPAD_MAX_X)
status_msg.touch1.y = report_msg.trackpad_touch1_y / float(
Controller.TOUCHPAD_MAX_Y)
# Battery
if report_msg.battery == Controller.BATTERY_FULL_CHARGING:
status_msg.battery_full_charging = True
status_msg.battery_percentage = 1.0
else:
status_msg.battery_full_charging = False
status_msg.battery_percentage = float(
report_msg.battery) / Controller.BATTERY_MAX
# Plugs
plug_attrs = [
attr for attr in dir(report_msg) if attr.startswith('plug_')
]
for attr in plug_attrs:
val = getattr(report_msg, attr)
setattr(status_msg, attr, val)
return status_msg