ROS标定摄像头|ROSROS机器人|摄像头
abstract Welcome to my blog, enter password to read.
Read more
解读NodeHandle（一）构造函数与析构函数|ROSROS程序和源码分析

ros::NodeHandle构造函数会调用ros::start(), 最后一个ros::NodeHandle销毁时，将调用ros::shutdown()。如果想自定义节点生存期，可以用这两个函数。

检查关闭节点的两种方法是ros::ok()和ros::isShuttingDown()

NodeHandle源码

NodeHandle类是非常重要的一个类，可以说是ROS程序的核心，发布、订阅就是它完成的，分析一下它的源码。

NodeHandle类中有几个私有成员变量:

1
2
3
4
5
std::string namespace_;
// 回调接口指针，主要用于advertise, subscribe,advertiseService和createTimer函数
CallbackQueueInterface* callback_queue_;

NodeHandleBackingCollection* collection_;

构造函数

NodeHandle的构造函数调用层级如下：

源码:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
NodeHandle::NodeHandle(const std::string& ns, const M_string& remappings)
  : namespace_(this_node::getNamespace())
  , callback_queue_(0)	// 空指针
  , collection_(0)		// 空指针
{
  std::string 　tilde_resolved_ns;
  if (!ns.empty() && ns[0] == '~')// starts with tilde
    tilde_resolved_ns = names::resolve(ns);
  else
    tilde_resolved_ns = ns;

  construct(tilde_resolved_ns, true);

  initRemappings(remappings);
}

getNamespace和ros::names::resolve

首先看初始化时的getNamespace，其原型是

1
2
const std::string & ros::this_node::getNamespace()	
{  return namespace_;  }    //Returns the namespace of the current node.

返回当前节点的namespace。比如节点初始化和启动是这样实现的:
1
2
ros::init(argc,argv,"locateTag");
ros::NodeHandle n("~node");

那么getNamespace()的结果就是/locateTag/node，但是节点名称有命名规范，否则编译正确但运行会报错:
1
2
3
terminate called after throwing an instance of 'ros::InvalidNameException'
  what():  Character [-] at element [6] is not valid in Graph Resource Name [health---Status].  Valid characters are a-z, A-Z, 0-9, / and _.
已放弃 (核心已转储)

接下来的names::resolve是对参数ns进行处理。如果ns不为空而且以~开头，使用resolve函数解析，还以上面的例子，解析结果仍然是/locateTag/node，否则直接赋给tilde_resolved_ns。resolve函数返回的叫做Graph Resource Names，这是ROS中的继承性命名系统，命名必须符合下面特征:

  1. 第一个字符只能是a-z|A-Z或者~ /
  2. 之后的字符是0-9|a-z|A-Z，_ /
  3. 基本名称不能有 / ~

上面说了这么多，其实在使用时，一般ns都是空，此时的getNamespace()和resolveName都返回/

construct函数

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
void NodeHandle::construct(const std::string& ns, bool validate_name)
{
  if (!ros::isInitialized())     // 若没有调用 ros::init，报错: 必须先init()然后创建NodeHandle
  {
    ROS_FATAL("You must call ros::init() before creating the first NodeHandle");
    ROS_BREAK();
  }
  collection_ = new NodeHandleBackingCollection;
  unresolved_namespace_ = ns;
  // if callback_queue_ is nonnull, we are in a non-nullary constructor

  if (validate_name)
    namespace_ = resolveName(ns, true);
  else
      namespace_ = resolveName(ns, true, no_validate());
    
  ok_ = true;
  boost::mutex::scoped_lock    lock(g_nh_refcount_mutex);
   //此参数为全局变量，初始为0
  if (g_nh_refcount == 0 && !ros::isStarted())
  {
    g_node_started_by_nh = true;
    ros::start();	// 启动ros
  }
  ++g_nh_refcount;
}

若此时没有调用ros::init，报严重错误然后终止，所以 ros::init()必须在NodeHandle创建之前。 有时先调用了ros::init也会出这个错误，很可能是在NodeHandle之前先用了ROS的东西

然后创建NodeHandleBackingCollection指针，下面是一些赋值，g_nh_refcount为初始为0的全局变量，即全局引用计数，如果此时没有启动ros，将调用ros::start()，然后将g_nh_refcount加１

ros::start()太复杂了，它是ROS架构的核心，在另一篇文章分析

initRemappings比较简单，而且不重要，就不分析了

析构函数

类的析构函数只调用了destruct():

1
2
3
4
5
6
7
8
9
10
11
12
13
void NodeHandle::destruct()
{
  delete collection_;

  boost::mutex::scoped_lock lock(g_nh_refcount_mutex);

  --g_nh_refcount;

  if (g_nh_refcount == 0 && g_node_started_by_nh)
  {
    ros::shutdown();
  }
}

析构函数更简单，释放collection_，引用计数减１，如果变成了０，就关闭ros


参考：
node_handle.cpp源码
ROS Names
node_handle.h源码

ROS编程常用函数|ROSROS程序和源码分析

接受中文注释

setlocale(LC_ALL, "");

ros::Time 和 double 转换

1
2
double x=ros::Time::now().toSec();     //把时间戳转化成浮点型格式
ros::Time stamp = ros::Time().fromSec(x);    //把浮点型变成时间戳

读取环境变量

跨平台函数 bool get_environment_variable (std::string &str, const char *environment_variable)

clamp函数

