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 changeValuePosition_lander2(self, value):
self.value_hans_axis_1 = value
msg = GoalRotaryServo()
msg.velocity = 0.404
msg.position = self.value_hans_axis_1 * 3.1416/180
self.publisher_lander2.publish(msg)
self.labelPosition_lander2.setText("Position " + str(value))
def changeValuePosition_axis2(self, value):
self.value_hans_axis_2 = value
msg = GoalRotaryServo()
msg.position = self.value_hans_axis_2 * 3.1416/180
msg.velocity = 0.404
msg.control_type = 1
self.publisher_axis2.publish(msg)
self.labelPosition_axis2.setText("Position " + str(value))
def changeValuePosition_hebi(self, value):
self.value_hebi = value
msg = GoalRotaryServo()
msg.position = self.value_hebi * 3.1416/180
msg.velocity = 0.404
msg.control_type = 1
self.publisher_hebi.publish(msg)
self.labelPosition_hebi.setText("Position " + str(self.value_hebi))
def changeValuePosition_pal(self, value):
self.value_pal = value
msg = GoalRotaryServo()
msg.position = self.value_pal * 3.1416 / 180
msg.velocity = 0.01
msg.control_type = 4
self.publisher_pal.publish(msg)
self.labelPosition_pal.setText("Position " + str(value))
def __init__(self):
# Initialize Node with name "mara_minimal_publisher"
super().__init__('mara_minimal_publisher')
# Create a publisher on topic "/hrim_actuation_servomotor_000000000001/goal_axis1"
self.pub_ = self.create_publisher(
GoalRotaryServo,
'/hrim_actuator_rotaryservo_000000000001/goal_axis1',
qos_profile=qos_profile_sensor_data)
# Create message with the same type as the topic, GoalRotaryServo
self.msg = GoalRotaryServo()
# Create a timer to publish the messages periodically
timer_period = 1.0 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
self.i = 0 # Iteration counter
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()
# -------- #
rclpy.init(args=None)
# Create Node with name "mara_minimal_publisher"
node = rclpy.create_node("mara_minimal_publisher")
# Create a publisher on topic "/hrim_actuation_servomotor_000000000001/goal_axis1"
pub = node.create_publisher(
GoalRotaryServo,
'/hrim_actuator_rotaryservo_000000000001/goal_axis1',
qos_profile=qos_profile_sensor_data)
# Create message with the same type as the topic, GoalRotaryServo
msg = GoalRotaryServo()
# Desired position in degrees
position_deg = 30.
# Loop
i = 1 # Loop counter
while rclpy.ok():
# Fill message content
msg.position = position_deg * 3.1416 / 180 # Position to rads
msg.velocity = 0.4 # Velocity in rads/s
msg.control_type = 4 # Position and velocity control
# Publish message!
pub.publish(msg)
def changeValuePosition(self, value):
self.value_hans_axis_1 = value
msg = GoalRotaryServo()
msg.position = self.value_hans_axis_1 * 3.1416 / 180
self.publisher.publish(msg)
self.labelPosition.setText("Position " + str(value))
def update(self):
# rclpy.spin_once(self.node)
msg = GoalRotaryServo()
msg.position = self.value_pal * 3.1416 / 180
msg.control_type = 1
msg.velocity = 0.1
self.publisher_pal.publish(msg)
self.labelPosition_pal.setText("Position " + str(self.value_pal))
msg = GoalRotaryServo()
msg.position = self.value_hans_axis_2 * 3.1416 / 180
self.publisher_axis2.publish(msg)
self.labelPosition_axis2.setText("Position " +
str(self.value_hans_axis_2))
msg = GoalRotaryServo()
msg.position = self.value_hans_axis_1 * 3.1416 / 180
self.publisher.publish(msg)
self.labelPosition.setText("Position " + str(self.value_hans_axis_1))
#!/usr/bin/python3
# ROS 2.0
import rclpy
from rclpy.qos import qos_profile_default, qos_profile_sensor_data
# HRIM
from hrim_actuator_rotaryservo_msgs.msg import GoalRotaryServo
rclpy.init(args=None)
node = rclpy.create_node('test_finger_control_service')
publisher = node.create_publisher(
GoalRotaryServo,
'/hros_actuation_servomotor_000000000001/command',
qos_profile=qos_profile_sensor_data)
value = 90 # in degrees
msg = GoalRotaryServo()
msg.position = value * 3.1416 / 180
msg.velocity = 0.404
msg.control_type = 1
publisher.publish(msg)
rclpy.spin(node)
