机械臂代码优化

catkin_ws/src/lstrobot_planning/scripts/visual.py

@@ -1,94 +0,0 @@
-import rospy
-import moveit_commander
-import moveit_visual_tools
-import numpy as np
-import os, sys
-
-from geometry_msgs.msg import Pose
-from math import pi, cos, sin
-
-def euler2quat(phi, theta, psi):
-    c1 = cos(phi / 2)
-    c2 = cos(theta / 2)
-    c3 = cos(psi / 2)
-    s1 = sin(phi / 2)
-    s2 = sin(theta / 2)
-    s3 = sin(psi / 2)
-    quat = [
-        c1 * c2 * c3 + s1 * s2 * s3,
-        s1 * c2 * c3 - c1 * s2 * s3,
-        c1 * s2 * c3 + s1 * c2 * s3,
-        c1 * c2 * s3 - s1 * s2 * c3
-    ]
-    return quat
-
-def read_points(file_path):
-    with open(file_path, 'r') as file:
-        lines = file.readlines()
-    return lines
-
-def split_str(src, delimiter):
-    return [elem for elem in src.split(delimiter) if elem]
-
-def main():
-    rospy.init_node("visual_node", anonymous=True)
-
-    moveit_commander.roscpp_initialize(sys.argv)
-    visual_tools = moveit_visual_tools.MoveItVisualTools('base_link')
-
-    visual_tools.deleteAllMarkers()
-    visual_tools.loadRemoteControl()
-    
-    text_pose = visual_tools.get_identity_pose()
-    text_pose.position.z = 1.75
-
-    waypoints = [[] for _ in range(20)]
-
-    # 从参数服务器获取文件路径
-    folder_path = rospy.get_param("folder_path", "")
-    file_path = os.path.join(folder_path, "result.txt")
-
-    num = 0
-    lines = read_points(file_path)
-    for line in lines:
-        l = split_str(line, "sp,/")
-
-        target_pose1 = Pose()
-        target_pose2 = Pose()
-
-        target_pose1.position.x = float(l[0]) / 1000
-        target_pose1.position.y = float(l[1]) / 1000
-        target_pose1.position.z = float(l[2]) / 1000
-
-        target_pose2.position.x = float(l[3]) / 1000
-        target_pose2.position.y = float(l[4]) / 1000
-        target_pose2.position.z = float(l[5]) / 1000
-
-        target_pose1.orientation.x = float(l[6])
-        target_pose1.orientation.y = float(l[7])
-        target_pose1.orientation.z = float(l[8])
-        target_pose1.orientation.w = float(l[9])
-
-        target_pose2.orientation.x = float(l[10])
-        target_pose2.orientation.y = float(l[11])
-        target_pose2.orientation.z = float(l[12])
-        target_pose2.orientation.w = float(l[13])
-
-        waypoints[num].append(target_pose1)
-        waypoints[num].append(target_pose2)
-        num += 1
-
-    visual_tools.deleteAllMarkers()
-    visual_tools.publishText(text_pose, "robot", visual_tools.WHITE, visual_tools.XLARGE)
-
-    for k in range(num):
-        visual_tools.publishPath(waypoints[k], visual_tools.PINK, visual_tools.XXXSMALL)
-        for i, wp in enumerate(waypoints[k]):
-            label = f"point{i}  {wp.position.x:.2f} {wp.position.y:.2f} {wp.position.z:.2f}"
-            visual_tools.publishAxisLabeled(wp, label, visual_tools.XXXSMALL)
-        visual_tools.trigger()
-
-    moveit_commander.roscpp_shutdown()
-
-if __name__ == '__main__':
-    main()