ROS学习笔记（一）补充篇 参考创客制造

我将ＲＯＳ的ＣＰＰ部分分成7个部分：
1、基础的node param
2、动态调节参数
3、关于ＴＦ变换
4、actionlib
5、插件技术
6、movebase
7、nodelet技术

前言

相比于ros学习笔记一，ros学习笔记一补充篇将会更加注重代码的讲解。ros学习笔记一只是像大观园一样，看看ROS这个领域的山山水水。而ros学习笔记一补充篇将会深入的补充这些山山水水的来源

节点node

rosrun 使用格式：
rosrun 包名 节点名
rosnode list 将会显示出正在运行的节点，其实我们平时都是直接用rosrun rqt_graph rqt_graph来吧正在运行的节点和话题之间的信息给显示出来

话题

节点node1和节点node2是通过话题来通信，节点node1发布topic1，而节点node2订阅topic1
我们可以输入 rostopic echo 话题1来显示出话题的信息
如何查看一个node到底能够发布多少个话题，用
rostopic list
在这个下面列出来的topic都是可以被订阅的

当然也 可以用

rostopic echo /

然后 不停的按tab回车来显示出来

发布publish

使用rostopic pub可以吧数据发布到某个正在广播的话题上面
1、我们可以用rostopic type 话题名
来查看这个话题的数据类型
2、然后rosmsg show 话题的数据类型名
3、有了1,2,这两个参数我们就可以发布话题了
示例：

rostopic pub -1 话题名称 话题的数据类型 需要改变的参数

rostopic pub -1 /turtle1/cmd_vel geometry_msgs/Twist -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, 1.8]' 

---

编写简单的消息发布器和订阅器 (C++) catkin方式

前提准备：
talker.cpp

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
  ros::Rate loop_rate(10);
  int count = 0;
  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();
    ROS_INFO("%s", msg.data.c_str());
    chatter_pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }
  return 0;
}

listener.cpp

#include "ros/ros.h"
#include "std_msgs/String.h"

void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
  ros::spin();

  return 0;
}

我下面将两种编译方式，看情况而定，反正我都会哈哈~
当然，个人比较倾向于用IDE，但是用终端编译是最准确的
先讲用终端命令行进行编译吧。
这个是个很经典的框架，很值得记忆！！！！

cd Desktop/
mkdir -p dao/src 这里建立一个stack叫做dao
cd huazi/src/
catkin_init_workspace
cd ..
catkin_make
cd src
catkin_create_pkg daodao roscpp rospy std_msgs genmsg 这里建立一个package叫做daodao

然后把talk.cpp文件放到里面
然后在CMakeLists.txt中修改，这里说的是修改，就是把几个分号给去掉，或者说直接把语句添加上

 add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker dao_generate_messages_cpp)当然这句话不加也是可以的

要说明的一点就是，这里dao_generate_messages_cpp当中的dao就是我的这个stack的名字
然后做完这一切工作之后，就是stack的目录下进行catkin_make

cd ..
catkin_make

然后运行一些这个节点

roscore
source ~/Desktop/dao/devel/setup.bash
rosrun daodao talker

---

第二种编译方式
打开Roboware
在桌面上创建一个叫dao111的stack
然后新建一个daodao的包，如果你不知道如何新建一个包的话，就请到我的Roboware学习笔记那篇里面有如何新建一个包的过程
然后新建一个talker.cpp的文件，然后输入代码

值得注意的一点就是，这里的find_package 就要自己写了

另外，很重要很重要的框架
就是按照这个样子来建立文件

然后你就可以在这里看到package 和 node 的名字

然后运行

好到这里两种调试方法结束啦~
在这里我想做个总结，就是packagename自己在创建文件夹的时候时候自己定义的，node的名字值在写cpp文件当中自己的定义的，其中cpp的文件名字就是node的名字

---

代码解析：
框架：

#include"ros/ros.h"
#include"std_msgs/String.h"
int main(int argc,char **argv)
{
    ros::init(argc,argv,"节点名")
    ros::NodeHandle n;
}

一个很经典的语句：

ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);

就像是int a=1;在这句话中返回值数据类型是int
因此在上一条语句中返回值的数据类型是ros::Publisher，调用句柄n的一个advertise的函数，这个函数的返回值的数据类型是
（引用一句很经典的话：告诉master,我们要在chatter上发布一个的数据类型) 这里chatter就是topic的话题的名称，1000就是按照我的理解是，在1000字符以内吧

ros::Rate loop_rate(10);
loop_rate.sleep();

这里的10表示的是10hz，如果是1的话，就是1Hz，也即是没1s更新一次

ROS_INFO("%s", msg.data.c_str());

ros::spinOnce();这个地方应该是回调机制

chatter_pub.publish(msg);这里向所有订阅chatter（话题）的节点发布消息我觉的可以记住这个框架，实例化两个对象，

   std_msgs::String msg;
    std::stringstream ss;
    ss<

---

小例子1、

我用上面的框架，写了一个自己的小例子，每隔1秒，向终端ros发布1,2,3…
程序：

#include<sstream>
#include"ros/ros.h"
#include"std_msgs/String.h"
int main(int argc, char **argv)
{
    ros::init(argc,argv,"Num");
    ros::NodeHandle n;
    ros::Publisher chatter_pub=n.advertise<std_msgs::String>("getNum",1000);//使用这条语句发布信息
    ros::Rate loop_rate(1);
    int count=0;
    while(ros::ok())
    {
        std_msgs::String msg;
        std::stringstream ss;
        ss<<count;
        msg.data=ss.str();
        ROS_INFO("%s", msg.data.c_str());
        chatter_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }
}

另外一定要注意大小写。

---

小例子2

然后下面我想讲的就是吧这个节点发布的消息给了另外一个节点，然后这这个节点打印出来
这是我的目标

效果：

Num.cpp

#include<sstream>
#include"ros/ros.h"
#include"std_msgs/String.h"
int main(int argc, char **argv)
{
    ros::init(argc,argv,"Num");
    ros::NodeHandle n;
    ros::Publisher chatter_pub=n.advertise<std_msgs::String>("getNum",1000);//使用这条语句发布信息
    ros::Rate loop_rate(1);
    int count=0;
    while(ros::ok())
    {
        std_msgs::String msg;
        std::stringstream ss;
        ss<<count;
        msg.data=ss.str();
        chatter_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }
}

get.cpp

#include"ros/ros.h"
#include"std_msgs/String.h"
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("%s", msg->data.c_str());
}
int main(int argc, char **argv)
{
    ros::init(argc,argv,"get");
    ros::NodeHandle n;
    ros::Subscriber sub=n.subscribe("getNum",1000, chatterCallback);
    ros::spin();
}

代码解析和总结：

这两句命令在在一起记忆：

ros::spinOnce();
ros::spin();

在publish的cpp文件当中

ros::spinOnce();如果没有这个函数的话，就没有回调信息

在subscribe的cpp文件当中

ros::spin();产生回调信息

然后这个框架我真的觉得应该记住

        std_msgs::String msg;实例化类型
        std::stringstream ss;
        ss<;将输出数值给了ss
        msg.data=ss.str();将ss和msg相互关联，这样的话后面的输出做准备
        chatter_pub.publish(msg);向刚刚的msg的话题进行发布
        ros::spinOnce();产生对应的回调函数，在getcpp当中哟对应的
        loop_rate.sleep();
        ++count;

这个输出的回调函数应该是要记住的

void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("%s", msg->data.c_str());
}

这个在主函数的当中调用机制也是应该熟悉的！

ros::Subscriber sub=n.subscribe("getNum",1000, chatterCallback);

最后别忘记在CMakeLists.txt当中添加2条语句哈。

---

python的实现

1、发布一个话题

第一种方式，使用终端实现
在工作区间下

cd catkin_ws/src
catkin_create_pkg daodao roscpp rospy std_msgs genmsg
cd daodao
mkdir scripts
cd scripts
vim talker.py
chmod +x talker.py

