def __start_arm_control(self):
# nodes and publishers for six angle joints
self.arm_cmd_nodes_pubs = []
for x in range(1, 4):
for y in range(1, 3):
node = rclpy.create_node("mara_arm_cmd_" + str(x) + "_" +
str(y))
self.arm_cmd_nodes_pubs.append([
node,
node.create_publisher(
GoalRotaryServo,
'/hrim_actuator_rotaryservo_00000000000' + str(x) +
'/goal_axis' + str(y),
qos_profile=qos_profile_sensor_data)
])
# declare joint angle messages
self.arm_cmd_msgs = []
for i in range(6):
msg = GoalRotaryServo()
msg.position = 0.0 # Position to rads
msg.velocity = 30. # Velocity in rads/s
msg.control_type = 4 # Position and velocity control
self.arm_cmd_msgs.append(msg)
# gripper node and service client
node = rclpy.create_node("mara_gripper_cmd")
client = node.create_client(
ControlFinger, "/hrim_actuator_gripper_000000000004/fingercontrol")
self.arm_cmd_nodes_pubs.append([node, client])
# declare gripper service request
req = ControlFinger.Request()
req.goal_velocity = 250.
req.goal_angularposition = 0.0
self.arm_cmd_msgs.append(req)
def __init__(self):
super().__init__('mara_minimal_client')
# Create a client for service "/hrim_actuation_gripper_000000000004/goal"
self.client = self.create_client(ControlFinger, "/hrim_actuation_gripper_000000000004/goal")
# Wait for service to be avaiable before calling it
while not self.client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service not available, waiting again...')
# Create request with the same type as the service, ControlFinger
self.req = ControlFinger.Request()
def moveGripper(self):
print('moveGripper', self.vel, self.pos, self.effort)
req = ControlFinger.Request()
req.goal_velocity = self.vel # velocity range: 30 - 250 mm/s
req.goal_effort = self.effort # forces range: 10 - 125 N
req.goal_angularposition = self.pos # position range 0 - 0.87 rad
future = self.cli.call_async(req)
rclpy.spin_until_future_complete(self.node, future)
if future.result() is not None:
self.node.get_logger().info('Goal accepted: %d: ' %
future.result().goal_accepted)
else:
self.node.get_logger().error(
'Exception while calling service: %r' % future.exception())
def initUI(self, gripper, gripperService):
self.gripper = gripper
self.msg = GoalRotaryServo()
self.msg.velocity = 0.1
self.msg.control_type = 4
grid = QGridLayout()
self.setLayout(grid)
if self.gripper != 'None':
self.client = self.node.create_client(ControlFinger,
gripperService)
while not self.client.wait_for_service(timeout_sec=1.0):
print(gripperService +
' service not available, waiting again...')
self.req = ControlFinger.Request()
if self.gripper == '140':
self.req.goal_velocity = 30.0
else:
self.req.goal_velocity = 20.0
self.labelGripperPos = QLabel("Gripper: 0")
self.sldGripperPos = QSlider(Qt.Horizontal, self)
self.sldGripperPos.setMinimum(0)
self.sldGripperPos.setMaximum(87)
self.sldGripperPos.setFocusPolicy(Qt.NoFocus)
self.sldGripperPos.setGeometry(30, 40, 100, 30)
self.sldGripperPos.setTickInterval(1)
self.sldGripperPos.valueChanged[int].connect(
self.changePositionGripper)
self.labelGripperVel = QLabel("Speed: 30")
self.sldGripperVel = QSlider(Qt.Horizontal, self)
if self.gripper == '140':
self.sldGripperVel.setMinimum(300)
self.sldGripperVel.setMaximum(2500)
else:
self.sldGripperVel.setMinimum(1500)
self.sldGripperVel.setMaximum(200)
self.sldGripperVel.setFocusPolicy(Qt.NoFocus)
self.sldGripperVel.setGeometry(30, 40, 100, 30)
self.sldGripperVel.setTickInterval(1)
self.sldGripperVel.valueChanged[int].connect(
self.changeVelocityGripper)
grid.addWidget(self.labelGripperPos, 10, 0)
grid.addWidget(self.sldGripperPos, 10, 1)
grid.addWidget(self.labelGripperVel, 11, 0)
grid.addWidget(self.sldGripperVel, 11, 1)
self.labelJoint1 = QLabel("Joint 1: 0")
self.sldJoint1 = QSlider(Qt.Horizontal, self)
self.sldJoint1.setMinimum(-180)
self.sldJoint1.setMaximum(180)
self.sldJoint1.setFocusPolicy(Qt.NoFocus)
self.sldJoint1.setGeometry(30, 40, 100, 30)
self.sldJoint1.setTickInterval(0.1)
self.sldJoint1.valueChanged[int].connect(self.changePositionJoint1)
self.labelJoint2 = QLabel("Joint 2: 0")
self.sldJoint2 = QSlider(Qt.Horizontal, self)
self.sldJoint2.setMinimum(-180)
self.sldJoint2.setMaximum(180)
self.sldJoint2.setFocusPolicy(Qt.NoFocus)
self.sldJoint2.setGeometry(30, 40, 100, 30)
self.sldJoint2.setTickInterval(0.1)
self.sldJoint2.valueChanged[int].connect(self.changePositionJoint2)
self.labelJoint3 = QLabel("Joint 3: 0")
self.sldJoint3 = QSlider(Qt.Horizontal, self)
self.sldJoint3.setMaximum(180)
self.sldJoint3.setMinimum(-180)
self.sldJoint3.setFocusPolicy(Qt.NoFocus)
self.sldJoint3.setGeometry(30, 40, 100, 30)
self.sldJoint3.setTickInterval(0.1)
self.sldJoint3.valueChanged[int].connect(self.changePositionJoint3)
self.labelJoint4 = QLabel("Joint 4: 0")
self.sldJoint4 = QSlider(Qt.Horizontal, self)
self.sldJoint4.setMinimum(-180)
self.sldJoint4.setMaximum(180)
self.sldJoint4.setFocusPolicy(Qt.NoFocus)
self.sldJoint4.setGeometry(30, 40, 100, 30)
self.sldJoint4.setTickInterval(0.1)
self.sldJoint4.valueChanged[int].connect(self.changePositionJoint4)
self.labelJoint5 = QLabel("Joint 5: 0")
self.sldJoint5 = QSlider(Qt.Horizontal, self)
self.sldJoint5.setMinimum(-180)
self.sldJoint5.setMaximum(180)
self.sldJoint5.setFocusPolicy(Qt.NoFocus)
self.sldJoint5.setGeometry(30, 40, 100, 30)
self.sldJoint5.setTickInterval(0.1)
self.sldJoint5.valueChanged[int].connect(self.changePositionJoint5)
self.labelJoint6 = QLabel("Joint 6: 0")
self.sldJoint6 = QSlider(Qt.Horizontal, self)
self.sldJoint6.setMinimum(-180)
self.sldJoint6.setMaximum(180)
self.sldJoint6.setFocusPolicy(Qt.NoFocus)
self.sldJoint6.setGeometry(30, 40, 100, 30)
self.sldJoint6.setTickInterval(0.1)
self.sldJoint6.valueChanged[int].connect(self.changePositionJoint6)
self.labelVelocity = QLabel("Speed: 0.3")
self.sldVelocity = QSlider(Qt.Horizontal, self)
self.sldVelocity.setMinimum(10)
self.sldVelocity.setMaximum(157)
self.sldVelocity.setFocusPolicy(Qt.NoFocus)
self.sldVelocity.setGeometry(30, 40, 100, 30)
self.sldVelocity.setTickInterval(1)
self.sldVelocity.valueChanged[int].connect(self.changeVelocity)
grid.addWidget(self.labelJoint1, 0, 0)
grid.addWidget(self.sldJoint1, 0, 1)
grid.addWidget(self.labelJoint2, 1, 0)
grid.addWidget(self.sldJoint2, 1, 1)
grid.addWidget(self.labelJoint3, 2, 0)
grid.addWidget(self.sldJoint3, 2, 1)
grid.addWidget(self.labelJoint4, 3, 0)
grid.addWidget(self.sldJoint4, 3, 1)
grid.addWidget(self.labelJoint5, 4, 0)
grid.addWidget(self.sldJoint5, 4, 1)
grid.addWidget(self.labelJoint6, 5, 0)
grid.addWidget(self.sldJoint6, 5, 1)
grid.addWidget(self.labelVelocity, 6, 0)
grid.addWidget(self.sldVelocity, 6, 1)
self.buttonHome = QPushButton('Go Home', self)
self.buttonHome.clicked.connect(self.goHome)
self.buttonA = QPushButton('Go A', self)
self.buttonA.clicked.connect(self.goA)
self.buttonB = QPushButton('Go B', self)
self.buttonB.clicked.connect(self.goB)
grid.addWidget(self.buttonHome, 7, 0)
grid.addWidget(self.buttonA, 8, 0)
grid.addWidget(self.buttonB, 8, 1)
self.setWindowTitle("MARA")
self.show()
from hrim_actuator_gripper_srvs.srv import ControlFinger
# -------- #
rclpy.init(args=None)
# Create Node with name "mara_minimal_client"
node = rclpy.create_node("mara_minimal_client")
# Create a client for service "/hrim_actuation_gripper_000000000004/goal"
client = node.create_client(ControlFinger,
"/hrim_actuation_gripper_000000000004/goal",
qos_profile=qos_profile_services_default)
# Create request with the same type as the service, ControlFinger
req = ControlFinger.Request()
# Position range 0 - 0.87 rad
# TODO: The range is wider for me (up to 9.0 sometimes)
req.goal_linearposition = 0.87
# Wait for service to be avaiable before calling it
while not client.wait_for_service(timeout_sec=1.0):
node.get_logger().info('Service not available, waiting again...')
# Call service and spin
future = client.call_async(req)
rclpy.spin_until_future_complete(node, future)
# Analyze the result
if future.result() is not None: