ROS订阅力矩传感器数据

下面是一个完整的 ROS 订阅者代码，它将订阅你发布的 WrenchStamped 消息，并输出力和力矩信息。假设你的话题名称为 /sensor_wrench，你可以根据实际的情况调整话题名称。

wrench_listener.cpp

#include <ros/ros.h>
#include <geometry_msgs/WrenchStamped.h>  // 引入 WrenchStamped 消息类型

// 定义一个回调函数来处理接收到的 WrenchStamped 消息
void wrenchCallback(const geometry_msgs::WrenchStamped::ConstPtr& msg)
{
    // 打印消息中的力和力矩信息
    ROS_INFO("Received Wrench data:");
    ROS_INFO("Force: [%.3f, %.3f, %.3f]", msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z);
    ROS_INFO("Torque: [%.3f, %.3f, %.3f]", msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z);
}

int main(int argc, char** argv)
{
    // 初始化 ROS 节点
    ros::init(argc, argv, "wrench_listener");
    ros::NodeHandle nh;

    // 订阅传感器数据话题 (假设话题名称为 /sensor_wrench)
    ros::Subscriber sub = nh.subscribe("/sensor_wrench", 10, wrenchCallback);

    // 进入循环，等待回调函数
    ros::spin();

    return 0;
}

代码解析：

1. #include <geometry_msgs/WrenchStamped.h>：

   • 该头文件包含了 ROS 消息类型 WrenchStamped，它用于存储力（force）和力矩（torque）的数据。

2. wrenchCallback 函数：

   • 这个回调函数会在接收到 WrenchStamped 消息时被触发，打印消息中的力和力矩数据。
   • msg->wrench.force.x、msg->wrench.force.y 和 msg->wrench.force.z 分别代表三个方向的力，msg->wrench.torque.x、msg->wrench.torque.y 和 msg->wrench.torque.z 分别代表力矩的三个分量。

3. ros::init(argc, argv, "wrench_listener")：

   • 初始化 ROS 节点，wrench_listener 是该节点的名称。

4. ros::NodeHandle nh：

   • 创建一个 ROS 节点句柄 nh，它用于与 ROS 系统进行交互。

5. ros::Subscriber sub = nh.subscribe("/sensor_wrench", 10, wrenchCallback)：

   • 订阅话题 /sensor_wrench，回调函数为 wrenchCallback，队列大小设置为 10，即最多保存 10 条未处理的消息。

6. ros::spin()：

   • 进入 ROS 的事件循环，等待消息到达并触发回调函数。ros::spin() 会阻塞主线程直到节点被关闭或 ROS 系统停止。

编译和运行：

1. 创建一个 ROS 包（假设你已经创建了一个名为 sensor_listener 的包）：

   如果你还没有创建 ROS 包，可以使用以下命令：

cd ~/catkin_ws/src
catkin_create_pkg sensor_listener std_msgs roscpp geometry_msgs

1. 将代码文件放到 src 文件夹：

   把 wrench_listener.cpp 文件放在 sensor_listener/src 文件夹内。

2. 修改 CMakeLists.txt：

   打开 sensor_listener/CMakeLists.txt，确保以下部分已启用：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  geometry_msgs
)

add_executable(wrench_listener src/wrench_listener.cpp)
target_link_libraries(wrench_listener ${catkin_LIBRARIES})
add_dependencies(wrench_listener ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

编译 ROS 包：

在 catkin_ws 目录下，运行以下命令：

cd ~/catkin_ws
catkin_make

运行订阅节点：

在 ROS 环境中运行以下命令启动订阅节点：

source devel/setup.bash
rosrun sensor_listener wrench_listener

1. 启动发布者节点（假设发布者节点已经运行）：

   确保发布者节点（即你之前提供的代码）正在运行并且正确发布 WrenchStamped 消息到 /sensor_wrench 话题。

结果：

在终端中，你会看到类似如下的输出（假设发布者正在发布力和力矩数据）

[INFO] [1683491282.390956]: Received Wrench data:
[INFO] [1683491282.391023]: Force: [1.234, -0.567, 2.345]
[INFO] [1683491282.391089]: Torque: [0.123, -0.234, 0.345]

这表明订阅者成功地接收了力和力矩信息并将其打印出来。