diff --git a/mycobot_320/new_mycobot_320/scripts/mycobot_320_slider.py b/mycobot_320/new_mycobot_320/scripts/mycobot_320_slider.py index f1f3e75e..2944a416 100755 --- a/mycobot_320/new_mycobot_320/scripts/mycobot_320_slider.py +++ b/mycobot_320/new_mycobot_320/scripts/mycobot_320_slider.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python # -*- coding:utf-8 -*- """[summary] This file obtains the joint angle of the manipulator in ROS, @@ -8,7 +8,8 @@ port: serial prot string. Defaults is '/dev/ttyUSB0' baud: serial prot baudrate. Defaults is 115200. """ - +import math +import time import rospy from sensor_msgs.msg import JointState @@ -22,11 +23,11 @@ def callback(data): data_list = [] for index, value in enumerate(data.position): - data_list.append(value) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) - mc.send_radians(data_list, 80) - - # time.sleep(0.5) + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): global mc @@ -38,7 +39,9 @@ def listener(): baud = rospy.get_param("~baud", 115200) print(port, baud) mc = MyCobot(port, baud) - + time.sleep(0.05) + mc.set_fresh_mode(1) + time.sleep(0.05) # spin() simply keeps python from exiting until this node is stopped print("spin ...") rospy.spin() diff --git a/mycobot_320/new_mycobot_320_gripper_moveit/launch/moveit.rviz b/mycobot_320/new_mycobot_320_gripper_moveit/launch/moveit.rviz index f803bd31..1d535a1f 100644 --- a/mycobot_320/new_mycobot_320_gripper_moveit/launch/moveit.rviz +++ b/mycobot_320/new_mycobot_320_gripper_moveit/launch/moveit.rviz @@ -42,7 +42,7 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 0.1 + - Acceleration_Scaling_Factor: 0.5000000000000001 Class: moveit_rviz_plugin/MotionPlanning Enabled: true Move Group Namespace: "" @@ -263,7 +263,7 @@ Visualization Manager: Show Robot Collision: false Show Robot Visual: true Value: true - Velocity_Scaling_Factor: 0.1 + Velocity_Scaling_Factor: 0.5000000000000001 Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/mycobot_320/new_mycobot_320_gripper_moveit/scripts/sync_plan.py b/mycobot_320/new_mycobot_320_gripper_moveit/scripts/sync_plan.py index 953196ae..966bfc64 100755 --- a/mycobot_320/new_mycobot_320_gripper_moveit/scripts/sync_plan.py +++ b/mycobot_320/new_mycobot_320_gripper_moveit/scripts/sync_plan.py @@ -28,12 +28,12 @@ def callback(data): data_list = data_list[:7] print("radians:%s"%data_list[:6]) - mc.send_radians(data_list[:6], 80) + mc.send_radians(data_list[:6], 25) gripper_value = int(abs(-0.7-data_list[6])* 117) print("gripper_value:%s"%gripper_value) # mc.set_gripper_mode(0) # time.sleep(1) - mc.set_gripper_value(gripper_value, 100) + mc.set_gripper_value(gripper_value, 90) def listener(): global mc @@ -46,10 +46,15 @@ def listener(): baud = rospy.get_param("~baud", 115200) print(port, baud) mc = MyCobot(port, baud) - + time.sleep(0.05) + mc.set_fresh_mode(1) + time.sleep(0.05) # spin() simply keeps python from exiting until this node is stopped print("spin ...") - rospy.spin() + # rospy.spin() + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + rate.sleep() if __name__ == "__main__": diff --git a/mycobot_320/new_mycobot_320_moveit/launch/moveit.rviz b/mycobot_320/new_mycobot_320_moveit/launch/moveit.rviz index cc4f6fe2..ba9f9813 100644 --- a/mycobot_320/new_mycobot_320_moveit/launch/moveit.rviz +++ b/mycobot_320/new_mycobot_320_moveit/launch/moveit.rviz @@ -11,7 +11,7 @@ Panels: - /MotionPlanning1/Planned Path1 - /TF1 Splitter Ratio: 0.7425600290298462 - Tree Height: 134 + Tree Height: 95 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -44,7 +44,7 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 1 + - Acceleration_Scaling_Factor: 0.5000000000000001 Class: moveit_rviz_plugin/MotionPlanning Enabled: true Move Group Namespace: "" @@ -195,7 +195,7 @@ Visualization Manager: Show Robot Collision: false Show Robot Visual: false Value: true - Velocity_Scaling_Factor: 1 + Velocity_Scaling_Factor: 0.5000000000000001 - Class: rviz/TF Enabled: false Filter (blacklist): "" @@ -252,7 +252,7 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 954 + Height: 906 Help: collapsed: false Hide Left Dock: false @@ -261,7 +261,7 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002ad00000360fc0200000007fb000000100044006900730070006c006100790073010000003d00000117000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007200000001860000004a0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000015a000002430000017d00ffffff000004850000036000000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f300000330fc0200000007fb000000100044006900730070006c006100790073010000003d000000f0000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000133000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000017a000001f30000017d00ffffff0000053f0000033000000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false Width: 1848 diff --git a/mycobot_320/new_mycobot_320_moveit/scripts/sync_plan.py b/mycobot_320/new_mycobot_320_moveit/scripts/sync_plan.py index 188a119e..93cfd214 100755 --- a/mycobot_320/new_mycobot_320_moveit/scripts/sync_plan.py +++ b/mycobot_320/new_mycobot_320_moveit/scripts/sync_plan.py @@ -1,6 +1,7 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 # -*- coding:utf-8 -*- import time +import math import subprocess import rospy from sensor_msgs.msg import JointState @@ -12,29 +13,34 @@ def callback(data): - rospy.loginfo(rospy.get_caller_id() + "%s", 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) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) - mc.send_radians(data_list, 80) + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): global mc rospy.init_node("mycobot_reciver", anonymous=True) - port = rospy.get_param("~port", "/dev/ttyUSB0") + port = rospy.get_param("~port", "/dev/ttyACM0") baud = rospy.get_param("~baud", 115200) - # 1000000 - mc = MyCobot(port, baud) + mc = MyCobot(port, baud) + time.sleep(0.05) + mc.set_fresh_mode(1) + time.sleep(0.05) rospy.Subscriber("joint_states", JointState, callback) # spin() simply keeps python from exiting until this node is stopped - rospy.spin() + # rospy.spin() + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + rate.sleep() if __name__ == "__main__": diff --git a/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_slider.py b/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_slider.py index 829982e5..cdaab1ad 100755 --- a/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_slider.py +++ b/mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_slider.py @@ -9,7 +9,8 @@ port: serial prot string. Defaults is '/dev/ttyAMA0' baud: serial prot baudrate. Defaults is 115200. """ - +import math +import time import rospy from sensor_msgs.msg import JointState @@ -24,12 +25,11 @@ def callback(data): print(data.position) data_list = [] for index, value in enumerate(data.position): - data_list.append(value) - - mc.send_radians(data_list, 80) + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) - print('data_list:', data_list) - # time.sleep(0.5) + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + mc.send_angles(data_list, 25) def listener(): @@ -41,6 +41,9 @@ def listener(): baud = rospy.get_param("~baud", 115200) print(port, baud) mc = MyCobot(port, baud) + time.sleep(0.05) + mc.set_fresh_mode(1) + time.sleep(0.05) # spin() simply keeps python from exiting until this node is stopped print("spin ...") diff --git a/mycobot_320/new_mycobot_320_pi_moveit/launch/moveit.rviz b/mycobot_320/new_mycobot_320_pi_moveit/launch/moveit.rviz index a4a0f5e8..6f3b77a3 100644 --- a/mycobot_320/new_mycobot_320_pi_moveit/launch/moveit.rviz +++ b/mycobot_320/new_mycobot_320_pi_moveit/launch/moveit.rviz @@ -43,7 +43,7 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 1 + - Acceleration_Scaling_Factor: 0.5000000000000001 Class: moveit_rviz_plugin/MotionPlanning Enabled: true Move Group Namespace: "" @@ -194,7 +194,7 @@ Visualization Manager: Show Robot Collision: false Show Robot Visual: false Value: true - Velocity_Scaling_Factor: 1 + Velocity_Scaling_Factor: 0.5000000000000001 Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/mycobot_320/new_mycobot_320_pi_moveit/scripts/sync_plan.py b/mycobot_320/new_mycobot_320_pi_moveit/scripts/sync_plan.py index 15b1e9f8..79ecebe5 100755 --- a/mycobot_320/new_mycobot_320_pi_moveit/scripts/sync_plan.py +++ b/mycobot_320/new_mycobot_320_pi_moveit/scripts/sync_plan.py @@ -18,9 +18,9 @@ def callback(data): for index, value in enumerate(data.position): radians_to_angles = round(math.degrees(value), 2) data_list.append(radians_to_angles) + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) - # mc.send_radians(data_list, 80) - mc.send_angles(data_list, 60) + mc.send_angles(data_list, 25) def listener(): @@ -31,7 +31,9 @@ def listener(): baud = rospy.get_param("~baud", 115200) # 115200 mc = MyCobot(port, baud) - + time.sleep(0.05) + mc.set_fresh_mode(1) + time.sleep(0.05) rospy.Subscriber("joint_states", JointState, callback) # spin() simply keeps python from exiting until this node is stopped