1.
#!/usr/bin/env python
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseArray, Pose
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
from moveit_commander import roscpp_initialize, roscpp_shutdown
from std_msgs.msg import String
import sys
import threading
class TrajectoryReceiver:
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('trajectory_receiver', anonymous=True)
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
self.group = MoveGroupCommander("arm_group")
self.group.set_planning_time(30) # 增加规划时间
self.trajectory_subscriber = rospy.Subscriber("robot_arm_trajectory", PoseArray, self.trajectory_callback)
self.waypoints = []
self.lock = threading.Lock()
def trajectory_callback(self, msg):
rospy.loginfo("Received trajectory with %d points", len(msg.poses))
with self.lock:
self.waypoints = list(msg.poses)
threading.Thread(target=self.process_trajectory).start()
def process_trajectory(self):
with self.lock:
waypoints = self.waypoints
if not waypoints:
return
rospy.loginfo("Processing trajectory with %d points", len(waypoints))
# 获取机械臂的当前状态
current_pose = self.group.get_current_pose().pose
# 将当前状态添加为第一个路径点
waypoints.insert(0, current_pose)
# 将所有四元数设置为默认值 (0, 0, 0, 1)
for i in range(len(waypoints)):
waypoints[i].orientation.x = 0.0
waypoints[i].orientation.y = 0.0
waypoints[i].orientation.z = 0.0
waypoints[i].orientation.w = 1.0
# 打印每个路径点的信息
rospy.loginfo("Waypoint %d: x=%f, y=%f, z=%f, qx=%f, qy=%f, qz=%f, qw=%f",
i, waypoints[i].position.x, waypoints[i].position.y, waypoints[i].position.z,
waypoints[i].orientation.x, waypoints[i].orientation.y, waypoints[i].orientation.z, waypoints[i].orientation.w)
plan, fraction = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.05, # eef_step
0.2) # jump_threshold
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
self.group.execute(plan, wait=True)
else:
rospy.logwarn("Path planning failed with only %.2f%% success.", fraction * 100)
def start(self):
rospy.spin()
if __name__ == "__main__":
try:
traj_receiver = TrajectoryReceiver()
traj_receiver.start()
except rospy.ROSInterruptException:
pass
finally:
roscpp_shutdown()
2.test_trajectory.py
#!/usr/bin/env python
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
from moveit_commander import roscpp_initialize, roscpp_shutdown
import geometry_msgs.msg
import sys
from copy import deepcopy
def main():
roscpp_initialize(sys.argv)
rospy.init_node('test_trajectory', anonymous=True)
robot = RobotCommander()
scene = PlanningSceneInterface()
group = MoveGroupCommander("arm_group")
group.set_planning_time(10)
# 获取终端link的名称
end_effector_link = group.get_end_effector_link()
# 获取当前位姿数据作为机械臂运动的起始位姿
start_pose = group.get_current_pose(end_effector_link).pose
# 打印初始位姿
rospy.loginfo("Start Pose:")
rospy.loginfo("Position: x = %f, y = %f, z = %f", start_pose.position.x, start_pose.position.y, start_pose.position.z)
rospy.loginfo("Orientation: x = %f, y = %f, z = %f, w = %f", start_pose.orientation.x, start_pose.orientation.y, start_pose.orientation.z, start_pose.orientation.w)
# 设置终点
end_pose = geometry_msgs.msg.Pose()
end_pose.position.x = 0.21
end_pose.position.y = -0.12
end_pose.position.z = 0.24
end_pose.orientation.x = start_pose.orientation.x
end_pose.orientation.y = start_pose.orientation.y
end_pose.orientation.z = start_pose.orientation.z
end_pose.orientation.w = start_pose.orientation.w
waypoints = [start_pose, end_pose]
# 进行笛卡尔路径规划
fraction = 0.0 # 路径规划覆盖率
maxtries = 100 # 最大尝试规划次数
attempts = 0 # 已经尝试规划次数
# 设置机械臂当前的状态作为运动初始状态
group.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径,依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = group.compute_cartesian_path(
waypoints, # 路点列表
0.01, # eef_step,终端步进值
0.0) # jump_threshold,跳跃阈值
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# 如果路径规划成功(覆盖率100%),则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
group.execute(plan, wait=True)
rospy.loginfo("Path execution complete.")
else:
rospy.logwarn("Path planning failed with only %.2f%% success after %d attempts.", fraction * 100, attempts)
roscpp_shutdown()
if __name__ == "__main__":
try:
main()
except rospy.ROSInterruptException:
pass