有一次机器人没有装雷达和相机，打算随便跑跑。于是在通用代价地图的障碍层，不设置传感器数据来源，运行move_base出现频繁报警

1

The /scan observation buffer has not been updated for 22.06 seconds, and it should be updated every 5.00 seconds.

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

bool ObservationBuffer::isCurrent() const
{
  if (expected_update_rate_ == ros::Duration(0.0))
    	return true;
  // last_updated_ 没有赋值
  bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
  if (!current)
  {
    ROS_WARN(
        "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
        topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
  }
  return current;
}

此时发导航命令，又有报警

1

[/move_base]:Sensor data is out of date, we're not going to allow commanding of the base for safety

1
2
3
4
5
6

if(!controller_costmap_ros_->isCurrent())
{
  ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());
  publishZeroVelocity();
  return false;
}

1
2
3
4
5
6
7
8
9

bool LayeredCostmap::isCurrent()
{
  current_ = true;
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
      ++plugin)
  {
    current_ = current_ && (*plugin)->isCurrent();
  }
}

从网上下载一个包含雷达数据的bag，又设置了通用代价地图和tf树后，发现报警依然，应该还是时间戳问题，懒得再对齐了。

• Packages, Topics / Services, 文件、库的命名都采用 under_scored

• Classes / Types的命名采用 CamelCased，比如class ExampleClass, class HokuyoURGLaser。 函数命名采用 camelCased，比如int exampleMethod(int example_arg)

• 普通变量和成员变量命名、命名空间都使用 under_scored，循环中的临时变量使用i,j,k，i on the outer loop, j on the next inner loop

• 常量命名采用 ALL_CAPITALS

• 全局变量采用 g_ 开头的 under_scored

• 每行最多120个字符

• 所有头文件要包含#ifndef，比如:

1
2
3
4

#ifndef PACKAGE_PATH_FILE_H
#define PACKAGE_PATH_FILE_H
	......
#endif

这部分应该在license之后，#endif在头文件的最后

• 不使用宏

预处理所用的宏，比如

1
2
3

#if DEBUG
        temporary_debugger_break();
#endif

• 函数的输出参数使用指针，而不是引用: int exampleMethod(FooThing input, BarThing* output);

• 头文件里不要用using namespace关键字，在源文件里可使用using，但不要using namespace std;，而是使用using std::list;, using std::vector;，否则引入了std所有内容。

• 建议使用Exceptions，而不是returning integer error codes。 析构函数不要抛出异常。 不直接触发的回调函数，不要抛出异常。

• 保证代码是 exception-safe: When your code can be interrupted by exceptions, you must ensure that resources you hold will be deallocated when stack variables go out of scope. In particular, mutexes must be released, and heap-allocated memory must be freed. Accomplish this safety by using the following mutex guards and smart pointers

枚举

1
2
3
4
5
6
7
8
9
10

namespace Choices
{
  enum Choice
  {
     Choice1,
     Choice2,
     Choice3
  };
}
typedef Choices::Choice Choice;

This prevents enums from polluting the namespace they’re inside. Individual items within the enum are referenced by: Choices::Choice1, but the typedef still allows declaration of the Choice enum without the namespace.

• If you are using C++11 and above, you can use scoped enumeration

1
2
3
4
5
6
7

enum class Choice
{
    Choice1,
    Choice2,
    Choice3
};
Choice c = Choice::Choice1;

• 不建议使用全局变量和函数，尤其前者。更不能在多线程中使用。大多数变量和函数都该在类的体内，其他应当在命名空间里。

• 类不要使用静态变量。

• 只在 well-defined exit point 调用exit()函数

• 使用assertions之类的比条件判断语句好，比如ROS_ASSERT, ROS_ASSERT_MSG, ROS_ASSERT_CMD, ROS_BREADK

Depending on compilation settings, the assertion may not be executed.

It is typical to develop software with assertion-checking enabled, in order to catch violations. When nearing software completion and when assertions are found to always be true in the face of extensive testing, you build with a flag that removes assertions from compilation, so they take up no space or time.

The following option to catkin_make will define the NDEBUG macro for all your ROS packages, and thereby remove assertion checks.
catkin_make -DCMAKE_CXX_FLAGS:STRING="-DNDEBUG"

Note: cmake will rebuild all your software when you run it with this command, and will remember the setting through subsequent catkin_make runs until you delete your build and devel directories and rebuild.

