#!/usr/bin/env python3
"""
S3 60机器人控制节点 - 运动完成确认修复版
基于关节角度比较确认运动完成,到达目标后等待0.5s返回
修复:解决MoveIt2 Action Client执行器上下文冲突问题
修复:关节数据重排为标准顺序进行对比
"""
import socket
import json
import time
from threading import Thread, Lock
import traceback
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from sensor_msgs.msg import JointState
from moveit_msgs.msg import DisplayTrajectory
from pymoveit2 import MoveIt2
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
# -------------------------- 配置参数 --------------------------
JOINT_NAMES = ["Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"]
BASE_LINK_NAME = "base_link"
END_EFFECTOR_NAME = "link6"
MOVE_GROUP_ARM = "arm"
PLANNER_ID = "RRTConnectkConfigDefault"
SERVER_IP = "192.168.101.111"
SERVER_PORT = 8888
MAX_VELOCITY = 0.5
MAX_ACCELERATION = 0.5
JOINT_TOLERANCE = 0.03 # 关节角度容差(弧度)
MAX_POINTS = 5 # 最大点数限制
class S360RobotControlNode(Node):
def __init__(self):
super().__init__("s360_robot_full_control")
self.get_logger().info("=== S3 60机器人控制节点启动 ===")
# 状态管理
self.is_shutting_down = False
self.current_joint_positions = [0.0] * len(JOINT_NAMES)
self.joints_lock = Lock()
self.joint_state_received = False # 标记是否收到过关节状态
self.planned_trajectory = None # 整合轨迹监控器功能
self.trajectory_lock = Lock()
# 关节状态消息中的关节顺序(用于重排)
self.joint_state_order = JOINT_NAMES.copy()
# -------------------------- 整合子系统功能 --------------------------
# 1. 轨迹发布器(原RobotMovePublisher)
self.trajectory_pub = self.create_publisher(JointTrajectory, 'RobotMove', 10)
self.get_logger().info("✅ 轨迹发布器初始化完成")
# 2. 轨迹监控器(原TrajectoryMonitor)
self.trajectory_sub = self.create_subscription(
DisplayTrajectory,
'/display_planned_path',
self.trajectory_callback,
10
)
self.get_logger().info("✅ 轨迹监控器初始化完成")
# 3. 关节状态订阅器
self.joint_state_sub = self.create_subscription(
JointState,
'joint_states',
self.joint_state_callback,
10
)
self.get_logger().info("✅ 关节状态订阅器初始化完成")
# 4. MoveIt2初始化(延迟到执行器启动后,避免上下文冲突)
self.moveit2 = None
self.init_moveit2()
# 5. Socket服务器
self.socket_thread = Thread(
target=self.start_socket_server,
daemon=True
)
self.socket_thread.start()
self.get_logger().info(f"✅ Socket服务器启动: {SERVER_IP}:{SERVER_PORT}")
# -------------------------- 原RobotMovePublisher方法(整合) --------------------------
def publish_joint_trajectory(self, target_joints):
"""发布关节轨迹到RobotMove - 单点"""
try:
traj_msg = JointTrajectory()
traj_msg.joint_names = JOINT_NAMES
point = JointTrajectoryPoint()
point.positions = target_joints
point.time_from_start = rclpy.duration.Duration(seconds=2.0).to_msg()
traj_msg.points.append(point)
self.trajectory_pub.publish(traj_msg)
self.get_logger().info(f"📢 发布关节轨迹到RobotMove: {[f'{x:.3f}' for x in target_joints]}")
return True
except Exception as e:
self.get_logger().error(f"❌ 轨迹发布失败: {e}")
return False
def publish_planned_trajectory(self, trajectory_points):
"""发布MoveIt2规划的轨迹到RobotMove - 最多5个点"""
try:
if not trajectory_points:
self.get_logger().error("❌ 轨迹点为空")
return False
traj_msg = JointTrajectory()
traj_msg.joint_names = JOINT_NAMES
# 限制最多5个点
if len(trajectory_points) > MAX_POINTS:
step = len(trajectory_points) // MAX_POINTS
sampled_points = []
for i in range(MAX_POINTS):
idx = min(i * step, len(trajectory_points) - 1)
sampled_points.append(trajectory_points[idx])
# 确保包含起点和终点
if len(trajectory_points) > 1:
sampled_points[0] = trajectory_points[0]
sampled_points[-1] = trajectory_points[-1]
trajectory_points = sampled_points
# 发布轨迹点
for i, point in enumerate(trajectory_points[:MAX_POINTS]):
traj_point = JointTrajectoryPoint()
traj_point.positions = point.positions
# 重新计算时间戳(总时间2秒)
total_time = 2.0
time_ratio = i / (len(trajectory_points[:MAX_POINTS]) - 1) if len(trajectory_points[:MAX_POINTS]) > 1 else 1.0
traj_point.time_from_start = rclpy.duration.Duration(seconds=total_time * time_ratio).to_msg()
traj_msg.points.append(traj_point)
self.trajectory_pub.publish(traj_msg)
self.get_logger().info(f"📢 发布规划轨迹到RobotMove: {len(traj_msg.points)}个点")
return True
except Exception as e:
self.get_logger().error(f"❌ 规划轨迹发布失败: {e}")
return False
def publish_smooth_trajectory(self, start_joints, target_joints, num_points=5):
"""发布平滑插值轨迹到RobotMove - 最多5个点"""
try:
traj_msg = JointTrajectory()
traj_msg.joint_names = JOINT_NAMES
# 确保不超过最大点数
num_points = min(num_points, MAX_POINTS)
# 生成平滑插值点
for i in range(num_points):
ratio = i / (num_points - 1) if num_points > 1 else 1.0
point = JointTrajectoryPoint()
point.positions = [
start + ratio * (target - start)
for start, target in zip(start_joints, target_joints)
]
point.time_from_start = rclpy.duration.Duration(seconds=2.0 * ratio).to_msg()
traj_msg.points.append(point)
self.trajectory_pub.publish(traj_msg)
self.get_logger().info(f"📢 发布平滑轨迹到RobotMove: {num_points}个点")
return True
except Exception as e:
self.get_logger().error(f"❌ 平滑轨迹发布失败: {e}")
return False
# -------------------------- 原TrajectoryMonitor方法(整合) --------------------------
def trajectory_callback(self, msg):
"""处理MoveIt2规划的轨迹"""
try:
if msg.trajectory and len(msg.trajectory) > 0:
joint_trajectory = msg.trajectory[0].joint_trajectory
if joint_trajectory.points:
with self.trajectory_lock:
self.planned_trajectory = joint_trajectory.points
self.get_logger().info(f"📋 收到规划轨迹: {len(joint_trajectory.points)}个点 (将采样为最多{MAX_POINTS}个点)")
except Exception as e:
self.get_logger().error(f"❌ 轨迹处理异常: {e}")
def get_planned_trajectory(self):
"""获取规划的轨迹"""
with self.trajectory_lock:
return self.planned_trajectory
def clear_trajectory(self):
"""清空轨迹缓存"""
with self.trajectory_lock:
self.planned_trajectory = None
# -------------------------- 核心功能方法 --------------------------
def init_moveit2(self):
"""初始化MoveIt2(独立方法,确保上下文正确)"""
try:
self.moveit2 = MoveIt2(
node=self, # 绑定到主节点,避免跨节点上下文冲突
joint_names=JOINT_NAMES,
base_link_name=BASE_LINK_NAME,
end_effector_name=END_EFFECTOR_NAME,
group_name=MOVE_GROUP_ARM
)
self.moveit2.planner_id = PLANNER_ID
self.moveit2.max_velocity = MAX_VELOCITY
self.moveit2.max_acceleration = MAX_ACCELERATION
self.get_logger().info("✅ MoveIt2接口初始化完成")
except Exception as e:
self.get_logger().error(f"❌ MoveIt2初始化失败: {e}")
self.moveit2 = None
def joint_state_callback(self, msg):
"""关节状态回调 - 获取当前关节位置"""
try:
if all(joint in msg.name for joint in JOINT_NAMES):
# 记录关节状态消息中的关节顺序
self.joint_state_order = msg.name
with self.joints_lock:
for i, name in enumerate(JOINT_NAMES):
if name in msg.name:
idx = msg.name.index(name)
self.current_joint_positions[i] = msg.position[idx]
self.joint_state_received = True
except Exception as e:
self.get_logger().warn(f"⚠️ 关节状态处理异常: {e}")
def start_socket_server(self):
"""启动Socket服务器"""
server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
server_socket.bind((SERVER_IP, SERVER_PORT))
server_socket.listen(5)
server_socket.settimeout(1.0)
self.get_logger().info(f"📡 Socket服务器就绪: {SERVER_IP}:{SERVER_PORT}")
while rclpy.ok() and not self.is_shutting_down:
try:
client_socket, addr = server_socket.accept()
self.get_logger().info(f"🔌 客户端连接: {addr}")
Thread(
target=self.handle_client,
args=(client_socket,),
daemon=True
).start()
except socket.timeout:
continue
except Exception as e:
if rclpy.ok() and not self.is_shutting_down:
self.get_logger().error(f"❌ Socket接受异常: {e}")
server_socket.close()
def handle_client(self, client_socket):
"""处理客户端请求"""
try:
client_socket.settimeout(30.0)
data = client_socket.recv(1024).decode().strip()
if not data:
return
# 解析指令
if ':' in data:
cmd_type, cmd_json = data.split(':', 1)
cmd_params = json.loads(cmd_json)
self.get_logger().info(f"📥 收到指令: {cmd_type}, 参数: {cmd_params}")
# 执行对应指令
success = False
if cmd_type == "p1":
success = self.execute_joint_motion(cmd_params)
elif cmd_type in ["p2", "p3"]:
success = self.execute_pose_motion(cmd_params)
else:
self.get_logger().error(f"❌ 未知指令类型: {cmd_type}")
# 发送响应
response = "True" if success else "False"
client_socket.sendall(response.encode())
self.get_logger().info(f"📤 发送响应: {response}")
except Exception as e:
self.get_logger().error(f"❌ 客户端处理异常: {e}")
try:
client_socket.sendall("False".encode())
except:
pass
finally:
try:
client_socket.close()
except:
pass
self.get_logger().info("🔌 客户端连接关闭")
def execute_joint_motion(self, target_joints):
"""执行关节空间运动"""
if not self.moveit2:
self.get_logger().error("❌ MoveIt2未初始化")
return False
if len(target_joints) != len(JOINT_NAMES):
self.get_logger().error(f"❌ 关节数不匹配: 期望{len(JOINT_NAMES)}, 收到{len(target_joints)}")
return False
try:
self.get_logger().info(f"🔄 开始关节运动: {[f'{x:.3f}' for x in target_joints]}")
# 获取当前关节状态
with self.joints_lock:
current_joints = self.current_joint_positions.copy()
# 发布平滑插值轨迹
self.publish_smooth_trajectory(current_joints, target_joints, num_points=5)
# 执行MoveIt2运动(阻塞到规划完成)
self.moveit2.move_to_configuration(target_joints)
# 等待运动完成
success = self.wait_for_motion_completion(target_joints, timeout=30.0)
if success:
self.get_logger().info("✅ 关节运动完成")
else:
self.get_logger().warning("⚠️ 关节运动超时或未完全到达")
return success
except Exception as e:
self.get_logger().error(f"❌ 关节运动执行异常: {e}")
return False
def execute_pose_motion(self, target_pose):
"""执行笛卡尔空间运动"""
if not self.moveit2:
self.get_logger().error("❌ MoveIt2未初始化")
return False
if len(target_pose) != 2 or len(target_pose[0]) != 3 or len(target_pose[1]) != 4:
self.get_logger().error("❌ 位姿格式错误")
return False
try:
target_pos, target_quat = target_pose
self.get_logger().info(f"🔄 开始位姿运动: 位置={target_pos}, 姿态={target_quat}")
# 清空之前的轨迹缓存
self.clear_trajectory()
# 计算逆运动学获取目标关节位置
ik_result = self.moveit2.compute_ik(target_pos, target_quat)
if not ik_result:
self.get_logger().error("❌ 逆运动学求解失败")
return False
# 提取关节位置
target_joints = self._extract_joint_positions_from_state(ik_result)
if not target_joints:
self.get_logger().error("❌ 无法从逆运动学结果中提取关节位置")
return False
self.get_logger().info(f"🎯 逆运动学解算结果: {[f'{x:.3f}' for x in target_joints]}")
# 执行MoveIt2位姿运动(非笛卡尔路径)
self.moveit2.move_to_pose(
position=target_pos,
quat_xyzw=target_quat,
cartesian=False
)
# 等待规划完成并获取轨迹
planned_trajectory = self.wait_for_planned_trajectory(timeout=10.0)
if planned_trajectory:
self.publish_planned_trajectory(planned_trajectory)
self.get_logger().info("✅ 成功发布MoveIt2规划轨迹到RobotMove(最多5个点)")
else:
self.get_logger().warning("⚠️ 无法获取规划轨迹,使用备用平滑轨迹")
with self.joints_lock:
current_joints = self.current_joint_positions.copy()
self.publish_smooth_trajectory(current_joints, target_joints, num_points=5)
# 等待运动完成
success = self.wait_for_motion_completion(target_joints, timeout=30.0)
if success:
self.get_logger().info("✅ 位姿运动完成")
else:
self.get_logger().warning("⚠️ 位姿运动超时")
return success
except Exception as e:
self.get_logger().error(f"❌ 位姿运动执行异常: {e}")
self.get_logger().error(f"详细错误: {traceback.format_exc()}")
return False
def _extract_joint_positions_from_state(self, joint_state):
"""从JointState消息中提取关节位置"""
try:
if not joint_state or not hasattr(joint_state, 'position'):
self.get_logger().error("❌ 无效的JointState对象")
return None
extracted_joints = [0.0] * len(JOINT_NAMES)
success_count = 0
for i, joint_name in enumerate(JOINT_NAMES):
if joint_name in joint_state.name:
idx = joint_state.name.index(joint_name)
extracted_joints[i] = joint_state.position[idx]
success_count += 1
else:
self.get_logger().warning(f"⚠️ 在JointState中未找到关节: {joint_name}")
if success_count == len(JOINT_NAMES):
return extracted_joints
else:
self.get_logger().error(f"❌ 只找到了 {success_count}/{len(JOINT_NAMES)} 个关节")
return None
except Exception as e:
self.get_logger().error(f"❌ 提取关节位置失败: {e}")
return None
def wait_for_planned_trajectory(self, timeout=10.0):
"""等待并获取MoveIt2规划的轨迹"""
start_time = time.time()
while rclpy.ok() and time.time() - start_time < timeout:
planned_trajectory = self.get_planned_trajectory()
if planned_trajectory:
return planned_trajectory
time.sleep(0.1)
return None
def wait_for_motion_completion(self, target_joints, timeout=30.0):
"""基于关节角度比较确认运动完成,到达目标后等待0.5s返回"""
if not self.joint_state_received:
self.get_logger().warning("⚠️ 尚未收到关节状态数据,等待运动完成可能不准确")
self.get_logger().info("⏳ 开始等待运动完成...")
start_time = time.time()
target_reached_time = None
while rclpy.ok() and time.time() - start_time < timeout:
# 获取当前关节位置
with self.joints_lock:
current_joints = self.current_joint_positions.copy()
# 检查是否到达目标位置 - 重排关节顺序到标准顺序
if self._joints_reached_target(current_joints, target_joints):
if target_reached_time is None:
target_reached_time = time.time()
self.get_logger().info("🎯 首次到达目标位置,开始稳定等待...")
else:
# 稳定0.5秒后确认完成
if time.time() - target_reached_time >= 0.5:
self.get_logger().info(f"✅ 运动完成!稳定时间: {time.time() - target_reached_time:.2f}s")
return True
else:
# 未到达目标,重置计时
if target_reached_time is not None:
self.get_logger().info("⚠️ 位置偏移,重置稳定计时")
target_reached_time = None
# 每5秒打印一次进度
elapsed = time.time() - start_time
if elapsed % 5.0 < 0.1:
max_error = self._calculate_max_joint_error(current_joints, target_joints)
self.get_logger().info(f"⏱️ 等待中... 已等待{elapsed:.1f}s, 最大关节误差: {max_error:.4f}rad")
time.sleep(0.1)
# 超时处理
if target_reached_time is not None:
stable_duration = time.time() - target_reached_time
self.get_logger().warning(f"⚠️ 运动部分完成,稳定{stable_duration:.2f}s后超时")
else:
with self.joints_lock:
current_joints = self.current_joint_positions.copy()
max_error = self._calculate_max_joint_error(current_joints, target_joints)
self.get_logger().warning(f"⚠️ 运动超时,最大关节误差: {max_error:.4f}rad")
return False
def _joints_reached_target(self, current_joints, target_joints):
"""检查所有关节是否在容差范围内到达目标"""
# 重新排序当前关节数据到标准顺序
reordered_current = self._reorder_joints_to_standard(current_joints)
if len(reordered_current) != len(target_joints):
return False
for current, target in zip(reordered_current, target_joints):
if abs(current - target) > JOINT_TOLERANCE:
return False
return True
def _calculate_max_joint_error(self, current_joints, target_joints):
"""计算最大关节误差"""
# 重新排序当前关节数据到标准顺序
reordered_current = self._reorder_joints_to_standard(current_joints)
if len(reordered_current) != len(target_joints):
return float('inf')
max_error = 0.0
for current, target in zip(reordered_current, target_joints):
error = abs(current - target)
if error > max_error:
max_error = error
return max_error
def _reorder_joints_to_standard(self, joints_data):
"""
将关节数据重排为标准顺序 ["Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"]
基于当前关节状态话题的实际顺序进行重排
"""
try:
# 如果关节状态消息中的顺序与标准顺序一致,直接返回
if self.joint_state_order == JOINT_NAMES:
return joints_data
# 否则,按照实际关节状态消息的顺序重排到标准顺序
if len(joints_data) == 6 and len(self.joint_state_order) == 6:
# 创建从实际顺序到标准顺序的映射
reordered = [0.0] * 6
for i, joint_name in enumerate(JOINT_NAMES):
if joint_name in self.joint_state_order:
idx = self.joint_state_order.index(joint_name)
reordered[i] = joints_data[idx]
else:
self.get_logger().warning(f"⚠️ 在关节状态消息中未找到关节: {joint_name}")
reordered[i] = joints_data[i] # 使用原位置作为备用
# 如果检测到您提供的特定顺序 ['Joint2','Joint3','Joint1','Joint4','Joint5','Joint6']
if (len(self.joint_state_order) == 6 and
self.joint_state_order[0] == "Joint2" and
self.joint_state_order[1] == "Joint3" and
self.joint_state_order[2] == "Joint1"):
# 使用硬编码的重排逻辑确保正确性
reordered = [
joints_data[2], # Joint1 (在原数组索引2)
joints_data[0], # Joint2 (在原数组索引0)
joints_data[1], # Joint3 (在原数组索引1)
joints_data[3], # Joint4 (在原数组索引3)
joints_data[4], # Joint5 (在原数组索引4)
joints_data[5] # Joint6 (在原数组索引5)
]
return reordered
else:
self.get_logger().warning(f"⚠️ 关节数据长度不匹配: 数据{len(joints_data)}, 顺序{len(self.joint_state_order)}")
return joints_data
except Exception as e:
self.get_logger().error(f"❌ 关节数据重排失败: {e}")
return joints_data
def shutdown(self):
"""关闭节点"""
self.is_shutting_down = True
self.get_logger().info("🛑 开始关闭节点...")
def main():
rclpy.init()
node = None
try:
node = S360RobotControlNode()
# 执行器只添加主节点(关键修复:避免多节点上下文冲突)
executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(node)
node.get_logger().info("🚀 节点开始运行(按Ctrl+C退出)")
executor.spin()
except KeyboardInterrupt:
node.get_logger().info("🛑 收到中断信号,关闭节点...")
except Exception as e:
if node:
node.get_logger().error(f"❌ 节点运行异常: {e}")
else:
print(f"❌ 节点初始化异常: {e}")
finally:
if node:
node.shutdown()
node.destroy_node()
rclpy.shutdown()
print("✅ 节点已安全关闭")
if __name__ == "__main__":
main()
name:
- Joint2
- Joint3
- Joint1
- Joint4
- Joint5
- Joint6
position:
- -0.11739741912
- 1.5494284625348966
- -0.0340351848
- -0.1387772676
- 1.5710593366799999
- -1.60547801652
velocity: []
effort: []
[INFO] [1761221208.042075563] [s360_robot_full_control]: ⏱️ 等待中... 已等待0.0s, 最大关节误差: 1.5621rad
[INFO] [1761221213.085019225] [s360_robot_full_control]: ⏱️ 等待中... 已等待5.0s, 最大关节误差: 1.5621rad
[INFO] [1761221218.132077655] [s360_robot_full_control]: ⏱️ 等待中... 已等待10.1s, 最大关节误差: 1.5621rad
[INFO] [1761221223.078469150] [s360_robot_full_control]: ⏱️ 等待中... 已等待15.0s, 最大关节误差: 1.5621rad
[INFO] [1761221228.111193687] [s360_robot_full_control]: ⏱️ 等待中... 已等待20.1s, 最大关节误差: 1.5621rad
^C[INFO] [1761221230.534367902] [s360_robot_full_control]: 🛑 收到中断信号,关闭节点...
[INFO] [1761221208.035660440] [CodroidIO]: 监听回调
{"joint_names": ["Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"], "points": [{"positions": [-0.02599377318, 0.31809715146, 1.5268861447148965, 0.27448657488, 1.57111925778, -1.59747255756], "velocities": [], "accelerations": []}, {"positions": [-0.02682495239343273, 0.27309715146000013, 1.5292177054939136, 0.23178313590264085, 1.5711143145327557, -1.5983018962505733], "velocities": [], "accelerations": []}, {"positions": [-0.02931848988091759, 0.13809715973330774, 1.5362123874023041, 0.10367282682164504, 1.5710994847919322, -1.6007899121698186], "velocities": [], "accelerations": []}, {"positions": [-0.03240979190073338, -0.02926577312589769, 1.5448838729604328, -0.055148790743353965, 1.5710810999840064, -1.6038743689606618], "velocities": [], "accelerations": []}, {"positions": [-0.034037705688143, -0.1174009421117632, 1.5494503730328655, -0.13878600875551295, 1.5710714183410908, -1.6054986779748885], "velocities": [], "accelerations": []}]}
这是怎么回事,关节数据没有更新,已经到达目标的点位了依然没有返回true,借鉴下面代码中解决关节数据更新的方法解决上诉代码的问题
#!/usr/bin/env python3
"""
优化版:根据坐标类型自动区分控制方式
- 关节坐标 → 关节空间控制
- 笛卡尔坐标 → 笛卡尔空间控制
- 支持p1-p5等多种指令标识(仅作标识用)
- 精简日志输出
"""
from threading import Thread, Lock
import socket
import json
import rclpy
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.node import Node
from pymoveit2 import MoveIt2
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from moveit_msgs.msg import DisplayTrajectory
from codroid_msgs.msg import RobotInfo
import time
import concurrent.futures
# -------------------------- 配置参数 --------------------------
PYCHARM_PORT = 8888
JOINT_TOLERANCE = 0.03
STABLE_COUNT_THRESHOLD = 5
P2_PLAN_TIMEOUT = 10
P2_MOVE_TIMEOUT = 60
# 机器人参数
joint_names = ["Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"]
base_link_name = "base_link"
end_effector_name = "link6"
MOVE_GROUP_ARM = "arm"#test_group
# -------------------------- 轨迹发布类 --------------------------
class RobotMovePublisher(Node):
def __init__(self):
super().__init__('robot_move_publisher')
self.qos_profile = QoSProfile(
reliability=ReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
depth=10
)
self.pub = self.create_publisher(JointTrajectory, 'RobotMove', self.qos_profile)
self.get_logger().info("轨迹发布器就绪")
def publish_direct_joint(self, target_joints):
"""发布关节目标"""
try:
traj_msg = JointTrajectory()
traj_msg.joint_names = joint_names
point = JointTrajectoryPoint()
point.positions = target_joints
traj_msg.points.append(point)
self.pub.publish(traj_msg)
self.get_logger().info(f"发布关节目标")
return True
except Exception as e:
self.get_logger().error(f"关节发布失败: {e}")
return False
def publish_smooth_trajectory(self, start_joints, end_joints):
"""发布平滑轨迹"""
try:
traj_msg = JointTrajectory()
traj_msg.joint_names = joint_names
# 5点平滑插值
for ratio in [0.0, 0.25, 0.5, 0.75, 1.0]:
point = JointTrajectoryPoint()
point.positions = [start + ratio * (end - start) for start, end in zip(start_joints, end_joints)]
traj_msg.points.append(point)
self.pub.publish(traj_msg)
self.get_logger().info("发布平滑轨迹")
return True
except Exception as e:
self.get_logger().error(f"轨迹发布失败: {e}")
return False
# -------------------------- 轨迹管理器 --------------------------
class TrajectoryManager(Node):
def __init__(self):
super().__init__('trajectory_manager')
self.traj_pub = RobotMovePublisher()
self.plan_end_joints = None
self.traj_published = False
self.sub = self.create_subscription(
DisplayTrajectory, '/display_planned_path', self.plan_callback, 10
)
self.get_logger().info("轨迹管理器就绪")
def plan_callback(self, msg):
"""处理规划轨迹"""
if not msg.trajectory or not msg.trajectory[0].joint_trajectory.points:
return
if not self.traj_published:
joint_traj = msg.trajectory[0].joint_trajectory
start_joints = list(joint_traj.points[0].positions)
self.plan_end_joints = list(joint_traj.points[-1].positions)
if self.traj_pub.publish_smooth_trajectory(start_joints, self.plan_end_joints):
self.traj_published = True
self.get_logger().info("轨迹发布完成")
# -------------------------- 核心规划与通信类 --------------------------
class CommunicationPlanner(Node):
def __init__(self):
super().__init__('communication_planner')
# 状态管理
self.real_joints = None
self.joints_lock = Lock()
# 子系统初始化
self.traj_manager = TrajectoryManager()
self.callback_group = ReentrantCallbackGroup()
self.moveit2 = MoveIt2(
node=self,
joint_names=joint_names,
base_link_name=base_link_name,
end_effector_name=end_effector_name,
group_name=MOVE_GROUP_ARM,
callback_group=self.callback_group,
)
# MoveIt2参数配置
self.moveit2.max_velocity = 0.5
self.moveit2.max_acceleration = 0.5
self.moveit2.planner_id = "RRTConnectkConfigDefault"
# 线程池
self.thread_pool = concurrent.futures.ThreadPoolExecutor(max_workers=4)
# 订阅器
self.create_subscription(
RobotInfo, '/RobotInfo', self.robot_info_callback, 10
)
# 启动Socket服务器
self.socket_thread = Thread(target=self.start_socket_server, daemon=True)
self.socket_thread.start()
self.get_logger().info("系统就绪,等待指令...")
def robot_info_callback(self, msg):
"""更新实时关节数据"""
if msg.joint_positions and len(msg.joint_positions) == 6:
with self.joints_lock:
self.real_joints = msg.joint_positions
def wait_for_real_joints(self, timeout=10.0):
"""等待关节数据就绪"""
start_time = time.time()
while rclpy.ok():
with self.joints_lock:
if self.real_joints is not None:
return True
if time.time() - start_time > timeout:
self.get_logger().error("关节数据获取超时")
return False
time.sleep(0.1)
def is_joint_reached(self, target_joints):
"""检查关节是否到达目标"""
with self.joints_lock:
current = self.real_joints
if current is None:
return False
# 检查所有关节误差
return all(abs(c - t) <= JOINT_TOLERANCE
for c, t in zip(current, target_joints))
def identify_coordinate_type(self, data):
"""自动识别坐标类型"""
if isinstance(data, list):
if len(data) == 6 and all(isinstance(x, (int, float)) for x in data):
return "joint" # 6关节坐标
elif (len(data) == 2 and isinstance(data[0], list) and
isinstance(data[1], list) and len(data[0]) == 3 and
len(data[1]) == 4):
return "cartesian" # 笛卡尔坐标
return "unknown"
def handle_joint_control(self, target_joints, command_id="unknown"):
"""处理关节空间控制"""
self.get_logger().info(f"[{command_id}] 执行关节控制")
if not self.wait_for_real_joints():
return False
if not self.traj_manager.traj_pub.publish_direct_joint(target_joints):
return False
# 等待稳定到达
start_time = time.time()
stable_count = 0
while rclpy.ok() and time.time() - start_time < 30:
if self.is_joint_reached(target_joints):
stable_count += 1
if stable_count >= STABLE_COUNT_THRESHOLD:
self.get_logger().info(f"[{command_id}] 关节控制完成")
return True
else:
stable_count = 0
time.sleep(0.1)
self.get_logger().warning(f"[{command_id}] 关节控制超时")
return False
def _cartesian_planning_thread(self, target_pos, target_quat):
"""笛卡尔规划线程"""
try:
self.moveit2.move_to_pose(position=target_pos, quat_xyzw=target_quat)
return True
except Exception as e:
self.get_logger().error(f"笛卡尔规划失败: {e}")
return False
def handle_cartesian_control(self, target_pos, target_quat, command_id="unknown"):
"""处理笛卡尔空间控制"""
self.get_logger().info(f"[{command_id}] 执行笛卡尔控制")
if not self.wait_for_real_joints():
return False
# 重置状态
self.traj_manager.plan_end_joints = None
self.traj_manager.traj_published = False
# 设置规划起点
with self.joints_lock:
self.moveit2.joint_value_target = self.real_joints
self.moveit2.update_planning_scene()
# 异步规划
planning_future = self.thread_pool.submit(
self._cartesian_planning_thread, target_pos, target_quat
)
try:
if not planning_future.result(timeout=P2_PLAN_TIMEOUT):
return False
except concurrent.futures.TimeoutError:
self.get_logger().error(f"[{command_id}] 笛卡尔规划超时")
return False
# 等待规划结果
start_time = time.time()
while rclpy.ok() and self.traj_manager.plan_end_joints is None:
if time.time() - start_time > P2_PLAN_TIMEOUT:
self.get_logger().warning(f"[{command_id}] 未获取规划终点")
return False
time.sleep(0.1)
target_joints = self.traj_manager.plan_end_joints
return self._wait_for_joints_non_blocking(target_joints, command_id)
def _wait_for_joints_non_blocking(self, target_joints, command_id="unknown"):
"""非阻塞等待关节到达"""
start_time = time.time()
stable_count = 0
while rclpy.ok() and time.time() - start_time < P2_MOVE_TIMEOUT:
rclpy.spin_once(self, timeout_sec=0)
if self.is_joint_reached(target_joints):
stable_count += 1
if stable_count >= STABLE_COUNT_THRESHOLD:
self.get_logger().info(f"[{command_id}] 笛卡尔控制完成")
return True
else:
stable_count = 0
time.sleep(0.1)
self.get_logger().warning(f"[{command_id}] 笛卡尔控制超时")
return False
def process_command(self, command_id, data):
"""根据坐标类型自动选择控制方式"""
coord_type = self.identify_coordinate_type(data)
if coord_type == "joint":
return self.handle_joint_control(data, command_id)
elif coord_type == "cartesian":
if len(data) == 2:
return self.handle_cartesian_control(data[0], data[1], command_id)
else:
self.get_logger().error(f"[{command_id}] 笛卡尔坐标格式错误")
return False
else:
self.get_logger().error(f"[{command_id}] 无法识别的坐标格式")
return False
def start_socket_server(self):
"""Socket服务器主循环"""
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as server_socket:
server_socket.bind(("0.0.0.0", PYCHARM_PORT))
server_socket.listen(1)
self.get_logger().info(f"Socket服务启动,端口: {PYCHARM_PORT}")
while rclpy.ok():
conn, addr = server_socket.accept()
with conn:
self.get_logger().info(f"客户端连接: {addr}")
try:
data = conn.recv(1024).decode("utf-8")
if not data:
continue
self.get_logger().info(f"接收指令: {data}")
result = False
# 解析指令格式: p1:[data], p2:[data], p3:[data] 等
if ":" in data:
command_id, json_str = data.split(":", 1)
try:
command_data = json.loads(json_str)
result = self.process_command(command_id, command_data)
except json.JSONDecodeError:
self.get_logger().error("JSON解析失败")
result = False
else:
# 如果没有指令标识,使用默认标识
try:
coord_data = json.loads(data)
result = self.process_command("default", coord_data)
except json.JSONDecodeError:
self.get_logger().error("数据格式错误")
result = False
conn.sendall(str(result).encode("utf-8"))
self.get_logger().info(f"返回结果: {result}")
except Exception as e:
self.get_logger().error(f"指令处理异常: {e}")
conn.sendall("error".encode("utf-8"))
def shutdown(self):
"""清理资源"""
if hasattr(self, 'thread_pool'):
self.thread_pool.shutdown(wait=False)
# -------------------------- 程序入口 --------------------------
def main():
rclpy.init()
planner = None
try:
planner = CommunicationPlanner()
executor = rclpy.executors.MultiThreadedExecutor(4)
executor.add_node(planner)
executor.add_node(planner.traj_manager)
executor.add_node(planner.traj_manager.traj_pub)
executor.spin()
except KeyboardInterrupt:
planner.get_logger().info("程序终止")
except Exception as e:
if planner is not None:
planner.get_logger().error(f"系统异常: {e}")
else:
print(f"系统异常: {e}")
finally:
if planner is not None:
planner.shutdown()
rclpy.shutdown()
if __name__ == "__main__":
main()
最新发布