ROS节点控制实体机械臂运动

#include<ros/ros.h>
#include<moveit/move_group_interface/move_group_interface.h>
#include<moveit/robot_state/robot/state.h>
#include<moveit_msgs/DisplayTrajactory.h>//moveit！消息类型，显示轨迹将规划好的轨迹发送到Rviz
#include<bost/property_tree/ptree.hpp>
#include<bost/property_tree/ptree/xml_parser.hpp>//boost中的属性树组件，用于处理xml格式文件
#include<boost/asio.hpp>//boost库中的网络通信库，可与外部设备网络通信（TCP/IP、串口等）
#include<ros/subscriber.h>//接收ROS话题外部控制指令
#include<moveit_msgs/ExecuteTrajetoryActionGoal.h>//向动作服务器发送轨迹执行请求namespace pt=boost::property_tree;// 为boost属性树创建别名，简化代码
using boost::asio::ip::tcp; // 引入tcp命名空间，方便使用TCP相关类
class KukaRealControl{
private:
ros::NodeHandle nh_; // ROS节点句柄，用于ROS通信
moveit::planning_interface::MoveGrouplnterface move_group_;// MoveIt!运动组接口
ros::Subscriber execute_goal_sub_;// ROS订阅者，用于接收轨迹执行目标
std::unique_ptr<tcp::socket>socket; // 智能指针管理TCP socket对象
bool is_connected_;  // 标记是否与机器人建立连接
std::vector<std::string>joint names_; // 存储机器人关节名称列表
double send_frequency_; // 数据发送频率（Hz）
std::vector<double>last_sent_position_; // 上一次发送的关节位置public:
KukaRealControl()://构造函数
nh_("~"),// 初始化ROS私有节点句柄（命名空间为~）
move_group_("kuka_arm"),// 初始化运动组，指定组名为"kuka_arm"
is_connected_(false),// 初始化为未连接状态
send_frequency_(10.0)// 默认发送频率10Hz
{
joint_names_=move_group_.getVariableNames();// 获取运动组的关节名称列表
nh_.param("send_frequency",send_frequency_,send_frequency_);// 从ROS参数服务器获取发送频率
initEKI();// 初始化EKI通信（连接机器人）execute_goal_sub_=nh,subscribe("/execute_trajectory/goal",1,&KukaRealControl::execuateGoalCallback,this);// 订阅轨迹执行目标话题
//初始化最后发送的位置
last_sent_position_resize(joint_names_.size(),0.0); // 初始化最后发送的关节位置（大小与关节数量一致，初始值0）
}
void initEKI()//// 初始化EKI通信，建立与KUKA机器人的TCP连接,KukaRealControl类的私有成员函数
{
try{
io_service_=std::make_unique<boost::asio::io_serbice>();// 创建Boost.Asio的I/O服务对象，用于管理网络I/O操作
socket_=std::make_unique<tcp::socket>(*io_service_);// 创建TCP套接字对象，并与I/O服务绑定tcp:endpoint endpoint(boost::asio::ip::adress::from_string("192.168.1.10"),54600); // 定义机器人的网络端点（IP地址和端口），IP为192.168.1.10，端口54600（EKI默认端口）
socket->connect(endpoint);// 尝试与机器人建立TCP连接
is_connect_=true;// 连接成功，更新连接状态
ROS_INFO("Successfully connected to KUKA robot via EKI");// 输出连接成功日志} 
}
catch(const std::exception& e)// 捕获连接过程中可能抛出的异常
{
ROS ERROR("Faild connected to KUKA robot:%s",e.what()); // 输出连接失败日志，包含具体错误信息
is_connected_=false;// 连接失败，更新连接状态
}
}
void executeGoalCallback(const moveit_msgs::ExecuteTrajectoryActionGoal::ConstPtr& msg)// 轨迹执行目标回调函数，处理接收到的轨迹指令
{
if(!is_connected_)// 检查是否与机器人建立连接
{
ROS_WARN("Not connected to KUKA robot,skipping execution"); // 未连接时输出警告
return;// 提前返回，终止执行
}
const auto&joint_traj=msg->goal.trajectory.joint_trajectory;/ 从消息中获取关节轨迹数据（使用auto自动推导类型，简化代码）
if(joint_traj.points.empty())// 检查轨迹点是否为空
{
ROS_WARN("Empty trajectory received,skipping execuation");// 输出轨迹执行信息：轨迹点数量和发送频率
return;
}
ROS_INFO("Executing trajectory with %lu points on real robot at %1f Hz",joint_traj,points,size(),send_frequency_);// 输出轨迹执行信息：轨迹点数量和发送频率
ros::Rate rate(send_frequency_);// 创建ROS速率对象，控制发送频率
for(const auto& point:joint_traj.points) // 遍历轨迹中的所有点（范围for循环，C++11特性）
{
if(!ros::ok()) break;// 检查ROS节点是否正常运行，否则退出循环sendJointStateToRobot(point.positions);// 将当前轨迹点的关节位置发送给机器人
last_sent_positions_=point.positions;// 更新最后发送的关节位置
rate.sleep();// 按照设定频率休眠，控制发送节奏
}
sendCompletionSignal(); // 发送完所有轨迹点后，发送完成信号（如停止机器人）
ROS_INFO("Trajectory execution completed,sent completion signal");
}void sendJointStateToRobot(const std::vector<double>&positions)// 将关节位置转换为XML并发送给机器人
{
pt::ptree pt;// 创建属性树对象，用于构建XML结构
if(position.size()!=joint_names_.size()) // 检查关节数量是否匹配
{
ROS_ERROR("Joint count mismatch:expected %d,received %d",joint_name_.size(),position.size());
return;
}
for(size_t i=0,i<position.size();++i)// 遍历所有关节，将弧度转换为角度并放入XML节点
{
double angle_deg=positions[i]*180.0/M_PI; // 弧度转角度
std::string joint_name=joint_name[i];
if(joint_name=="j1") pt.put("Vision.PosData.PosX",angle_deg);
if(joint_name=="j2") pt.put("Vision.PosData.PosY",angle_deg-90);
if(joint_name=="j3") pt.put("Vision.PosData.PosZ",angle_deg+90);
if(joint_name=="j4") pt.put("Vision.PosData.PosA",angle_deg);
if(joint_name=="j5") pt.put("Vision,PosData.PosB",angle_deg);
if(joint_name=="j6") pt.put("Vision,PosData.PosC",angle_deg); // 根据关节名映射到XML中的对应位置
}
pt.put(Vision,PosData.SPD",0.015);// 设置速度参数sendXmlData(pt);// 将XML数据发送给机器人
}void sendCompletionSignal()
{
pt::ptree pt;// 创建一个 ptree 对象，用于构建XML或JSON格式的数据结构for(size_t i=0;i<last_sent_position_.size();++i)// 遍历向量中的每一个元素
{
double angle_deg=last_sent_position[i]*180/M_PI;
std::string joint_name=joint_names_[i];// 获取当前关节的名称
if(joint_name=="j1") pt.put("Vision.PosData.PosX",angle_deg);
if(joint_name=="j2") pt.put("Vision.PosData.PosY",angle_deg);
if(joint_name=="j3") pt.put("Vision.PosData.PosZ",angle_deg);
if(joint_name=="j4") pt.put("Vision.PosData.PosA",angle_deg);
if(joint_name=="j5") pt.put("Vision.PosData.PosB",angle_deg);
if(joint_name=="j6") pt.put("Vision.PosData.PosC",angle_deg); // 如果关节名是 "j6"，将其角度存入 ptree 的 "Vision.PosData.PosC" 路径
}
pt.put("Vision.PosData.SPD",0.0);// 在 ptree 中添加一个新的键值对，键为 "Vision.PosData.SPD"，值为 0.0。
sendXmlData(pt);// 调用 sendXmlData 函数，并将构建好的 ptree 对象 pt 作为参数传递进去。
}
void sendXmlData(const pt::ptree&pt)
{
std::stringstream ss; // 创建字符串流
pt::write_xml(ss,pt); // 将property tree写入XML格式
std::string xml_str=ss.str();// 转换为字符串size_t pos=xml_str.find("?>");// 查找XML声明结束标记
if(pos!=std::string::npos)
{
xml_str.erase(0,pos+2); // 删除XML声明部分（如<?xml version="1.0"?>）
}try{
boost::asio::write(*socket_,boost::asio::buffer(xml_str));// 发送XML数据
std::vector<char>response(1024); // 创建接收缓冲区
socket_->read_some(boost::asio::buffer(response));// 读取服务器响应
}
catch(const std::exception& e)
{
ROS_ERROR(Communication error:%s,e.what()); // 记录错误日志
is_connected_false;// 设置连接状态为false
init EKI();// 重新初始化EKI连接
}
}
};int main(int argc,char** argv)
{
ros::init(argv,argv,"kuka_real_control_node");// 初始化ROS节点
KukaRealControl controller;// 创建KUKA机器人控制器实例
ros::spin(); // 进入ROS事件循环，等待回调
return 0;   // 程序正常退出
}

1.Boost 库是什么？

Boost 是一个开源的 C++ 库集合，提供了大量高质量的工具类和函数，弥补了 C++ 标准库的不足，被广泛用于工业级软件开发（包括机器人、网络、图形等领域）。

XML（可扩展标记语言）是一种用于存储和传输数据的标记语言，其结构清晰、易于人类阅读和机器解析，常被用作配置文件格式。

2.什么是 XML 配置文件？

XML（可扩展标记语言）是一种用于存储和传输数据的标记语言，其结构清晰、易于人类阅读和机器解析，常被用作配置文件格式。在机器人编程或其他软件开发中，XML 配置文件的作用包括：

• 存储程序运行所需的参数（如机器人关节限位、运动速度阈值等）
• 定义系统的配置信息（如节点通信方式、模块启动顺序）
• 保存结构化数据（如预设的运动路径点、坐标系定义）
• 例如，一个简单的机器人关节配置 XML 可能长这样：