这个类有时也有用，但是不熟悉就容易用错。它具备单例模式 static const TopicManagerPtr& instance()，使用方便。

• size_t getNumPublishers (const std::string &_topic)    Return the number of publishers connected to this node on a particular topic.

• size_t getNumSubscribers (const std::string &_topic)    Return the number of subscribers a node has for a particular topic

• void getAdvertisedTopics (V_string &topics)    Get the list of topics advertised by this node

• void getSubscribedTopics (V_string &topics)    Get the list of topics subscribed to by this node

根据说明，对以上几个函数，不能直接传一个话题名做参数，以获知订阅者和发布者的数量，结果往往不是你想要的。

1
2
3
4

std::vector<string> topics;
ros::TopicManager::instance()->getSubscribedTopics(topics);
for(int i=0; i< topics.size(); i++)
    std::cout << topics.at(i) << "  ";

这里获得的话题只有clock，而不是所有被订阅的节点。

• PublicationPtr lookupPublication(const std::string &topic)    Lookup an advertised topic.

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

#include<iostream>
  
int main()
{
  #if defined __linux__
      std::cout<<"linux system"<<std::endl;
  #elif defined _WIN32
      std::cout<<"windows system"<<std::endl;
  #endif
   
  #if defined  __aarch64__
      std::cout<<"this is arm cpu"<<std::endl;
  #elif defined __x86_64__
      std::cout<<"this id x86 cpu"<<std::endl;
  #endif
      return 0;
}

cmake 中判断CPU 架构，操作系统类型

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

cmake_minimum_required(VERSION 3.10.0)
 
message(${CMAKE_HOST_SYSTEM_NAME})
message(${CMAKE_HOST_SYSTEM_PROCESSOR})
 
if(CMAKE_HOST_SYSTEM_NAME MATCHES "Linux")
    message("this is Linux")
elseif(CMAKE_HOST_SYSTEM_NAME MATCHES "Windows")
  message("this is Windows")
endif()
if(CMAKE_HOST_SYSTEM_PROCESSOR MATCHES "aarch64")
    message("this is aarch64 cpu")
elseif(CMAKE_HOST_SYSTEM_PROCESSOR MATCHES "x86_64")
  message("this is x86_64 cpu")
endif()

源码其实并不复杂

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

bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
  global_pose.setIdentity();
  tf::Stamped < tf::Pose > robot_pose;
  robot_pose.setIdentity();
  robot_pose.frame_id_ = robot_base_frame_;
  robot_pose.stamp_ = ros::Time();
  ros::Time current_time = ros::Time::now();  // save time for checking tf delay later
  try
  {
    tf_.transformPose(global_frame_, robot_pose, global_pose);
  }
  catch (tf::LookupException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ConnectivityException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ExtrapolationException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  // check global_pose timeout
  if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_)
  {
    ROS_WARN_THROTTLE(1.0,
                      "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
                      current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance_);
    return false;
  }
  return true;
}

核心是函数 transformPose，tf_在这里是tf::TransformListener，但函数实际是继承自基类tf::Transformer，有两个重载，常用的是第一个

1
2
3
4
5
6
7
8
9
10
11

void Transformer::transformPose(const std::string&   target_frame,
                                const Stamped< tf::Pose >&   stamped_in,
                                Stamped< tf::Pose >&   stamped_out 
) const

void Transformer::transformPose(const std::string&   target_frame,
                                const ros::Time&   target_time,
                                const Stamped< tf::Pose >&   stamped_in,
                                const std::string&   fixed_frame,
                                Stamped< tf::Pose >&   stamped_out 
) const

1
2
3
4
5
6
7
8

// Get the transform between two frames by frame ID assuming fixed frame.
void Transformer::lookupTransform ( const std::string &   target_frame,
	const ros::Time &   target_time,
	const std::string &   source_frame,
	const ros::Time &   source_time,
	const std::string &   fixed_frame,
	StampedTransform &  transform 
)   const

• target_frame The frame to which data should be transformed
• target_time The time to which the data should be transformed. (0 will get the latest)
• source_frame The frame where the data originated
• source_time The time at which the source_frame should be evaluated. (0 will get the latest)
• fixed_frame The frame in which to assume the transform is constant in time.
• transform The transform reference to fill.

可能抛出的异常

1
2
3
4
5
6
7
8

namespace tf{
  // Pass through exceptions from tf2
  typedef tf2::TransformException TransformException;
  typedef tf2::LookupException LookupException;
  typedef tf2::ConnectivityException ConnectivityException;
  typedef tf2::ExtrapolationException ExtrapolationException;
  typedef tf2::InvalidArgumentException InvalidArgument; 
 }