talker表示的是发布一个话题
往talker.py中写这些

#!/usr/bin/python
# -*- coding: UTF-8 -*-
import rospy
from std_msgs.msg import String

def talker():
    pub = rospy.Publisher('chatter', String, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        hello_str = "hello world %s" % rospy.get_time()
        rospy.loginfo(hello_str)
        pub.publish(hello_str)
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

然后

cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash
rosrun daodao talker.py

直接就运行成功了

第二种编译方式：
使用roboware

同样使用roboware的时候，还是需要安装一些软件的
为了支持python的调试功能，需要安装pylint

sudo apt-get install python-pip
sudo python -m pip install pylint

同时为了获取更好的代码补齐体验，需要安装clang

sudo apt-get install clang

然后把代码写进去，就可以运行了，在这次调试的过程中，我发现直接在调试窗口，更好一些，能够犯得错误更少
意外情况1：

这是因为roscore没开
另外，指的注意的一点是仍要写cmakelists文件，

意外情况2：

这是因为python没有加#!/usr/bin/python
意外情况3：

这是因为每天添加可执行文件chmod +x talker.py
成功运行：

建议吧python的代码放到scripts这个文件夹中 src中放Cpp文件

代码解析：

#!/usr/bin/python
# -*- coding: UTF-8 -*-
import rospy
from std_msgs.msg import String

def talker():
    pub = rospy.Publisher('chatter', String, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        hello_str = "hello world %s" % rospy.get_time()
        rospy.loginfo(hello_str)
        pub.publish(hello_str)
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

#!/usr/bin/python
# -*- coding: UTF-8 -*-

让编译器找到python

import rospy
from std_msgs.msg import String

导入rospy客户端，和std_msg/String 这个数据类型
queue_size表示的对应的大小

pub = rospy.Publisher('chatter', String, queue_size=10)

这个是话题的名称chatter

rospy.init_node('talker')

初始化节点，节点的名字要唯一，并且不能

rate=rospy.Rate(10)

创建rate对象，与sleep（）函数结合使用，控制话题消息发布的频率

while not rospy.is_shutdown():
    hello_str = "hello world %s" % rospy.get_time()
    rospy.loginfo(hello_str)
    pub.publish(hello_str)
    rate.sleep()

rospy.is_shutdown()函数是什么意思，目前还不知道。
rate.sleep（）用于控制发布的频率
rospy.loginfor(str) 函数在屏幕输出信息，我们把这个信息存储在hello_str当中

---

2、订阅一个话题

#!/usr/bin/python
import rospy
from std_msgs.msg import String

def callback(data):
    rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)

def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("chatter", String, callback)
    rospy.spin()
if __name__ == '__main__':
    listener()

最后别忘记给listener.py添加可执行权限chmod +x listener.py

测试可以通过

代码分析

def callback(data):
    rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)

这里是定义一个回调函数

def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("chatter", String, callback)
    rospy.spin()

定义listener函数，
第一行，初始化一个节点，后面anony=False ，默认参数是不匿名的，但是现在你也可以选择匿名。匿名之后就不会被重复了
第二行是订阅话题的名称，使用回调函数来输出话题的名称
第三行rospy.spin()是保持你的节点一直运行，直到程序关闭。

---

接下来这篇博客当中，我希望写的是ros cpp剩下的内容，和python的所有的东西。
编写服务器和客户端
到自己的依赖项里面然后

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

添加message_generation

 add_message_files(
   FILES
   Num.msg
#   Message2.msg
)

 add_service_files(
   FILES
#   Service1.srv
AddTwoInts.srv
 )

 generate_messages(
   DEPENDENCIES
   std_msgs
 )

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES mypackage
  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

在package.xml里面添加

  <build_depend>message_generationbuild_depend>
  <run_depend>message_runtimerun_depend>

这个地方的文件名就是你当时创建的srv文件的名称
beginner_tutorials/AddTwoInts.h是由编译系统自动根据我们先前创建的srv文件生成的对应该srv文件的头文件。
里面一般就是有一个输入的参数和一个输出的参数，然后

bool add(mypackage::AddTwoInts::Request &req,mypackage::AddTwoInts::Response &res)
{
    res.sum=req.a+req.b;
    ROS_INFO_STREAM("res.sum="<<res.sum);
    return true;
}

在srv当中，我们使用—将request和respon进行分开。

int64 a
int64 b
---
int64 sum

重要的就是这一句，其实就是这个service的名称和回调函数。

 ros::ServiceServer service=nh.advertiseService("add_two_ints",add);

这个头文件姑且可以理解为stdio.h

#include 

利用这个头文件，能够让我们输出参数

    mypackage::AddTwoInts srv;
    srv.request.a=atoll(argv[1]);
    srv.request.b=atoll(argv[2]);

利用atoll函数来在终端传递参数。

总结：service 和client 就像他们的名称一样，服务器和客户端，服务一直都在等待数据的输入，clinet进行输入的输入 这个advertise 的 subscribise我感觉还是有很大差异的。

这个就是客户端的框架，首先要看到底订阅谁

    ros::ServiceClient client=nh.serviceClient("add_two_ints");
    mypackage::AddTwoInts srv;
    srv.request.a=atoll(argv[1]);
    srv.request.b=atoll(argv[2]);
    client.call(srv);

这个要跟advertise对应起来，就是那,特别是话题的名称，然后就实例化传入参数的对应，然后吧这些对象都放入到call这个呼叫的函数当中，完成一个服务器和客户端之间的通信。

 ros::ServiceServer service=nh.advertiseService("add_two_ints",add);

#include 
#include "mypackage/AddTwoInts.h"
#include 
int main(int argc,char **argv)
{
    ros::init(argc, argv, "add_two_ints_client");
    ros::NodeHandle nh;
    ros::ServiceClient client=nh.serviceClient("add_two_ints");
    mypackage::AddTwoInts srv;
    srv.request.a=atoll(argv[1]);
    srv.request.b=atoll(argv[2]);
    client.call(srv);
    ROS_INFO("Sum: %ld", (long int)srv.response.sum);
    return true;
}

使用参数
getparam函数有一个bool类型的返回值，来判断这个参数是不是成功。

std::string s;
if (n.getParam("my_param", s))
{
  ROS_INFO("Got param: %s", s.c_str());
}
else
{
  ROS_ERROR("Failed to get param 'my_param'");
}

std::string s;
n.getParam("my_param", s);

param函数，在没有获得参数的时候，可以设置默认值

int i;
n.param（“myparam”,i,11）;

这里的n是实例化的句柄

std::string s;
n.param<std::string>("my_param", s, "default_value");

设置参数

n.setParam("my_param", "hello there");

NodeHandle类的私有名称

ros::init(argc, argv, "my_node_name");
ros::NodeHandle nh1("~");
ros::NodeHandle nh2("~foo");

nh1的命名空间是 /my_node_name
nh2的命名空间是/my_node_name/foo.

然后对这个私有的命名空间进行访问

ros::NodeHandle nh;
nh.getParam("~name", ... );

这两种访问等价的。

ros::NodeHandle nh("~");
nh.getParam("name", ... );

使用类方法作为回调函数

例如在订阅的时候，原来的时候之要做一个回调函数，现在我们不做了，就想这些回调函数封装到一个类当中。

ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

public:
  void callback(const std_msgs::String::ConstPtr& msg);
};
// %EndTag(CLASS_WITH_DECLARATION)%

void Listener::callback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

  ros::Subscriber sub = n.subscribe("chatter", 1000, &Listener::callback, &listener);

总体的框架

#include "ros/ros.h"
#include "std_msgs/String.h"
class Listener
{
public:
  void callback(const std_msgs::String::ConstPtr& msg);
};
void Listener::callback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}
int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener_class");
  ros::NodeHandle n;
  Listener listener;
  ros::Subscriber sub = n.subscribe("chatter", 1000, &Listener::callback, &listener);
  ros::spin()；
  return 0;
}

同样在制作服务器的时候

#include "ros/ros.h"
#include "roscpp_tutorials/TwoInts.h"