1
2
3
4
5
6
7
8
9
template<class T>
T clamp(T x, T min, T max)
{
    if (x > max)
        return max;
    if (x < min)
        return min;
    return x;
}
1
2
3
4
// 注意最后一个路径点，是 end() 的前一个点
geometry_msgs::PoseStamped  global_end = *(std::prev(global_plan_.end()) );
geometry_msgs::PoseStamped  base_end;
transformPose("base_link", global_end,  base_end);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
bool  transformPose(
    const std::string frame,
    geometry_msgs::PoseStamped &in_pose,
    geometry_msgs::PoseStamped &out_pose) const
{
    if (in_pose.header.frame_id == frame)
    {
        out_pose = in_pose;
        return true;
    }
    try
    {
        out_pose.header.frame_id = frame;
        tf_->transformPose(frame, in_pose, out_pose);
        out_pose.header.seq = in_pose.header.seq;
        return true;
    }
    catch (tf2::TransformException &ex)
    {
        ROS_ERROR("Exception in transformPose : %s", ex.what()  );
    }
    return false;
}

四元数转欧拉角yaw

1
2
3
4
5
6
7
8
9
10
11
12
13
14
double quaternionToYaw(const geometry_msgs::Quaternion&  quat)
{
    tf::Quaternion tf_quat;
    tf::quaternionMsgToTF(quat, tf_quat);
    double roll, pitch, yaw;
    tf::Matrix3x3(tf_quat).getRPY(roll, pitch, yaw);
    return yaw;
}

double quaternionToYaw(const geometry_msgs::Quaternion&  quat)
{
	tf::Quaternion quat(0,0, quat.z, quat.w);
	return tf::getYaw(quat);
}

欧拉角 —— 四元数

1
2
3
4
5
6
7
8
9
//只通过yaw计算四元数，用于平面小车
static geometry_msgs::Quaternion  tf::createQuaternionMsgFromYaw(double yaw);

static tf::Quaternion  createQuaternionFromYaw (double yaw)

static tf::Quaternion  createQuaternionFromRPY(double roll, double pitch, double yaw)

static geometry_msgs::Quaternion  createQuaternionMsgFromRollPitchYaw( double roll, 
    double pitch,   double yaw)

获得当前节点名称

ros::this_node::getName()

bool ros::master::getNodes (V_string &nodes) 是获得当前所有运行的节点，效果和rosnode list一样。但这里有个缺陷，使用pkill杀死的节点，在rosnode list里仍然能看到，实际已经退出了。

规范角度 [-π, π)

调用g2o::normalize_theta(double theta)或者

1
2
3
4
5
6
7
void normAngle ( double& angle )
{
    if( angle >= M_PI)
        angle -= 2 * M_PI;
    if( angle < -M_PI)
        angle += 2 * M_PI;
}

判断机器人是否静止

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
bool robotIsStill()
{
    bool velStatus;
    boost::shared_ptr<nav_msgs::Odometry const> twistEdge;
    twistEdge = ros::topic::waitForMessage<nav_msgs::Odometry>("odom",ros::Duration(0.3) );
    if(twistEdge != NULL)
    {
        double velX = twistEdge->twist.twist.linear.x;
        double angularZ = twistEdge->twist.twist.angular.z;
        if(velX == 0 && angularZ == 0)
            velStatus = true;
        else
            velStatus = false;
    }
    else velStatus = false;
    return velStatus;
}

移动一定时间，也可以表示移动一定距离

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = -0.12;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;

auto t1 = std::chrono::steady_clock::now();
auto t2 = std::chrono::steady_clock::now();
ROS_INFO(" -------------- backward begin ! ------------------ ");
while( std::chrono::duration<double>(t2-t1).count() < 3 )
{
    vel_pub.publish(cmd_vel);
    t2 = std::chrono::steady_clock::now();
}
cmd_vel.linear.x = 0;
vel_pub.publish(cmd_vel);
ROS_INFO(" -------------- backward done, now continue !  ------------------ ");

获取当前速度

1
2
3
4
5
6
7
8
tf::Stamped<tf::Pose> robot_vel_tf;
base_local_planner::OdometryHelperRos odom_helper_;
odom_helper_.getRobotVel(robot_vel_tf);

geometry_msgs::Twist  robot_vel_;
robot_vel_.linear.x = robot_vel_tf.getOrigin().getX();
robot_vel_.linear.y = robot_vel_tf.getOrigin().getY();
robot_vel_.angular.z = tf::getYaw(robot_vel_tf.getRotation());

poseStampedMsgToTF

1
2
3
4
5
6
7
8
//get the starting pose of the robot
tf::Stamped<tf::Pose> global_pose;
if(!planner_costmap_ros_->getRobotPose(global_pose)) {
  ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
  return false;
}
geometry_msgs::PoseStamped start_pose;
tf::poseStampedTFToMsg(global_pose, start_pose);

还有类似的函数: poseTFToMsg 和 transformMsgToTF

angles包

  • static double from_degrees (double degrees): Convert degrees to radians

  • static double normalize_angle (double angle)

  • static double shortest_angular_distance (double from, double to)

  • static double to_degrees (double radians): Convert radians to degrees

  • static double two_pi_complement (double angle): returns the angle in [-2*M_PI, 2*M_PI] going the other way along the unit circl

tf 命名空间

tf的命名空间有很多有用的函数，先列出一部分:

  • TFSIMD_FORCE_INLINE tfScalar angle (const Quaternion &q1, const Quaternion &q2)

Return the half angle between two quaternions

  • TFSIMD_FORCE_INLINE tfScalar angleShortestPath (const Quaternion &q1, const Quaternion &q2)

Return the shortest angle between two quaternions

  • void assertQuaternionValid (const geometry_msgs::Quaternion &q)

Throw InvalidArgument if quaternion is malformed

  • void assertQuaternionValid (const tf::Quaternion &q)

Throw InvalidArgument if quaternion is malformed

  • TFSIMD_FORCE_INLINE Quaternion shortestArcQuat (const Vector3 &v0, const Vector3 &v1)

