---

前言

之前基于ros1写过一篇读取机器人移动轨迹并在Rviz中显示，随着ros2的普遍使用，本文将ros1的代码进行迁移，以期能够在ros2中使用。

---

一、准备工作

为方便后续操作，使用服务通信来实现轨迹录制和加载。

1.自定义服务接口

cd catkin_ws/src
ros2 pkg create --build-type ament_cmake base_interfaces_demo
cd base_interfaces_demo
mkdir srv
cd srv
touch SetPathName.srv

编写srv文件

string path_name
---

编辑配置文件
package.xml添加如下：

<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

CMakeLists.txt添加如下：

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/SetPathName.srv"
)

编译验证

cd catkin_ws
colcon build --packages-select base_interfaces_demo
source install/setup.bash
ros2 interface show base_interfaces_demo/srv/SetPathName

输出和SetPathName.srv文件中一样的数据。

二、实现

1.功能包创建

cd catkin_ws/src
ros2 pkg create path_server --build-type ament_cmake --dependencies rclcpp base_interfaces_demo std_msgs tf2 tf2_ros std_srvs tf2_geometry_msgs nav_msgs geometry_msgs --node-name record_name

2.轨迹保存

record_path.cpp：

#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2/utils.h"
#include "tf2_ros/buffer.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_msgs/msg/tf_message.hpp"
#include "base_interfaces_demo/srv/set_path_name.hpp"
#include "std_srvs/srv/empty.hpp"
#include <fstream>
#include <iomanip>
#include <boost/filesystem.hpp>

using std::placeholders::_1;
using std::placeholders::_2;
using base_interfaces_demo::srv::SetPathName;

class RecordPath: public rclcpp::Node{
public:
  RecordPath():Node("record_path"),first_record_(true),record_flag_(false){

    this->declare_parameter<std::string>("map_frame","map");
    this->declare_parameter<std::string>("base_link_frame","base_link");
    this->declare_parameter<std::string>("odom_topic","odom");
    this->declare_parameter<double>("distance_interval",0.05);

    // this->get_parameter("map_frame",map_frame_);
    map_frame_ = this->get_parameter("map_frame").as_string();
    base_link_frame_ = this->get_parameter("base_link_frame").as_string();
    odom_topic_ = this->get_parameter("odom_topic").as_string();
    distance_interval_ = this->get_parameter("distance_interval").as_double();

    tf_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
    tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);

    path_pub_ = this->create_publisher<nav_msgs::msg::Path>("recorded_path",1) ;
    odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>("/odom",10,
                std::bind(&RecordPath::record_callback,this,_1));
    start_record_server_ = this->create_service<SetPathName>("start_record_path",std::bind(&RecordPath::start_record,this,_1,_2));
    stop_record_server_ = this->create_service<std_srvs::srv::Empty>("stop_record_path",std::bind(&RecordPath::stop_record,this,_1,_2));
    
    path_data_.poses.clear();
    path = "/home/robot/catkin_ws/src/path_server/path_data/";
  }
  ~RecordPath(){
    output_file_.close();
  }

  void record_data(double x, double y, double yaw){
    path_data_.header.stamp = this->get_clock()->now();
    path_data_.header.frame_id = map_frame_;
    
    if(output_file_){
      output_file_ << x <<" "<< y <<" "<< yaw << std::endl;
    }
    cache_x_ = x ;
    cache_y_ = y ;
    cache_yaw_ = yaw ;

    geometry_msgs::msg::PoseStamped poseStamped;
    poseStamped.header.stamp = this->get_clock()->now();
    poseStamped.header.frame_id = map_frame_;
    poseStamped.pose.position.x = x;
    poseStamped.pose.position.y = y;
    tf2::Quaternion q;
    q.setRPY(0,0,yaw);
    poseStamped.pose.orientation.x = q.x();
    poseStamped.pose.orientation.y = q.y();
    poseStamped.pose.orientation.z = q.z();
    poseStamped.pose.orientation.w = q.w();
    path_data_.poses.push_back(poseStamped);
    path_pub_->publish(path_data_);

  }
  
  double norm2(double x,double y, double yaw){
    return x * x + y * y + yaw * yaw;
  }