// %Tag(CLASS_DECLARATION)%
class AddTwo
{
public:
  bool add(roscpp_tutorials::TwoInts::Request& req,
           roscpp_tutorials::TwoInts::Response& res);
};
// %EndTag(CLASS_DECLARATION)%

bool AddTwo::add(roscpp_tutorials::TwoInts::Request& req,
                 roscpp_tutorials::TwoInts::Response& res)
{
  res.sum = req.a + req.b;
  ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
  ROS_INFO("  sending back response: [%ld]", (long int)res.sum);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_server");
  ros::NodeHandle n;

// %Tag(SERVICE_SERVER)%
  AddTwo a;
  ros::ServiceServer ss = n.advertiseService("add_two_ints", &AddTwo::add, &a);
// %EndTag(SERVICE_SERVER)%

  ros::spin();

  return 0;
}

关键的部分抽出来分析

class AddTwo
{
public:
  bool add(roscpp_tutorials::TwoInts::Request& req,
           roscpp_tutorials::TwoInts::Response& res);
};
bool AddTwo::add(roscpp_tutorials::TwoInts::Request& req,
                 roscpp_tutorials::TwoInts::Response& res)
{
  res.sum = req.a + req.b;
  ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
  ROS_INFO("  sending back response: [%ld]", (long int)res.sum);
  return true;
}
AddTwo a;
  ros::ServiceServer ss = n.advertiseService("add_two_ints", &AddTwo::add, &a);

启动就是一个是这个函数的引用，另一个是这类实例化对象的引用

总结，就是subscribe或者advertiseService的时候，类的就，就是在这个名称或者话题的后面，对这个函数的引用，和对这个实例化对象的引用。
Timer类
使用定时器，可向定时器一起的创建订阅

#include "ros/ros.h"
void callback1(const ros::TimerEvent&)
{
  ROS_INFO("Callback 1 triggered");
}

void callback2(const ros::TimerEvent&)
{
  ROS_INFO("Callback 2 triggered");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  ros::Timer timer1 = n.createTimer(ros::Duration(0.1), callback1);
  ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2);
  ros::spin();
  return 0;
}

两个回调函数用来执行任务ros::Duration(0.1)每隔0.1秒执行一次。
设置动态调节参数
http://www.ncnynl.com/archives/201608/508.html
msg文件描述的是ROS中使用的消息类型。实际上就是声明一个数据类型和变量名
srv描述的一项服务，那么就有request 和 respon两个部分，他们用—进行划分。
ROS当中的Header他含有时间戳和坐标信息，因此在msg文件当汇总，第一行经常可以看到

  string child_frame_id
  geometry_msgs/PoseWithCovariance pose
  geometry_msgs/TwistWithCovariance twist
  string first_name
string last_name
uint8 age
uint32 score

msg的数据类型

int8, int16, int32, int64 (plus uint*) float32, float64 string time, duration other msg files variable-length array[] and fixed-length array[C]

并且在package.xml;添加

  <build_depend>message_generationbuild_depend>
  <run_depend>message_runtimerun_depend>

在cmakelist添加

find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation)


catkin_package(
  ...
  CATKIN_DEPENDS message_runtime ...
  ...)

如何在launch文件当中配置msg
我们定义的msg文件

string message
int32 a
int32 b

我们使用的launch文件

 "node_example" type="talker" name="talker">
    <param name="a" value="1"/>
    <param name="b" value="2"/>
    <param name="message" value="hello"/>
    <param name="rate" value="1"/>
    from="example" to="chatter"/>
  

其实就是给参数，然后velue就可以了。

首先创建动态配置文件 .cfg

#! /usr/bin/env python

PACKAGE='node_example'
import roslib
roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()
#       Name       Type      Level Description     Default Min   Max
gen.add("message", str_t,    0,    "The message.", "hello")
gen.add("a",       int_t,    0,    "First number.", 1,     -100, 100)
gen.add("b",       int_t,    0,    "First number.", 2,     -100, 100)

exit(gen.generate(PACKAGE, "node_example", "nodeExample"))

然后给这个文件添加执行权限

chmod 755 cfg/nodeExample.cfg

然后cmakelist里面修改

generate_dynamic_reconfigure_options(
  cfg/nodeExample.cfg
)
catkin_package(
  CATKIN_DEPENDS dynamic_reconfigure message_runtime roscpp rospy std_msgs
)

然后这里的写法是比较标准的

add_executable(talker src/nodes/talker_node.cpp src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker node_example_gencfg node_example_generate_messages_cpp)

在添加可执行文件的时候， 需要多加一句add_dependencies

在talker_node.cpp文件中

#include 
int main(int argc, char **argv)
{
  // Set up ROS.
  ros::init(argc, argv, "talker");
  ros::NodeHandle nh;

  // Create a new node_example::Talker object.
  node_example::ExampleTalker node(nh);

  // Let ROS handle all callbacks.
  ros::spin();

  return 0;
}

这个里面的头文件

#ifndef NODE_EXAMPLE_TALKER_H
#define NODE_EXAMPLE_TALKER_H

// ROS includes.
#include 
#include 

// Custom message includes. Auto-generated from msg/ directory.
#include 

// Dynamic reconfigure includes.
#include 
// Auto-generated from cfg/ directory.
#include 

namespace node_example
{
class ExampleTalker
{
 public:
  //! Constructor.
  explicit ExampleTalker(ros::NodeHandle nh);

  //! Callback function for dynamic reconfigure server.
  void configCallback(node_example::nodeExampleConfig &config, uint32_t level);

  //! Timer callback for publishing message.
  void timerCallback(const ros::TimerEvent &event);

 private:
  //! The timer variable used to go to callback function at specified rate.
  ros::Timer timer_;

  //! Message publisher.
  ros::Publisher pub_;

  //! Dynamic reconfigure server.
  dynamic_reconfigure::Server dr_srv_;

  //! The actual message.
  std::string message_;

  //! The first integer to use in addition.
  int a_;

  //! The second integer to use in addition.
  int b_;
};
}

C++中的explicit关键字只能用于修饰只有一个参数的类构造函数, 它的作用是表明该构造函数是显示的
然后在talker.cpp文件当中，具体写一些这些函数的定义

#include <node_example/talker.h>

namespace node_example
{
ExampleTalker::ExampleTalker(ros::NodeHandle nh) : message_("hello"), a_(1), b_(2)
{
  // Set up a dynamic reconfigure server.
  // Do this before parameter server, else some of the parameter server values can be overwritten.
  dynamic_reconfigure::Server<node_example::nodeExampleConfig>::CallbackType cb;
  cb = boost::bind(&ExampleTalker::configCallback, this, _1, _2);
  dr_srv_.setCallback(cb);

  // Declare variables that can be modified by launch file or command line.
  int rate;

  // Initialize node parameters from launch file or command line. Use a private node handle so that multiple instances
  // of the node can be run simultaneously while using different parameters.
  ros::NodeHandle pnh("~");
  pnh.param("a", a_, a_);
  pnh.param("b", b_, b_);
  pnh.param("message", message_, message_);
  pnh.param("rate", rate, 1);

  // Create a publisher and name the topic.
  pub_ = nh.advertise<node_example::NodeExampleData>("example", 10);

  // Create timer.
  timer_ = nh.createTimer(ros::Duration(1 / rate), &ExampleTalker::timerCallback, this);
}

void ExampleTalker::timerCallback(const ros::TimerEvent &event)
{
  node_example::NodeExampleData msg;
  msg.message = message_;
  msg.a = a_;
  msg.b = b_;

  pub_.publish(msg);
}

void ExampleTalker::configCallback(node_example::nodeExampleConfig &config, uint32_t level)
{
  // Set class variables to new values. They should match what is input at the dynamic reconfigure GUI.
  message_ = config.message.c_str();
  a_ = config.a;
  b_ = config.b;
}
}

