From 1da87726e125871d5739ad72f834ecdf1dd7db40 Mon Sep 17 00:00:00 2001 From: YueErro Date: Wed, 8 May 2019 15:53:30 +0200 Subject: [PATCH 1/3] remove unused stuff --- .../HROSCognitionMaraComponents.hpp | 0 mara_description/CMakeLists.txt | 8 +- mara_description/launch/mara_demo_launch.py | 47 -- .../scripts/mara_state_publisher.py | 209 ------ mara_gazebo/CMakeLists.txt | 2 +- mara_gazebo/worlds/empty_speed_up.world | 30 - mara_gazebo/worlds/mara_on_a_table.world | 658 ------------------ 7 files changed, 2 insertions(+), 952 deletions(-) rename hros_cognition_mara_components/include/{hros_cognition_mara_components => }/HROSCognitionMaraComponents.hpp (100%) delete mode 100644 mara_description/launch/mara_demo_launch.py delete mode 100644 mara_description/scripts/mara_state_publisher.py delete mode 100644 mara_gazebo/worlds/empty_speed_up.world delete mode 100644 mara_gazebo/worlds/mara_on_a_table.world diff --git a/hros_cognition_mara_components/include/hros_cognition_mara_components/HROSCognitionMaraComponents.hpp b/hros_cognition_mara_components/include/HROSCognitionMaraComponents.hpp similarity index 100% rename from hros_cognition_mara_components/include/hros_cognition_mara_components/HROSCognitionMaraComponents.hpp rename to hros_cognition_mara_components/include/HROSCognitionMaraComponents.hpp diff --git a/mara_description/CMakeLists.txt b/mara_description/CMakeLists.txt index 4413d67..01d96e8 100644 --- a/mara_description/CMakeLists.txt +++ b/mara_description/CMakeLists.txt @@ -12,16 +12,10 @@ endif() find_package(ament_cmake REQUIRED) find_package(urdf REQUIRED) -install(DIRECTORY meshes urdf material launch rviz +install(DIRECTORY meshes urdf material rviz DESTINATION share/${PROJECT_NAME} ) -install( - PROGRAMS - scripts/mara_state_publisher.py - DESTINATION lib/${PROJECT_NAME} -) - ament_export_dependencies(xacro) ament_export_dependencies(urdf) ament_export_dependencies(material) diff --git a/mara_description/launch/mara_demo_launch.py b/mara_description/launch/mara_demo_launch.py deleted file mode 100644 index 1ee9ee0..0000000 --- a/mara_description/launch/mara_demo_launch.py +++ /dev/null @@ -1,47 +0,0 @@ -import sys -import os -import launch - -from ament_index_python.packages import get_package_share_directory, get_package_prefix -from launch import LaunchDescription -from launch import LaunchService -from launch.actions.execute_process import ExecuteProcess -from launch_ros.actions import Node - -def generate_launch_description(): - urdf = os.path.join(get_package_share_directory('mara_description'), 'urdf', 'mara_robot_gripper_140.urdf') - install_dir = get_package_prefix('mara_description') - - print("install_dir ", install_dir) - - if 'GAZEBO_MODEL_PATH' in os.environ: - os.environ['GAZEBO_MODEL_PATH'] = os.environ['GAZEBO_MODEL_PATH'] + ':' + install_dir + '/share' - else: - os.environ['GAZEBO_MODEL_PATH'] = install_dir + "/share" - - if 'GAZEBO_PLUGIN_PATH' in os.environ: - os.environ['GAZEBO_PLUGIN_PATH'] = os.environ['GAZEBO_PLUGIN_PATH'] + ':' + install_dir + '/lib' - else: - os.environ['GAZEBO_PLUGIN_PATH'] = install_dir + '/lib' - - try: - envs = {} - for key in os.environ.__dict__["_data"]: - key = key.decode("utf-8") - if (key.isupper()): - envs[key] = os.environ[key] - except Exception as e: - print("Error with Envs: " + str(e)) - return None - - rviz_config_path = os.path.join(get_package_share_directory('mara_description'), - 'rviz', 'visualization.rviz') - ld = LaunchDescription([ - ExecuteProcess( - cmd=['rviz2', '-d', rviz_config_path], output='screen', - env=envs - ), - Node(package='robot_state_publisher', node_executable='robot_state_publisher', output='screen', arguments=[urdf]), - Node(package='mara_description', node_executable='mara_state_publisher.py', output='screen'), - ]) - return ld diff --git a/mara_description/scripts/mara_state_publisher.py b/mara_description/scripts/mara_state_publisher.py deleted file mode 100644 index fac0e21..0000000 --- a/mara_description/scripts/mara_state_publisher.py +++ /dev/null @@ -1,209 +0,0 @@ -#!/usr/bin/python3 -# -*- coding: utf-8 -*- - -from time import sleep - -import rclpy -import sys - -from sensor_msgs.msg import JointState - -#Qt5 -from PyQt5.QtWidgets import (QWidget, QSlider, QGridLayout, QPushButton, - QLabel, QApplication) -from PyQt5.QtCore import Qt, pyqtSignal, QObject -from PyQt5.QtGui import QPixmap - -# We do not recommend this style as ROS 2 provides timers for this purpose, -# and it is recommended that all nodes call a variation of spin. -# This example is only included for completeness because it is similar to examples in ROS 1. -# For periodic publication please see the other examples using timers. -class DoubleSlider(QSlider): - - # create our our signal that we can connect to if necessary - doubleValueChanged = pyqtSignal(float) - - def __init__(self, decimals=3, *args, **kargs): - super(DoubleSlider, self).__init__( Qt.Horizontal, **kargs) - self._multi = 10 ** decimals - - self.valueChanged.connect(self.emitDoubleValueChanged) - - def emitDoubleValueChanged(self): - value = float(super(DoubleSlider, self).value())/self._multi - self.doubleValueChanged.emit(value) - - def value(self): - return float(super(DoubleSlider, self).value()) / self._multi - - def setMinimum(self, value): - return super(DoubleSlider, self).setMinimum(value * self._multi) - - def setMaximum(self, value): - return super(DoubleSlider, self).setMaximum(value * self._multi) - - def setSingleStep(self, value): - return super(DoubleSlider, self).setSingleStep(value * self._multi) - - def singleStep(self): - return float(super(DoubleSlider, self).singleStep()) / self._multi - - def setValue(self, value): - super(DoubleSlider, self).setValue(int(value * self._multi)) - -class Example(QWidget): - - def __init__(self): - super().__init__() - - self.initUI() - - def sendJointStates(self): - msg = JointState() - msg.name.append("motor1") - msg.position.append(self.motor1) - msg.name.append("motor2") - msg.position.append(self.motor2) - msg.name.append("motor3") - msg.position.append(self.motor3) - msg.name.append("motor4") - msg.position.append(self.motor4) - msg.name.append("motor5") - msg.position.append(self.motor5) - msg.name.append("motor6") - msg.position.append(self.motor6) - self.publisher.publish(msg) - - def initUI(self): - - rclpy.init(args=sys.argv) - - node = rclpy.create_node('joint_states_publisher_dh_robotics') - - self.publisher = node.create_publisher(JointState, '/joint_states') - - grid = QGridLayout() - self.setLayout(grid) - - labelMotor1 = QLabel("Motor1") - self.labelMotor1_value = QLabel("") - labelMotor2 = QLabel("Motor2") - self.labelMotor2_value = QLabel("") - labelMotor3 = QLabel("Motor3") - self.labelMotor3_value = QLabel("") - labelMotor4 = QLabel("Motor4") - self.labelMotor4_value = QLabel("") - labelMotor5 = QLabel("Motor5") - self.labelMotor5_value = QLabel("") - labelMotor6 = QLabel("Motor6") - self.labelMotor6_value = QLabel("") - - sldMotor1 = DoubleSlider(decimals=3) - sldMotor1.setMinimum(-3.1416) - sldMotor1.setMaximum(3.1416) - sldMotor1.setFocusPolicy(Qt.NoFocus) - sldMotor1.setGeometry(30, 40, 100, 30) - sldMotor1.doubleValueChanged[float].connect(self.changeValueMotor1) - - sldMotor2 = DoubleSlider(decimals=3) - sldMotor2.setMinimum(-3.1416) - sldMotor2.setMaximum(3.1416) - sldMotor2.setFocusPolicy(Qt.NoFocus) - sldMotor2.setGeometry(30, 40, 100, 30) - sldMotor2.doubleValueChanged[float].connect(self.changeValueMotor2) - - sldMotor3= DoubleSlider(decimals=3) - sldMotor3.setMinimum(-3.1416) - sldMotor3.setMaximum(3.1416) - sldMotor3.setFocusPolicy(Qt.NoFocus) - sldMotor3.setGeometry(30, 40, 100, 30) - sldMotor3.doubleValueChanged[float].connect(self.changeValueMotor3) - - sldMotor4 = DoubleSlider(decimals=3) - sldMotor4.setMinimum(-3.1416) - sldMotor4.setMaximum(3.1416) - sldMotor4.setFocusPolicy(Qt.NoFocus) - sldMotor4.setGeometry(30, 40, 100, 30) - sldMotor4.doubleValueChanged[float].connect(self.changeValueMotor4) - - sldMotor5 = DoubleSlider(decimals=3) - sldMotor5.setMinimum(-3.1416) - sldMotor5.setMaximum(3.1416) - sldMotor5.setFocusPolicy(Qt.NoFocus) - sldMotor5.setGeometry(30, 40, 100, 30) - sldMotor5.doubleValueChanged[float].connect(self.changeValueMotor5) - - sldMotor6 = DoubleSlider(decimals=3) - sldMotor6.setMinimum(-3.1416) - sldMotor6.setMaximum(3.1416) - sldMotor6.setFocusPolicy(Qt.NoFocus) - sldMotor6.setGeometry(30, 40, 100, 30) - sldMotor6.doubleValueChanged[float].connect(self.changeValueMotor6) - - grid.addWidget(labelMotor1, 0, 0) - grid.addWidget(sldMotor1, 0, 1) - grid.addWidget(self.labelMotor1_value, 0, 2) - - grid.addWidget(labelMotor2, 1, 0) - grid.addWidget(sldMotor2, 1, 1) - grid.addWidget(self.labelMotor2_value, 1, 2) - - grid.addWidget(labelMotor3, 2, 0) - grid.addWidget(sldMotor3, 2, 1) - grid.addWidget(self.labelMotor3_value, 2, 2) - - grid.addWidget(labelMotor4, 3, 0) - grid.addWidget(sldMotor4, 3, 1) - grid.addWidget(self.labelMotor4_value, 3, 2) - - grid.addWidget(labelMotor5, 4, 0) - grid.addWidget(sldMotor5, 4, 1) - grid.addWidget(self.labelMotor5_value, 4, 2) - - grid.addWidget(labelMotor6, 5, 0) - grid.addWidget(sldMotor6, 5, 1) - grid.addWidget(self.labelMotor6_value, 5, 2) - - self.setWindowTitle("MARA joint states publisher") - self.show() - self.motor1 = 0.0 - self.motor2 = 0.0 - self.motor3 = 0.0 - self.motor4 = 0.0 - self.motor5 = 0.0 - self.motor6 = 0.0 - - def changeValueMotor1(self, value): - self.motor1 = float(value) - self.sendJointStates() - self.labelMotor1_value.setText(str(self.motor1)) - - def changeValueMotor2(self, value): - self.motor2 = float(value) - self.sendJointStates() - self.labelMotor2_value.setText(str(self.motor2)) - - def changeValueMotor3(self, value): - self.motor3 = float(value) - self.sendJointStates() - self.labelMotor3_value.setText(str(self.motor3)) - - def changeValueMotor4(self, value): - self.motor4 = float(value) - self.sendJointStates() - self.labelMotor4_value.setText(str(self.motor4)) - - def changeValueMotor5(self, value): - self.motor5 = float(value) - self.sendJointStates() - self.labelMotor5_value.setText(str(self.motor5)) - - def changeValueMotor6(self, value): - self.motor6 = float(value) - self.sendJointStates() - self.labelMotor6_value.setText(str(self.motor6)) - -if __name__ == '__main__': - app = QApplication(sys.argv) - ex = Example() - sys.exit(app.exec_()) diff --git a/mara_gazebo/CMakeLists.txt b/mara_gazebo/CMakeLists.txt index 0c375ab..6e7558a 100644 --- a/mara_gazebo/CMakeLists.txt +++ b/mara_gazebo/CMakeLists.txt @@ -12,7 +12,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(urdf REQUIRED) -install(DIRECTORY worlds launch +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) diff --git a/mara_gazebo/worlds/empty_speed_up.world b/mara_gazebo/worlds/empty_speed_up.world deleted file mode 100644 index 79f2d65..0000000 --- a/mara_gazebo/worlds/empty_speed_up.world +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - model://sun - - - - model://ground_plane - - - - 0.00105 - 0 - - - 5 - - - - - - \ No newline at end of file diff --git a/mara_gazebo/worlds/mara_on_a_table.world b/mara_gazebo/worlds/mara_on_a_table.world deleted file mode 100644 index 53c468c..0000000 --- a/mara_gazebo/worlds/mara_on_a_table.world +++ /dev/null @@ -1,658 +0,0 @@ - - - - - model://ground_plane - - - - model://sun - - - - false - - - - - 0 0 0.02 0 -0 0 - - 0 -5e-06 0.68145 0 -0 0 - 7.033 - - 1.14783 - -1e-05 - -5.29396e-23 - 1.14782 - -3.38813e-21 - 1.29472 - - - - 0 0 0.6 0 -0 0 - - - 1.5 1 0.05 - - - - - 0 0 0.65 0 -0 0 - - - 0.001 0.001 0.001 - model://mara_description/meshes/H-ROS_Robot_BASE.stl - - - - - - - - - - - - - 0 0 0.6 0 -0 0 - - - 1.5 1 0.05 - model://mara_description/urdf/table.obj - - - - - -0.65 -0.4 0.29 0 -0 0 - - - 0.1 0.1 0.58 - model://mara_description/urdf/table.obj - - - - - -0.65 0.4 0.29 0 -0 0 - - - 0.1 0.1 0.58 - model://mara_description/urdf/table.obj - - - - - 0.65 -0.4 0.29 0 -0 0 - - - 0.1 0.1 0.58 - model://mara_description/urdf/table.obj - - - - - 0.65 0.4 0.29 0 -0 0 - - - 0.1 0.1 0.58 - model://mara_description/urdf/table.obj - - - - - 0 0 0.65 0 -0 0 - - - 0.001 0.001 0.001 - model://mara_description/meshes/H-ROS_Robot_BASE.stl - - - - - - - 1 - - 1 - - - table - world - - - 0 0 0.67625 0 -0 0 - - 0 -5e-06 0.03145 0 -0 0 - 5.8 - - 0.101059 - 0.000211 - -0.004 - 0.0938662 - -0.0169854 - 0.0144994 - - - - 0 0 0 0 -0 0 - - - 0.001 0.001 0.001 - model://mara_description/meshes/H-ROS_Robot_mara1.stl - - - - - - - - - - - - - 0 0 0 0 -0 0 - - - 0.001 0.001 0.001 - model://mara_description/meshes/H-ROS_Robot_mara1.stl - - - - - - - 1 - - - true - 30 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - - - /hrim_actuation_servomotor_000000000001 - ~/out:=imu - - - - - - motor1_link - table - - 0 0 -1 - - -2.3562 - 2.3562 - 330 - 3.1416 - - - - - 0 -0.1005 0.83075 0 -0 0 - - 0 -5e-06 0.03145 0 -0 0 - 1.03237 - - 0.0148652 - 0 - 0 - 0.01557 - 0.00071 - 0.00092 - - - - 0 0 0 0 -0 0 - - - 0.001 0.001 0.001 - model://mara_description/meshes/H-ROS_Robot_LINK1.stl - - - - - 0 0 0 0 -0 0 - - - 0.001 0.001 0.001 - model://mara_description/meshes/H-ROS_Robot_LINK1.stl - - - - - - - 1 - - - motor2_link - motor1_link - - 0 1 0 - - -3.13 - 3.13 - 330 - 3.1416 - - - - - 0 -0.101 1.03075 0 -0 0 - - 0 -5e-06 0.03145 0 -0 0 - 3.8 - - 0.04517 - 2e-05 - 0 - 0.01587 - 0.01444 - 0.03303 - - - - 0 0 0 0 -0 0 - - - 0.001 0.001 0.001 - model://mara_description/meshes/H-ROS_Robot_mara2.stl - - - - - 0 0 0 0 -0 0 - - - 0.001 0.001 0.001 - model://mara_description/meshes/H-ROS_Robot_mara2.stl - - - - - - - 1 - 1 - - 0 0 0 0 3.14 0 - true - 30 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - - - /hrim_actuation_servomotor_000000000002 - ~/out:=imu - - - - - - motor3_link - motor2_link - - 0 -1 0 - - -3.13 - 3.13 - 330 - 3.1416 - - - - - 0 -0.00101 1.16075 0 -0 0 - - 0 -5e-06 0.03145 0 -0 0 - 1.1 - - 0.01464 - 0 - 0 - 0.00895 - -0.00631 - 0.00725 - - - - 0 0 0 0 -0 0 - - - 0.001 0.001 0.001 - model://mara_description/meshes/H-ROS_Robot_LINK2.stl - - - - - 0 0 0 0 -0 0 - - - 0.001 0.001 0.001 - model://mara_description/meshes/H-ROS_Robot_LINK2.stl - - - - - - - 1 - - 1 - - - motor4_link - motor3_link - - 0 0 -1 - - -3.13 - 3.13 - 330 - 3.1416 - - - - - 0 -0.098009 1.31981 0 -0 0 - - 0 -5e-06 0.03145 0 -0 0 - 2.9 - - 0.03406 - 0 - 0 - 0.01066 - 0.00989 - 0.02563 - - - - 0 0 0 0 -0 0 - - - 0.001 0.001 0.001 - model://mara_description/meshes/H-ROS_Robot_mara3.stl - - - - - 0 0 0 0 -0 0 - - - 0.001 0.001 0.001 - model://mara_description/meshes/H-ROS_Robot_mara3.stl - - - - - - - 1 - - true - 30 - 0 0 0 0 3.14 0 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - - - /hrim_actuation_servomotor_000000000003 - ~/out:=imu - - - - - - motor5_link - motor4_link - - 0 -1 0 - - -3.13 - 3.13 - 330 - 3.1416 - - - - - 0 -0.001009 1.44631 0 -0 0 - - 0 -5e-06 0.03145 0 -0 0 - 2.9 - - 0.0005 - 0 - 0 - 0.0005 - 0 - 0.0003 - - - - 0 0 0 0 -0 1.04667 - - - 0.001 0.001 0.001 - model://mara_description/meshes/gripper_support.stl - - - - - 0 0 0 0 -0 1.04667 - - - 0.001 0.001 0.001 - model://mara_description/meshes/gripper_support.stl - - - - - - - 1 - - - motor6_link - motor5_link - - 0 0 -1 - - -3.13 - 3.13 - 330 - 3.1416 - - - - 0 - - - - - ~/out:=joint_states - - 250 - motor1 - motor2 - motor3 - motor4 - motor5 - motor6 - - - - - - - - motor2 - motor1 - - - - - - - - motor3 - motor4 - - - - - - - - motor5 - motor6 - - - - - - From 69b4c397eba2fa63e8dbb3fbcbfe1ba78e582229 Mon Sep 17 00:00:00 2001 From: YueErro Date: Wed, 8 May 2019 16:07:39 +0200 Subject: [PATCH 2/3] fix include paths --- .../src/HROSCognitionMaraComponents.cpp | 2 +- hros_cognition_mara_components/src/TopicCommand.cpp | 2 +- hros_cognition_mara_components/src/TopicState.cpp | 2 +- hros_cognition_mara_components/src/main.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/hros_cognition_mara_components/src/HROSCognitionMaraComponents.cpp b/hros_cognition_mara_components/src/HROSCognitionMaraComponents.cpp index 01d823c..770d11e 100644 --- a/hros_cognition_mara_components/src/HROSCognitionMaraComponents.cpp +++ b/hros_cognition_mara_components/src/HROSCognitionMaraComponents.cpp @@ -1,4 +1,4 @@ -#include "hros_cognition_mara_components/HROSCognitionMaraComponents.hpp" +#include "HROSCognitionMaraComponents.hpp" HROSCognitionMaraComponentsNode::HROSCognitionMaraComponentsNode(const std::string & node_name, int argc, char **argv, bool intra_process_comms ) diff --git a/hros_cognition_mara_components/src/TopicCommand.cpp b/hros_cognition_mara_components/src/TopicCommand.cpp index 0c5c307..203d9b9 100644 --- a/hros_cognition_mara_components/src/TopicCommand.cpp +++ b/hros_cognition_mara_components/src/TopicCommand.cpp @@ -1,4 +1,4 @@ -#include "hros_cognition_mara_components/HROSCognitionMaraComponents.hpp" +#include "HROSCognitionMaraComponents.hpp" void HROSCognitionMaraComponentsNode::commandCallback(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg) { diff --git a/hros_cognition_mara_components/src/TopicState.cpp b/hros_cognition_mara_components/src/TopicState.cpp index 3ef1d41..d4b0bed 100644 --- a/hros_cognition_mara_components/src/TopicState.cpp +++ b/hros_cognition_mara_components/src/TopicState.cpp @@ -1,4 +1,4 @@ -#include "hros_cognition_mara_components/HROSCognitionMaraComponents.hpp" +#include "HROSCognitionMaraComponents.hpp" void HROSCognitionMaraComponentsNode::stateCallback(std::string motor_name, float velocity, float position, float effort) { diff --git a/hros_cognition_mara_components/src/main.cpp b/hros_cognition_mara_components/src/main.cpp index 87073d1..ae1737e 100644 --- a/hros_cognition_mara_components/src/main.cpp +++ b/hros_cognition_mara_components/src/main.cpp @@ -1,4 +1,4 @@ -#include "hros_cognition_mara_components/HROSCognitionMaraComponents.hpp" +#include "HROSCognitionMaraComponents.hpp" #include #include From 2fb8a48112b8147bced7f5a0bf7bb9981022a7c7 Mon Sep 17 00:00:00 2001 From: YueErro Date: Fri, 10 May 2019 11:23:13 +0200 Subject: [PATCH 3/3] remove test and modules --- .../test_python/move_motors.py | 152 ----------------- mara_description/urdf/module.urdf | 159 ------------------ mara_description/urdf/module.urdf.xacro | 144 ---------------- 3 files changed, 455 deletions(-) delete mode 100644 hros_cognition_mara_components/test_python/move_motors.py delete mode 100644 mara_description/urdf/module.urdf delete mode 100644 mara_description/urdf/module.urdf.xacro diff --git a/hros_cognition_mara_components/test_python/move_motors.py b/hros_cognition_mara_components/test_python/move_motors.py deleted file mode 100644 index 4de18e9..0000000 --- a/hros_cognition_mara_components/test_python/move_motors.py +++ /dev/null @@ -1,152 +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 hrim_sensor_camera_msgs.msg import CompressedImage -from rclpy.qos import qos_profile_default, qos_profile_sensor_data - -import rclpy - -import cv2 -import numpy as np -import os - - -class Example(QWidget): - - def chatter_callback(self, msg): - imagen_numpy = np.asarray(msg.data, dtype=np.uint8) - img = cv2.imdecode(imagen_numpy, 1) - cv2.imshow("Image window", img) - cv2.waitKey(3) - print('chatter_callback') - - 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_B827EB79FCFB/Command') - self.publisher_axis2 = self.node.create_publisher(GoalRotaryServo, '/hrim_actuation_servomotor_B827EBCB8804/Command') - - self.publisher_pal = self.node.create_publisher(GoalRotaryServo, '/hrim_actuation_servomotor_B827EBFA06A8/Command') - - grid = QGridLayout() - self.setLayout(grid) - - self.labelPosition = QLabel("Position") - sldPosition = QSlider(Qt.Horizontal, self) - sldPosition.setMinimum(-90) - sldPosition.setMaximum(90) - sldPosition.setFocusPolicy(Qt.NoFocus) - sldPosition.setGeometry(30, 40, 100, 30) - sldPosition.setTickInterval(0.1) - sldPosition.valueChanged[int].connect(self.changeValuePosition) - - self.labelPosition_axis2 = QLabel("Position") - sldPosition_axis2 = QSlider(Qt.Horizontal, self) - sldPosition_axis2.setMinimum(-90) - sldPosition_axis2.setMaximum(90) - sldPosition_axis2.setFocusPolicy(Qt.NoFocus) - sldPosition_axis2.setGeometry(30, 40, 100, 30) - sldPosition_axis2.setTickInterval(0.1) - sldPosition_axis2.valueChanged[int].connect(self.changeValuePosition_axis2) - - self.labelPosition_pal = QLabel("Position") - sldPosition_pal = QSlider(Qt.Horizontal, self) - sldPosition_pal.setMaximum(90) - sldPosition_pal.setMinimum(-90) - sldPosition_pal.setFocusPolicy(Qt.NoFocus) - sldPosition_pal.setGeometry(30, 40, 100, 30) - sldPosition_pal.setTickInterval(0.1) - sldPosition_pal.valueChanged[int].connect(self.changeValuePosition_pal) - - grid.addWidget(self.labelPosition, 0, 0) - grid.addWidget(sldPosition, 0, 1) - grid.addWidget(self.labelPosition_axis2, 1, 0) - grid.addWidget(sldPosition_axis2, 1, 1) - grid.addWidget(self.labelPosition_pal, 2, 0) - grid.addWidget(sldPosition_pal, 2, 1) - - self.setWindowTitle("HANS motor") - 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; - - subscription = self.node.create_subscription(CompressedImage, - '/hrim_sensing_camera_B827EBF38D04/compressed_image', - self.chatter_callback, - qos_profile=qos_profile_sensor_data) - - timer = QTimer(self) - timer.timeout.connect(self.update) - timer.start(50) - - 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)) - - 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 changeValuePosition_axis2(self, value): - self.value_hans_axis_2 = value - msg = GoalRotaryServo() - msg.position = self.value_hans_axis_2 * 3.1416/180 - 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.position = self.value_hans_axis_1 * 3.1416/180 - self.publisher.publish(msg) - self.labelPosition.setText("Position " + str(value)) - -if __name__ == '__main__': - - app = QApplication(sys.argv) - ex = Example() - sys.exit(app.exec_()) diff --git a/mara_description/urdf/module.urdf b/mara_description/urdf/module.urdf deleted file mode 100644 index 0cd24c6..0000000 --- a/mara_description/urdf/module.urdf +++ /dev/null @@ -1,159 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - True - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - motor2 - motor1 - series14 - - - diff --git a/mara_description/urdf/module.urdf.xacro b/mara_description/urdf/module.urdf.xacro deleted file mode 100644 index c95cb26..0000000 --- a/mara_description/urdf/module.urdf.xacro +++ /dev/null @@ -1,144 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - True - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - motor2 - motor1 - series14 - - -