对位置点的YAML读取

建立工程包

cd catkin_ws/
cd src/
catkin_create_pkg yaml_test std_msgs rospy roscpp tf geometry_msgs
其中三个依赖是实时发布TF关系必须用的
cd catkin_ws/
catkin_make

从官方git源https://github.com/jbeder/yaml-cpp/releases/tag/release-0.5.2下载 tar.gz的源码包，按照以下步骤做：

    cd yaml-cpp # 进入克隆的文件夹
     
    mkdir build
     
    cd build
     
    cmake -DBUILD_SHARED_LIBS=ON ..    默认生成的是静态库，这里要生成动态库调用
     
    make
     
    sudo make install

直接附上代码，

/*作者：路
  时间：2019-11-26
工作目的：取base_link相对于map的TF记录在yaml文件中，供以后的导航应用
2019-11-26～27
1.单独完成了yaml文件读写
2.得到了base_link相对于odom的关系
3.将手柄程序应用到其中，可以接收到手柄的数据。

2019-11-28
1.将三者程序结合，并且写位c++的标准格式，为了以后的调用方便。
2.可以实现手柄按下后，将tf消息读入yaml文件
3.仍存在的问题是读入的文件格式不是最后需要的数组形式，仍需要调试
*/

//--------------------two_tf_transform-----------
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <iostream>
#include <cmath>
//--------------------write_yaml----------------
#include <iostream>
#include "/usr/local/include/yaml-cpp/yaml.h"   //这里应该还需要改动，因为这部分代码换个电脑就GG
#include "std_msgs/String.h"
#include <fstream>
//--------------------joy----------------------
#include <geometry_msgs/Twist.h>  
#include <sensor_msgs/Joy.h>  

using namespace std;
class Simple_point
{
public:
   Simple_point():
      node_point("~")
   {
        node_point.param<int>("axis_linear",axis_lin,1); //默认axes[1]接收速度   
	node_point.param<int>("axis_angular",axis_ang,2);//默认axes[2]接收角度
	node_point.param<double>("vel_linear",vlinear,1);//默认线速度1 m/s
	node_point.param<double>("vel_angular",vangular,1);//默认角速度1 单位rad/s
	pub_cmdvel = node_point.advertise<geometry_msgs::Twist>("/cmd_vel",1);//将速度发给乌龟
	sub_joy = node_point.subscribe<sensor_msgs::Joy>("/joy",10,&Simple_point::joy_callback,this); 
        //订阅游戏手柄发来的数据
   }

struct Pose
{
  double position[3];
  double orientation[4];
};

struct WayPoint
{
  std::string name;
  struct Pose pose;
  int work_type;
  int interval_num;