仿照上面的例子，我打算自己也写一个出来。
命名说明：
节点的名称：talker
msg名称：Num.msg
config名称：my_cfg.cfg
命名空间：my_namespace
类名：my_class
nodecpp文件名：talker_node.cpp
cpp文件名：talker.cpp
头文件，talker.h
文件名（package name）:my_stack

首先将talker_node.cpp文件贴出来

#include <talker.h>
int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");
    ros::NodeHandle nh;
//只是将自己实例化的句柄传入,将自己的做的这个对象付给初始值
    my_namespace::my_class node(nh);
    ros::spin();
}

为什么这个包含头问的时候只有talker.h
因为在cmakellist。txt这个里面已经include目录包含进行

include_directories(include ${catkin_INCLUDE_DIRS})

如果你在include 文件夹下面创建一个a文件夹，然后在a文件夹下面创建b.h
那么你的#include 应该这么写：#include
然后肥西上面的代码，其实就是直接在my_namespace空间下实例化一个my_class的对象，然后就将ros的句柄传递进去。

然后分析头文件

#ifndef MY_STACK_TALKER_H
#define MY_STACK_TALKER_H
//ROS的库添加
#include
#include
//由msg文件自动生成
#include
//添加动态调节参数的库
#include
//由cfg文件自动生成
#include
namespace my_namespace
{
    class my_class
    {
        public:
            //构造函数
             my_class(ros::NodeHandle nh);
            //call函数，用于对reconfigure进行服务器
            void ConfigCallback(my_stack::mycfgConfig &config, int32_t level);
            //发送时间的回调函数
            void timerCallback(const ros::TimerEvent &event);
        private:

            ros::Timer timer_;
            //消息的发布
            ros::Publisher pub_;
            //定义动态调节参数的server
            dynamic_reconfigure::Server dr_srv_;
            //提供的message
            std::string message_;
            //第一个添加的数
            int a_;
            //第二个参加的数
            int b_;
    };
}
#endif //MY_STACK_LKER_H

需要注意的是msg文件对应的Num.msg然后他的头文件就是Num,h，然后my_stack是我的包的名称，其实就是你的packagename或者文件夹的名称

#include

另外一点是：我的cfg文件夹下面的参数mycfg，然后他对应的头文件是mycfgConfig.h

#include

然后添加动态调节参数的库

#include

然后可以看到my_class的构造函数my_class()传入的就是ros的句柄
然后关于命名空间，要注意的地方就是mycfgConfig的命名空间都是my_stack

void timerCallback(const ros::TimerEvent &event);

然后是talker.cpp文件

#include 
namespace my_namespace
{
    my_class::my_class(ros::NodeHandle nh):message_("hello"),a_(1),b_(0)
    {
        //设置动态调节参数的服务器
        dynamic_reconfigure::Server::mycfgConfig>::CallbackType cb;
        //然后boost::bind函数进行参数的回调
        cb=boost::bind(& my_class::ConfigCallback,this,_1,_2);
        //然后等待响应
        dr_srv_.setCallback(cb);
        //声明变量
        int rate;
        //初始化节点的参数，这些参数可以用launch文件中进行设置，使用私有的命名空间
        ros::NodeHandle pnh("~");
        pnh.param("a",a_,a_);//可以给这个东西设置默认的数值
        pnh.param("b",b_,b_);
        pnh.param("message",message_,message_);
        pnh.param("rate",rate,1);
        //我感觉这里是给变量赋值
        pub_=nh.advertise<my_stack::Num>("chatter",10);
        //进行时间上的回调
        timer_=nh.createTimer(ros::Duration(1/rate),& my_class::timerCallback,this);
    }
    void my_class::timerCallback(const ros::TimerEvent &event)
    {
        my_stack::Num msg;
        msg.message=message_;
        msg.a=a_;
        msg.b=b_;
        pub_.publish(msg);
    }
    void my_class::ConfigCallback(my_stack::mycfgConfig &config, int32_t level)
    {
        //将这个变量设置成新的数值
        message_=config.message.c_str();
        a_=config.a;
        b_=config.b;
    }
}

然后我cmakelists.txt函数贴在这里

cmake_minimum_required(VERSION 2.8.3)
project(my_stack)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED
rospy
roscpp 
std_msgs
dynamic_reconfigure
message_generation
)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
add_message_files(
  FILES
  Num.msg
)

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs
)

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
  cfg/mycfg.cfg
)

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
  CATKIN_DEPENDS
  roscpp
  rospy
  std_msgs
  message_runtime
  dynamic_reconfigure 
#  INCLUDE_DIRS include
#  LIBRARIES my_node
#  CATKIN_DEPENDS other_catkin_pkg
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(include ${catkin_INCLUDE_DIRS})
## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/my_node.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/my_node_node.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_my_node.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

add_executable(talker src/node/talker_node.cpp src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})

其实就是将最后的相关的cpp文件包含进去

add_executable(talker src/node/talker_node.cpp src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})

然后我吧package.xml文件也贴下来

<package>
  <name>my_stackname>
  <version>0.0.0version>
  <description>The my_node packagedescription>

  
  
  
  <maintainer email="[email protected]">davidhanmaintainer>


  
  
  
  <license>TODOlicense>


  
  
  
  


  
  
  
  


  
  
  
  
  
  
  
  
  
  
  
  <buildtool_depend>catkinbuildtool_depend>


  
  <export>
    

  export>

<build_depend>roscppbuild_depend>
<build_depend>rospybuild_depend>
<build_depend>std_msgsbuild_depend>
<build_depend>message_generationbuild_depend>
<build_depend>dynamic_reconfigurebuild_depend>
<run_depend>dynamic_reconfigurerun_depend>
<run_depend>roscpprun_depend>
<run_depend>rospyrun_depend>
<run_depend>std_msgsrun_depend>
<run_depend>message_runtimerun_depend>
package>

最重要的就是下面的这个，就是每个build要和一个run对应起来

<build_depend>roscppbuild_depend>
<build_depend>rospybuild_depend>
<build_depend>std_msgsbuild_depend>
<build_depend>message_generationbuild_depend>
<build_depend>dynamic_reconfigurebuild_depend>
<run_depend>dynamic_reconfigurerun_depend>
<run_depend>roscpprun_depend>
<run_depend>rospyrun_depend>
<run_depend>std_msgsrun_depend>
<run_depend>message_runtimerun_depend>

然后就是在

<build_depend>message_generationbuild_depend>
<run_depend>message_runtimerun_depend>

这两个是一一对应的
然后就是Num.msg文件的创建
Num.msg

string message
int32 a
int32 b

然后对应的cmakelist是

generate_messages(
  DEPENDENCIES
  std_msgs
)

然后cfg文件的创建

#! /usr/bin/env python

PACKAGE='my_stack'
import roslib
roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()
#       Name       Type      Level Description     Default Min   Max
gen.add("message", str_t,    0,    "The message.", "hello")
gen.add("a",       int_t,    0,    "First number.", 1,     -100, 100)
gen.add("b",       int_t,    0,    "First number.", 2,     -100, 100)

exit(gen.generate(PACKAGE, "my_stack", "mycfg"))

这个地方，我自己写了一个但是，没有写对，最后还是直接copy的，在最后一行当中
就是my_stack 就是你的packagename，然后记得要给这个文件赋予权限，chmod 777

exit(gen.generate(PACKAGE, "my_stack", "mycfg"))

这个文件会自动生成，我们刚刚说的mycigConfig，h的头文件

对应的cmakelist.txt

 generate_dynamic_reconfigure_options(
  cfg/mycfg.cfg
)

然后同样做了一个简单的接受
listern.h

#ifndef MY_STACK_LINTERN_H
#define MY_STACK_LINTERN_H

#include 
#include 
#include 
namespace my_namespace
{
class listern 
{
    public: 
        listern(ros::NodeHandle nh);
        void messageCallback(const my_stack::Num::ConstPtr &msg);
    private:
        ros::Subscriber sub_; 
};
}
#endif

listern.cpp