TFSIMD_FORCE_INLINE Quaternion shortestArcQuatNormalize2 (Vector3 &v0, Vector3 &v1)

  • TFSIMD_FORCE_INLINE Quaternion slerp (const Quaternion &q1, const Quaternion &q2, const tfScalar &t)

Return the result of spherical linear interpolation betwen two quaternions

  • std::string strip_leading_slash (const std::string &frame_name)

  • TFSIMD_FORCE_INLINE tfScalar tfAngle (const Vector3 &v1, const Vector3 &v2)

Return the angle between two vectors

  • TFSIMD_FORCE_INLINE Vector3 tfCross (const Vector3 &v1, const Vector3 &v2)

Return the cross product of two vectors

  • TFSIMD_FORCE_INLINE tfScalar tfDistance (const Vector3 &v1, const Vector3 &v2)

Return the distance between two vectors

  • TFSIMD_FORCE_INLINE tfScalar tfDistance2 (const Vector3 &v1, const Vector3 &v2)

Return the distance squared between two vectors

  • TFSIMD_FORCE_INLINE tfScalar tfDot (const Vector3 &v1, const Vector3 &v2)

Return the dot product between two vectors

goalToGlobalFrame

geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg)
这个函数的核心功能是获得机器人坐标系和全局地图坐标系的关系：

1
tf_.transformPose(global_frame, goal_pose, global_pose);

计算机器人的内接圆和外切圆半径

1
2
3
4
5
std::vector<geometry_msgs::Point> footprint = planner_costmap_ros_->getRobotFootprint();
costmap_2d::calculateMinAndMaxDistances(footprint, robot_inscribed_radius_, robot_circumscribed_radius);
// 或者这样
planner_costmap_ros_->getLayeredCostmap()->getInscribedRadius()
planner_costmap_ros_->getLayeredCostmap()->getCircumscribedRadius()

通过footprint的代价值判断是否撞障碍

与上一条结合，参考TEB代码

1
2


计算 直角三角形的斜边长

#include <math.h>

double hypot(double x, double y)，输入是两个直角边，用法: hypot(p1.x - p2.x, p1.y - p2.y)

ros::shutdown()

关闭节点并且从主节点登出，会终结所有的订阅、发布、服务，连ROS_INFO也不能再用。一般不用调用此函数，当所有的NodeHandle销毁时，就会自动关闭节点。默认的roscpp也会安装SIGINT句柄用来检测Ctrl-C，并自动为你关闭节点。

ros::waitForShutdown ()

等待节点关闭，无论时通过Ctrl-C还是ros::shutdown()

ros::isShuttingDown()

一旦ros::shutdown()调用（注意是刚开始调用，而不是调用完毕）就返回true。一般建议用ros::ok()，特殊情况可以用ros::isShuttingDown()

ros::NodeHandle

获取节点的句柄，ros::init是初始化节点，这个是启动节点。
如果不想通过对象的生命周期来管理节点的开始和结束，可以通过ros::start()和ros::shutdown()来自己管理节点。

ros::ok()

判断是否退出节点，如果返回false，说明可能发生了以下事件

  • 调用了ros::shutdown()
  • 被另一同名节点踢出ROS网络
  • 节点中的所有ros::NodeHandles都已经被销毁

ros::Time::now()

获取当前时间: ros::Time time = ros::Time::now()
使用模拟时间时，当 /clock节点接受到第一条消息时，ros::Time::now()返回时刻 0，此时客户端还不知道时钟时间。

ros::Rate

1
2
3
4
5
6
ros::Rate rate(10);
while(ros::ok())
{
    // do something
    rate.sleep();
}

Rate类的构造函数参数是频率，不是时间。rate(10)对应时间间隔为1/10秒，也就是100毫秒。Rate::sleep()是进行睡眠，实际调用Duration::sleep()，间隔就是从构造函数开始计算，源码处理比较复杂。

获得当前运行的所有节点名

1
2
3
typedef std::vector<std::string> ros::V_string
// Retreives the currently-known list of nodes from the master. 放入
ROSCPP_DECL bool   getNodes (V_string &nodes)

用这个函数很简单：

1
2
3
4
5
6
7
std::vector<std::string> nodes;
ros::master::getNodes(nodes);	// bool
int len = nodes.size();
for (int i =0; i < len; i ++)
{
	cout<<"node: "<<nodes.at(i)<<endl;
}

获得当前运行的所有话题名

1
2
3
4
5
6
7
8
9
10
11
struct ros::master::TopicInfo
{
	TopicInfo ()
 	TopicInfo (const std::string &_name, const std::string &_datatype)
public:
	std::string 	datatype    // Datatype of the topic. 
	std::string 	name        // Name of the topic. 
}
typedef std::vector<TopicInfo> ros::master::V_TopicInfo

ROSCPP_DECL bool    getTopics (V_TopicInfo &topics)

使用是类似的：

1
2
3
4
5
6
7
8
std::vector<ros::master::TopicInfo> topics;
ros::master::getTopics(topics);
int len = topics.size();
for (int i =0; i < len; i ++)
{
	cout<<"topic: "<<topics.at(i).name<<"   type: "
		<<topics.at(i).datatype<<endl;
}

其他可能用到的函数

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
ros::isInitialized ()    // 判断ros::init()是否被调用

ros::isStarted ()	　// 判断节点是否已经通过ros::start()启动

ros::requestShutdown ()　　// 在节点的一个线程内要求ROS关闭

ros::this_node::getName       // 返回当前节点的名称

getAdvertisedTopics()　　//返回节点注册的话题
getSubscribedTopics()   //返回节点订阅的主题

getNamespace()	//返回节点的命名空间

ROS_ASSERT(a<b)			//同assert()，若条件不满足，程序会终止

ROS_BREAK()				//直接终止程序，报警FATAL


 // Get the hostname where the master runs. 
ROSCPP_DECL const std::string &     getHost ()