private:
  std::string map_frame_, base_link_frame_;
  std::string odom_topic_;
  double distance_interval_;
  std::ofstream output_file_;
  bool first_record_ , record_flag_;
  std::string path;

  std::unique_ptr<tf2_ros::Buffer> tf_;
  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;

  double cache_x_, cache_y_, cache_yaw_;
  nav_msgs::msg::Path path_data_;

  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
  rclcpp::Service<SetPathName>::SharedPtr start_record_server_;
  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_record_server_;

  bool stop_record(const std_srvs::srv::Empty::Request::SharedPtr req,const std_srvs::srv::Empty::Response::SharedPtr res){
    RCLCPP_INFO(this->get_logger(),"Recv stop record path");
    output_file_.close();
    record_flag_ = false;
    return true;
  }

  bool start_record(const SetPathName::Request::SharedPtr req, const SetPathName::Response::SharedPtr res){
    std::string filename = req->path_name;
    if(filename.empty()){
      RCLCPP_INFO(this->get_logger(),"filename is empty!");
      return false;
    }
    output_file_.close();
    output_file_ = std::ofstream(path + filename + ".path");

    record_flag_  =true;
    first_record_ = true;
    return true;
  }

  void record_callback(const nav_msgs::msg::Odometry::SharedPtr odom){
    
    // rclcpp::Time rclcpp_time = now();
    // tf2::TimePoint tf2_time(std::chrono::nanoseconds(rclcpp_time.nanoseconds()));

    if(record_flag_&& tf_->canTransform(map_frame_,base_link_frame_,tf2::TimePointZero)){
      auto map_to_baselink = tf_->lookupTransform(map_frame_,base_link_frame_,tf2::TimePointZero);
      double x = map_to_baselink.transform.translation.x;
      double y = map_to_baselink.transform.translation.y;
      
      tf2::Quaternion quat(map_to_baselink.transform.rotation.x,
                          map_to_baselink.transform.rotation.y,
                          map_to_baselink.transform.rotation.z,
                          map_to_baselink.transform.rotation.w);      
      double roll, pitch, yaw;
      tf2::Matrix3x3 m(quat);
      m.getRPY(roll, pitch, yaw);

      if(first_record_){
        record_data(x,y,yaw);
        first_record_ = false;
      } else if(norm2(cache_x_ - x,cache_y_ - y, 0) > (distance_interval_ * distance_interval_)){
        record_data(x,y,yaw);
      }
    }
  }

};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);
  rclcpp::spin(std::make_shared<RecordPath>());
  rclcpp::shutdown();
  return 0;
}

保存路径：catkin_ws/src/path_server/path_data/

3.轨迹加载

load_path.cpp：

#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2/transform_datatypes.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/utils.h"
#include "tf2_ros/buffer.h"
#include "tf2/convert.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "nav_msgs/msg/path.hpp"
#include <fstream>
#include "base_interfaces_demo/srv/set_path_name.hpp"

using base_interfaces_demo::srv::SetPathName;
using std::placeholders::_1;
using std::placeholders::_2;

class LoadPath: public rclcpp::Node{
public:
    LoadPath():Node("load_path"){
        this->declare_parameter<std::string>("map_frame","map");
        this->get_parameter("map_frame",map_frame_);

        this->declare_parameter<std::string>("path_file","777");
        this->get_parameter("path_file",path_file_);
        full_path_pub_ = this->create_publisher<nav_msgs::msg::Path>("loaded_path",10);
        load_path_server_ = this->create_service<SetPathName>("load_path_server",std::bind(&LoadPath::load_path_callback,this,_1,_2));
    
        if(!path_file_.empty()){
            std::string  path = "/home/robot/catkin_ws/src/path_server/path_data/";
            if(access(path.c_str(),0) != -1){
                load_path(path + path_file_ + ".path");
            }
        }
    }
    bool load_path(std::string path){
        if(path.empty()){
            RCLCPP_WARN(this->get_logger(),"path file cannot be empty!");
            return false;
        } 
        std::ifstream is(path);
        if(!is) {
            RCLCPP_WARN_STREAM(this->get_logger(),"file: " + path + " not exist!");
            return false;
        } else {
            load_path_.poses.clear();
            load_path_.header.frame_id = map_frame_;
            load_path_.header.stamp = this->get_clock()->now();
            while(!is.eof()) {
                double x,y, yaw;
                geometry_msgs::msg::PoseStamped poseStamped;
                is >> x >> y >> yaw;
                poseStamped.header.frame_id = map_frame_;
                poseStamped.header.stamp = this->get_clock()->now();
                poseStamped.pose.position.x = x;
                poseStamped.pose.position.y = y;
                poseStamped.pose.position.z = 0.0;

                tf2::Quaternion q;
                q.setRPY(0,0,yaw);
                geometry_msgs::msg::Quaternion quat;
                tf2::convert(q,quat);
                poseStamped.pose.orientation = quat;

                load_path_.poses.push_back(poseStamped);
            }
            full_path_pub_->publish(load_path_);
        }
    }
private:
    std::unique_ptr<tf2_ros::Buffer> tf_;
    std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
    nav_msgs::msg::Path load_path_;
    std::string map_frame_, path_file_;
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr full_path_pub_;
    rclcpp::Service<SetPathName>::SharedPtr load_path_server_;

    bool load_path_callback(const SetPathName::Request::SharedPtr req,const SetPathName::Response::SharedPtr res){
        std::string filename = req->path_name;
        if(filename.empty()){
            RCLCPP_WARN(this->get_logger(),"path file cannot be empty!");
            return false;
        } else {
            std::string path = "/home/robot/catkin_ws/src/path_server/path_data/";
            return load_path(path + filename + ".path");
        }
    }
};

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto load_path_nopde = std::make_shared<LoadPath>();
    rclcpp::spin(load_path_nopde);
    rclcpp::shutdown();
    return 0;
}

---

运行加载路径

ros2 run path_server load_path
ros2 service call /load_path_server base_interfaces_demo/srv/SetPathName path_name:\ \'666\'\ 

结果如下：