Robot actually moves actual real hardware now
Actually Gear ratios subject to change Need to move state publisher into ar3_hardware/joint_state_broadcaster
This commit is contained in:
parent
70568e30b1
commit
0528093f41
5 changed files with 228 additions and 86 deletions
|
@ -111,10 +111,10 @@ def generate_launch_description():
|
||||||
parameters=[robot_description]),
|
parameters=[robot_description]),
|
||||||
|
|
||||||
# Fake robot state publisher
|
# 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'),
|
||||||
])
|
])
|
||||||
|
|
|
@ -6,9 +6,8 @@ Panels:
|
||||||
Expanded:
|
Expanded:
|
||||||
- /Global Options1
|
- /Global Options1
|
||||||
- /Status1
|
- /Status1
|
||||||
- /RobotModel1
|
|
||||||
Splitter Ratio: 0.5
|
Splitter Ratio: 0.5
|
||||||
Tree Height: 616
|
Tree Height: 218
|
||||||
- Class: rviz_common/Selection
|
- Class: rviz_common/Selection
|
||||||
Name: Selection
|
Name: Selection
|
||||||
- Class: rviz_common/Tool Properties
|
- Class: rviz_common/Tool Properties
|
||||||
|
@ -43,64 +42,159 @@ Visualization Manager:
|
||||||
Plane Cell Count: 10
|
Plane Cell Count: 10
|
||||||
Reference Frame: <Fixed Frame>
|
Reference Frame: <Fixed Frame>
|
||||||
Value: true
|
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
|
Enabled: true
|
||||||
Links:
|
Move Group Namespace: ""
|
||||||
All Links Enabled: true
|
MoveIt_Allow_Approximate_IK: false
|
||||||
Expand Joint Details: false
|
MoveIt_Allow_External_Program: false
|
||||||
Expand Link Details: false
|
MoveIt_Allow_Replanning: false
|
||||||
Expand Tree: false
|
MoveIt_Allow_Sensor_Positioning: false
|
||||||
Link Tree Style: Links in Alphabetic Order
|
MoveIt_Planning_Attempts: 10
|
||||||
base_link:
|
MoveIt_Planning_Time: 5
|
||||||
Alpha: 1
|
MoveIt_Use_Cartesian_Path: false
|
||||||
Show Axes: false
|
MoveIt_Use_Constraint_Aware_IK: false
|
||||||
Show Trail: false
|
MoveIt_Warehouse_Host: 127.0.0.1
|
||||||
Value: true
|
MoveIt_Warehouse_Port: 33829
|
||||||
link_1:
|
MoveIt_Workspace:
|
||||||
Alpha: 1
|
Center:
|
||||||
Show Axes: false
|
X: 0
|
||||||
Show Trail: false
|
Y: 0
|
||||||
Value: true
|
Z: 0
|
||||||
link_2:
|
Size:
|
||||||
Alpha: 1
|
X: 2
|
||||||
Show Axes: false
|
Y: 2
|
||||||
Show Trail: false
|
Z: 2
|
||||||
Value: true
|
Name: MotionPlanning
|
||||||
link_3:
|
Planned Path:
|
||||||
Alpha: 1
|
Color Enabled: false
|
||||||
Show Axes: false
|
Interrupt Display: false
|
||||||
Show Trail: false
|
Links:
|
||||||
Value: true
|
All Links Enabled: true
|
||||||
link_4:
|
Expand Joint Details: false
|
||||||
Alpha: 1
|
Expand Link Details: false
|
||||||
Show Axes: false
|
Expand Tree: false
|
||||||
Show Trail: false
|
Link Tree Style: Links in Alphabetic Order
|
||||||
Value: true
|
base_link:
|
||||||
link_5:
|
Alpha: 1
|
||||||
Alpha: 1
|
Show Axes: false
|
||||||
Show Axes: false
|
Show Trail: false
|
||||||
Show Trail: false
|
Value: true
|
||||||
Value: true
|
link_1:
|
||||||
link_6:
|
Alpha: 1
|
||||||
Alpha: 1
|
Show Axes: false
|
||||||
Show Axes: false
|
Show Trail: false
|
||||||
Show Trail: false
|
Value: true
|
||||||
Value: true
|
link_2:
|
||||||
Name: RobotModel
|
Alpha: 1
|
||||||
TF Prefix: ""
|
Show Axes: false
|
||||||
Update Interval: 0
|
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
|
Value: true
|
||||||
Visual Enabled: true
|
Velocity_Scaling_Factor: 0.1
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Global Options:
|
Global Options:
|
||||||
Background Color: 48; 48; 48
|
Background Color: 48; 48; 48
|
||||||
|
@ -144,7 +238,7 @@ Visualization Manager:
|
||||||
Views:
|
Views:
|
||||||
Current:
|
Current:
|
||||||
Class: rviz_default_plugins/Orbit
|
Class: rviz_default_plugins/Orbit
|
||||||
Distance: 10
|
Distance: 2.1567115783691406
|
||||||
Enable Stereo Rendering:
|
Enable Stereo Rendering:
|
||||||
Stereo Eye Separation: 0.05999999865889549
|
Stereo Eye Separation: 0.05999999865889549
|
||||||
Stereo Focal Distance: 1
|
Stereo Focal Distance: 1
|
||||||
|
@ -165,12 +259,16 @@ Visualization Manager:
|
||||||
Yaw: 0.785398006439209
|
Yaw: 0.785398006439209
|
||||||
Saved: ~
|
Saved: ~
|
||||||
Window Geometry:
|
Window Geometry:
|
||||||
|
"":
|
||||||
|
collapsed: false
|
||||||
|
" - Trajectory Slider":
|
||||||
|
collapsed: false
|
||||||
Displays:
|
Displays:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Height: 846
|
Height: 846
|
||||||
Hide Left Dock: false
|
Hide Left Dock: false
|
||||||
Hide Right Dock: false
|
Hide Right Dock: false
|
||||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002f3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000002f3000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002f3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
QMainWindow State: 000000ff00000000fd0000000400000000000001e5000002f3fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000165000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffbffffffff01000001a9000001880000017d00ffffff000000010000010f000002f3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002f3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000001b0000002f300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
Selection:
|
Selection:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Tool Properties:
|
Tool Properties:
|
||||||
|
@ -178,5 +276,5 @@ Window Geometry:
|
||||||
Views:
|
Views:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Width: 1200
|
Width: 1200
|
||||||
X: 1068
|
X: 582
|
||||||
Y: 195
|
Y: 35
|
||||||
|
|
|
@ -118,12 +118,12 @@ hardware_interface::return_type Ar3SystemHardware::start()
|
||||||
};
|
};
|
||||||
thread_executor_spin_ = std::thread(spin);
|
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;
|
status_ = hardware_interface::status::STARTED;
|
||||||
|
|
||||||
|
|
|
@ -1,5 +1,10 @@
|
||||||
import rclpy
|
import rclpy, re
|
||||||
from rclpy.node import Node
|
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
|
from std_msgs.msg import Float32
|
||||||
|
|
||||||
|
@ -20,32 +25,71 @@ class CobotMove(Node):
|
||||||
|
|
||||||
self._serial = Serial('/dev/ttyACM0', 115200)
|
self._serial = Serial('/dev/ttyACM0', 115200)
|
||||||
|
|
||||||
|
self._cur = [0.0] * 6
|
||||||
# self._buf = [90, 90, 90] # 3 servos
|
# self._buf = [90, 90, 90] # 3 servos
|
||||||
# self._prev_buf = [90, 90, 90]
|
# self._prev_buf = [90, 90, 90]
|
||||||
|
|
||||||
base_topic = 'move/'
|
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+'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+'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')
|
self._serial.write(b'STA0.0.1\n')
|
||||||
print(self._serial.readline())
|
print(self._serial.readline())
|
||||||
|
|
||||||
def _move_j1_cb(self, msg):
|
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):
|
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):
|
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 ?
|
# def _push ?
|
||||||
|
|
||||||
|
|
|
@ -12,10 +12,10 @@ class JoyMove(Node):
|
||||||
|
|
||||||
self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data)
|
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):
|
def _joy_cb(self, msg):
|
||||||
asd = (msg.axes[1] + 1) / 2 * 180.0
|
asd = msg.axes[1]
|
||||||
self.base_pub.publish(Float32(data=asd))
|
self.base_pub.publish(Float32(data=asd))
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in a new issue