// Get the port where the master runs. 
ROSCPP_DECL uint32_t    getPort ()

// Get the full URI to the master (eg. http://host:port/) 
ROSCPP_DECL const std::string &     getURI ()

ros_Time_now不能在NodeHandle之前.png

参考：
ros 初始化和关闭

解析ros::init（一）|ROSROS程序和源码分析

在ROS程序中，初始化ROS节点是函数ros::init()，启动节点是实例化类ros::NodeHandle，这两步是ROS程序必不可少的，先后顺序不能变，否则会报错：

本篇分析ros::init，另一篇分析NodeHandle。

原型

使用任何roscpp函数前，必须调用ros::init()，一般有两种形式：

1
2
3
ros::init(argc, argv, "my_node_name");
ros::init(argc, argv, "my_node_name",   ros::init_options::AnonymousName);
ros::init(argc, argv, "my_node_name",   ros::init_options::NoSigintHandler);

argc和argv用于处理remapping参数，使用这种形式后，在命令行中使用参数就无效了。如果还想在命令行中处理，需要在ros::init之后调用ros::removeROSArgs()

一般只能有一个同名节点在运行，如果再运行一个，前一个节点会自动关闭。使用第二种重载就可以同时运行多个同名节点，比如rviz和rostopic就是如此，ROS会在节点名后面加上UTC时间以示区别。

运行同一个节点还有更好的方法，比如已经运行了一个名为Pub的节点：rosrun pub Pub，我们指定参数__name可以运行同一个可执行文件，但是节点名不同：

1
rosrun pub Pub __name:=newPub

默认发SIGINT信号会终结节点，也就是Ctral+C会退出节点，但也可以自定义信号处理函数，前提是使用第三种重载的ros::init。

ros::init 源码

ros::init有三种重载形式，以最简单的一种为例

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
void init(const M_string& remappings, const std::string& name, uint32_t options)
{
  if (!g_atexit_registered)
  {
    g_atexit_registered = true;   // 状态
    // 注册终止函数，在调用exit()或终止main函数时关闭shutdown()函数
    atexit(atexitCallback);
  }

  if (!g_global_queue)
  {
    g_global_queue.reset(new CallbackQueue);
  }
  //上面做了一些预处理，主要部分在下面：
  if (!g_initialized)   // 若未初始化
  {
    g_init_options = options;
    g_ok = true;

    ROSCONSOLE_AUTOINIT; //在console.h中的一段宏定义：Initializes the rosconsole library. 
    // Disable SIGPIPE
#ifndef WIN32     // 如果不是windows系统，执行
    signal(SIGPIPE, SIG_IGN);
#endif
    // 重点是5个init
    network::init(remappings);//初始化网络,实现在network.cpp中
    master::init(remappings); //初始化master
    // names:: namespace is initialized by this_node
    this_node::init(name, remappings, options); //初始化当前节点
    file_log::init(remappings);
    param::init(remappings);
    g_initialized = true;//置上初始化标记
  }
}

分析一下代码，首先是注册终止函数，也就是调用exit函数时执行的回调函数：

1
2
3
4
5
6
7
8
9
void atexitCallback()
{
  if (ok() && !isShuttingDown())
  {
    ROSCPP_LOG_DEBUG("shutting down due to exit() or end of main() without cleanup of all NodeHandles");
    g_started = false; // don't shutdown singletons, because they are already destroyed
    shutdown();         // 关闭节点
  }
}

g_global_queue的定义是CallbackQueuePtr g_global_queue;，再查发现类型其实是一个boost共享指针：typedef boost::shared_ptr<CallbackQueue> CallbackQueuePtr;，这里是共享指针丢弃原来的指向对象，销毁掉，重新指向新new的对象

再往下是一个宏ROSCONSOLE_AUTOINIT，内容不必太详细看，功能就是初始化rosconsole库。

然后执行signal(SIGPIPE, SIG_IGN);，功能是收到RST包后，不要关闭节点。

最后就是很重要的5个init，我们一一分析

network:init()

这个函数主要就是给g_host和g_tcpros_server_port赋值

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
void init(const M_string& remappings) //该函数在init.cpp中被调用
{
  //模块1：
  M_string::const_iterator it = remappings.find("__hostname");
  if (it != remappings.end())
  {
    g_host = it->second;
  }
  else
  {
    it = remappings.find("__ip");
    if (it != remappings.end())
    {
      g_host = it->second;
    }
  }
  //模块2
  it = remappings.find("__tcpros_server_port");
  if (it != remappings.end())
  {
    try
    {           //  尝试将对应元素的值（std::string）转化成uint16_t类型，boost 转化
                //  g_tcpros_server_port初始值为0
      g_tcpros_server_port = boost::lexical_cast<uint16_t>(it->second);
    }
    catch (boost::bad_lexical_cast & )        // 如果上述类型转化发生异常，捕捉
    {
      throw ros::InvalidPortException("__tcpros_server_port [" + it->second + "] was not specified as a number within the 0-65535 range");
    }
  }
  //模块3，如果上面未能赋值，调用函数determineHost赋值
  if (g_host.empty())
  {
    g_host = determineHost();
  }
}

M_string其实是std::map<std::string, std::string>，也就是标准map容器。

boost::lexical_cast用于string 和数值之间的转换比如：将一个字符串”712”转换成整数712，代码如下：

1
2
3
4
5
6
string s = "712";  
int a = lexical_cast<int>(s); 

s="1523408.78";
float num = boost::lexical_cast<float>(s);
cout<<num<<endl;

浮点数比较大时，会四舍五入用科学计数法表示．
这种方法的好处是：如果转换发生了意外，lexical_cast会抛出一个bad_lexical_cast异常，可以在程序中进行捕捉。

