建立工程包
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)