kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-05 19:47:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

fix 320 moveit bug

Browse source
This commit is contained in:
weiquan 2021-12-10 18:10:51 +08:00
parent 744e89c73d
commit 601941d65a
4 changed files with 190 additions and 521 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

611
mycobot_320_moveit/launch/moveit.rviz
View file

@ -5,8 +5,12 @@ Panels:
Property Tree Widget: Property Tree Widget:
Expanded: Expanded:
- /MotionPlanning1 - /MotionPlanning1
Splitter Ratio: 0.74256 - /MotionPlanning1/Scene Geometry1
Tree Height: 664 - /MotionPlanning1/Scene Robot1
- /MotionPlanning1/Planning Request1
- /MotionPlanning1/Planned Path1
Splitter Ratio: 0.742560029
Tree Height: 218
- Class: rviz/Help - Class: rviz/Help
Name: Help Name: Help
- Class: rviz/Views - Class: rviz/Views
@ -14,6 +18,8 @@ Panels:
- /Current View1 - /Current View1
Name: Views Name: Views
Splitter Ratio: 0.5 Splitter Ratio: 0.5
Toolbars:
toolButtonStyle: 2
Visualization Manager: Visualization Manager:
Class: "" Class: ""
Displays: Displays:
@ -23,7 +29,7 @@ Visualization Manager:
Color: 160; 160; 164 Color: 160; 160; 164
Enabled: true Enabled: true
Line Style: Line Style:
Line Width: 0.03 Line Width: 0.0299999993
Value: Lines Value: Lines
Name: Grid Name: Grid
Normal Cell Count: 0 Normal Cell Count: 0
@ -35,302 +41,87 @@ Visualization Manager:
Plane Cell Count: 10 Plane Cell Count: 10
Reference Frame: <Fixed Frame> Reference Frame: <Fixed Frame>
Value: true Value: true
- Class: moveit_rviz_plugin/MotionPlanning - Acceleration_Scaling_Factor: 1
Class: moveit_rviz_plugin/MotionPlanning
Enabled: true 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_Goal_Tolerance: 0 MoveIt_Goal_Tolerance: 0
MoveIt_Planning_Attempts: 10
MoveIt_Planning_Time: 5 MoveIt_Planning_Time: 5
MoveIt_Use_Constraint_Aware_IK: true MoveIt_Use_Constraint_Aware_IK: true
MoveIt_Warehouse_Host: 127.0.0.1 MoveIt_Warehouse_Host: 127.0.0.1
MoveIt_Warehouse_Port: 33829 MoveIt_Warehouse_Port: 33829
MoveIt_Workspace:
Center:
X: 0
Y: 0
Z: 0
Size:
X: 2
Y: 2
Z: 2
Name: MotionPlanning Name: MotionPlanning
Planned Path: Planned Path:
Color Enabled: false
Interrupt Display: false
Links: Links:
base_bellow_link: All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
base_footprint: gripper:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
base_link: link1:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
bl_caster_l_wheel_link: link2:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
bl_caster_r_wheel_link: link3:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
bl_caster_rotation_link: link4:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
br_caster_l_wheel_link: link5:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
br_caster_r_wheel_link: link6:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
br_caster_rotation_link: Loop Animation: false
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
double_stereo_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_l_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_r_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_l_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_r_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_kinect_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_kinect_rgb_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_prosilica_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_plate_frame:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_tilt_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_elbow_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_forearm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_l_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_l_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_motor_accelerometer_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_palm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_r_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_r_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_shoulder_lift_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_shoulder_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_upper_arm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_wrist_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_wrist_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
laser_tilt_mount_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_elbow_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_forearm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_l_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_l_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_motor_accelerometer_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_palm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_r_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_r_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_shoulder_lift_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_shoulder_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_upper_arm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_wrist_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_wrist_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
sensor_mount_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
torso_lift_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Loop Animation: true
Robot Alpha: 0.5 Robot Alpha: 0.5
Robot Color: 150; 50; 150
Show Robot Collision: false Show Robot Collision: false
Show Robot Visual: true Show Robot Visual: true
Show Trail: false Show Trail: false
State Display Time: 0.05 s State Display Time: 0.05 s
Trail Step Size: 1
Trajectory Topic: move_group/display_planned_path Trajectory Topic: move_group/display_planned_path
Planning Metrics: Planning Metrics:
Payload: 1 Payload: 1
@ -338,15 +129,16 @@ Visualization Manager:
Show Manipulability: false Show Manipulability: false
Show Manipulability Index: false Show Manipulability Index: false
Show Weight Limit: false Show Weight Limit: false
TextHeight: 0.0799999982
Planning Request: Planning Request:
Colliding Link Color: 255; 0; 0 Colliding Link Color: 255; 0; 0
Goal State Alpha: 1 Goal State Alpha: 1
Goal State Color: 250; 128; 0 Goal State Color: 250; 128; 0
Interactive Marker Size: 0 Interactive Marker Size: 0
Joint Violation Color: 255; 0; 255 Joint Violation Color: 255; 0; 255
Planning Group: left_arm Planning Group: arm_group
Query Goal State: true Query Goal State: true
Query Start State: false Query Start State: true
Show Workspace: false Show Workspace: false
Start State Alpha: 1 Start State Alpha: 1
Start State Color: 0; 255; 0 Start State Color: 0; 255; 0
@ -355,300 +147,69 @@ Visualization Manager:
Scene Geometry: Scene Geometry:
Scene Alpha: 1 Scene Alpha: 1
Scene Color: 50; 230; 50 Scene Color: 50; 230; 50
Scene Display Time: 0.2 Scene Display Time: 0.200000003
Show Scene Geometry: true Show Scene Geometry: true
Voxel Coloring: Z-Axis Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels Voxel Rendering: Occupied Voxels
Scene Robot: Scene Robot:
Attached Body Color: 150; 50; 150 Attached Body Color: 150; 50; 150
Links: Links:
base_bellow_link: All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
base_footprint: gripper:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
base_link: link1:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
bl_caster_l_wheel_link: link2:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
bl_caster_r_wheel_link: link3:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
bl_caster_rotation_link: link4:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
br_caster_l_wheel_link: link5:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
br_caster_r_wheel_link: link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
br_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
double_stereo_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_l_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_r_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_l_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_r_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_kinect_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_kinect_rgb_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_prosilica_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_plate_frame:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_tilt_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_elbow_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_forearm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_l_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_l_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_motor_accelerometer_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_palm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_r_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_r_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_shoulder_lift_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_shoulder_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_upper_arm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_wrist_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_wrist_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
laser_tilt_mount_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_elbow_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_forearm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_l_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_l_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_motor_accelerometer_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_palm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_r_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_r_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_shoulder_lift_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_shoulder_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_upper_arm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_wrist_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_wrist_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
sensor_mount_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
torso_lift_link:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
Show Trail: false Show Trail: false
Value: true Value: true
Robot Alpha: 0.5 Robot Alpha: 0.5
Show Scene Robot: true Show Robot Collision: false
Show Robot Visual: false
Value: true Value: true
Velocity_Scaling_Factor: 1
Enabled: true Enabled: true
Global Options: Global Options:
Background Color: 48; 48; 48 Background Color: 48; 48; 48
Fixed Frame: /base Default Light: true
Fixed Frame: base
Frame Rate: 30
Name: root Name: root
Tools: Tools:
- Class: rviz/Interact - Class: rviz/Interact
@ -659,31 +220,41 @@ Visualization Manager:
Views: Views:
Current: Current:
Class: rviz/XYOrbit Class: rviz/XYOrbit
Distance: 2.9965 Distance: 2.99650002
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point: Focal Point:
X: 0.113567 X: 0.113567002
Y: 0.10592 Y: 0.105920002
Z: 2.23518e-07 Z: 2.23518001e-07
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View Name: Current View
Near Clip Distance: 0.01 Near Clip Distance: 0.00999999978
Pitch: 0.509797 Pitch: 0.509796977
Target Frame: /base Target Frame: base
Value: XYOrbit (rviz) Value: XYOrbit (rviz)
Yaw: 5.65995 Yaw: 5.65994978
Saved: ~ Saved: ~
Window Geometry: Window Geometry:
Displays: Displays:
collapsed: false collapsed: false
Height: 1337 Height: 926
Help: Help:
collapsed: false collapsed: false
Hide Left Dock: false Hide Left Dock: false
Hide Right Dock: false Hide Right Dock: false
Motion Planning: MotionPlanning:
collapsed: false collapsed: false
QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 MotionPlanning - Trajectory Slider:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000100000000000002ad00000358fc0200000007fb000000100044006900730070006c00610079007301000000280000016f000000d700fffffffb0000000800480065006c00700000000342000000bb0000007300fffffffb0000000a0056006900650077007300000003b0000000b0000000ad00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c0069006400650072010000019d0000004d0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001f0000001900000018300ffffff0000048c0000035800000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Views: Views:
collapsed: false collapsed: false
Width: 1828 Width: 1855
X: 459 X: 65
Y: -243 Y: 24