最后，如果之前未能给g_host成功赋值，会调用determineHost函数，它依次获取环境变量ROS_HOSTNAME和ROS_IP，如果没有设置这两个环境变量，就调用gethostname函数获取主机名，但不能取localhost，如果这样还不能获取到，只好返回127.0.0.1

master::init()

这个是处理主机节点

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
43
void init(const M_string& remappings)
{
  //构建迭代器，查找remappings中键为"__master"的节点。
  M_string::const_iterator it = remappings.find("__master");
  if (it != remappings.end())
  {
    g_uri = it->second;
  }
  //如果g_uri没有被赋值（即刚刚没找到相应节点）
  if (g_uri.empty())
  {
    char *master_uri_env = NULL;
    #ifdef _MSC_VER     // 根据编译器使用不同函数
      _dupenv_s(&master_uri_env, NULL, "ROS_MASTER_URI");
    #else
      master_uri_env = getenv("ROS_MASTER_URI");
    #endif

    if (!master_uri_env)
    {
      ROS_FATAL( "ROS_MASTER_URI is not defined in the environment. Either " \
                 "type the following or (preferrably) add this to your " \
                 "~/.bashrc file in order set up your " \
                 "local machine as a ROS master:\n\n" \
                 "export ROS_MASTER_URI=http://localhost:11311\n\n" \
                 "then, type 'roscore' in another shell to actually launch " \
                 "the master program.");
      ROS_BREAK();
    }
    g_uri = master_uri_env;

#ifdef _MSC_VER
    free(master_uri_env);
#endif
  }

  //对g_uri进行解析，把g_uri中去掉协议部分赋值给g_host，并将端口赋值给g_port。
  if (!network::splitURI(g_uri, g_host, g_port))
  {
    ROS_FATAL( "Couldn't parse the master URI [%s] into a host:port pair.", g_uri.c_str());
    ROS_BREAK();
  }
}

如果没有获得环境变量ROS_MASTER_URI，那么节点报错退出，可见这个环境变量必不可少，最后把值赋给g_uri。最后的splitURI函数显然是一个字符串的处理函数，过程比较复杂，我们知道功能即可

spin与spinOnce|ROSROS程序和源码分析

在话题发布和订阅中，消息订阅器一旦知道话题上面有消息发布，就会将消息的值作为参数传入回调函数中，把回调函数放到了一个回调函数队列中，它们的函数名一样，只是实参不一样，这就是subscribe函数的作用。但是此时还没有执行callback函数，当spinOnce函数被调用时，spinOnce就会调用回调函数队列中第一个callback函数，此时回调函数才被执行，然后等到下次spinOnce函数又被调用时，回调函数队列中第二个回调函数就会被调用，以此类推。

注意：因为回调函数队列的长度是有限的，如果发布数据的速度太快，spinOnce函数调用的频率太少，就会导致队列溢出，一些回调函数就会被挤掉，导致没被执行。

对于spin函数，一旦进入spin函数，它就不会返回了，相当于它在自己的函数里阻塞。只要回调函数队列里面有回调函数在，它就会马上去执行。如果没有的话，它就会阻塞，不会占用CPU。

spin()的目的是启动一个新的线程去获取队列中的回调函数并调用它，而回调函数本身不是线程 ，有单线程，同步多线程和异步多线程等情况，这些都有内置的语句。所有用户的调用程序将从 ros::spin()开始调用，只到节点关闭，ros::spin()才有返回值。

ros::spin其实就相当于

1
2
3
4
5
while(ros::ok())
{
    // Do Something
    ros::spinOnce();
}

发布和订阅话题都不一定要使用spinOnce()，如果仅仅只是响应topic，就用ros::spin()，当程序中除了响应回调函数还有其他重复性工作的时候，那就在循环中做那些工作，然后调用ros::spinOnce()

spinOnece的注意事项

我仔细试验了这几个参数，没有发现缺失回调函数的情况，一般不需要太注意

ros::spinOnce()的用法相对来说很灵活，但往往需要考虑调用消息的时机，调用频率，以及消息池的大小。

比如下面的程序，消息送达频率为10Hz，ros::spinOnce()的调用频率为5Hz，那么消息池的大小就一定要大于2，才能保证数据不丢失，无延迟，跟这里的发布消息池容量无关

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//发送频率为10Hz(1秒发10次)  消息池最大容量1000。
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
ros::Rate loop_rate(10);
while (ros::ok())
{
	// 发布消息
}


ros::Subscriber sub = n.subscribe("chatter", 2, chatterCallback);
ros::Rate loop_rate(5);
while (ros::ok())
{
    /*...TODO...*/ 
    ros::spinOnce();
    loop_rate.sleep();
}

看一下源码：

1
2
3
4
void spinOnce()
{
  g_global_queue->callAvailable(ros::WallDuration());
}

调用队列中的所有回调函数，如果一个回调还没有准备好调用，再推回队列

发布和订阅|ROSROS程序和源码分析

话题通信的媒介是消息，消息的产生和消费是解偶的，二者之间没有强行绑定的关系，ROS节点不在乎是谁在发布Topic，哪个节点在订阅Topic，它只关心topic的名字以及当前的消息类型时候和要求的匹配。所以说话题通信是多对多的方式

publisher注册时的消息类型,如果和发布时的消息类型不一致,编译不报错,运行报错

消息是以Boost共享指针的形式传输，我们可以存储它而又不需要复制数据。回调函数最常用的原型是这样的：

1
void callback(const std_msgs::StringConstPtr& str)

其实它就是void callback(const boost::shared_ptr<std_msgs::String const>&);，还有以下几种：
1
2
3
4
5
6
void callback(boost::shared_ptr<std_msgs::String const>);
void callback(std_msgs::StringConstPtr);
void callback(std_msgs::String::ConstPtr);
void callback(const std_msgs::String&);
void callback(std_msgs::String);
void callback(const ros::MessageEvent<std_msgs::String const>&);