报警 Costmap2DROS transform timeout

报警对应的transform_tolerance_是在构造函数里写死的0.3，一开始我以为这个数太小了，于是改为2，结果没有改善。

以下报警的根源都是Costmap2DROS::getRobotPose

尝试的改善措施(全都无效):

• Costmap2DROS构造函数中设置: transform_tolerance_(2)
• timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this); 0.1秒间隔太短了。增大到1.5，稍有改善，但未解决根本问题，反而降低了代价地图的性能。
• 降低两个代价地图的update_frequency

所有报警基本来自Costmap2DROS::getRobotPose，或者说map—->base_link的tf变换不及时。

1. 如果不启动VSLAM，启动诠视相机的xv_sdk.launch，使用static_transform_republisher建立假的TF树，启动导航框架，一个报警也没有。因此问题还在VSLAM，怀疑是CPU而不是网络问题。
2. 启动诠视相机的xv_sdk.launch和VSLAM，但不发布tf，使用假的tf树，启动导航框架，仍然没有报警。
3. 继续缩小问题范围，发布假的tf: world—->map—->odom—-> base_link，导航框架不报警。但是如果由VSLAM发布map—->odom，就有报警了。

所以问题的原因：

1. VSLAM的map—->odom—-> base_link变换。
2. Nano的CPU没有使用到极致，还有多余算力

在此情况下规划路径时，全局路径可生成，因为全局规划频率是0，虽然有很多报警，总能成功规划一次。但局部路径就不同了，需要频繁规划。
只看TF的频率和tf echo有没有数据还不够，应该看Rviz里，base_link坐标系在map中的坐标是否能随着机器人的移动实时更新。先满足这一点，再运行导航框架。

头文件

1
2
3
4
5
6

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <rosbag/message_instance.h>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include <nav_msgs/Path.h>

读写bag文件

open函数默认是读模式。

写bag文件：

1
2
3
4
5
6
7
8
9
10

rosbag::Bag bag;
try{
    bag.open("/home/user/plan.bag", rosbag::bagmode::Write);
}
catch (const rosbag::BagException& ex) {
    ROS_ERROR("open rosbag error: %s", ex.what() );
    return;
}
bag.write("record_plan",ros::Time::now(), coverage_plan);
bag.close();

1
2
3

Write   
Read    
Append  

读bag文件一定要用try和catch

下面的例子，bag文件只有一个话题/move_base/transformed_plan，一堆nav_msgs/Path消息

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

rosbag::Bag bag;
try{
    bag.open("/home/user/plan.bag", rosbag::bagmode::Read);
}
catch (const rosbag::BagException& ex) {
    ROS_ERROR("open rosbag error: %s", ex.what() );
    return;
}        
std::vector<std::string> topics;
bool bag_read_once = false;

topics.push_back(std::string("record_plan") );
rosbag::View view(bag, rosbag::TopicQuery(topics) );

BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
    nav_msgs::Path::ConstPtr i = m.instantiate<nav_msgs::Path>();
    plan_from_bag.header.frame_id = i->header.frame_id;
    plan_from_bag.header.stamp = i->header.stamp;
    plan_from_bag.header.seq = i->header.seq;

    unsigned int path_size = i->poses.size();
    ROS_INFO("plan from bag size: %zu",i->poses.size() );
    plan_from_bag.poses.reserve(path_size);
    if(bag_read_once)    break;

    plan_from_bag.poses = i->poses;
    bag_read_once = true;
}
bag.close();

用到的类如下：

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

/* Create a view and add a query
 * param bag             The bag file on which to run this query
 * param query           The actual query to evaluate which connections to include
 * param start_time      The beginning of the time range for the query
 * param end_time        The end of the time range for the query
 * param reduce_overlap  If multiple views return the same messages, reduce them to a single message
 */
View(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query,
     ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX, bool const& reduce_overlap = false);

class ROSBAG_DECL TopicQuery
{
public:
    TopicQuery(std::string const& topic);
    TopicQuery(std::vector<std::string> const& topics);

    bool operator()(ConnectionInfo const*) const;

private:
    std::vector<std::string> topics_;
};

rosbag::View的常用函数

• iterator begin() Simply copy the merge_queue state into the iterator.
• iterator end() Default constructed iterator signifies end.
• ros::Time getBeginTime()
• ros::Time getEndTime()
• uint32_t size () 获得bag文件中所含消息的个数，不是文件大小