    friend std::ostream& operator<<(std::ostream& os, WayPoint& wp)
  {
    os << "name:" << wp.name << " x:" << wp.pose.position[0] << " y:" << wp.pose.position[1] << " z:"
        << wp.pose.position[2] << " ox:" << wp.pose.orientation[0] << " oy:" << wp.pose.orientation[1] << " oz:"
        << wp.pose.orientation[2] << " ow:" << wp.pose.orientation[3];
    return os;
  }
};

std::vector<WayPoint> simple_points;
std::vector<WayPoint> simple_points_finish;
void tf_print()
   {
    
   try{
	//得到坐标odom和坐标base_link之间的关系
	   listener.waitForTransform("odom", "base_link", ros::Time(0), ros::Duration(3.0));
	   listener.lookupTransform("odom", "base_link",ros::Time(0), transform);
       }
    catch (tf::TransformException &ex) 
       {
	   ROS_ERROR("%s",ex.what());
	   ros::Duration(1.0).sleep();
       }
 
     Translation_x=transform.getOrigin().x();
     Translation_y=transform.getOrigin().y();
     Translation_z=transform.getOrigin().z();
     cout<<"Translation: "<<Translation_x<<" "<<Translation_y<<" "<<Translation_z<<endl;
    //两种不同的表示方法，来表示getRotation
     Rotation_x=transform.getRotation()[0];
     Rotation_y=transform.getRotation()[1];
     Rotation_z=transform.getRotation()[2];
     Rotation_w=transform.getRotation()[3];
     cout<<"Rotation:"<<Rotation_x<<" "<<Rotation_y<<" "<<Rotation_z<<" "<<Rotation_w<<endl;
  }

void read_yamlfile()
     {
         cout << "name:" << config["name"].as<string>() << endl;
         cout << "sex:" << config["sex"].as<string>() << endl;
         cout << "age:" << config["age"].as<int>() << endl;
     }

void write_yamlfile()
     {
        ofstream fout("/home/hao/catkin_ws/src/config1.yaml");
        config_write1["score"]["math"] = 99;
       //config_write1["score"]["english"] = 1;
        fout << config_write1;
        fout.close();
     }



void joy_callback(const sensor_msgs::Joy::ConstPtr& Joy)  
     {   
	geometry_msgs::Twist v;
	v.linear.x =Joy->axes[axis_lin]*vlinear; //将游戏手柄的数据乘以你想要的速度，然后发给乌龟
 	v.angular.z =Joy->axes[axis_ang]*vangular;  
 	//ROS_INFO("linear:%.3lf angular:%.3lf",v.linear.x,v.angular.z);   
	pub_cmdvel.publish(v);  
          

	if(Joy->buttons[0])    //手柄上面的Y键位
	    { 
             try{
	    //得到坐标odom和坐标base_link之间的关系
	    listener.waitForTransform("map", "base_link", ros::Time(0), ros::Duration(3.0));
	    listener.lookupTransform("map", "base_link",ros::Time(0), transform);
	       }
	    catch (tf::TransformException &ex) 
	       {
		   ROS_ERROR("%s",ex.what());
		   ros::Duration(1.0).sleep();
	       }
             Translation_x=transform.getOrigin().x();
	     Translation_y=transform.getOrigin().y();
	     Translation_z=transform.getOrigin().z();
	     cout<<"Translation: "<<Translation_x<<" "<<Translation_y<<" "<<Translation_z<<endl;
	    //两种不同的表示方法，来表示getRotation
	     Rotation_x=transform.getRotation()[0];
	     Rotation_y=transform.getRotation()[1];
	     Rotation_z=transform.getRotation()[2];
	     Rotation_w=transform.getRotation()[3];
	     cout<<"Rotation:"<<Rotation_x<<" "<<Rotation_y<<" "<<Rotation_z<<" "<<Rotation_w<<endl;


             std::string marker_name = "point" + std::to_string(count+1);
	     std::cout << "marker_name :" << marker_name << std::endl;

             WayPoint wp_simple;
             wp_simple.name = marker_name;
             if(Translation_x==0) Translation_x=0.0000001;
             wp_simple.pose.position[0] = Translation_x;
             if(Translation_y==0) Translation_y=0.0000001;
             wp_simple.pose.position[1] = Translation_y;
             if(Translation_z==0) Translation_z=0.0000001;
             wp_simple.pose.position[2] = Translation_z;
             if(Rotation_x==0) Rotation_x=0.0000001;
             wp_simple.pose.orientation[0] = Rotation_x;
             if(Rotation_y==0) Rotation_y=0.0000001;
	     wp_simple.pose.orientation[1] = Rotation_y;
             if(Rotation_z==0) Rotation_z=0.0000001;
	     wp_simple.pose.orientation[2] = Rotation_z;
             if(Rotation_w==0) Rotation_w=0.0000001;
	     wp_simple.pose.orientation[3] = Rotation_w;
             wp_simple.work_type = 0;
             wp_simple.interval_num = 0;
             simple_points.push_back(wp_simple);
             cout<<"count = "<<count++<<endl;
            }
       if(Joy->buttons[1]&&Joy->buttons[2]&&Joy->buttons[3]) 
         {
            std::ofstream fout("/home/hao/dlos_ws/src/dalu_robot/config/patrol_points.yaml");
	    fout << "waypoints:";
            fout << '\n';
            for(int i=0;i<count;i++)
             {
             //int i=0;
             config_write[i]["name"] = simple_points[i].name;
             config_write[i]["pose"]["position"]["x"] = simple_points[i].pose.position[0];
             config_write[i]["pose"]["position"]["y"] = simple_points[i].pose.position[1];
             config_write[i]["pose"]["position"]["z"] = simple_points[i].pose.position[2];
             config_write[i]["pose"]["orientation"]["x"] = simple_points[i].pose.orientation[0];
             config_write[i]["pose"]["orientation"]["y"] = simple_points[i].pose.orientation[1];
             config_write[i]["pose"]["orientation"]["z"] = simple_points[i].pose.orientation[2];
             config_write[i]["pose"]["orientation"]["w"] = simple_points[i].pose.orientation[3];
             config_write[i]["work_type"] = simple_points[i].work_type;
             config_write[i]["interval_num"] = simple_points[i].interval_num;
             }

             emitter << YAML::BeginSeq;
     //      emitter << YAML::Key <<waypoints:;
             for(int i=0;i<count;i++)
             emitter << config_write[i];
             emitter << YAML::EndSeq;
             fout <<config_write1;
	     fout << emitter.c_str();
	     fout << '\n';
	     fout.close();
         }

     }  

private:
    double Translation_x,Translation_y,Translation_z,Rotation_x,Rotation_y,Rotation_z,Rotation_w;
    ros::NodeHandle node_point;
    ros::Subscriber sub_joy ;  
    ros::Publisher pub_cmdvel ;  
    double vlinear,vangular;//我们控制乌龟的速度，是通过这两个变量调整  
    int axis_ang,axis_lin;  //axes[]的键  
    tf::TransformListener listener;
    tf::StampedTransform transform;
    YAML::Node config = YAML::LoadFile("/home/hao/catkin_ws/src/config.yaml");
    YAML::Node config_write[1000];
    YAML::Node config_write1;
    int count=0;
    YAML::Emitter emitter;
};