  <robot_config><joint name="joint1"><min_angle>-180</min_angle><max_angle>180</max_angle><speed>0.5</speed></joint>
  </robot_config>

3.EKI 协议，一种 KUKA 机器人常用的以太网通信协议

4.智能指针：std::unique_ptr是 C++11 引入的智能指针，用于自动管理动态内存，避免内存泄漏。

5.catch块捕获所有从try块中抛出的std::exception类型异常

6.ROS_ERROR：ROS 的错误日志函数，输出连接失败信息

7.e.what()：调用异常对象的what()方法，获取错误描述字符串（如 "Connection refused"、"Invalid argument" 等）

8.弧度（ROS/MoveIt! 默认单位）转换为角度（KUKA 机器人常用单位）

9.const 是 C++ 中的一个关键字，主要用于声明常量和只读数据，帮助程序员写出更安全、更清晰的代码。

核心含义

• 只读：被 const 修饰的变量或对象的值不能被修改
• 保护数据：防止意外修改，提高代码安全性
• 编译器优化：编译器知道这是常量，可能进行性能优化
• 接口清晰：向其他程序员表明哪些数据是只读的

10.

// 如果没有 std::，编译器不知道你要用哪个vector
std::vector<std::string> joint_names_;  // 明确使用标准库的vector
// 而不是可能存在的其他vector实现

11.

• 构造函数：对象的"出生设置"，让创建对象更方便

• STL：编程的"瑞士军刀"，提供各种现成好用的数据容器和算法

就像你买手机：

• 构造函数决定了手机出厂时的默认配置

• STL就像手机里的各种APP（通讯录、备忘录等），都是现成好用的工具