From 0528093f416b617ded52d9802d45e65928cd5485 Mon Sep 17 00:00:00 2001 From: Thomas Muller Date: Thu, 9 Sep 2021 13:38:30 +0000 Subject: [PATCH] Robot actually moves actual real hardware now Actually Gear ratios subject to change Need to move state publisher into ar3_hardware/joint_state_broadcaster --- src/ar3_config/launch/dev.launch.py | 12 +- src/ar3_config/rviz/ar3.rviz | 222 ++++++++++++++++------ src/ar3_hardware/src/ar3_system.cpp | 12 +- src/cobot_platform/cobot_platform/move.py | 64 ++++++- src/joystickmove/joystickmove/move.py | 4 +- 5 files changed, 228 insertions(+), 86 deletions(-) diff --git a/src/ar3_config/launch/dev.launch.py b/src/ar3_config/launch/dev.launch.py index 512db52..695243b 100644 --- a/src/ar3_config/launch/dev.launch.py +++ b/src/ar3_config/launch/dev.launch.py @@ -111,10 +111,10 @@ def generate_launch_description(): parameters=[robot_description]), # Fake robot state publisher - Node( - package='ar3_description', - executable='state_publisher', - name='state_publisher', - emulate_tty=True, - output='screen'), + # Node( + # package='ar3_description', + # executable='state_publisher', + # name='state_publisher', + # emulate_tty=True, + # output='screen'), ]) diff --git a/src/ar3_config/rviz/ar3.rviz b/src/ar3_config/rviz/ar3.rviz index 16a5970..3561aa9 100644 --- a/src/ar3_config/rviz/ar3.rviz +++ b/src/ar3_config/rviz/ar3.rviz @@ -6,9 +6,8 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /RobotModel1 Splitter Ratio: 0.5 - Tree Height: 616 + Tree Height: 218 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -43,64 +42,159 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - TF Prefix: "" - Update Interval: 0 + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: ar3_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true Value: true - Visual Enabled: true + Velocity_Scaling_Factor: 0.1 Enabled: true Global Options: Background Color: 48; 48; 48 @@ -144,7 +238,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10 + Distance: 2.1567115783691406 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -165,12 +259,16 @@ Visualization Manager: Yaw: 0.785398006439209 Saved: ~ Window Geometry: + "": + collapsed: false + " - Trajectory Slider": + collapsed: false Displays: collapsed: false Height: 846 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002f3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000002f3000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002f3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001e5000002f3fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000165000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffbffffffff01000001a9000001880000017d00ffffff000000010000010f000002f3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002f3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000001b0000002f300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: @@ -178,5 +276,5 @@ Window Geometry: Views: collapsed: false Width: 1200 - X: 1068 - Y: 195 + X: 582 + Y: 35 diff --git a/src/ar3_hardware/src/ar3_system.cpp b/src/ar3_hardware/src/ar3_system.cpp index f16abad..ae55b64 100644 --- a/src/ar3_hardware/src/ar3_system.cpp +++ b/src/ar3_hardware/src/ar3_system.cpp @@ -118,12 +118,12 @@ hardware_interface::return_type Ar3SystemHardware::start() }; thread_executor_spin_ = std::thread(spin); - j1_pub_ = nh_->create_publisher("/net_robot/drives/j1", 10); - j2_pub_ = nh_->create_publisher("/net_robot/drives/j2", 10); - j3_pub_ = nh_->create_publisher("/net_robot/drives/j3", 10); - j4_pub_ = nh_->create_publisher("/net_robot/drives/j4", 10); - j5_pub_ = nh_->create_publisher("/net_robot/drives/j5", 10); - j6_pub_ = nh_->create_publisher("/net_robot/drives/j6", 10); + j1_pub_ = nh_->create_publisher("/move/j1", 10); + j2_pub_ = nh_->create_publisher("/move/j2", 10); + j3_pub_ = nh_->create_publisher("/move/j3", 10); + j4_pub_ = nh_->create_publisher("/move/j4", 10); + j5_pub_ = nh_->create_publisher("/move/j5", 10); + j6_pub_ = nh_->create_publisher("/move/j6", 10); status_ = hardware_interface::status::STARTED; diff --git a/src/cobot_platform/cobot_platform/move.py b/src/cobot_platform/cobot_platform/move.py index 824fba0..479c082 100644 --- a/src/cobot_platform/cobot_platform/move.py +++ b/src/cobot_platform/cobot_platform/move.py @@ -1,5 +1,10 @@ -import rclpy +import rclpy, re from rclpy.node import Node +from rclpy.qos import QoSProfile + +from sensor_msgs.msg import JointState + +from math import pi from std_msgs.msg import Float32 @@ -20,32 +25,71 @@ class CobotMove(Node): self._serial = Serial('/dev/ttyACM0', 115200) + self._cur = [0.0] * 6 # self._buf = [90, 90, 90] # 3 servos # self._prev_buf = [90, 90, 90] base_topic = 'move/' self.create_subscription(Float32, base_topic+'j1', self._move_j1_cb, rclpy.qos.qos_profile_sensor_data) - self.create_subscription(Float32, base_topic+'j2', self._move_j2_cb, rclpy.qos.qos_profile_sensor_data) + self.create_subscription(Float32, base_topic+'j2', self._move_j2_cb, rclpy.qos.qos_profile_sensor_data) self.create_subscription(Float32, base_topic+'j3', self._move_j3_cb, rclpy.qos.qos_profile_sensor_data) + self.create_subscription(Float32, base_topic+'j4', self._move_j4_cb, rclpy.qos.qos_profile_sensor_data) + self.create_subscription(Float32, base_topic+'j5', self._move_j5_cb, rclpy.qos.qos_profile_sensor_data) + self.create_subscription(Float32, base_topic+'j6', self._move_j6_cb, rclpy.qos.qos_profile_sensor_data) + + qos = QoSProfile(depth=10) + self._joint_pub = self.create_publisher(JointState, 'joint_states', qos) + self._joint_state = JointState() self._serial.write(b'STA0.0.1\n') print(self._serial.readline()) def _move_j1_cb(self, msg): - self._move_servo(Servos.J1, msg.data) + self._cur[0] = int(msg.data / pi / 2. * 2000) + self._move_servo() def _move_j2_cb(self, msg): - self._move_servo(Servos.J2, msg.data) + self._cur[1] = int(msg.data / pi / 2. * 3840.64) + self._move_servo() def _move_j3_cb(self, msg): - self._move_servo(Servos.J3, msg.data) + self._cur[2] = int(msg.data / pi / 2. * 10000) + self._move_servo() - def _move_servo(self, serv_num, pos): - cmd = 'MT' - # self._buf[serv_num] = pos - self._serial.write(f'{cmd}A{int(pos * 10000)}B0C0D0E0F0\n'.encode('UTF-8')) - print(self._serial.readline()) + def _move_j4_cb(self, msg): + self._cur[3] = int(msg.data / pi / 2. * 2000) + self._move_servo() + + def _move_j5_cb(self, msg): + self._cur[4] = int(msg.data / pi / 2. * 2000) + self._move_servo() + + def _move_j6_cb(self, msg): + self._cur[5] = int(msg.data / pi / 2. * 2000) + self._move_servo() + + def _move_servo(self): + self._serial.write(f'MTA{self._cur[0]}B{self._cur[1]}C{self._cur[2]}D{self._cur[3]}E{self._cur[4]}F{self._cur[5]}\n'.encode('UTF-8')) + + + line = self._serial.readline().decode('UTF-8') + + blocks = re.findall(r'[ABCDEF]-*\d+', line) + a = float(blocks[0][1:]) / 2000. * pi * 2. + b = float(blocks[1][1:]) / 3840.64 * pi * 2. + c = float(blocks[2][1:]) / 10000. * pi * 2. + d = float(blocks[3][1:]) / 2000. * pi * 2. + e = float(blocks[4][1:]) / 2000. * pi * 2. + f = float(blocks[5][1:]) / 2000. * pi * 2. + + self._joint_state.header.stamp = self.get_clock().now().to_msg() + self._joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] + self._joint_state.position = [a, b, c, d, e, f] + + self._joint_pub.publish(self._joint_state) + + print(self._joint_state.position) # def _push ? diff --git a/src/joystickmove/joystickmove/move.py b/src/joystickmove/joystickmove/move.py index 692356f..a2f1afa 100644 --- a/src/joystickmove/joystickmove/move.py +++ b/src/joystickmove/joystickmove/move.py @@ -12,10 +12,10 @@ class JoyMove(Node): self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data) - self.base_pub = self.create_publisher(Float32, '/move/body', 10) + self.base_pub = self.create_publisher(Float32, '/move/j1', 10) def _joy_cb(self, msg): - asd = (msg.axes[1] + 1) / 2 * 180.0 + asd = msg.axes[1] self.base_pub.publish(Float32(data=asd))