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