57
mycobot_320_moveit/launch/mycobot320_moveit.launch Normal file
View file

@ -0,0 +1,57 @@
<launch>
<!-- By default, we do not start a database (it can be large) -->
<arg name="db" default="false" />
<!-- Allow user to specify database location -->
<arg name="db_path" default="$(find mycobot_320_moveit)/default_warehouse_mongo_db" />
<!-- By default, we are not in debug mode -->
<arg name="debug" default="false" />
<!--
By default, hide joint_state_publisher's GUI
MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher.
The latter one maintains and publishes the current joint configuration of the simulated robot.
It also provides a GUI to move the simulated robot around "manually".
This corresponds to moving around the real robot without the use of MoveIt.
-->
<arg name="use_gui" default="false" />
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
<include file="$(find mycobot_320_moveit)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<!-- If needed, broadcast static tf for robot root -->
<!-- We do not have a robot connected, so publish fake joint states -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="$(arg use_gui)"/>
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
</node>
<!-- Given the published joint states, publish tf for the robot links -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find mycobot_320_moveit)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="true"/>
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
</include>
<!-- Run Rviz and load the default config to see the state of the move_group node -->
<include file="$(find mycobot_320_moveit)/launch/moveit_rviz.launch">
<arg name="config" value="true"/>
<arg name="debug" value="$(arg debug)"/>
</include>
<!-- If database loading was enabled, start mongodb as well -->
<include file="$(find mycobot_320_moveit)/launch/default_warehouse_db.launch" if="$(arg db)">
<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
</include>
</launch>

41
mycobot_320_moveit/scripts/sync_plan.py Normal file
View file

@ -0,0 +1,41 @@
#!/usr/bin/env python2
import time
import subprocess
import rospy
from sensor_msgs.msg import JointState
from pymycobot.mycobot import MyCobot
mc = None
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "%s", data)
data_list = []
for index, value in enumerate(data.position):
# if index != 2:
# value *= -1
data_list.append(value)
mc.send_radians(data_list, 80)
def listener():
global mc
rospy.init_node("mycobot_reciver", anonymous=True)
port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
shell=True)
port = rospy.get_param("~port", port)
baud = rospy.get_param("~baud", 1000000)
mc = MyCobot(port, baud)
rospy.Subscriber("joint_states", JointState, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == "__main__":
listener()