int main(int argc, char** argv)
   {
       ros::init(argc, argv, "simple_point");
       Simple_point simple_point;
    //   ros::Rate loop_rate(10);
     //  simple_point.read_yamlfile();
    //  simple_point.write_yamlfile();
    //     while (ros::ok())
    //    {
    //     simple_point.tf_print();    //打印tf关系 odom与base_link
    //     loop_rate.sleep();
 	 ROS_INFO("linear: angular:");   
         ros::spin();
    //    }
       return 0;
   }

上述代码的功能为按手柄的Y键开始录制点集，为了防止误操作，当XBA同时按下时将所有的保存。

注意：1.第一个点不要取坐标原点

            2.保存完后不要重复保存，会使得文件冗余不可读，需加入控制逻辑，保存依次之后失效。

            3.文件中点集合只能录取1000个，需加入动态数组改善这一情况。

得到的yaml文件如下。

waypoints:
- name: point1
  pose:
    position:
      x: -1.6123593695728566
      y: -1.8915670950076069
      z: 0.017000000000000001
    orientation:
      x: 9.9999999999999995e-08
      y: 9.9999999999999995e-08
      z: -0.031761722291953565
      w: 0.9994954692228718
  work_type: 0
  interval_num: 0
- name: point2
  pose:
    position:
      x: 2.7226182828472587
      y: -2.0086066304568448
      z: 0.016999999999999998
    orientation:
      x: 9.9999999999999995e-08
      y: 9.9999999999999995e-08
      z: -0.023536016431103306
      w: 0.99972298959789607
  work_type: 0
  interval_num: 0
- name: point3
  pose:
    position:
      x: 2.3144254070075632
      y: 1.3082720447433922
      z: 0.016999999999999998
    orientation:
      x: 9.9999999999999995e-08
      y: 9.9999999999999995e-08
      z: 0.84230217407645824
      w: 0.53900560993933244
  work_type: 0
  interval_num: 0
- name: point4
  pose:
    position:
      x: -1.0459579318226968
      y: 0.16889750489303135
      z: 0.017000000000000001
    orientation:
      x: 9.9999999999999995e-08
      y: 9.9999999999999995e-08
      z: -0.80639213133053256
      w: 0.59138120576173325
  work_type: 0
  interval_num: 0

cmakelist文件

add_definitions(-std=c++11)
link_directories(/usr/local/lib)
include_directories(/usr/local/include/yaml-cpp)

add_executable(writeyaml src/writeyaml.cpp)
target_link_libraries(writeyaml ${catkin_LIBRARIES} yaml-cpp)

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

add_executable(simple_point src/simple_point.cpp)
target_link_libraries(simple_point ${catkin_LIBRARIES} yaml-cpp)