From 2aae90cc3c2fe686c96b538afeda56a11ad30522 Mon Sep 17 00:00:00 2001 From: YueErro Date: Fri, 10 May 2019 14:32:31 +0200 Subject: [PATCH 1/2] update and unify mara control scripts --- README.md | 2 +- mara_utils_scripts/CMakeLists.txt | 3 +- .../scripts/gripper_interface.py | 110 ------ mara_utils_scripts/scripts/mara_control.py | 361 ++++++++++++++++++ .../scripts/mara_joint_control.py | 258 ------------- 5 files changed, 363 insertions(+), 371 deletions(-) delete mode 100644 mara_utils_scripts/scripts/gripper_interface.py create mode 100644 mara_utils_scripts/scripts/mara_control.py delete mode 100644 mara_utils_scripts/scripts/mara_joint_control.py diff --git a/README.md b/README.md index 355ea2e..d392628 100644 --- a/README.md +++ b/README.md @@ -136,7 +136,7 @@ ros-crystal-gazebo-ros \ ros-crystal-gazebo-ros-pkgs sudo apt install -y \ -python3-vcstool python3-numpy wget +python3-vcstool python3-numpy wget python3-pyqt5 ``` ### Create a ROS 2.0 workspace diff --git a/mara_utils_scripts/CMakeLists.txt b/mara_utils_scripts/CMakeLists.txt index 8918131..beb4573 100644 --- a/mara_utils_scripts/CMakeLists.txt +++ b/mara_utils_scripts/CMakeLists.txt @@ -14,8 +14,7 @@ find_package(ament_cmake REQUIRED) install( PROGRAMS - scripts/gripper_interface.py - scripts/mara_joint_control.py + scripts/mara_control.py scripts/spawn_mara.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/mara_utils_scripts/scripts/gripper_interface.py b/mara_utils_scripts/scripts/gripper_interface.py deleted file mode 100644 index 5158b3c..0000000 --- a/mara_utils_scripts/scripts/gripper_interface.py +++ /dev/null @@ -1,110 +0,0 @@ -#!/usr/bin/python3 -# -*- coding: utf-8 -*- - -#Qt5 -from PyQt5.QtWidgets import (QWidget, QSlider, QGridLayout, QPushButton, - QLabel, QApplication) -from PyQt5.QtCore import Qt -from PyQt5.QtGui import QPixmap - -#system -import sys -import time - -#ROS 2.0 -from hrim_actuator_gripper_srvs.srv import ControlFinger -import rclpy - -class Example(QWidget): - - def __init__(self): - super().__init__() - - self.initUI() - - 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_linearposition = 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()) - - # print('Goal accepted: %d' % self.cli.response.goal_accepted) - - def initUI(self): - - rclpy.init(args=None) - self.node = rclpy.create_node('test_finger_control_service') - - self.cli = self.node.create_client(ControlFinger, '/hrim_actuation_gripper_000000000004/goal') #Change me! - - grid = QGridLayout() - self.setLayout(grid) - - labelSpeed = QLabel("Speed") - labelEffort = QLabel("Effort") - labelPosition = QLabel("Position") - - button = QPushButton("Goto") - - sldSpeed = QSlider(Qt.Horizontal, self) - sldSpeed.setMinimum(30) - sldSpeed.setMaximum(250) - sldSpeed.setFocusPolicy(Qt.NoFocus) - sldSpeed.valueChanged[int].connect(self.changeValueSpeed) - - sldPosition = QSlider(Qt.Horizontal, self) - sldPosition.setMinimum(0) - sldPosition.setMaximum(86) - sldPosition.setFocusPolicy(Qt.NoFocus) - sldPosition.setGeometry(30, 40, 100, 30) - sldPosition.valueChanged[int].connect(self.changeValuePosition) - - sldEffort = QSlider(Qt.Horizontal, self) - sldEffort.setMinimum(10) - sldEffort.setMaximum(125) - sldEffort.setFocusPolicy(Qt.NoFocus) - sldEffort.valueChanged[int].connect(self.changeValueEffort) - - grid.addWidget(labelPosition, 0, 0) - grid.addWidget(sldPosition, 0, 1) - - grid.addWidget(labelSpeed, 1, 0) - grid.addWidget(sldSpeed, 1, 1) - - grid.addWidget(labelEffort, 2, 0) - grid.addWidget(sldEffort, 2, 1) - - grid.addWidget(button, 3, 0, 1, 2) - button.clicked.connect(self.clickedOnMoveGripper) - - self.setWindowTitle("Robotiq 140 interface") - self.show() - self.pos = 0.0 - self.vel = 30.0 - self.effort = 10.0 - - - def clickedOnMoveGripper(self): - self.moveGripper() - - def changeValueSpeed(self, value): - self.vel = float(value) - - def changeValuePosition(self, value): - self.pos = float(value/100.0) - - def changeValueEffort(self, value): - self.vel = float(value) - -if __name__ == '__main__': - app = QApplication(sys.argv) - ex = Example() - sys.exit(app.exec_()) diff --git a/mara_utils_scripts/scripts/mara_control.py b/mara_utils_scripts/scripts/mara_control.py new file mode 100644 index 0000000..395481d --- /dev/null +++ b/mara_utils_scripts/scripts/mara_control.py @@ -0,0 +1,361 @@ +#!/usr/bin/python3 +# -*- coding: utf-8 -*- + +#TODO +""" +For loop WIP +The rest of the script will works once the naming is fixed +""" + +import os +import sys +import time +import yaml +import rclpy +import argparse + +from hrim_actuator_gripper_srvs.srv import ControlFinger +from hrim_actuator_rotaryservo_msgs.msg import GoalRotaryServo +from rclpy.qos import qos_profile_default, qos_profile_sensor_data +from ament_index_python.packages import get_package_share_directory +from PyQt5.QtCore import Qt, QThread +from PyQt5.QtWidgets import QWidget, QSlider, QGridLayout, QPushButton, QLabel, QApplication + +class Thread(QThread): + + def __init__(self, example): + + QThread.__init__(self) + self.example = example + + def __del__(self): + self.wait() + + def run(self): + self.example.buttonStopLoop.clicked.connect(self.example.stopLoop) + +class Example(QWidget): + + def __init__(self, goalTopics, gripper, gripperService): + super().__init__() + + rclpy.init(args=None) + self.node = rclpy.create_node('mara_control') + + self.pubs = [] + for gt in goalTopics: + self.createPublisher(gt) + + self.initUI(gripper, gripperService) + + def createPublisher(self, topicName): + pub = self.node.create_publisher(GoalRotaryServo, topicName, qos_profile=qos_profile_sensor_data) + self.pubs.append(pub) + + def initUI(self, gripper, gripperService): + self.gripper = gripper + + self.msg = GoalRotaryServo() + self.msg.velocity = 0.1 + self.msg.control_type = 4 + + self.loop = False + + 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) + self.buttonLoop = QPushButton('LOOP', self) + self.buttonLoop.clicked.connect(self.startLoop) + self.buttonStopLoop = QPushButton('STOP LOOP', self) + # self.buttonStopLoop.clicked.connect(self.stopLoop) + + grid.addWidget(self.buttonHome, 7, 0) + grid.addWidget(self.buttonA, 8, 0) + grid.addWidget(self.buttonB, 8, 1) + grid.addWidget(self.buttonLoop, 9, 0) + grid.addWidget(self.buttonStopLoop, 9, 1) + + self.setWindowTitle("MARA") + self.show() + + def update(self): + rclpy.spin_once(self.node) + + def changePositionJoint1(self, value): + self.msg.position = value * 3.1416/180 + self.pubs[0].publish(self.msg) + self.labelJoint1.setText("Joint 1: " + str(value)) + + def changePositionJoint2(self, value): + self.msg.position = value * 3.1416/180 + self.pubs[1].publish(self.msg) + self.labelJoint2.setText("Joint 2: " + str(value)) + + def changePositionJoint3(self, value): + self.msg.position = value * 3.1416/180 + self.pubs[2].publish(self.msg) + self.labelJoint3.setText("Joint 3: " + str(value)) + + def changePositionJoint4(self, value): + self.msg.position = value * 3.1416/180 + self.pubs[3].publish(self.msg) + self.labelJoint4.setText("Joint 4: " + str(value)) + + def changePositionJoint5(self, value): + self.msg.position = value * 3.1416/180 + self.pubs[4].publish(self.msg) + self.labelJoint5.setText("Joint 5: " + str(value)) + + def changePositionJoint6(self, value): + self.msg.position = value * 3.1416/180 + self.pubs[5].publish(self.msg) + self.labelJoint6.setText("Joint 6: " + str(value)) + + def changeVelocity(self, value): + self.msg.velocity = value/100 + self.labelVelocity.setText("Speed: " + str(value/100)) + + def goHome(self): + self.sldJoint1.setValue(0) + self.sldJoint2.setValue(0) + self.sldJoint3.setValue(0) + self.sldJoint4.setValue(0) + self.sldJoint5.setValue(0) + self.sldJoint6.setValue(0) + + def goA(self): + self.sldJoint1.setValue(40) + self.sldJoint2.setValue(40) + self.sldJoint3.setValue(40) + self.sldJoint4.setValue(40) + self.sldJoint5.setValue(40) + self.sldJoint6.setValue(40) + + def goB(self): + self.sldJoint1.setValue(-61) + self.sldJoint2.setValue(-60) + self.sldJoint3.setValue(-39) + self.sldJoint4.setValue(-63) + self.sldJoint5.setValue(21) + self.sldJoint6.setValue(-36) + + def startLoop(self): + self.loop = True + + thread = Thread(self) + thread.start() + + while self.loop: + self.sldJoint1.setValue(40) + self.sldJoint2.setValue(40) + self.sldJoint3.setValue(40) + self.sldJoint4.setValue(40) + self.sldJoint5.setValue(40) + self.sldJoint6.setValue(40) + + self.sldJoint1.setValue(-40) + self.sldJoint2.setValue(-40) + self.sldJoint3.setValue(-40) + self.sldJoint4.setValue(-40) + self.sldJoint5.setValue(-40) + self.sldJoint6.setValue(-40) + + def stopLoop(self): + self.loop = False + + def changePositionGripper(self, value): + if self.gripper == 'hande': + self.req.goal_linearposition = value/100 + + else: + self.req.goal_angularposition = value/100 + + future = self.client.call_async(self.req) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is None: + self.node.get_logger().error('Exception while calling service: %r' % future.exception()) + else: + self.labelGripperPos.setText("Gripper: " + str(value/100)) + + def changeVelocityGripper(self, value): + self.req.goal_velocity = value/10 + + future = self.client.call_async(self.req) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is None: + self.node.get_logger().error('Exception while calling service: %r' % future.exception()) + else: + self.labelGripperVel.setText("Speed: " + str(value/10)) + +class YAML(): + def __init__(self, yamlpath, gripper): + self.yamlFile = open(yamlpath, 'r') + self.gripper = gripper + self.goalTopics = [] + self.goalGripperService = None + + def getTopics(self, yamlFile, env, rdi): + try: + y = yaml.safe_load(yamlFile) + if env == "sim": + motors = y['simulated_motors'] + if self.gripper != 'None': + self.goalGripperService = y['simulated_grippers'][1] + elif env == "real": + motors = y['real_motors'] + if self.gripper != 'None': + self.goalGripperService = y['real_grippers'][1] + + os.environ["ROS_DOMAIN_ID"] = rdi + os.environ["RMW_IMPLEMENTATION"] = "rmw_opensplice_cpp" + + delimiter = "trajectory" + for m in motors: + name = m.split(delimiter)[0] + axis = m.split(delimiter)[1] + goaltopic = name + "goal" + axis + self.goalTopics.append(goaltopic) + + except yaml.YAMLError as exc: + print(exc) + +if __name__ == '__main__': + + parser = argparse.ArgumentParser(description='MARA simulated or real') + parser.add_argument('-env', '--environment', type=str , choices=['sim', 'real'], default='sim', help="simulated or real") + parser.add_argument('-id', '--rdi', type=str, default='22', help="ROS_DOMAIN_ID of the SoM") + parser.add_argument('-g', '--gripper', type=str, choices=['None', '140', '85', 'hande'], default='None', help="MARA robot gripper") + args = parser.parse_args() + + yamlpath = os.path.join(get_package_share_directory('hrim_cognition_mara_components'), 'mara.yaml') + yamlOBJ = YAML(yamlpath, args.gripper) + yamltopics = yamlOBJ.getTopics(yamlOBJ.yamlFile, args.environment, args.rdi) + + app = QApplication(sys.argv) + ex = Example(yamlOBJ.goalTopics, args.gripper, yamlOBJ.goalGripperService) + sys.exit(app.exec_()) diff --git a/mara_utils_scripts/scripts/mara_joint_control.py b/mara_utils_scripts/scripts/mara_joint_control.py deleted file mode 100644 index 8124595..0000000 --- a/mara_utils_scripts/scripts/mara_joint_control.py +++ /dev/null @@ -1,258 +0,0 @@ -#!/usr/bin/python3 -# -*- coding: utf-8 -*- - -#Qt5 -from PyQt5.QtWidgets import (QWidget, QSlider, QGridLayout, QPushButton, - QLabel, QApplication) -from PyQt5.QtCore import Qt -from PyQt5.QtGui import QPixmap -from PyQt5.QtCore import QTimer -#system -import sys -import time - -#ROS 2.0 -from hrim_actuator_rotaryservo_msgs.msg import GoalRotaryServo -from rclpy.qos import qos_profile_default, qos_profile_sensor_data - -import rclpy - -import numpy as np -import os -beloop= False - -class Example(QWidget): - - def __init__(self): - super().__init__() - - self.initUI() - - def initUI(self): - - rclpy.init(args=None) - - self.node = rclpy.create_node('test_finger_control_service') - - self.publisher = self.node.create_publisher(GoalRotaryServo, '/hrim_actuation_servomotor_000000000001/goal_axis2', qos_profile=qos_profile_sensor_data) - self.publisher_axis2 = self.node.create_publisher(GoalRotaryServo, '/hrim_actuation_servomotor_000000000001/goal_axis1', qos_profile=qos_profile_sensor_data) - - self.publisher_pal = self.node.create_publisher(GoalRotaryServo, '/hrim_actuation_servomotor_000000000002/goal_axis1', qos_profile=qos_profile_sensor_data) - self.publisher_hebi = self.node.create_publisher(GoalRotaryServo, '/hrim_actuation_servomotor_000000000002/goal_axis2', qos_profile=qos_profile_sensor_data) - - self.publisher_lander = self.node.create_publisher(GoalRotaryServo, '/hrim_actuation_servomotor_000000000003/goal_axis1', qos_profile=qos_profile_sensor_data) - self.publisher_lander2 = self.node.create_publisher(GoalRotaryServo, '/hrim_actuation_servomotor_000000000003/goal_axis2', qos_profile=qos_profile_sensor_data) - - grid = QGridLayout() - self.setLayout(grid) - - self.labelPosition = QLabel("Position") - self.sldPosition = QSlider(Qt.Horizontal, self) - self.sldPosition.setMinimum(-180) - self.sldPosition.setMaximum(180) - self.sldPosition.setFocusPolicy(Qt.NoFocus) - self.sldPosition.setGeometry(30, 40, 100, 30) - self.sldPosition.setTickInterval(0.1) - self.sldPosition.valueChanged[int].connect(self.changeValuePosition) - - self.labelPosition_axis2 = QLabel("Position") - self.sldPosition_axis2 = QSlider(Qt.Horizontal, self) - self.sldPosition_axis2.setMinimum(-180) - self.sldPosition_axis2.setMaximum(180) - self.sldPosition_axis2.setFocusPolicy(Qt.NoFocus) - self.sldPosition_axis2.setGeometry(30, 40, 100, 30) - self.sldPosition_axis2.setTickInterval(0.1) - self.sldPosition_axis2.valueChanged[int].connect(self.changeValuePosition_axis2) - - self.labelPosition_pal = QLabel("Position") - self.sldPosition_pal = QSlider(Qt.Horizontal, self) - self.sldPosition_pal.setMaximum(180) - self.sldPosition_pal.setMinimum(-180) - self.sldPosition_pal.setFocusPolicy(Qt.NoFocus) - self.sldPosition_pal.setGeometry(30, 40, 100, 30) - self.sldPosition_pal.setTickInterval(0.1) - self.sldPosition_pal.valueChanged[int].connect(self.changeValuePosition_pal) - - self.labelPosition_hebi = QLabel("Position") - self.sldPosition_hebi = QSlider(Qt.Horizontal, self) - self.sldPosition_hebi.setMaximum(180) - self.sldPosition_hebi.setMinimum(-180) - self.sldPosition_hebi.setFocusPolicy(Qt.NoFocus) - self.sldPosition_hebi.setGeometry(30, 40, 100, 30) - self.sldPosition_hebi.setTickInterval(0.1) - self.sldPosition_hebi.valueChanged[int].connect(self.changeValuePosition_hebi) - - self.labelPosition_lander = QLabel("Position") - self.sldPosition_lander = QSlider(Qt.Horizontal, self) - self.sldPosition_lander.setMaximum(180) - self.sldPosition_lander.setMinimum(-180) - self.sldPosition_lander.setFocusPolicy(Qt.NoFocus) - self.sldPosition_lander.setGeometry(30, 40, 100, 30) - self.sldPosition_lander.setTickInterval(0.1) - self.sldPosition_lander.valueChanged[int].connect(self.changeValuePosition_lander) - - self.labelPosition_lander2 = QLabel("Position") - self.sldPosition_lander2 = QSlider(Qt.Horizontal, self) - self.sldPosition_lander2.setMaximum(180) - self.sldPosition_lander2.setMinimum(-180) - self.sldPosition_lander2.setFocusPolicy(Qt.NoFocus) - self.sldPosition_lander2.setGeometry(30, 40, 100, 30) - self.sldPosition_lander2.setTickInterval(0.1) - self.sldPosition_lander2.valueChanged[int].connect(self.changeValuePosition_lander2) - - - grid.addWidget(self.labelPosition, 0, 0) - grid.addWidget(self.sldPosition, 0, 1) - grid.addWidget(self.labelPosition_axis2, 1, 0) - grid.addWidget(self.sldPosition_axis2, 1, 1) - grid.addWidget(self.labelPosition_pal, 2, 0) - grid.addWidget(self.sldPosition_pal, 2, 1) - grid.addWidget(self.labelPosition_hebi, 3, 0) - grid.addWidget(self.sldPosition_hebi, 3, 1) - grid.addWidget(self.labelPosition_lander, 4, 0) - grid.addWidget(self.sldPosition_lander, 4, 1) - grid.addWidget(self.labelPosition_lander2, 5, 0) - grid.addWidget(self.sldPosition_lander2, 5, 1) - - - self.button = QPushButton('Go 0', self) - self.button.clicked.connect(self.gotozero) - self.button2 = QPushButton('Go A', self) - self.button2.clicked.connect(self.gotoa) - self.button3 = QPushButton('Go B', self) - self.button3.clicked.connect(self.gotob) - self.button4 = QPushButton('LOOP', self) - self.button4.clicked.connect(self.goloop) - self.button5 = QPushButton('STOP LOOP', self) - self.button5.clicked.connect(self.stoploop) - - grid.addWidget(self.button, 6, 0) - grid.addWidget(self.button2, 7, 0) - grid.addWidget(self.button3,8, 0) - grid.addWidget(self.button4,9, 0) - grid.addWidget(self.button5,9, 1) - - self.setWindowTitle("MAIRA") - self.show() - self.pos = 0.0 - self.vel = 30.0 - self.effort = 10.0 - - self.value_hans_axis_1 = 0; - self.value_hans_axis_2 = 0; - self.value_pal = 0; - self.value_hebi = 0; - self.value_lander = 0; - self.value_lander2 = 0; - - def update(self): - rclpy.spin_once(self.node) - - 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.404 - msg.control_type = 1 - self.publisher_pal.publish(msg) - self.labelPosition_pal.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(self, value): - self.value_hans_axis_1 = value - msg = GoalRotaryServo() - msg.velocity = 0.404 - msg.control_type = 1 - msg.position = self.value_hans_axis_1 * 3.1416/180 - self.publisher.publish(msg) - self.labelPosition.setText("Position " + str(value)) - - def changeValuePosition_lander(self, value): - - self.value_lander = value - msg = GoalRotaryServo() - msg.velocity = 0.404 - msg.position = self.value_lander * 3.1416/180 - self.publisher_lander.publish(msg) - self.labelPosition_lander.setText("Position " + str(value)) - - 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 gotozero(self): - print("gotozero") - self.sldPosition.setValue(0) - self.sldPosition_axis2.setValue(0) - self.sldPosition_pal.setValue(0) - self.sldPosition_hebi.setValue(0) - self.sldPosition_lander.setValue(0) - self.sldPosition_lander2.setValue(0) - def gotoa(self): - print("gotozero") - self.sldPosition.setValue(40) - self.sldPosition_axis2.setValue(40) - self.sldPosition_pal.setValue(40) - self.sldPosition_hebi.setValue(40) - self.sldPosition_lander.setValue(40) - self.sldPosition_lander2.setValue(40) - def gotob(self): - print("gotozero") - self.sldPosition.setValue(-61) - self.sldPosition_axis2.setValue(-60) - self.sldPosition_pal.setValue(-39) - self.sldPosition_hebi.setValue(-63) - self.sldPosition_lander.setValue(21) - self.sldPosition_lander2.setValue(-36) - def goloop(self): - global beloop - beloop = True - while beloop: - print("go Loop 40") - self.sldPosition.setValue(40) - self.sldPosition_axis2.setValue(40) - self.sldPosition_pal.setValue(40) - self.sldPosition_hebi.setValue(40) - self.sldPosition_lander.setValue(40) - self.sldPosition_lander2.setValue(40) - - time.sleep(14) - print("go Loop 0") - self.sldPosition.setValue(-40) - self.sldPosition_axis2.setValue(-40) - self.sldPosition_pal.setValue(-40) - self.sldPosition_hebi.setValue(-40) - self.sldPosition_lander.setValue(-40) - self.sldPosition_lander2.setValue(-40) - time.sleep(14) - - def stoploop(self): - global beloop - beloop = False -if __name__ == '__main__': - - app = QApplication(sys.argv) - ex = Example() - sys.exit(app.exec_()) From b762e263cd754cb0b826acfd0a25b20e25ffda58 Mon Sep 17 00:00:00 2001 From: YueErro Date: Fri, 10 May 2019 15:55:32 +0200 Subject: [PATCH 2/2] remove loop so far not useful --- mara_utils_scripts/scripts/mara_control.py | 55 +--------------------- 1 file changed, 2 insertions(+), 53 deletions(-) diff --git a/mara_utils_scripts/scripts/mara_control.py b/mara_utils_scripts/scripts/mara_control.py index 395481d..84221ae 100644 --- a/mara_utils_scripts/scripts/mara_control.py +++ b/mara_utils_scripts/scripts/mara_control.py @@ -1,12 +1,6 @@ #!/usr/bin/python3 # -*- coding: utf-8 -*- -#TODO -""" -For loop WIP -The rest of the script will works once the naming is fixed -""" - import os import sys import time @@ -16,24 +10,11 @@ from hrim_actuator_gripper_srvs.srv import ControlFinger from hrim_actuator_rotaryservo_msgs.msg import GoalRotaryServo -from rclpy.qos import qos_profile_default, qos_profile_sensor_data +from rclpy.qos import qos_profile_sensor_data from ament_index_python.packages import get_package_share_directory -from PyQt5.QtCore import Qt, QThread +from PyQt5.QtCore import Qt from PyQt5.QtWidgets import QWidget, QSlider, QGridLayout, QPushButton, QLabel, QApplication -class Thread(QThread): - - def __init__(self, example): - - QThread.__init__(self) - self.example = example - - def __del__(self): - self.wait() - - def run(self): - self.example.buttonStopLoop.clicked.connect(self.example.stopLoop) - class Example(QWidget): def __init__(self, goalTopics, gripper, gripperService): @@ -59,8 +40,6 @@ def initUI(self, gripper, gripperService): self.msg.velocity = 0.1 self.msg.control_type = 4 - self.loop = False - grid = QGridLayout() self.setLayout(grid) @@ -189,16 +168,10 @@ def initUI(self, gripper, gripperService): self.buttonA.clicked.connect(self.goA) self.buttonB = QPushButton('Go B', self) self.buttonB.clicked.connect(self.goB) - self.buttonLoop = QPushButton('LOOP', self) - self.buttonLoop.clicked.connect(self.startLoop) - self.buttonStopLoop = QPushButton('STOP LOOP', self) - # self.buttonStopLoop.clicked.connect(self.stopLoop) grid.addWidget(self.buttonHome, 7, 0) grid.addWidget(self.buttonA, 8, 0) grid.addWidget(self.buttonB, 8, 1) - grid.addWidget(self.buttonLoop, 9, 0) - grid.addWidget(self.buttonStopLoop, 9, 1) self.setWindowTitle("MARA") self.show() @@ -264,30 +237,6 @@ def goB(self): self.sldJoint5.setValue(21) self.sldJoint6.setValue(-36) - def startLoop(self): - self.loop = True - - thread = Thread(self) - thread.start() - - while self.loop: - self.sldJoint1.setValue(40) - self.sldJoint2.setValue(40) - self.sldJoint3.setValue(40) - self.sldJoint4.setValue(40) - self.sldJoint5.setValue(40) - self.sldJoint6.setValue(40) - - self.sldJoint1.setValue(-40) - self.sldJoint2.setValue(-40) - self.sldJoint3.setValue(-40) - self.sldJoint4.setValue(-40) - self.sldJoint5.setValue(-40) - self.sldJoint6.setValue(-40) - - def stopLoop(self): - self.loop = False - def changePositionGripper(self, value): if self.gripper == 'hande': self.req.goal_linearposition = value/100