update myarm ros and add moveit avoidance script

elephantrobotics · Jul 25, 2023 · 1128f97 · 1128f97
1 parent 26f52fa
commit 1128f97
Show file tree

Hide file tree

Showing 5 changed files with 20 additions and 161 deletions.
diff --git a/myArm/myarm/scripts/slider_control.py b/myArm/myarm/scripts/slider_control.py
@@ -8,7 +8,7 @@
     port: serial prot string. Defaults is '/dev/ttyUSB0'
     baud: serial prot baudrate. Defaults is 115200.
 """
-
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -23,9 +23,11 @@ def callback(data):
     print(data.position)
     data_list = []
     for index, value in enumerate(data.position):
-        data_list.append(value)
+        radians_to_angles = math.degrees(value)
+        data_list.append(radians_to_angles)
 
-    mc.send_radians(data_list, 80)
+    # mc.send_radians(data_list, 80)
+    mc.send_angles(data_list, 80)
     # time.sleep(0.5)
 
 

diff --git a/myArm/myarm_moveit/launch/moveit.rviz b/myArm/myarm_moveit/launch/moveit.rviz
@@ -236,9 +236,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5
+      Pitch: 0.4847961962223053
       Target Frame: base
-      Yaw: -0.6232355833053589
+      Yaw: 5.471311569213867
     Saved: ~
 Window Geometry:
   Displays:

diff --git a/myArm/myarm_moveit/scripts/obstacle_test.py → ...moveit/scripts/obstacle_avoidance_demo.py b/myArm/myarm_moveit/scripts/obstacle_test.py → ...moveit/scripts/obstacle_avoidance_demo.py
@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+# -*- coding: utf-8 -*-
 
 import sys
 import rospy
@@ -22,14 +23,7 @@ def __init__(self):
 
         # 创建MoveGroupCommander对象
         self.arm_group = moveit_commander.MoveGroupCommander("arm_group")
-        # 切换规划器为ompl
-        # self.arm_group.set_planner_id("ompl")
-        # planner_params = {"clearance": 0.1}  # 设置障碍物清除参数为0.1米（可以根据实际情况调整值）
-        # arm_group.set_planner_params("ompl", planner_params)
-
-        # 设置 OMPL 规划器的名称
-        # self.arm_group.set_planner_id("RRTConnectkConfigDefault")
-
+
         # 获取末端关节的名称
         self.end_effector_link = self.arm_group.get_end_effector_link()
 
@@ -85,7 +79,7 @@ def robot_move(self):
         # 控制机械臂回到初始化位置
         self.arm_group.set_named_target('init_pose')
         self.arm_group.go()
-        rospy.sleep(5)
+        rospy.sleep(3)
 
 
         # 设置机械臂运动的目标点，使用笛卡尔空间坐标位置表示（单位：米），姿态使用四元数表示
@@ -100,10 +94,6 @@ def robot_move(self):
         target_pose.pose.orientation.z = 0.0
         target_pose.pose.orientation.w = 0.014
 
-        # 移除所有障碍物
-        # scene.remove_world_object("cylinder1")
-        # scene.remove_world_object("cylinder2")
-
         # 更新当前的位姿
         self.arm_group.set_start_state_to_current_state()
 
@@ -120,7 +110,7 @@ def robot_move(self):
         # print('plan point:', plan[1])
         # 执行运动
         self.arm_group.execute(plan[1])
-        rospy.sleep(5)
+        rospy.sleep(3)
         # 获取末端执行器的姿态
         end_effector_pose = self.arm_group.get_current_pose().pose
 
@@ -146,8 +136,8 @@ def robot_move(self):
 
     def run(self):
         # 移除所有障碍物
-        self.scene.remove_world_object("cylinder1")
-        self.scene.remove_world_object("cylinder2")
+        # self.scene.remove_world_object("cylinder1")
+        # self.scene.remove_world_object("cylinder2")
         # 没有障碍物运行一次
         # self.robot_move()
 

diff --git a/myArm/myarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/myArm/myarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
diff --git a/myArm/myarm_moveit/scripts/sync_plan.py b/myArm/myarm_moveit/scripts/sync_plan.py
@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+# -*- coding: utf-8 -*-
 
 """[summary]
 This file obtains the joint angle of the manipulator in ROS,
@@ -8,7 +9,7 @@
     port: serial prot string. Defaults is '/dev/ttyUSB0'
     baud: serial prot baudrate. Defaults is 115200.
 """
-
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -23,9 +24,11 @@ def callback(data):
     print(data.position)
     data_list = []
     for index, value in enumerate(data.position):
-        data_list.append(value)
+        radians_to_angles = math.degrees(value)
+        data_list.append(radians_to_angles)
 
-    mc.send_radians(data_list, 80)
+    # mc.send_radians(data_list, 80)
+    mc.send_angles(data_list, 80)
     # time.sleep(0.5)
 
 
@@ -38,7 +41,7 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyArm(port, baud)
-
+  
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止
     print("spin ...")