定义模板类时，函数声明和实现都必须放在头文件里，否则编译失败，准确地讲，是在链接阶段报错．原因在于编译器虽然也编译了包含模板定义的源码文件，但是该文件仅仅是模板的定义，而并没有真正的实例化出具体的函数来。因此在链接阶段，编译器进行符号查找时，发现源码文件中的符号，在所有二进制文件中都找不到相关的定义，因此就报错了。
不过，函数的实现可以放在头文件的类外，不一定必须在类体内．

参考：
C++模板编译模型

new运算是先分配内存再执行构造函数，delete是先执行析构函数再释放内存。

STL allocator将这些操作做了精密分工：内存配置有alloc::allocate()负责，内存释放由alloc::deallocate()负责； 对象构造由::construct()负责，对象析构由::destroy()负责。

STL采用了两级配置器，当分配的空间大小超过128字节时，会使用第一级空间配置器，直接使用malloc()、realloc()、free()函数进行内存空间的分配和释放；当分配的空间小于128字节时，为减少申请小内存造成的内存碎片和额外负担问题，将使用第二级空间配置器，它采用了内存池技术，通过16个free-list来配置和回收内存，free-list对内存的需求量按8的倍数处理，也就是16个free-list分别管理8,16,24……128字节的内存区块。

C++所有的标准容器类都接受一个allocator类作为其模板参数；这个参数有一个默认值，比如std::vector是 vector >的简写。

1
2
3
4

template <class T, class Alloc = alloc>  // 预设使用 alloc 为配置器
class vector {

}

1
2

std::string s="123";
printf("%s",s);

结果报错： cannot pass non-trivial object of type ‘std::string’ (aka ‘basic_string, allocator >’) to variadic function; expected type from format string was char*

也有可能不报错但是显示不正常，这跟编译器有关。

在main函数中访问回调函数中处理的变量

做法是让类的成员函数做回调函数，在回调函数中处理相应变量，在main函数中实例化类，然后通过返回对象的成员进行访问

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

class Listener 
{
public:
    double theta1f;
    double theta2f;
    double theta3f;

    void callback(const std_msgs::Float32MultiArray::ConstPtr& msg);
};

void Listener::callback(const std_msgs::Float32MultiArray::ConstPtr& msg) 
{
    theta1f = msg->data[0];
    theta2f = msg->data[1];
    theta3f = msg->data[2];
}

int main(int argc, char **argv) 
{
    ros::init(argc, argv, "node");
    ros::NodeHandle nh;
    ros::Rate loop_rate(30);
    Listener listener;
    ros::Subscriber sub = nh.subscribe<std_msgs::float32multiarray>("topic_subscribed", 1, &Listener::callback, &listener);
    while (ros::ok())
    {
        ros::spinOnce();
        ROS_INFO("This is theta1f: %.2f", listener.theta1f);
        ROS_INFO("This is theta2f: %.2f", listener.theta2f);
        ROS_INFO("This is theta2f: %.2f", listener.theta2f);
        loop_rate.sleep();
    }

    return 0;
}

回调函数中访问main函数中的变量赋值

以前我都是用全局变量来实现，但这种方法总感觉不保险，后来发现ROS中有专门的解决方法，NodeHandle::subscribe有一个重载可以处理这种情况，用到了boost::bind，其实用std::bind也能实现。代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

#include <ros/ros.h>
#include <std_msgs/Int8.h>
//回调函数里访问main中的一个结构体
typedef struct _Struct
{
  int        m;
  double     n;
} myStruct;