其他常用函数

1
2
3
4
5
6
7

// Get the filename of the bag
std::string  getFileName () const

// Get the current size of the bag file (a lower bound) More...
uint64_t  getSize () const
 
bool    isOpen () const

问题

bag文件 unindexed

这种情况下如果没有用try和catch就会出错

我查来查去，没发现C++ API里有类似rosbag reindex命令的函数，只能不处理。似乎python里有，没有深入研究，

参考：
rosbag API

Costmap2DROS类是2D Costmap的ROS封装类，处理订阅的话题，这些话题提供了对障碍物的观测，方式为点云或激光扫描消息。 costmap_2d::Costmap2DROS给用户提供了纯2D的接口，这意味着查询障碍只能在列上进行。例如，在XY平面上位于同一位置的桌子和鞋，虽然在Z方向上有差异但是它们在costmap_2d::Costmap2DROS对象代价地图中对应的cell上拥有相同的代价值。

A ROS wrapper for a 2D Costmap. Handles subscribing to topics that provide observations about obstacles in either the form of PointCloud or LaserScan messages.

• Costmap2D * getCostmap ()

实际是return layered_costmap_->getCostmap();，跟getLayeredCostmap()->getCostmap()一样. Return a pointer to the “master” costmap which receives updates from all the layers.

1

costmap_2d::Costmap2D* global_costmap = planner_costmap_ros_->getCostmap();

• bool getRobotPose(tf::Stamped& global_pose) const;

获取机器人在代价地图的global frame中的位姿，成功获取则返回true

1
2
3
4
5
6
7
8
9
10
11

// 这个返回类型很不方便
tf::Stamped<tf::Pose> pose;
if(!costmap->getRobotPose(pose))
{
	ROS_ERROR("Cannot clear map because pose cannot be retrieved");
	return;
}
double x = pose.getOrigin().x();
double y = pose.getOrigin().y();
double yaw = tf::getYaw(pose.getRotation());
double yaw_norm = angles::normalize_angle(tf::getYaw(pose.getRotation() ) );

• std::string getGlobalFrameID() 和 std::string getBaseFrameID()

这两个返回的就是代价地图yaml里配置的全局和局部坐标系

• LayeredCostmap* getLayeredCostmap()

• std::vector<geometry_msgs::Point> getRobotFootprint() 获取机器人边界（在机器人坐标系下，包含padding），返回的是vector<geometry_msgs::Point>，索引顺序就是yaml中设置的顺序

• std::vector<geometry_msgs::Point>getUnpaddedRobotFootprint() 获取机器人边界（在机器人坐标系下，不包含padding）

• geometry_msgs::Polygon getRobotFootprintPolygon() 获取机器人边界（在机器人坐标系下，包含padding）

LayeredCostmap类是Costmap2DROS的类成员，它是主地图，也能够管理各层地图，因为它含有指向各层子地图的指针，能够调用子地图的类方法，开启子地图的更新。 每一层在Costmap2DROS中以插件方式被实例化，并被添加到LayeredCostmap。每一层可以独立编译，且可使用C++接口对代价地图随意修改，即LayerdCostmap为Costmap2DROS（用户接口）提供了加载地图层的插件机制，每个插件（即地图层）都是Layer类型的

LayeredCostmap的所有函数其实都是通过Costmap2D和各个Layer的函数执行的。

Layer调用的函数: isCurrent, onFootprintChanged, updateBounds, matchSize

Costmap2D调用的函数: updateOrigin, resizeMap, setDefaultValue

• getCostmap()也可以获得costmap_2d::Costmap2D *costmap指针

• getInscribedRadius() 获得内切圆半径

• getCircumscribedRadius() 获得外接圆半径

• 地图有效性 current

对于各层，都有current_参数，该参数继承自Layer类，且通过Layer::isCurrent查询。总的地图的current是通过各层current的与操作计算出来的, 可看LayeredCostmap::isCurrent(),主要的操作是对操作的实时性提供保证，提供是否发生超时的信息。

对于静态层，只要初始化（onInitialize）完成后current_就一直为true

对于障碍物层，其current_参数是各个观察缓冲区ObservationBuffer是否current的与操作，而ObservationBuffer的current取决于缓冲区更新时间（新的观测数据到来的时间）与expected_update_rate参数的对比。

对于膨胀层，也是只要初始化完成后current_就一直为true