MessageEvent类允许你在订阅的回调函数内获取信息的元数据。

其他常用的ros::Subscriber函数

1
2
3
4
5
6
// 注意： 只能判断话题是否发布，不能判断有没有发布消息
uint32_t 	getNumPublishers () const    //获得连接订阅者的发布者个数
 
std::string 	getTopic () const	//获得所订阅的话题

uint32_t 	getNumSubscribers () const　　//获得连接发布者的订阅者个数

订阅话题后，回调函数的参数类型必须是对应话题的消息类型，否则能订阅成功但不运行回调。

发布者的回调函数

用于检测是否有订阅者

1
2
3
4
5
6
7
8
9
10
11
12
ros::Publisher pub = nh.advertise<std_msgs::Int8>("topic", 50, connectCb,disconnectCb);

// 回调函数如下
void connectCb(const ros::SingleSubscriberPublisher& pub)
{
    ROS_INFO("topic Name: %s", pub.getTopic().c_str());
    ROS_INFO("Subscriber Name: %s", pub.getSubscriberName().c_str());
}
void disconnectCb(const ros::SingleSubscriberPublisher& pub)
{
    ROS_INFO("disconnectCb");
}

ROS的回调函数里定义count变量，居然报错ambiguous，看来以后最好不用count变量
image-20211227101811232

特殊需求

有时需要在订阅者的回调函数中发布消息,而且只发布一次,这时没必要用service混合topic,用static变量就可以做到:

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
static ros::Publisher *pubPtr;
static bool published = false;

void callback(const std_msgs::Int8ConstPtr& msg)
{
    int num = msg->data;
    ROS_INFO("msg: %d", num);
    std_msgs::String str;
    str.data = std::string(to_string(num) );
    if(!published && num>8)   // 在num>8时发布一次,以后再也不发布
    {
      pubPtr->publish(str);
      published = true;
    }
    sleep(1);
}

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

    ros::Subscriber sub = nh.subscribe<std_msgs::Int8>("topic",20,callback);
    ros::Publisher pub = nh.advertise<std_msgs::String>("new",50,false);
    pubPtr = &pub;
    ros::spin();
    return 0;
}

让订阅者停止订阅

ROS官方有这么一段话：

1
2
3
4
5
function   subscribe() 　returns a Subscriber object that you must hold on to until you want to unsubscribe. When all copies of the Subscriber object go out of scope, this callback will automatically be unsubscribed from this topic.

function  shutdown()   Unsubscribe the callback associated with this Subscriber.
This method usually does not need to be explicitly called, as automatic shutdown happens when all copies of this Subscriber go out of scope
This method overrides the automatic reference counted unsubscribe, and immediately unsubscribes the callback associated with this Subscriber

根据说明，有以下三种方法：

  1. 一般让订阅者停止订阅的方法是等subscribe()生成的订阅者对象出作用域．

  2. 另一种方法就是explicitly调用shutdown()，典型方法是声明订阅者对象为全局变量，在回调函数中满足某条件时，对此对象调用shutdown()，但这样会直接退出程序，并不好

  3. 第三种方法其实与第二种类似，声明一个bool全局变量ok，订阅时执行

1
2
3
4
while(!ok)
{
    ros::spinOnce();
}

在回调函数中满足某条件时，让ok=true

queue_size和buff_size

发布者的原型是这样的

1
2
3
4
Publisher ros::NodeHandle::advertise(const std::string & topic,
    uint32_t    queue_size,
    bool    latch = false 
)

  • queue_size Maximum number of outgoing messages to be queued for delivery to subscribers。 消息队列是为了缓存发布节点发布的消息，一旦队列中消息的数量超过了queue_size，那么最先进入队列的（最老的）消息被舍弃。
  • latch (optional) If true, the last message published on this topic will be saved and sent to new subscribers when they connect

订阅者Subscriber ros::NodeHandle::subscribe函数里也有一个queue_size： Number of incoming messages to queue up for processing，消息队列是为了缓存节点接收到的信息，一旦自己处理的速度过慢，接收到的消息数量超过了queue_size，那么最先进入队列的（最老的）消息会被舍弃。

机器人应用中难免会遇到运算起来很费时间的操作，比如图像的特征提取、点云的匹配等等。有时候，我们需要在ROS的Subscriber的Callback回调函数中进行这些费时的操作。如果我们要是没有取舍的对于每个消息都调用一次回调函数，那么势必会导致计算越来越不实时，很有可能当下在处理的还是几秒以前的数据，之前的消息被舍弃了。所以，如果我们想只处理最新的消息，实际上只需要把两个queue_size都设置成1，那么系统不会缓存数据，自然处理的就是最新的消息。

rospy的订阅者还有个buff_size参数，要设置一个很大很大的数，这个缓冲区的大小是指消息队列使用的缓冲区物理内存空间大小。如果这个空间小于一个消息所需要的空间，比如消息是一副图片或者一帧点云，数据量超过了缓冲区的大小。这个时候为了保证通信不发生错误，就会触发网络通信的保护机制，TCP的Buffer会为你缓存消息。

但是roscpp里面没有buff_size参数，它获得 incoming message的大小，分配所需的缓冲区大小，然后一次读取整个incoming message

参考：
queue_size和buff_size的理解
ROS anwser的解释

ROS定时器|ROSROS Kinetic知识

roscpp的定时器会计划在某一频率下执行一次回调操作，在订阅或服务中的回调函数队列机制中使用。

创建定时器

通过ros::NodeHandle::createTimer()创建: ros::Timer timer = nh.createTimer(ros::Duration(0.1), timerCallback); 其实有多个重载，这是用的比较多的一种。