#include<listern.h>
namespace my_namespace
{
    listern::listern(ros::NodeHandle nh)
    {
        sub_=nh.subscribe("chatter",10,&listern::messageCallback,this);
    }
     void listern::messageCallback(const my_stack::Num::ConstPtr &msg)
    {
        ROS_INFO_STREAM("mess is "<<msg->message.c_str()<<"a+b="<<msg->a+msg->b);
    }
}

就是熟悉这种在类里面进行订阅话题的机制，这个代码可以在下面的地址下载到：
Timer定时器

作为function实现方法

void callback(const ros::TimerEvent& event)
{
...
}

...
ros::Timer timer = nh.createTimer(ros::Duration(0.1), callback);

作为类的实现方法

void Foo::callback(const ros::TimerEvent& event)
{
...
}

...
Foo foo_object;
ros::Timer timer = nh.createTimer(ros::Duration(0.1), &Foo::callback, &foo_object);

如果是在类里面进行定义的话

class Foo
{
public:
  void operator()(const ros::TimerEvent& event)
  {
    ...
  }
};

...
ros::Timer timer = nh.createTimer(ros::Duration(0.1), Foo());

获得参数
roscpp有两个版本get param接口：bare版和handle版。
nodehandle版

ros::NodeHandle nh;
std::string global_name, relative_name, default_param;
if (nh.getParam("/global_name", global_name))
{
  ...
}

if (nh.getParam("relative_name", relative_name))
{
...
}

// Default value version
nh.param("default_param", default_param, "default_value");

bare版本

std::string global_name, relative_name, default_param;
if (ros::param::get("/global_name", global_name))
{
  ...
}

if (ros::param::get("relative_name", relative_name))
{
...
}

// Default value version
ros::param::param("default_param", default_param, "default_value");

ros::NodeHandle::getParamCached() 和 ros::param::getCached() 能提供本地的缓存功能。
设置参数
nodeHandle版

ros::NodeHandle nh;
nh.setParam("/global_param", 5);
nh.setParam("relative_param", "my_string");
nh.setParam("bool_param", false);

bare版

ros::param::set("/global_param", 5);
ros::param::set("relative_param", "my_string");
ros::param::set("bool_param", false);

检查参数是否存在
nodehandle版

ros::NodeHandle nh;
if (nh.hasParam("my_param"))
{
  ...
}

bare版

if (ros::param::has("my_param"))
{
  ...
}

调用服务
Handle 方法：
Foo就是你的srv的

ros::ServiceClient client = nh.serviceClient("my_service_name");
my_package::Foo foo;
...
if (client.call(foo))
{
  ...
}

bare方法

//remember that ros::init(...) must have been called
my_package::Foo foo;
...
if (ros::service::call("my_service_name", foo))
{
  ...
}

来进行持久链接

ros::ServiceClient client = nh.serviceClient<my_package::Foo>("my_service_name", true);

函数作为回调函数
类方法

bool Foo::callback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
  return true;
}

Foo foo_object;
ros::ServiceServer service = nh.advertiseService("my_service", &Foo::callback, &foo_object);

函数的的对象

class Foo
{
public:
  bool operator()(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
  {
    return true;
  }
};

ＮodleHandles
用来指定nh的命名空间
你也可以指定私有的命名空间，例如~

ros::NodeHandle nh("~");

线程spin 和 callback函数

ros：：spin();

表示的是单个线程，只有当节点关闭或者ctrl+c的时候，才能关闭这个线程。
另外一个中模型是定期运行ros::spinonce()

ros::Rate r(10); // 10 hz
while (should_continue)
{
  ... do some work, publish some messages, etc. ...
  ros::spinOnce();
  r.sleep();
}

多线程
roscpp当中提供一些方式来进行多线程

ros::MultiThreadedSpinner spinner(4); // 新建4个线程
spinner.spin(); // spin() 将会返回，知道4个线程都跑完

大多数情况下使用的是：很有用的线程spinner是AsyncSpinner，它不是阻塞式的spin()调用它有start() 和stop()函数，当它销毁时候就会自动关闭：

ros::AsyncSpinner spinner(4); // Use 4 threads
spinner.start();
ros::waitForShutdown();

time
获取当前时间

ros::Time beign=ros::Time::now();

按照一定比例进行睡眠

ros::Rate r(10); // 10 hz
while (ros::ok())
{
  ... do some work ...
  r.sleep();
}

wailltime就是你挂钟时间
tf
基本的数据类型有Ｑuaternion,Vector,Point,Pose,Transform
tf::Stamped 模板对上述类型进行模板化，并且附带frame_id_和stamp_
tf::StampedTransform 是tf::Transforms的特例，它要求frame_id 、stamp 、child_frame_id.
将tf变换进行发送

void sendTransform(const StampedTransform & transform);
void sendTransform(const geometry_msgs::TransformStamped & transform);

waitForTransform()函数 返回bool值，评估变换是否有效。
lookupTransform()是一个更底层的方法用于返回两个坐标系的变换。

然后在终端做了一个简单ＴＦ订阅的东西

#include 
#include 
#include 
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
  ROS_INFO_STREAM("msg theta"<theta);
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_node");
  if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
  turtle_name = argv[1];
  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
  ros::spin();
  return 0;
};

发布机制

static tf::TransformBroadcaster br;
 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

进行做坐标变换的机制

tf::Transform transform;
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);

对于一个二位平面内的东西，那么我们就需要知道他的x,y和theta
然后在编写一个对上面这个tf变换进行接收的东西

   listener.lookupTransform("/turtle1", "/turtle2",
                               ros::Time(0), transform);

对这两个TF进行监听
然后利用

geometry_msgs::Twist vel_msg;
         vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
         vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
         turtle_vel.publish(vel_msg);

发布速度

atan2（double Y,double X）;返回的是X轴的夹角
pow(transform.getOrigin().x(), 2)表示x^2幂函数

代码的地址：
添加tf坐标系
其实你的

添加头文件
#include 
然后实例化发布的对象
 tf::TransformBroadcaster br;
实例化坐标变换的对象
tf::Transform transform;
然后构建一个以Rate=10HZ频率进行发布的循环
  ros::Rate rate(10.0);
  while(ros::ok())
  {


rate.sleep();
    }

然后在这个循环当中不断设置变换的 坐标原点，。使用setOrigin和vector3
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
然后发布出去
br.sendTransform(tf::StampedTransform());
通过tf::StampedTransform()函数
这个函数里面的定义
tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1")

从上面定义来看tf::StampedTransform函数

 StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id):

第一个参数是输入的TF，第二个参数是执行的时间，三个参数是当前TF的名字，第4个参数是子节点的名称
然后你打开rviz就可以看到变化添加坐标系，就可以看到变化。

添加移动的坐标系

其实，这个本质就是在这个循环当中，不断设置TF的位置，同样，打开RVIZ就能看到效果

transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );

等待TF变换

listener.waitForTransform("/turtle2", "/turtle1",
                              now, ros::Duration(3.0));

一般都是在监听TF之前有一个waitForTransform

