ROS（Robot Operating System）中，编写一个记录机器人速度并将其转换成轨迹

在ROS（Robot Operating System）中，编写一个记录机器人速度并将其转换成轨迹的类涉及到几个步骤。以下是一个简单的示例，展示了如何创建这样一个类，并将其放置在一个自定义的命名空间中。这个类将订阅机器人的速度信息，记录这些信息，并存储轨迹数据。
首先，你需要创建一个新的ROS包。如果你还没有创建，可以使用以下命令：

1、编写单一文件

cd ~/catkin_ws/src
catkin_create_pkg my_robot_tracker std_msgs roscpp
cd ~/catkin_ws
catkin_make
source devel/setup.bash

然后，在你的包中创建一个C++源文件，比如 tracker_node.cpp ，并添加以下代码：

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <vector>

namespace my_robot_tracker {

class TrajectoryRecorder {
public:
    TrajectoryRecorder() {
        // 初始化ROS节点
        ros::NodeHandle nh;
        // 订阅机器人的速度信息
        sub_ = nh.subscribe<nav_msgs::Odometry>("odom", 10, &TrajectoryRecorder::odometryCallback, this);
        // 发布轨迹信息
        pub_ = nh.advertise<geometry_msgs::PoseStamped>("trajectory", 10);
    }

    void odometryCallback(const nav_msgs::Odometry::ConstPtr& msg) {
        // 将速度信息转换为轨迹点
        geometry_msgs::PoseStamped pose;
        pose.header = msg->header;
        pose.pose = msg->pose.pose;
        trajectory_.push_back(pose);

        // 可以选择在这里发布轨迹点
        pub_.publish(pose);
    }

    // 可以添加一个方法来保存轨迹数据
    void saveTrajectory() {
        // 这里可以添加代码将trajectory_保存到文件
    }

private:
    ros::Subscriber sub_;
    ros::Publisher pub_;
    std::vector<geometry_msgs::PoseStamped> trajectory_;
};

} // namespace my_robot_tracker

int main(int argc, char** argv) {
    ros::init(argc, argv, "tracker_node");
    my_robot_tracker::TrajectoryRecorder recorder;
    ros::spin();
    return 0;
}

接下来，你需要在 CMakeLists.txt 文件中添加编译指令：

cmake_minimum_required(VERSION 2.8.3)
project(my_robot_tracker)

find_package(catkin REQUIRED COMPONENTS
  std_msgs
  roscpp
  nav_msgs
  geometry_msgs
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(tracker_node src/tracker_node.cpp)
target_link_libraries(tracker_node ${catkin_LIBRARIES})

最后，你需要在 package.xml 文件中添加依赖：

<package>
  <name>my_robot_tracker</name>
  <version>0.0.0</version>
  <description>A package for recording robot trajectory</description>
  <maintainer email="your_email@example.com">your_name</maintainer>
  <license>BSD</license>

  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>nav_msgs</build_depend>
  <build_depend>geometry_msgs</build_depend>

  <exec_depend>roscpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>nav_msgs</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
</package>

确保你的工作空间是最新的，然后编译你的包：

cd ~/catkin_ws
catkin_make
source devel/setup.bash

现在，你可以运行你的节点：

rosrun my_robot_tracker tracker_node

这个简单的示例展示了如何创建一个记录机器人速度并将其转换成轨迹的类，并将其放置在一个自定义的命名空间 my_robot_tracker 中。你可以根据需要扩展这个类，添加更多的功能，比如保存轨迹到文件等。

2、分文件编写

要将代码分成两个文件，我们可以将类定义和其成员函数的实现放在一个头文件和一个源文件中，而将 main 函数放在另一个源文件中。以下是如何组织这些代码：

1. 头文件（trajectory_recorder.h）

#ifndef TRAJECTORY_RECORDER_H
#define TRAJECTORY_RECORDER_H

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <vector>

namespace my_robot_tracker {

class TrajectoryRecorder {
public:
    TrajectoryRecorder();
    void odometryCallback(const nav_msgs::Odometry::ConstPtr& msg);
    void saveTrajectory();

private:
    ros::Subscriber sub_;
    ros::Publisher pub_;
    std::vector<geometry_msgs::PoseStamped> trajectory_;
};

} // namespace my_robot_tracker

#endif // TRAJECTORY_RECORDER_H

2. 源文件（trajectory_recorder.cpp）

#include "trajectory_recorder.h"

namespace my_robot_tracker {

TrajectoryRecorder::TrajectoryRecorder() {
    // 初始化ROS节点
    ros::NodeHandle nh;
    // 订阅机器人的速度信息
    sub_ = nh.subscribe<nav_msgs::Odometry>("odom", 10, &TrajectoryRecorder::odometryCallback, this);
    // 发布轨迹信息
    pub_ = nh.advertise<geometry_msgs::PoseStamped>("trajectory", 10);
}

void TrajectoryRecorder::odometryCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    // 将速度信息转换为轨迹点
    geometry_msgs::PoseStamped pose;
    pose.header = msg->header;
    pose.pose = msg->pose.pose;
    trajectory_.push_back(pose);

    // 可以选择在这里发布轨迹点
    pub_.publish(pose);
}

void TrajectoryRecorder::saveTrajectory() {
    // 这里可以添加代码将trajectory_保存到文件
}

} // namespace my_robot_tracker

3. 主函数文件（main.cpp）

#include <ros/ros.h>
#include "trajectory_recorder.h"

int main(int argc, char** argv) {
    ros::init(argc, argv, "tracker_node");
    my_robot_tracker::TrajectoryRecorder recorder;
    ros::spin();
    return 0;
}

更新CMakeLists.txt
确保你的 CMakeLists.txt 文件包含了新的文件：

cmake_minimum_required(VERSION 2.8.3)
project(my_robot_tracker)

find_package(catkin REQUIRED COMPONENTS
  std_msgs
  roscpp
  nav_msgs
  geometry_msgs
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(tracker_node src/main.cpp src/trajectory_recorder.cpp)
target_link_libraries(tracker_node ${catkin_LIBRARIES})

这样就将代码分成了三个文件：一个头文件 trajectory_recorder.h ，一个源文件 trajectory_recorder.cpp ，以及一个包含 main 函数的文件 main.cpp 。这种组织方式使得代码更加模块化，易于管理和维护。