34 """ PS4 dualshock teleop device implementation. """
37 from multiprocessing
import Process
39 from geometry_msgs.msg
import TwistStamped
40 from sensor_msgs.msg
import Joy
41 from std_srvs.srv
import Trigger
43 from dataclasses
import dataclass
57 RIGHT_STICK_X: int = 3
58 RIGHT_STICK_Y: int = 4
59 RIGHT_TRIGGER: int = 5
77 LEFT_STICK_TRIGGER: int = 11
78 RIGHT_STICK_TRIGGER: int = 12
84 A dataclass to store device config. This class follows the conventions of Joy message (http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Joy.html)
98 "A class which encapsulates teleoperation functionalities for ps4 dualshock device."
103 node_name: str =
"ps4_dualshock_teleop",
104 device_name: str =
"ps4_dualshock",
109 device_name=device_name,
110 device_config=device_config,
111 ee_frame_name=ee_frame_name,
113 self.
loggerlogger = rclpy.logging.get_logger(
"ps4_dualshock_teleop")
117 Publishes the teleop command.
121 twist = TwistStamped()
122 twist.twist.linear.z = data.axes[self.
device_configdevice_config.Axes.RIGHT_STICK_Y]
123 twist.twist.linear.y = data.axes[self.
device_configdevice_config.Axes.RIGHT_STICK_X]
125 lin_x_right = -0.5 * (data.axes[self.
device_configdevice_config.Axes.RIGHT_TRIGGER])
126 lin_x_left = 0.5 * (data.axes[self.
device_configdevice_config.Axes.LEFT_TRIGGER])
127 twist.twist.linear.x = lin_x_right + lin_x_left
129 twist.twist.angular.y = data.axes[self.
device_configdevice_config.Axes.LEFT_STICK_Y]
130 twist.twist.angular.x = data.axes[self.
device_configdevice_config.Axes.LEFT_STICK_X]
132 roll_positive = data.buttons[self.
device_configdevice_config.Buttons.R1]
133 roll_negative = -1 * data.buttons[self.
device_configdevice_config.Buttons.L1]
134 twist.twist.angular.z = float(roll_positive + roll_negative)
137 twist.header.stamp = self.get_clock().now().to_msg()
139 except Exception
as e:
140 self.
loggerlogger.info.error(e)
DualShock4 Device Mappings.
def __init__(self, str ee_frame_name, str node_name="ps4_dualshock_teleop", str device_name="ps4_dualshock", PS4DualShock device_config=PS4DualShock())
def publish_command(self, data)
void print(PropagationDistanceField &pdf, int numX, int numY, int numZ)