istener.waitForTransform("/turtle2", "/turtle1",
                              now, ros::Duration(3.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             now, transform);

waitForTransform() 四个参数：

1.需要等待变换从坐标系turtle2
2.到坐标系turtle1
3.在now时间
4.超时时间，不要等待超过此最大持续时间

关于时间travel
我们不让turtle2到turtle1当前时刻的地方，而是让turtle2到turtle1, 5秒前的地方

    ros::Time past = ros::Time::now() - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", "/turtle1",
                              past, ros::Duration(1.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             past, transform);

lookupTransform()API，有六个参数：

    变换从坐标系turtle2
    在now时间
    到turtle1坐标系
    在past时间
    指定不随时间改变的坐标系，这里是world
    变换结果保存的变量

走完了TF接下里才是ROS当中神奇的地方
编写简单的action服务器

catkin_create_pkg mypackage roscpp actionlib message_generation std_msgs actionlib_msgs

action有三种消息：goal, result, and feedback 他们通过.action文件自动生成的。
“

首先还是创建一个action
myaction.aciton

#goal defition
int32 order
---
#result defition
int32[] sequence
---
#feedback defition
int32[] sequence

定义goal result 和 feedback

这三个概念是在action里面 int32 和int32[]只是数据类型不同

然后在find_package(catkin REQUIRED COMPONENTS actionlib_msgs)
添加actionlib actionlib_msgs
然后是

add_action_files(FILES
  myaction.action
)
generate_messages(DEPENDENCIES
  actionlib_msgs
  std_msgs
)
catkin_package(
  CATKIN_DEPENDS
  message_runtime
  actionlib_msgs
#  INCLUDE_DIRS include
#  LIBRARIES mypackage
#  CATKIN_DEPENDS actionlib actionlib_msgs message_generation roscpp std_msgs
#  DEPENDS system_lib
)

然后你在你的工作空间caktin_make一下，就会产生这些头文件
给我的感觉actionlib就会生成很多的msg调试经验，尽量复制，因为自己一写就写错，很多都是因为自己的变量名称敲错，还不如直接复制。然后分析一下actionlib的代码

devel/include/mypackage/myaction
myactionActionFeedback.h  myactionActionResult.h    myactionResult.h
myactionActionGoal.h      myactionFeedback.h        
myactionAction.h          myactionGoal.h                      

因为要创建server,那么要添加这个头文件

#include

然后，下面这个头文件是由action自动生成的。他的命名方式跟cfg一样，我建立的myaction.action，那么我包含的头文件就因应该是myactionAcion.h

#include

class myclass
{
    private:
        ros::NodeHandle nh_;
        std::string actionname;
        actionlib::SimpleActionServer::myactionAction> as_;
        mypackage::myactionFeedback feedback_;
        mypackage::myactionResult result_;
    public:
        myclass(std::string name) : as_(nh_, name, boost::bind(&myclass::exectionCB, this, _1), false), actionname(name)
        {
            //在构造函数中开始线程
            as_.start();
        }
        ~myclass()
        {

        }
        void exectionCB(const mypackage::myactionGoalConstPtr &goal)
        {
            ros::Rate rate(1);
            bool success=true;
            feedback_.sequence.clear();
            feedback_.sequence.push_back(0);
            feedback_.sequence.push_back(1);
            ROS_INFO("%s,Executing,creation acion sequence is order %i whie %i and %i",actionname.c_str(),goal->order,feedback_.sequence[0],feedback_.sequence[1]);


            //开始执行action
            for(int i=1;iorder;i++)
            {
                if(as_.isPreemptRequested()|| !ros::ok())
                {
                 ROS_INFO("%s :",actionname.c_str());
                 as_.setPreempted();
                 success=false;   
                 break;
                }
                feedback_.sequence.push_back(feedback_.sequence[i]+feedback_.sequence[i-1]);
                as_.publishFeedback(feedback_);
                rate.sleep();
            }

            if(success)
            {
                result_.sequence=feedback_.sequence;
                ROS_INFO("%s success",actionname.c_str());
                as_.setSucceeded(result_);
            }

        }

};

然后定义一个类myclass,简明写一下这个类里面的东西
首先我们定义feedback_和result_

    mypackage::myactionFeedback feedback_;
        mypackage::myactionResult result_;
        actionlib::SimpleActionServer::myactionAction> as_;

然后注意里面写的是然后上面两个是当时在数据类型定义的feedback 和 result,然后在在构造函数当中为server 实例化的as 赋予初值

as_(nh_, name, boost::bind(&myclass::exectionCB, this, _1), false)

查看的他的定义，就和清楚了，地二个参数就是这个action的名字，false是说不直接开始。

   SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb, bool auto_start);

然后查看boost::bind的：功能是返回的一个函数对象，这个就跟上面对应起来，execute_cb就是一个执行的函数。
假设你有一个函数fun() 如下:
void fun(int x, int y) {
cout << x << ", " << y << endl;
}

第1种用法:
向原始函数 fun 绑定所有的参数
 boost::bind(&fun, 3, 4)     // bind的实参表依次为: 要绑定的函数的地址, 绑定到fun的第一个参数值, 第二个参数值...
        // fun有多少个参数, 这里就要提供多少个.
表示将 3 和 4 作为参数绑定到 fun 函数.
因为绑定了所有的参数. 现在我们调用bind所返回的函数对象:
 boost::bind(&fun, 3, 4)( );  //无参数.
就会输出 3, 4
第2种用法:
向原始函数 fun 绑定一部分参数
 boost::bind(&fun, 3, _1)    // bind的实参表依次还是: 要绑定的函数的地址, 要绑定到fun的第一个参数值, 然后注意
        // 因为我们不打算向fun绑定第2个参数(即我们希望在调用返回的Functor时再指定这个参数的值)
        // 所以这里使用 _1 来占位. 这里的 _1 代表该新函数对象被调用时. 实参表的第1个参数.
        // 同理下边还会用到 _2 _3 这样的占位符.
这里只为fun绑定了第一个参数3. 所以在调用bind返回的函数对象时. 需要:
 boost::bind(&fun, 3, _1)(4);  //这个4 会代替 _1 占位符.
输出 3, 4
同理 boost::bind(&fun, _1, 3)(4);
输出 4, 3

第3种用法:
不向 fun 绑定任何参数
 boost::bind(&fun, _1, _2)   // _1 _2 都是占位符. 上边已经说过了.
所以它就是 将新函数对象在调用时的实参表的第1个参数和第2个参数 绑定到fun函数. 
 boost::bind(&fun, _1, _2)(3, 4);    // 3将代替_1占位符, 4将代替_2占位符.
输出 3, 4
同理 boost::bind(&fun, _2, _1)(3, 4);   // 3将代替_1占位符, 4将代替_2占位符.
会输出 4, 3 
同理 boost::bind(&fun, _1, _1)(3);     // 3将代替_1占位符
会输出 3, 3

对于普通函数就这些. 对于函数对象. 如:
 struct Func {
  void operator()(int x) {
   cout << x << endl;
  }
 } f;
绑定的时候可能要指出返回值的类型:
 boost::bind<void>(f, 3)();  //指出返回值的类型 void

这里是第二种用啦，-1是一个占位符，那么当输入参数的是就可，然后boost::bind的就能，现来看就是绑定参数。
然后来看下面的exectionCB函数

        void exectionCB(const mypackage::myactionGoalConstPtr &goal)
        {
        //然后传入的goal，这样action里面所有的东西都用到了
            ros::Rate rate(1);

            bool success=true;
            feedback_.sequence.clear();
            feedback_.sequence.push_back(0);
            feedback_.sequence.push_back(1);
            ROS_INFO("%s,Executing,creation acion sequence is order %i whie %i and %i",actionname.c_str(),goal->order,feedback_.sequence[0],feedback_.sequence[1]);

            //开始执行action
            for(int i=1;iorder;i++)
            {
                if(as_.isPreemptRequested()|| !ros::ok())
                {
                 ROS_INFO("%s :",actionname.c_str());
                 as_.setPreempted();
                 success=false;   
                 break;
                }
                feedback_.sequence.push_back(feedback_.sequence[i]+feedback_.sequence[i-1]);
                as_.publishFeedback(feedback_);
                rate.sleep();
            }

            if(success)
            {
                result_.sequence=feedback_.sequence;
                ROS_INFO("%s success",actionname.c_str());
                as_.setSucceeded(result_);
            }

        }

一个类，类的方法可以访问类的属性吗？大果然是可以的

class A
{
    private:
     int a;
     public:
     void fun()
     {
         a=0;
     }
}

然后我就不看功能。
然后接着类似的写一个客户端
我吧核心的地方写出来，然后具体代码，我上传到github上面提供大家下再
添加actionlib的头文件

#include
#include

添加我们定义的action的数据类型的头文件（同样这是系统自动生成的）

#include

在主函数里面实例化一个client，然后给他命名，然后等待server,实例化goal给goal.order扶植，然后发送这个值给server.
然后判断一下这个server的状态。

  actionlib::SimpleActionClient::myactionAction> ac("acmyclient",true);
    ac.waitForServer();