void callback(const std_msgs::Int8ConstPtr& msg, myStruct p)
{
  ROS_INFO("msg: %d", msg->data);
  ROS_INFO("m: %d",p.m);
  ROS_INFO("n: %f",p.n);
  sleep(1);
}


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

  myStruct p;
  p.m = 7;
  p.n = 1.85;

  // "_1" 是占位符，对应 void callback(const std_msgs::Int8ConstPtr& msg, myStruct p)第一个参数
  /*  subscribe (
   * const std::string &base_topic,
   * uint32_t queue_size,
   * const boost::function< void(const std_msgs::Int8ConstPtr &)>   &callback  )
   */
  ros::Subscriber sub = nh.subscribe<std_msgs::Int8>("topic", 20, boost::bind(&callback, _1, p) );
  ros::spin();
  return 0;
}

myStruct是我自定义的结构体，在main函数里对其声明的变量赋值，回调的形参里多了它的变量。
其中subscribe函数的原型是这个：

1
2
3
4
5
6
7

template<class M >
Subscriber ros::NodeHandle::subscribe(const std::string& topic,
	uint32_t  	queue_size,
	const boost::function< void(const boost::shared_ptr< M const > &)> &  	callback,
	const VoidConstPtr &  	tracked_object = VoidConstPtr(),
	const TransportHints &  	transport_hints = TransportHints() 
) 	

与平时的重载形式不同，这次要用到话题的模板类型，第三个形参不是回调函数的指针，而是boost::bind，其中_1是一个占位符，要传给回调函数参数，在发生函数调用时才接受实参；最后是结构体的变量。

注意：回调函数第一个形参必须是const std_msgs::Int8ConstPtr&形式，其他形式比如const std_msgs::Int8Ptr&会报错，另外看到一篇博客说这里不能用引用，但是我用引用也没报错。对于回调函数callback，一般在bind里不用&，但是用了也可以，因为bind可接受函数或函数指针

参考：
How to make callback function called by several subscriber?
How to pass arguments to/from subscriber callback functions

程序演示

下面是我写的一个服务程序，服务文件ctrl.srv的构成为：

1
2
3

int8 cmd
---
bool ret

注意：最好是将所有自定义的服务和消息文件放到一个单独的package里面，否则会出现一个package的修改会影响到另一个用到它的消息/服务的package的编译．

新建srv文件后，容易忘记在CMakeLists里添加这个文件，导致编译失败

服务端

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

#include <ros/ros.h>
#include <riki_msgs/ctrl.h>

bool control(riki_msgs::ctrl::Request &req, riki_msgs::ctrl::Response &res)
{
    if(req.cmd==0)
    {
        ROS_INFO("shutting down camera");
        system("rosnode kill /usb_cam");
        ROS_INFO("close camera done");
        res.ret = 0;
    }
    else
    {
        ROS_INFO("starting camera");
        system("roslaunch usb_cam usb_cam.launch & ");
        ROS_INFO("camera is up");
        res.ret = 1;
    }
}

int main(int argc, char** argv)
{
    ros::init(argc,argv,"camServer");
    ros::NodeHandle nh;
    ros::ServiceServer server = nh.advertiseService("control_cam",control);
    ROS_INFO("------ waiting for client's request ------");
    ros::spin();
    return 0;
}

可以看出服务端的程序与话题中的订阅者程序高度相似，control函数就是个回调函数。

system函数调用roslaunch时，由于是在fork出的子进程里执行，launch的节点会一直阻塞不返回，在命令最后加&，让子进程返回

service回调函数里只能return true或false，若return整数会导致客户端的call结果为false，但实际成功，这样会影响判断。

service客户端发命令后，出现报错 ERROR: service [/service_name] responded with an error: b’’ ，原因在于service服务端的回调函数必须 return true

advertiseService的部分源码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

