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 __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_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 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))
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)
# Spin not really needed here since we don't have callbacks
#rclpy.spin_once(node)
# Sleep 1 second per loop
sleep(1.)
# Log
print("Iteration number", i)
i += 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)