最后使用

输入rostopic list -v 就可以看到所有的哦订阅和发布的所有的话题
rostopic list -v

Published topics:
 * /rosout [rosgraph_msgs/Log] 2 publishers
 * /acmyclient/goal [mypackage/myactionActionGoal] 1 publisher
 * /rosout_agg [rosgraph_msgs/Log] 1 publisher
 * /acmyclient/cancel [actionlib_msgs/GoalID] 1 publisher

Subscribed topics:
 * /rosout [rosgraph_msgs/Log] 1 subscriber
 * /acmyclient/feedback [mypackage/myactionActionFeedback] 1 subscriber
 * /acmyclient/result [mypackage/myactionActionResult] 1 subscriber
 * /acmyclient/status [actionlib_msgs/GoalStatusArray] 1 subscriber
 * /statistics [rosgraph_msgs/TopicStatistics] 1 subscriber

目前这个代存在的问题

rostopic echo /mypackage/result 
ERROR: Cannot load message class for [mypackage/myactionActionResult]. Are your messages built?

然后节点图倒是正常的

我就不花时间做这个事情了。
然后节点运行

rosrun mypackage myserver 
rosrun mypackage myclient

遇到的问题
在查看输出的的时候，遇到这个问题

 rostopic echo /mypackage/result 
ERROR: Cannot load message class for [mypackage/myactionActionResult]. Are your messages built?

解决办法：
分析原因：没有source 你的工作空间

source ~/catkin_ws02/devel/setup.bash
rostopic echo /mypackage/result eader: 
  seq: 2
  stamp: 
    secs: 1499474460
    nsecs:  70007426
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1499474441
      nsecs:  69487195
    id: /myclient-1-1499474441.69487195
  status: 3
  text: ''
result: 
  sequence: [0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 233, 377, 610, 987, 1597, 2584, 4181, 6765]
---

接下里使用goal的回调函数
使用平均数action服务器
然后现在代码还是有点问题的。但是一些概念我想记录在这里
创造一个简单的actionclient

 actionlib::SimpleActionClient::AverageAction> ac("ac_name");

之后要等待服务器

ac.waitForServer();

然后你要实例化你当时创建的actiongoal里面的东西，并且通过sendGoal进行发送

mypackage::AverageActionGoal goal;
    ac.sendGoal(goal);

    bool finished_before_timeout=ac.waitForResult(ros::Duration(30.0));
    if(finished_before_timeout)
    {
        actionlib::SimpleClientGoalState state=ac.getState();
        ROS_INFO("Action finished %s",state.toString().c_str());
    }
    else
        ROS_INFO("action did not finishe before time out");

这里是创造一个时延，30秒，通过actionlib：：simpleClientGoalState 来判断state的状态。

进行关闭节点

ros::shutdown()

使用boost::thread来创建多线程
第一种方法：

#include<iostream>
#include<boost/thread/thread.hpp>
void test()
{
std::cout<<"dfdf"<<std::endl;
}
int main()
{
    boost::thread my_thread(&hello);
    my_thread.join();
    return 0;
}

第二种方式，写在类里面

#include 
#include 
class my_class
{
   public:
   static void hello()
   {
    std::cout<<"hello"<<std::endl;
    }
    static void start()
    {
    boost::thread my_thread(hello);
     my_thread.join();//结束线程
    }
};
int main()
{
myclass::start();
return 0;
}

因为这里使用的static,所以可以这么写，这样写的前提的是，start 和 hello 都是static方法。
如果start和hello不是static修饰的话

#include 
#include 
#include 
class my_class
{
   public:
     void hello()
     {
     std::cout<<"dfdf"<::endl;
    }
    void start()
    {
    boost::function0 f=boost::bind(&myclass::hello,this);
    boost::thread thrd(f);
    thrd.join();
    }
};
int main()
{
myclass hello;
hello.start();
return 0;
}

相比与上一种方式，只是多了一个boos::function0类型的f 和boost::bind进行参数的绑定。

---

pluginlib
接下来开始学习插件
表示动态的插件工具
注意一点是，在进行这些地方的时候，一定要source一下你的工作空间。
首先创建pacakage

catkin_create_pkg mypackage_pluginlib roscpp pluginlib

首先分成4个部分：导出插件，描述插件，注册插件，使用插件
我们创建的一个插件的 基类，（也就是借口类）所有的定义都是从这个基类当中去继承
1、创建基类

#ifndef PLUGINLIB_TUTORIALS__POLYGON_BASE_H_
#define PLUGINLIB_TUTORIALS__POLYGON_BASE_H_

namespace mynamespacebase
{
  class myclassbase
  {
    public:
      virtual void initialize(double side_length) = 0;
      virtual double area() = 0;
      virtual ~ myclassbase(){}

    protected:
       myclassbase(){}
  };
};
#endif

2、从基类中继承，然后写插件的定义

#ifndef PLUGINLIB_TUTORIALS_POLYGON_PLUGINS_H_
#define PLUGINLIB_TUTORIALS_POLYGON_PLUGINS_H_

#include 
#include 

namespace mynamespaceuse 
{
  class Triangle : public mynamespacebase::myclassbase
  {
    public:
      Triangle() : side_length_() {}

      void initialize(double side_length)
      {
        side_length_ = side_length;
      }

      double area()
      {
        return 0.5 * side_length_ * getHeight();
      }

      double getHeight()
      {
        return sqrt((side_length_ * side_length_) - ((side_length_ / 2) * (side_length_ / 2)));
      }

    private:
      double side_length_;
  };

  class Square : public mynamespacebase::myclassbase
  {
    public:
      Square() : side_length_() {}

      void initialize(double side_length)
      {
        side_length_ = side_length;
      }

      double area()
      {
        return side_length_ * side_length_;
      }

    private:
      double side_length_;

  };
};
#endif

3、注册插件
( 现在来看可能是从路径的问题)

#include 
#include 
#include 

PLUGINLIB_EXPORT_CLASS(mynamespaceuse::Triangle,mynamespacebase::myclassbase);
PLUGINLIB_EXPORT_CLASS(mynamespaceuse::Square,mynamespacebase::myclassbase);

#include 

包含pluginlib的宏定义，以及作为插件的类。

PLUGINLIB_EXPORT_CLASS(mynamespaceuse::Square,mynamespacebase::myclassbase);

注册作为插件，这里面第一个参数是 插件类， 第二个参数是 这个插件的基类
这个代码还是有点问题的，原因我还是i没有找到，但是对于制作插件的基本方法我是清楚了，我吧这里里面的代码上传到我的github上面，你们如果知道怎么改的话，欢迎在博客下面留言
使用插件

#include 

#include 
#include 

int main(int argc, char** argv)
{
  pluginlib::ClassLoader::myclassbase> poly_loader("mypackage_pluginlib", "mynamespacebase::myclassbase");

  try
  {
    boost::shared_ptr::myclassbase> triangle = poly_loader.createInstance("mynamespaceuse::Triangle");//这里实际上写的上面的路径
    triangle->initialize(10.0);

    ROS_INFO("Triangle area: %.2f", triangle->area());
  }
  catch(pluginlib::PluginlibException& ex)
  {
    ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
  }

  try
  {
    boost::shared_ptr::myclassbase> square = poly_loader.createInstance("mynamespaceuse::Square");
    square->initialize(10.0);

    ROS_INFO("Square area: %.2f", square->area());
  }
  catch(pluginlib::PluginlibException& ex)
  {
    ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
  }

  return 0;
}

pluginlib::ClassLoader::myclassbase> poly_loader("mypackage_pluginlib", "mynamespacebase::myclassbase");

用于载入插件，第一个参数的是packagename 第二个参数是 基类

---

nodelet

然后下面我们进行nodelet的学习
nodelet，用来进行算法之间多种运行

