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)