回调函数只能是这种形式: void callback(const ros::TimerEvent&);
ros::TimerEvent结构体作为参数传入，它提供时间的相关信息，对于调试和配置非常有用。ros::TimerEvent结构体说明：

1
2
3
4
5
6
7
8
9
10
// 上次回调期望发生的时间
ros::Time last_expected    
// 上次回调实际发生的时间
ros::Time last_real       
// 本次回调期待发生的时间
ros::Time current_expected 
// 本次回调实际发生的时间
ros::Time current_real 
// 上次回调的时间间隔（结束时间-开始时间），是wall-clock时间。
ros::WallTime profile.last_duration

常用代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
void callback(const ros::TimerEvent&)
{
  ROS_INFO("Callback   triggered");
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "test_timer_node");
  signal(SIGINT, SigintHandler);

  ros::NodeHandle nh;
  ros::Timer timer = nh.createTimer(ros::Duration(0.5), callback);
  
  ros::spin();

  return(0);
}

实战

比如某些情况下，我们需要一直监视机器人的里程计数据状态，并在某个界面上显示出来。这种情况下可以用到定时器，

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
void odomTimerCb(const ros::TimerEvent& e);
void odomSubCb(const nav_msgs::OdometryConstPtr& m);
ros::Timer　odomTimer ;
riki_msgs::HealthStatus msg;

int main(int argc, char** argv)
{
    ros::init(argc,argv,"healthStatus");
    ros::NodeHandle nh;
    odomTimer = nh.createTimer(ros::Duration(5), odomTimerCb);
    ros::Subscriber odomSub = nh.subscribe("odom", 1000, odomSubCb);

    ros::Publisher pub = nh.advertise<riki_msgs::HealthStatus>("healthStatus", 1000);
    ros::Rate loop_rate(1);
    while(ros::ok())
    {
        pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }
}

void odomTimerCb(const ros::TimerEvent& e)
{
    msg.odomData = false;
}
void odomSubCb(const nav_msgs::OdometryConstPtr& m)
{
    msg.odomData = true;
    odomTimer.stop();
    odomTimer.start();
}

未发布odom主题时，不会进入订阅者的回调函数odomSubCb，只进入定时器的回调odomTimerCb，此时消息成员odomData一直是false。
发布odom后，进入订阅者回调，此时odomData成为true，此时需要先停止计时器再打开。如果不停止，还会进入计时器回调，让消息成员赋值为false，实际造成赋值混乱。但是还得打开，因为一旦odom话题关闭了，就不会再进入订阅者回调，需要设法调用计时器回调，赋值false。订阅odom话题成功再打开计时器后，不会进入计时器回调。

参考: ROS与C++入门教程-Timers

ROS只发布和订阅一次消息|ROSROS程序和源码分析

只发布一次消息

1
2
3
4
5
6
ros::init(argc,argv,"op_motor");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<std_msgs::Bool>("Switch",1000);
//不必判断订阅者，直接等待１秒钟，只发布一次
sleep(1);
pub.publish(msg);

只发布一次消息就简单了，不必用while循环，直接发布就可以，但是最好也延时一次，防止订阅者的网络状况不好，当然如果不考虑有没有订阅者就不必延时了，不过这样没什么意义。


ROS只订阅一次消息

1
2
3
4
5
6
7
8
9
10
11
12
ros::init(argc,argv,"node");
ros::NodeHandle nh;
boost::shared_ptr<riki_msgs::Battery const> Edge;    // 必须有const
Edge = ros::topic::waitForMessage<riki_msgs::Battery>("BMS",ros::Duration(0.1));
if(Edge != NULL)
{
    riki_msgs::Battery  bat = *Edge;
    cout<<"battery:"<<  bat.Ah  <<endl;
    cout<<"battery:"<<  Edge->Ah  <<endl;
}
else
    cout<<"no message from topic BMS"<<endl;

waitForMessage是等待一次消息，造成的网络传输会出现延时。如果是获得消息后再发布，新话题的发布频率会明显下降 为了保证发布频率，还是尽量使用spin()

这里用到的函数是ros::topic::waitForMessage，没有涉及回调函数，它有4个重载，常用的是两个:

1
2
3
template<class M> boost::shared_ptr< M const >   waitForMessage (const std::string &topic, ros::Duration timeout)

template<class M> boost::shared_ptr< M const >  waitForMessage (const std::string &topic, ros::NodeHandle &nh)

第一个是订阅话题topic，假如没有发布话题，那么等待时间timeout，超时后程序继续执行；第二个是如果没有发现话题topic，会一直阻塞，不能向下运行，除非执行rosnode kill。前一个用的比较多，注意时间不要设置太短，否则可能因为网络状况不好而订阅失败。

返回类型一看就知道是模板类的Boost共享指针，从这里就可以看出， 消息是以const Boost共享指针的形式传输，我们可以存储它而又不需要复制数据，waitForMessage就是返回了订阅到的消息，取指向就可以获得消息成员，也可以直接用指针获得消息成员。
注意模板类型有const，否则不符合重载声明

回调函数中常见的MsgType::ConstPtr参数类型其实就是boost::shared_ptr<const MsgType>

MsgType::Ptr就是boost::shared_ptr<MsgType>

参考:

How do I publish exactly one message
ros::topic Namespace

超声波传感器和电机驱动器的控制|ROSROS机器人
abstract Welcome to my blog, enter password to read.
Read more
ROS安装的常见问题|ROSROS Kinetic知识

新的电脑安装ubuntu16和18很容易出毛病，无线网卡不能用，驱动也装不成功。显卡驱动有问题，不能改变屏幕亮度和外接显示器等等问题。所以但是装新系统就不会有这些问题，因此ROS最好也用noetic，当然更新的是ROS2

安装ROS报错

