joystick works?
This commit is contained in:
parent
c0d20e2f7b
commit
ee0ba4cb33
6 changed files with 44 additions and 48 deletions
|
@ -4,7 +4,7 @@
|
||||||
use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
|
use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
|
||||||
|
|
||||||
## Properties of incoming commands
|
## Properties of incoming commands
|
||||||
command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
|
command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
|
||||||
scale:
|
scale:
|
||||||
# Scale parameters are only used if command_in_type=="unitless"
|
# Scale parameters are only used if command_in_type=="unitless"
|
||||||
linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
|
linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
|
||||||
|
|
|
@ -123,5 +123,6 @@ def generate_launch_description():
|
||||||
package='robot_state_publisher',
|
package='robot_state_publisher',
|
||||||
executable='robot_state_publisher',
|
executable='robot_state_publisher',
|
||||||
output='screen',
|
output='screen',
|
||||||
parameters=[robot_description]),
|
parameters=[robot_description]
|
||||||
|
),
|
||||||
])
|
])
|
||||||
|
|
|
@ -47,7 +47,7 @@ class StatePublisher(Node):
|
||||||
# joint_state.position = [random.random() * pi / 2] * 6
|
# joint_state.position = [random.random() * pi / 2] * 6
|
||||||
# joint_state.position = [angle * 16, pi/2., -25 / 180 * pi, 0., 0., 0.]
|
# joint_state.position = [angle * 16, pi/2., -25 / 180 * pi, 0., 0., 0.]
|
||||||
# joint_state.position = [angle * 0.01 * random.random()] * 6
|
# joint_state.position = [angle * 0.01 * random.random()] * 6
|
||||||
joint_state.position = [0., 0., 0., 0., 0., 0.]
|
# joint_state.position = [0., 0., 0., 0., 0., 0.]
|
||||||
|
|
||||||
# update transform
|
# update transform
|
||||||
# (moving in a circle with radius=2)
|
# (moving in a circle with radius=2)
|
||||||
|
|
|
@ -3,20 +3,12 @@ from rclpy.node import Node
|
||||||
from rclpy.qos import QoSProfile
|
from rclpy.qos import QoSProfile
|
||||||
|
|
||||||
from sensor_msgs.msg import JointState
|
from sensor_msgs.msg import JointState
|
||||||
|
from std_msgs.msg import Float32
|
||||||
|
|
||||||
from math import pi
|
from math import pi
|
||||||
|
|
||||||
from std_msgs.msg import Float32
|
|
||||||
|
|
||||||
from serial import Serial
|
from serial import Serial
|
||||||
|
|
||||||
from enum import Enum
|
|
||||||
|
|
||||||
class Servos(Enum):
|
|
||||||
J1 = 'A'
|
|
||||||
J2 = 'B'
|
|
||||||
J3 = 'C'
|
|
||||||
|
|
||||||
class CobotMove(Node):
|
class CobotMove(Node):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
super().__init__('cobot_move')
|
super().__init__('cobot_move')
|
||||||
|
@ -39,12 +31,12 @@ class CobotMove(Node):
|
||||||
self.create_subscription(Float32, base_topic+'j6', self._move_j6_cb, rclpy.qos.qos_profile_sensor_data)
|
self.create_subscription(Float32, base_topic+'j6', self._move_j6_cb, rclpy.qos.qos_profile_sensor_data)
|
||||||
|
|
||||||
publish_base_topic = 'update/'
|
publish_base_topic = 'update/'
|
||||||
self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', rclpy.qos.qos_profile_sensor_data)
|
self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', 10)
|
||||||
self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', rclpy.qos.qos_profile_sensor_data)
|
self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', 10)
|
||||||
self.s_j3 = self.create_publisher(Float32, publish_base_topic+'j3', rclpy.qos.qos_profile_sensor_data)
|
self.s_j3 = self.create_publisher(Float32, publish_base_topic+'j3', 10)
|
||||||
self.s_j4 = self.create_publisher(Float32, publish_base_topic+'j4', rclpy.qos.qos_profile_sensor_data)
|
self.s_j4 = self.create_publisher(Float32, publish_base_topic+'j4', 10)
|
||||||
self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', rclpy.qos.qos_profile_sensor_data)
|
self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', 10)
|
||||||
self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', rclpy.qos.qos_profile_sensor_data)
|
self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', 10)
|
||||||
|
|
||||||
qos = QoSProfile(depth=10)
|
qos = QoSProfile(depth=10)
|
||||||
self._joint_pub = self.create_publisher(JointState, 'joint_states', qos)
|
self._joint_pub = self.create_publisher(JointState, 'joint_states', qos)
|
||||||
|
@ -80,7 +72,6 @@ class CobotMove(Node):
|
||||||
def _move_servo(self):
|
def _move_servo(self):
|
||||||
self._serial.write(f'MTA{self._cur[0]}B{self._cur[1]}C{self._cur[2]}D{self._cur[3]}E{self._cur[4]}F{self._cur[5]}\n'.encode('UTF-8'))
|
self._serial.write(f'MTA{self._cur[0]}B{self._cur[1]}C{self._cur[2]}D{self._cur[3]}E{self._cur[4]}F{self._cur[5]}\n'.encode('UTF-8'))
|
||||||
|
|
||||||
|
|
||||||
line = self._serial.readline().decode('UTF-8')
|
line = self._serial.readline().decode('UTF-8')
|
||||||
|
|
||||||
blocks = re.findall(r'[ABCDEF]-*\d+', line)
|
blocks = re.findall(r'[ABCDEF]-*\d+', line)
|
||||||
|
@ -103,7 +94,6 @@ class CobotMove(Node):
|
||||||
self.s_j4.publish(Float32(data=d))
|
self.s_j4.publish(Float32(data=d))
|
||||||
self.s_j5.publish(Float32(data=e))
|
self.s_j5.publish(Float32(data=e))
|
||||||
self.s_j6.publish(Float32(data=f))
|
self.s_j6.publish(Float32(data=f))
|
||||||
|
|
||||||
# self._logger.info(f'published {a} {b} {c} {d} {e} {f}')
|
# self._logger.info(f'published {a} {b} {c} {d} {e} {f}')
|
||||||
# def _push ?
|
# def _push ?
|
||||||
|
|
||||||
|
|
|
@ -43,16 +43,16 @@ class JoyMove(Node):
|
||||||
self._logger.info('joystick init')
|
self._logger.info('joystick init')
|
||||||
|
|
||||||
JOY_TOPIC = '/joy'
|
JOY_TOPIC = '/joy'
|
||||||
# TWIST_TOPIC = '/servo_server/delta_twist_cmds'
|
TWIST_TOPIC = '/servo_server/delta_twist_cmds'
|
||||||
JOINT_TOPIC = '/servo_server/delta_joint_cmds'
|
JOINT_TOPIC = '/servo_server/delta_joint_cmds'
|
||||||
# EE = 'joint_6'
|
EE = 'joint_6'
|
||||||
# BASE = 'base_link'
|
BASE = 'base_link'
|
||||||
|
|
||||||
self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data)
|
self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data)
|
||||||
|
|
||||||
# self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10);
|
self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10);
|
||||||
self.joint_pub = self.create_publisher(JointJog, JOINT_TOPIC, 10);
|
self.joint_pub = self.create_publisher(JointJog, JOINT_TOPIC, 10);
|
||||||
# self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10);
|
self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10);
|
||||||
|
|
||||||
self.servo_client = self.create_client(Trigger, '/servo_server/start_servo')
|
self.servo_client = self.create_client(Trigger, '/servo_server/start_servo')
|
||||||
# Check if the a service is available
|
# Check if the a service is available
|
||||||
|
@ -74,35 +74,40 @@ class JoyMove(Node):
|
||||||
axes = msg.axes
|
axes = msg.axes
|
||||||
buttons = msg.buttons
|
buttons = msg.buttons
|
||||||
|
|
||||||
# ts = TwistStamped()
|
if buttons[Buttons.CROSS.value] or buttons[Buttons.CIRCLE.value] or buttons[Buttons.TRIANGLE.value] or buttons[Buttons.SQUARE.value] or axes[Axes.DPAD_UPDOWN.value] or axes[Axes.DPAD_LEFTRIGHT.value]:
|
||||||
jj = JointJog()
|
joint = JointJog()
|
||||||
|
|
||||||
jj.joint_names = ['joint_2']
|
joint.joint_names = ['joint_1', 'joint_2',
|
||||||
# jj.displacements = [axes[0]]
|
'joint_6', 'joint_5']
|
||||||
jj.velocities = [axes[0]]
|
joint.velocities = [float(axes[Axes.DPAD_UPDOWN.value]), float(axes[Axes.DPAD_LEFTRIGHT.value]),
|
||||||
# jj.duration = 1.0
|
float(buttons[Buttons.CIRCLE.value] - buttons[Buttons.CROSS.value]), float(buttons[Buttons.SQUARE.value] - buttons[Buttons.TRIANGLE.value])]
|
||||||
# ts.twist.linear.z = axes[Axes.RIGHT_Y.value]
|
joint.duration = 0.1
|
||||||
# ts.twist.linear.y = axes[Axes.RIGHT_X.value]
|
|
||||||
|
|
||||||
# lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1));
|
joint.header.stamp = self.get_clock().now().to_msg()
|
||||||
# lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1));
|
joint.header.frame_id = "link_3"
|
||||||
# ts.twist.linear.x = lin_x_right + lin_x_left
|
self.joint_pub.publish(joint)
|
||||||
|
return
|
||||||
|
|
||||||
# ts.twist.angular.y = axes[Axes.LEFT_Y.value]
|
ts = TwistStamped()
|
||||||
# ts.twist.angular.x = axes[Axes.LEFT_X.value]
|
|
||||||
|
|
||||||
# roll_positive = buttons[Buttons.R2.value]
|
ts.twist.linear.z = axes[Axes.RIGHT_Y.value]
|
||||||
# roll_negative = -1 * buttons[Buttons.L2.value]
|
ts.twist.linear.y = axes[Axes.RIGHT_X.value]
|
||||||
# ts.twist.angular.z = float(roll_positive + roll_negative)
|
|
||||||
|
|
||||||
# ts.header.stamp = self.get_clock().now().to_msg()
|
lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1));
|
||||||
# ts.header.frame_id = "base_link"
|
lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1));
|
||||||
|
ts.twist.linear.x = lin_x_right + lin_x_left
|
||||||
|
|
||||||
jj.header.stamp = self.get_clock().now().to_msg()
|
ts.twist.angular.y = axes[Axes.LEFT_Y.value]
|
||||||
jj.header.frame_id = "base_link"
|
ts.twist.angular.x = axes[Axes.LEFT_X.value]
|
||||||
|
|
||||||
# self.twist_pub.publish(ts)
|
roll_positive = buttons[Buttons.R2.value]
|
||||||
self.joint_pub.publish(jj)
|
roll_negative = -1 * buttons[Buttons.L2.value]
|
||||||
|
ts.twist.angular.z = float(roll_positive + roll_negative)
|
||||||
|
|
||||||
|
ts.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
ts.header.frame_id = "base_link"
|
||||||
|
|
||||||
|
self.twist_pub.publish(ts)
|
||||||
|
|
||||||
|
|
||||||
def main(args=None):
|
def main(args=None):
|
||||||
|
|
Loading…
Reference in a new issue