thomas/Cobots
2
1
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Poked it with joint jog

Browse source
This commit is contained in:
John Farrell 2021-09-28 11:31:54 -04:00
parent 3db39b9ec3
commit e401bd00fa
Signed by: john
GPG key ID: 10543A0DA2EC1E12
2 changed files with 22 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/ar3_config/config/ar3_joy.yaml
View file

@ -34,7 +34,7 @@ planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'wo
## Other frames
ee_frame_name: link_6 # The name of the end effector link, used to return the EE pose
robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector
robot_link_command_frame: link_6 # commands must be given in the frame of a robot link. Usually either the base or end effector
## Stopping behaviour
incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command

25
src/joystickmove/joystickmove/move.py
View file

@ -7,6 +7,7 @@ from std_msgs.msg import Float32
from sensor_msgs.msg import Joy
from geometry_msgs.msg import TwistStamped
from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
from control_msgs.msg import JointJog
from enum import Enum
@ -43,13 +44,14 @@ class JoyMove(Node):
JOY_TOPIC = '/joy'
TWIST_TOPIC = '/servo_server/delta_twist_cmds'
# JOINT_TOPIC = '/servo_server/delta_joint_cmds'
# EE = 'joint_6'
# BASE = 'base_link'
JOINT_TOPIC = '/servo_server/delta_joint_cmds'
EE = 'joint_6'
BASE = 'base_link'
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.joint_pub = self.create_publisher(JointJog, JOINT_TOPIC, 10);
self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10);
self.servo_client = self.create_client(Trigger, '/servo_server/start_servo')
@ -72,6 +74,21 @@ class JoyMove(Node):
axes = msg.axes
buttons = msg.buttons
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]:
joint = JointJog()
joint.joint_names = ['joint_1', 'joint_2',
'joint_6', 'joint_5']
joint.velocities = [float(axes[Axes.DPAD_UPDOWN.value]), float(axes[Axes.DPAD_LEFTRIGHT.value]),
float(buttons[Buttons.CIRCLE.value] - buttons[Buttons.CROSS.value]), float(buttons[Buttons.SQUARE.value] - buttons[Buttons.TRIANGLE.value])]
joint.duration = 1.0
joint.header.stamp = self.get_clock().now().to_msg()
joint.header.frame_id = "link_3"
self.joint_pub.publish(joint)
return
ts = TwistStamped()
ts.twist.linear.z = axes[Axes.RIGHT_Y.value]
@ -89,7 +106,7 @@ class JoyMove(Node):
ts.twist.angular.z = float(roll_positive + roll_negative)
ts.header.stamp = self.get_clock().now().to_msg()
ts.header.frame_id = "base_link"
ts.header.frame_id = "link_6"
self.twist_pub.publish(ts)