Ubuntu 16.04 安装ROS kinetic报错Depends: ros-kinetic-desktop but it is not going to be installed

  1. 删除ros-latest.list
  2. 修改源: vim /etc/apt/sources.list，改为下面内容，也就是中科大的安装源:
1
2
3
4
5
6
7
8
9
10
deb http://mirrors.ustc.edu.cn/ubuntu/ xenial main restricted universe multiverse
deb http://mirrors.ustc.edu.cn/ubuntu/ xenial-security main restricted universe multiverse
deb http://mirrors.ustc.edu.cn/ubuntu/ xenial-updates main restricted universe multiverse
deb http://mirrors.ustc.edu.cn/ubuntu/ xenial-proposed main restricted universe multiverse
deb http://mirrors.ustc.edu.cn/ubuntu/ xenial-backports main restricted universe multiverse
deb-src http://mirrors.ustc.edu.cn/ubuntu/ xenial main restricted universe multiverse
deb-src http://mirrors.ustc.edu.cn/ubuntu/ xenial-security main restricted universe multiverse
deb-src http://mirrors.ustc.edu.cn/ubuntu/ xenial-updates main restricted universe multiverse
deb-src http://mirrors.ustc.edu.cn/ubuntu/ xenial-proposed main restricted universe multiverse
deb-src http://mirrors.ustc.edu.cn/ubuntu/ xenial-backports main restricted universe multiverse

结果未变，再运行sudo apt-get -f install后安装了一个软件包，再安装ros-kinetic-desktop-full正常了

Failed to connect to raw.githubusercontent.com port 443

终端中输入vim /etc/hosts，加入185.199.110.133 raw.githubusercontent.com，或者从raw.Githubusercontent.com中选一个IP

退出后重新执行

安装时找不到软件包


从报错的内容找到安装包的地址，直接下载，有时一个软件包还有其他依赖包，都要下载

GPG error, signature were invalid


根据官方的文档，重新设置秘钥
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

编译缺少 libvtkproj4

编译时报错: No rule to make target '/usr/lib/x86_64-linux-gnu/libvtkproj4-6.2.so.6.2.0'

解决方法: sudo ln -s /usr/lib/x86_64-linux-gnu/libvtkCommonCore-6.2.so /usr/lib/x86_64-linux-gnu/libvtkproj4-6.2.so.6.2.0

E: 无法定位软件包 ros-kinetic-map-server

在软件和更新的其他软件里添加两项:

1
2
deb http://packages.ros.org/ros/ubuntu xenial main
deb http://packages.ros.org/ros-shadow-fixed/ubuntu xenial main

然后执行sudo apt-get update，应该就能安装了。
在/etc/apt/sources.list里添加也是同样效果，但两个地方不能都添加，否则update时会报错

without authentic

有时安装某个包出现下面情况，
without authentic.png
在命令最后添加--allow-unauthenticated即可

安装ROS包出现404 Not Found

1
2
Err:1 http://packages.ros.org/ros/ubuntu <YOUR_UBUNTU_VERSION>/main amd64 <SOME_ROS_PKG> amd64 0.13.3-0xenial-20190320-132757-0800
  404  Not Found [IP: 64.50.236.52 80]

运行sudo apt update也无效，打开上面的网页后，发现安装包的版本是更新的，看来我们的安装没有更新。
最后发现在2019年夏天，ROS官方更改了安装的Key，也就是第一次安装ROS时设置的Key。需要换成新的Key，步骤如下：

  • sudo apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 (Kinetic及以后的版本)

  • sudo -E apt-key adv —keyserver ‘hkp://keyserver.ubuntu.com:80’ —recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

  • sudo apt clean

  • sudo apt update

现在可以正常安装了，如果以后安装ROS，需要用这个新的Key，目前网上很多旧资料还是以前那个Key

参考：
apt update fails / cannot install pkgs
ROS报错“An error occurred during the signature verification”的解决办法

找不到***.deb

有时候我们解决了上面的问题，还是会出现下载ros包出现找不到的错误，这是因为ROS的包大部分都是在更新的，比如robot_state_publisher在2020年1月就更新了，如果没有apt-get update，是下载不到的

rqt_tf_tree 报警

执行rqt_tf_tree出现报警:

1
/opt/ros/melodic/lib/python2.7/dist-packages/rqt_tf_tree/dotcode_tf.py: 96: YAMLLoadWarning: calling yaml.load() without Loader=...is deprecated as the default Loader is unsafe. Please read https://msg.pyyaml.org/load for full details.

不理会也无影响，如果要解决，在dotcode_tf.py96行改为 yaml.load(input, Loader=yaml.FullLoader)

参考: yaml.load 问题

ImportError: No module named rospkg pyyaml

sudo pip3 install pyyaml 或者 sudo apt-get install python3-yaml

如果出现 Requirement already satisfied:，尝试下面命令

1
2
3
4
sudo apt-get install python3-catkin-pkg
sudo apt-get install python3-rospkg
sudo apt-get install python3-rospkg-modules
sudo apt-get install python3-catkin-pkg-modules

如果还有问题，那就是python版本问题，而不是安装的问题了，使用python3一般能解决，不过要explicitly指定，所以执行

1
sudo apt install python-is-python3

Could NOT find SDL (missing: SDL_LIBRARY SDL_INCLUDE_DIR)

在编译map_server时出错，是缺少了库文件：

1
2
sudo apt-get install libsdl-image1.2-dev
sudo apt-get install libsdl-dev

缺少某个包

使用catkin_make时，如果缺某个包，会提示缺*Config.cmake文件，平时都会使用apt install直接安装这个包，这并不是只添加了*Config.cmake文件，而是有一系列文件，所以不能直接从其他电脑复制。

liborocos-kdl_1.3.0

liborocos-kdl1.3_1.3.0的下载地址