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

@@ -111,10 +111,10 @@ def generate_launch_description():
             parameters=[robot_description]),
 
         # Fake robot state publisher
-        Node(
+        # Node(
-            package='ar3_description',
+        #     package='ar3_description',
-            executable='state_publisher',
+        #     executable='state_publisher',
-            name='state_publisher',
+        #     name='state_publisher',
-            emulate_tty=True,
+        #     emulate_tty=True,
-            output='screen'),
+        #     output='screen'),
     ])

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,18 +42,33 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Alpha: 1
+    - Acceleration_Scaling_Factor: 0.1
-      Class: rviz_default_plugins/RobotModel
+      Class: moveit_rviz_plugin/MotionPlanning
-      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
       Enabled: true
+      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
@@ -96,11 +110,91 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
-      Name: RobotModel
+        Loop Animation: false
-      TF Prefix: ""
+        Robot Alpha: 0.5
-      Update Interval: 0
+        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
-      Visual Enabled: 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
+      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
+  X: 582
-  Y: 195
+  Y: 35

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<std_msgs::msg::Float32>("/net_robot/drives/j1", 10);
+  j1_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j1", 10);
-  j2_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j2", 10);
+  j2_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j2", 10);
-  j3_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j3", 10);
+  j3_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j3", 10);
-  j4_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j4", 10);
+  j4_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j4", 10);
-  j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j5", 10);
+  j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j5", 10);
-  j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j6", 10);
+  j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j6", 10);
 
   status_ = hardware_interface::status::STARTED;
 

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,6 +25,7 @@ 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]
 
@@ -28,24 +34,62 @@ class CobotMove(Node):
         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+'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):
+    def _move_j4_cb(self, msg):
-        cmd = 'MT'
+        self._cur[3] = int(msg.data / pi / 2. * 2000)
-        #  self._buf[serv_num] = pos
+        self._move_servo()
-        self._serial.write(f'{cmd}A{int(pos * 10000)}B0C0D0E0F0\n'.encode('UTF-8'))
+
-        print(self._serial.readline())
+    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 ?
 

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))