nodelet usage:
nodelet load pkg/Type manager - Launch a nodelet of type pkg/Type on manager manager
nodelet standalone pkg/Type   - Launch a nodelet of type pkg/Type in a standalone node
nodelet unload name manager   - Unload a nodelet a nodelet by name from manager
nodelet manager               - Launch a nodelet manager node

可见nodelet是在launch文件当中进行的。
现在我就先跳过，这里的nodelet技术可以用于底盘的控制器。我不知道将来要看多少代码，才能写出这种水平.

---

关于navigation

ROS通过发布odometry信息。来将odom到base_link之间完成坐标变换。
导航的包，通过TF来确定机器人在世界中的位置。并且将传感器的数据与静态地图相互关联
在nav_msgs/Odometry当中

Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

(pose)对应于机器人在世界坐标系中的估计位置
速度(twist)对应于机器人在子坐标系的速度，通常是移动基站的坐标系，

#include 
#include 

添加必要的头文件
用来发布数据

ros::Publisher odom_pub = n.advertise("odom", 50);
 nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;

    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;



odom_pub.publish(odom);

从yaw当中创建四元数，并且就爱嗯这个四元数发布出去

  //从欧拉角当中创建四元数
        geometry_msgs::Quaternion odom_quat=tf::createQuaternionFromYaw(theta);
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp=current_time;
        odom_trans.header.frame_id="odom";
        odom_trans.child_frame_id="base_link";
        //然后将最后的量给了正确的东西，然后在这这些数值发布出来
        odom_trans.transform.translation.x=x;
        odom_trans.transform.translation.y=y;
        odom_trans.transform.translation.z=0;
        odom_trans.transform.rotation=odom_quat;
        //发布这个坐标变换
        odom_broadcaster.sendTransform(odom_trans);

通过创建current的东西来将TF发布出去
关于TF。既然只要我们知道激光雷达，那么通过TF变换树，就可以知道小车到障碍物的距离

任何一个TF之间的位置信息。

    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));

TransformBroadcaster发送转换需要五个参数。
功能是：创建了了一个在ROS 上发布base_laser → base_link转换的节点。
最后为路径规划做插件
为ROS添加一个新的全局路径规划器，新的路径规划器必须遵守在nav_core包中定义的nav_core::BaseGlobalPlanner C ++接口。一旦编写了全局路径规划器，它必须作为插件添加到ROS中，以便它可以被move_base包使用。
到这里，我觉得差不多了，是时候开始movebase的代码了。

后记：2017-09-04
建立自己的msg.

string state
float32 x
float32 y

在cpp当中导入自己的msg

#include<你的package的名字/msg的名字>

在使用的时候

   ros::NodeHandle nh;
  //实例化自定义msg
   你的package的名字：：msg的名字 msg;
   ros::Publisher pub_;
   pub_ = nh.advertise<你的package的名字：：msg的名字>("话题名称", 缓冲区大小建议是10);
   //对你的msg进行赋值
   这边用的是对成员函数进行赋值
   msg.state=state.c_str();
   //将msg发布出去
   pub_.publish(msg);

在接受方

  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("话题名称", 1000, 设置你的回调函数);
void 回调函数( const 你的package的名字：：msg的名字::ConstPtr &msg)
{  
这里记得是用->,因为这边是用的指针
   对msg进行处理，如何处理都可以
    ROS_INFO("Listener: GPS: distance = %f, state: %s",distance,msg->state.c_str());
}
对于string类型输出的时候，一定要加上.c_str()

对与python 文件的使用

from 你的package名字.msg import 你的msg名称
 pub = rospy.Publisher('chatter', 你的msg名称 , queue_size=10)
 pub.publish(你的msg的名字(state,x,y))

由于python没有实例化对象，因此发布的比较简单，建议参看链接
在接收方

   rospy.Subscriber('你的话题的名称', 你定义的数据类型, 定义的回调函数)
   def 回调函数：
   这边你就可以都用成员函数进行数据的计算。
   distance=math.sqrt(math.pow(gps.x-1,2)+math.pow(gps.y-2,2)) 
     rospy.loginfo('Listener: GPS: distance=%f, state: %s',distance,gps.state)

在自己定义的msg的话，cpp版本需要在最后，添加add_dependencies，否则自己添加的msg的头文件无法找到

add_executable(listener src/listener.cpp )
add_dependencies(listener topic_demo_generate_messages_cpp)
target_link_libraries(listener ${catkin_LIBRARIES})

---

ROS 当中四元数和欧拉角之间的转换
1、从欧拉角到四元数
公式：

对应的ROS代码：
核心的部分就是：geometry_msgs::Quaternion q;然后对q进行赋值值q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);通过q.w来向终端输入四元数中实部w的数值。要包含头文件tf/tf.h

 ros::init(argc, argv, "Euler2Quaternion");
  ros::NodeHandle node;
  geometry_msgs::Quaternion q;
  double roll,pitch,yaw;
  while(ros::ok())
  {
  //输入一个相对原点的位置
  std::cout<<"输入的欧拉角：roll,pitch,yaw:";
  std::cin>>roll>>pitch>>yaw;
  //输入欧拉角，转化成四元数在终端输出
  q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
  //ROS_INFO("输出的四元数为：w=%d,x=%d,y=%d,z=%d"，q.w,q.x,q.y,q.z);
  std::cout<<"输出的四元数为：w="<

2、从四元数到欧拉角
公式：

ROS的代码：
包含头文件nav_msgs/Odometry.h通过实例对象nav_msgs::Odometry position;来存储位置和方向的信息
然后通过tf::quaternionMsgToTF(position.pose.pose.orientation,RQ2); tf::Matrix3x3(RQ2).getRPY(roll,pitch,yaw);和将位置信息给了RQ2，通过RQ2.setRPY(roll,pitch,yaw)将数值给了roll，pitch,yaw值

  nav_msgs::Odometry position;
  tf::Quaternion RQ2;  
  double roll,pitch,yaw;
  while(ros::ok())
  {
  //输入一个相对原点的位置
  std::cout<<"输入的四元数：w,x,y,z:";
  std::cin>>position.pose.pose.orientation.w>>position.pose.pose.orientation.x>>position.pose.pose.orientation.y>>position.pose.pose.orientation.z;
  //输入四元数，转化成欧拉角数在终端输出
  tf::quaternionMsgToTF(position.pose.pose.orientation,RQ2);  
  tf::Matrix3x3(RQ2).getRPY(roll,pitch,yaw);  
  std::cout<<"输出的欧拉角为：roll="<

ROS发布TF变换
首先实例化static tf::TransformBroadcaster br;和TF变换tf::Transform transform;以及四元数tf::Quaternion q;然后输入RPY和xyz的数值，通过q.setRPY(roll,pitch,yaw);将RPY转化成四元数，然后通过transform.setOrigin(tf::Vector3(x,y,z));设置相对于原点的位置。通过transform.setRotation(q);来设置旋转，最后通过br发布变换br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"base_link","link1"));

  static tf::TransformBroadcaster br;
  tf::Transform transform;
  tf::Quaternion q;
  double roll,pitch,yaw,x,y,z;
  while(ros::ok())
  {
  //输入一个相对原点的位置
  std::cout<<"输入相对于原点的位置：";
  std::cin>>x>>y>>z;
  std::cout<<"输入的欧拉角：roll,pitch,yaw:";
  std::cin>>roll>>pitch>>yaw;
  //输入欧拉角，转化成四元数在终端输出
  q.setRPY(roll,pitch,yaw);
  transform.setOrigin(tf::Vector3(x,y,z));
  transform.setRotation(q);
 br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"base_link","link1"));
  ros::spinOnce();
  }

ROS监听TF变换
包含头文件tf/transform_listener.h和geometry_msgs/Twist.h然后实例化tf::TransformListener listener;然后必须在try 和catch当中监听 坐标点的位置点坐标存放在是transform.getOrigin().x()和旋转的信息存放在transform.getRotation().getW()

  tf::TransformListener listener;
  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
      listener.lookupTransform("/base_link", "/link1",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
    std::cout<<"输出的位置坐标：x="<