bool ServiceManager::advertiseService(const AdvertiseServiceOptions& ops)
{
   boost::recursive_mutex::scoped_lock shutdown_lock(shutting_down_mutex_);
   if (shutting_down_)
   {
     return false;
   }
   {
     boost::mutex::scoped_lock lock(service_publications_mutex_);
     // 如果service已经发布，就报错，然后返回，其实这里改成报警比较合适
     if (isServiceAdvertised(ops.service))
     {
       ROS_ERROR("Tried to advertise a service that is already advertised in this node [%s]", ops.service.c_str());
       return false;
     }

如果涉及到耗时的工作，回调函数应该这样写

1
2
3
4
5
6

bool serviceCB(mow_msgs::task::Request &req, mow_msgs::task::Response &res)
{
  // ......
  res.ret = "to charge";
  // 耗时的工作
}

客户端

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

#include <ros/ros.h>
#include <riki_msgs/ctrl.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "controlCam");
    if(argc!=2)
    {
        ROS_INFO("client need command");
        return 1;
    }
    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient<riki_msgs::ctrl>("control_cam");
    riki_msgs::ctrl srv;
    srv.request.cmd = atoi(argv[1]);
    if(client.exists())
    {
        ROS_INFO("service control_cam is up");
        ROS_INFO("service name:%s",client.getService().c_str());
    }
    else
    {
        ROS_ERROR("service control_cam is not available");
        return 1;
    }
    if(client.call(srv))
    {
        ROS_INFO("client calling srv !");
        if(srv.request.cmd>0)
            ROS_INFO("Sending command start!");
        else
            ROS_INFO("Sending command stop!");
    }
    else
    {
        ROS_ERROR("client calls srv failed !");
        return 1;
    }
    // 调用成功以后才能获取response
    cout<<"服务端的返回值: "<<srv.response.ret<<endl;
    return 0;
}

构建client的时候后面的路径要写绝对路径，有时候需要加个 /

客户端的程序与话题发布者的程序高度相似，exists()检查服务端的服务是否启动，若未启动则返回。call调用了服务，成功会返回true，而且调用完成后，可以在客户端程序里获得服务端的返回值

最后，先运行服务端，然后运行客户端，如果是rosrun control_cam controlCam 1则在服务端所在终端启动摄像头程序，若是0则关闭。

另一种使用方式

1
2
3
4
5
6
7
8
9

std_srvs::Empty srv;
ros::service::call("/move_base_node/clear_costmaps", srv);

while(!ros::service::call("static_map", req, resp))
{
    ROS_WARN("Request for map failed; trying again...");
    ros::Duration d(0.5);
    d.sleep();
}

常用函数

ros::ServiceClient常用函数：

1
2
3
4
5
6
7
8
9

bool call (Service &service)  //调用service，client发起通信。成功返回true

bool exists ()  //检查相应名称的服务是否可用

std::string getService ()  //返回客户端通信的服务名称
// 单例模式
static const ServiceManagerPtr &  instance ()

bool    unadvertiseService (const std::string &serv_name)

问题

切换算法时，会有个报警 Tried to advertise a service that is already advertised in this node。这个其实无任何影响，报警在ROS底层的源码，原因在于GlobalPlanner::initialize函数有一句:

1

make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

1
2
3
4

std::string makePlanServiceName = "/move_base/"+name+"/make_plan";

ros::ServiceManager::instance()->unadvertiseService(makePlanServiceName);
make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

md5不匹配

调用service时报错md5不匹配，其实是调用失败了，call的返回值是false，首先应检查客户端定义是否正确和服务是否存在:

客户端一直阻塞

我在move_base.cpp里写了一个函数 func，是rosservice的客户端，调用service clear_costmaps。在MoveBase的其他函数里，调用这个函数，结果发现会一直阻塞。后来注意到服务端程序就是MoveBase::clearCostmapsService，也就是在同一个类里

客户端call service阻塞的原因只有两个：

1. 多个客户端call 同一个service，而服务端一次只能处理一个请求
2. 服务端程序没有return值

服务端程序是MoveBase的，而且有了返回值，那么只可能是第一个原因了。

参考：blocking service callbacks

无法进入图形界面

![https://live.staticflickr.com/7870/46689319155_bc1d198671_z.jpg]
![https://live.staticflickr.com/7892/33727819068_7e69d575f5.jpg]

1.Ctrl+ALt+F1（F1~F5都可以）进入控制台

2.输入用户名和密码进入系统；

3.依次输入以下命令

1
2
3
4
5

cd /etc/X11

sudo cp xorg.conf.failsafe xorg.conf

sudo reboot

开机启动VMWare并启动指定系统

将VMWare的快捷方式放到C:\ProgramData\Microsoft\Windows\Start Menu\Programs\StartUp，然后打开快捷方式，修改目标：

改成

1

"....exe"  -x  "/path_to_system/Ubuntu 64.vmx"

如何让虚拟机网段与主机相同

先用管理员身份打开VMWare，再打开编辑-虚拟网络编辑器，将网络做如图设置：桥接模式，并选择网卡为主机真实网卡。这样进入虚拟机后，IP就同主机了，设置成DHCP即可。

无法打开虚拟机，提示要获得所有权，但还是失败

很简单，删除虚拟机文件夹下的lck文件夹即可，映像被lck锁定了。应当是上次没有正常关闭虚拟机导致的

硬盘空间太少

向虚拟机复制文件就是先在cache文件夹里面生成一个同样的文件，并使用拷贝的方式将其拷贝到拖拽放置的目录中。因此，如果不进行清理的话，cache文件夹中产生的文件，并不会自动删除或者释放。

该文件夹位于用户目录下/home/xxxx/.cache/vmware/drag_and_drop进入文件夹，可以见到每一次拖拽产生的文件，都在子文件夹中有一份。直接删除便可以腾出海量的空间。

vmware tool安装后仍然不能复制的问题

1
2
3
4

sudo apt-get upgrade
sudo  mkdir /mnt/cdrom 
sudo apt-get install open-vm-tools-desktop -y
sudo reboot

VM虚拟机增加磁盘空间

VM虚拟机增加磁盘空间

VMWare 分辨率设置

参考链接

服务和话题

rosservice call

rosservice call addTwoNum 3 4: 调用服务，addTwoNum是服务名称， 名称不加引号，3和4是服务中的req变量的值，注意必须按服务文件中的顺序赋值。

但上面是简单情况，srv文件稍微有点复杂时，就会怎么填也不对，个人认为这是ROS Service很失败的一个地方。
例如srv文件的请求部分是这样的：

1
2

string request_string
string album

1
2

rosservice call service_name "request_string: ''
album: ''"

call执行成功时，终端不会有任何结果。但是经常出现这样的错误： ERROR: service [/control_cam] responded with an error: ，但程序执行没有问题。原因是 程序中对srv文件中的应答没有处理

其他常用命令：

• rosservice list: 显示当前所有活动的服务
• rosservice info addTwoNum: 显示服务的信息
• rosservice type addTwoNum: 显示服务的变量类型

其实服务的相关命令和话题的很类似

ROS的服务类型

启动乌龟节点时，有一个服务叫clear，类型：

1
2
3

rosservice type clear

std_srvs/Empty

std_srvs包还有两个服务：std_srvs/Trigger和std_srvs/SetBool，但是很不常用

• locate的速度比find快，因为它并不是真的查找文件，而是查数据库。

• 新建的文件，我们立即用locate命令去查找，一般是找不到的，因为数据库的更新不是实时的，而是每天

• locate命令所搜索的后台数据库在/var/lib/mlocate这个目录下，可能有些Linux系统位置不同，具体我们可以用locate locate查询

• 并不是所有的目录下的文件都会用locate命令搜索到，/etc/updatedb.conf这个配置文件中，配置了一些locate命令的一些规则。

• updatedb会大致每天运行，这是靠系统的crontab命令实现的

• updatedb -U：更新指定目录相关的数据库信息。默认是整个系统，耗时比较长，因此可以使用该参数，比如sudo updatedb -U /home/user/

updatedb的配置文件 /etc/updatedb.conf

1
2
3
4
5

cat /etc/updatedb.conf 
PRUNE_BIND_MOUNTS = "yes"
PRUNEFS = "9p afs anon_inodefs auto autofs bdev binfmt_misc cgroup cifs coda configfs cpuset debugfs devpts ecryptfs exofs fuse fuse.sshfs fusectl gfs gfs2 gpfs hugetlbfs inotifyfs iso9660 jffs2 lustre mqueue ncpfs nfs nfs4 nfsd pipefs proc ramfs rootfs rpc_pipefs securityfs selinuxfs sfs sockfs sysfs tmpfs ubifs udf usbfs fuse.glusterfs ceph fuse.ceph"
PRUNENAMES = ".git .hg .svn"
PRUNEPATHS = "/afs /media /mnt /net /sfs /tmp /udev /var/cache/ccache /var/lib/yum/yumdb /var/spool/cups /var/spool/squid /var/tmp /var/lib/ceph"

• PRUNENAMES 搜索时不搜索的文件类型
• PRUNEPATHS 搜索时不搜索的路径
• PRUNE_BIND_MOUNTS = "yes" 开启搜索限制
• PRUNEFS 搜索时不搜索的文件系统

locate 常用命令

1
2
3
4
5
6
7

locate -c   # 查询指定文件的数目。(c为count的意思)
locate -e   # 只显示当前存在的文件条目。(e为existing的意思)
locate -i   # 查找时忽略大小写区别

# 使用正则表达式查找文件
locate -r makefile$		# 以makefile结尾的文件
locate -r ^/home/user/		# 以/home/user/开头的文件		

从结果中取出词尾是config2的

注意：locate的结果可能是不存在的文件，这时最好用locate -e

locate 查找文件tree.xml，也就是知道完整的文件名时，那么最好用 locate /tree.xml，如果不加/，会显示test_tree.xml的结果

find 常用命令

查找当前目录中(包括子目录)所有扩展名为cfg的文件:

1
2

# 或者 '*.cfg'
find -name *.cfg

• find . -name '*.cpp' -mmin -30 当前目录下，最近30分钟修改的cpp文件

• find . -name '*.cpp' -mtime 0 当前目录下，最近24小时修改的cpp文件

• find . -type f -mtime 0 当前目录下，最近24小时修改的常规文件

ROS启动乌龟追乌龟程序用的是roslaunch turtle_tf turtle_tf_demo.launch，其内容如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

<launch>

  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
  <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
  <!-- Axes -->
  <param name="scale_linear" value="2" type="double"/>
  <param name="scale_angular" value="2" type="double"/>
  <!-- 针对两个乌龟的tf广播 -->
  <node name="turtle1_tf_broadcaster" pkg="turtle_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle1" />
  </node>
  <node name="turtle2_tf_broadcaster" pkg="turtle_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle2" />
  </node>

  <node name="turtle_pointer" pkg="turtle_tf" type="turtle_tf_listener.py" respawn="false" output="screen" >
  </node>

</launch>

乌龟的程序,按下方向键,乌龟只走一段距离,是在cmd_vel的回调函数里更新乌龟的位置,取决于发布的twist消息和更新间隔dt,由Qt实现

tf包处理的是任一个点在所有坐标系之间的坐标变换问题，它把各种转换关系建立在一个树结构上，树的每个节点是坐标系，每个坐标系可以有多个child，但只能有一个parent，转换只能是从parent向child。比如Tb-a表示坐标系a向b转换，也就是说a是parent，b是child，这个变换描述的就是child坐标系中的点在parent坐标系下的姿态。要实现这个变换，就是用child坐标系在parent坐标系下的描述（一个矩阵）去描述（乘以）这个点在child坐标系下的描述（坐标）。world参考系是tf树最顶端的父参考系

如果打算用tf解决你的坐标变换问题，请一定要先清晰的画出这棵树的结构，再开始写程序。比较重要的类是tf::TransformBroadcaster, tf::TransformListener, tf::Transform, tf::StampedTransform

在tf的运行机制中，由于tf会把监听到的内容放到一个缓存中。我们通过transformPose获取变换关系，是通过查询这个缓存来实现的。获取的数据不能保证实时性，会有一定的延迟。也有可能无法获得，因此这个函数在运行过程中会抛出异常，所以这里使用try-catch语句捕获这个异常并返回。

tf::TransformBroadcaster类

sendtransform接口可以建立tf树，发布一个从已有的父坐标系到新的子坐标系的变换时，这棵树就会添加一个树枝，之后就是维护。TransformBroadcaster类就是一个publisher, 如果两个frame之间发生了相对运动，TransformBroadcaster类就会发布TransformStamped消息到tf话题，当多个节点向tf话题发消息时，就形成了tf树。

tf::Transform

建立坐标系之间的位移和旋转的关系，最后用于sendTransform函数。

它是一个坐标转换。成员有：Matrix3x3 m_basis，用3*3的矩阵表示旋转； Vector3 m_origin，用3*1的向量表示平移。

tf::Transform支持乘法运算符，实际的计算是先把旋转矩阵和平移量组合为变换矩阵，变换矩阵相乘后，再转换为tf::Transform类型

tf::Transform类的重要函数如下：

1
2
3
4
5

Matrix3x3 &   getBasis ()    //Return the basis matrix for the rotation
const Vector3 & getOrigin ()  //Return the origin vector translation
Quaternion  getRotation ()  //Return a quaternion representing the rotation
Transform   operator* (const Transform &t) const  //Return the product of this transform and the other.
Transform   inverse ()    //Return the inverse of this transform

tf::StampedTransform类继承自tf::Transform，它多了两个重要变量就是child_frame_id_和frame_id_。

tf::TransformListener

监听一个父坐标系到子坐标系的变换，waitForTransform是监听转换关系，可以指定监听的时间或一直阻塞；lookupTransform紧随其后，获取 tf::Transform

使用前需要#include <tf/transform_listener.h>

TransformListener构造函数有两个，常用的是

1
2
3
4

TransformListener::TransformListener(
    ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME),  
    bool spin_thread = true 
)

参考我写的程序test_costmap。开始，如果没有map—->base_link的TF转换，则报错No Transform available Error。此时发布TF变换，则不再报错。然后再关闭TF变换，test_costmap还能正常运行10s，然后报错 Extrapolation Error

transformPose

原型是void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const，

target_frame就是你要把源pose转换成哪个frame上的pose。假如你的源pose的frame_id是”odom”，你想转到”map”上，那么target_frame写成“map”就可以了。stamped_in就是源pose，而stamped_out就是目标数据了，也就是转换完成的数据。需要注意的是，从参数上来看，转换时是不需要指定源frame_id的，这是因为它已经包含在了stamped_in中，换句话说，就是这个函数一个隐含的使用条件是，stamped_in中必须指明源pose属于哪个frame

把odom坐标系的数据转换到map坐标系下

1
2
3
4
5
6
7
8
9
10
11
12
13

geometry_msgs::PoseStamped pose_odom;
pose_odom.header = odom->header;
pose_odom.pose = odom->pose.pose;

geometry_msgs::PoseStamped pose_map;
try{
    listener.transformPose("map", pose_odom, pose_map);
}
catch( tf::TransformException ex)
{
    ROS_WARN("transfrom exception : %s",ex.what());
    return;
}

有时会出现这样的报错： transfrom exception : “map” passed to lookupTransform argument target_frame does not exist ，但是使用tf_echo发现是正常的。需要检查代码是不是在回调函数里运行了， 不需要在回调函数里创建TransformListener对象， 将它作为类成员变量或者全局变量。

全局变量是在main函数之前完成构造函数的，如果用到的类构造函数用到NodeHandle，就会报错。比如tf::TransformListener，解决方法是用全局指针，比如boost::shared_ptr<T>，然后在main函数的ros::init()之后指向一个对象。

参考：tf::TransformListener::transformPose [exception] target_frame does not exist