雷达的拖尾现象和处理|激光SLAM雷达
abstract Welcome to my blog, enter password to read.
Read more
ssh自动登录远程机和expect的使用|脚本

从本机连接到远程机，发起SSH连接，执行命令cd ~/Downloads;ls

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
#!/bin/bash

# router parameters
ROUTER_USER="xiaoqiang"
ROUTER_HOST="192.168.0.110"
ROUTER_PASSWORD="1"
ROUTER_PORT="22"       # default

# start except
expect <<-EOF
set timeout -1
spawn ssh $ROUTER_USER@$ROUTER_HOST -p $ROUTER_PORT -o StrictHostKeyChecking=no -C "cd ~/Downloads;ls"
expect "*password:"
send "$ROUTER_PASSWORD\n\r\n"
expect eof;
EOF

首次远程连接时，会提示公钥确认，远程IP改变时，会提示公钥不一致，这种情况有时非常烦人。对于自动化任务，我们希望跳过公钥确认，修改/etc/ssh/ssh_config，加入：
1
StrictHostKeyChecking no

或者在命令行中使用-o参数： ssh -o StrictHostKeyChecking=no

运行结果:

1
2
3
4
5
6
7
spawn ssh xiaoqiang@192.168.0.110 -p 22 -o StrictHostKeyChecking=no -C cd ~/Downloads;ls
xiaoqiang@192.168.0.110's password: 
c-ares-1.17.0.tar.gz
cd1folder.zsh
CursorTheme
darcula.css
g2o_curve_fitting

在password一行之后为命令执行的结果，一直没法实现换行，不太完美

修改roslaunch源码|ROSROS Kinetic知识
abstract Welcome to my blog, enter password to read.
Read more
编译链接 ${catkin_LIBRARIES} 的问题|cmake/qmake|编译调试

编译xvisio相机驱动时，遇到了这样的错误

显然是ddynamic_reconfigure包的问题，于是安装对应的ROS包，但是问题依旧，真是奇怪

于是在cmake中使用MESSAGE(${catkin_LIBRARIES})看看链接的文件有什么问题，结果发现其他的ROS库都是链接到了ROS安装目录，比如 /opt/ros/noetic/lib/libcamera_info_manager.so， /usr/lib/x86_64-linux-gnu/libtinyxml2.so。 唯独ddynamic_reconfigure链接到了我之前放的库文件： /home/user/catkin_ws/devel/lib/libddynamic_reconfigure.so

这真是太奇怪了，环境变量catkin_LIBRARIES是如何换到另一个so库文件的??? 于是我把ROS安装目录的libddynamic_reconfigure.so覆盖catkin_ws的so库文件，重新编译成功了，相机驱动可以正常运行。

但是这样其实没有解决根本问题，最后只好把target_link_libraries的${catkin_LIBRARIES}换成一个个的so文件，在开头要LINK_DIRECTORIES(/opt/ros/noetic/lib)

cartographer的杂项模块|激光SLAMCartographer源码解读

detect_floors.cc

detect_floors.h定义了关于3D扫描楼层的数据结构。

1
2
3
4
5
6
7
8
9
10
struct Timespan {
	common::Time start;
	common::Time end;
};
Timespan表征扫描的时间范围。

struct Floor {
	std::vector< Timespan> timespans;
	double z; //z轴的中值
};

一个楼层对应多个扫描timespan：有可能重复的扫描多次，但只有一个高度z。

std::vector DetectFloors(const proto::Trajectory& trajectory); 使用启发式搜索寻找building的不同楼层的z值。

normal_estimation_2d.cc

用于计算TSDF地图中的点云法向量，可以不看。 normal_estimation_2d_test.cc是测试文件，但是没有在CMakeLists中设置 。

优雅地订阅话题|ROSROS程序和源码分析

这段代码其实是模仿cartographer的LaunchSubscribers函数，订阅多个传感器的数据话题，同时可以获得主函数里多个参数的赋值

主函数长这样

1
2
3
4
5
6
7
8
int main(int argc, char** argv)
{
    ros::init(argc, argv, "launch_subscribers");
    ros::NodeHandle nh;
    Node node;
    node.LaunchSubscribers("base_scan", 1, "/base_odometry/odom", 1, "/torso_lift_imu/data", 1);
    ros::spin();
}

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
44
45
46
void Node::LaunchSubscribers(const std::string& scan_topic, const int scan_num,
                             const std::string& odom_topic, const int use_odom,
                             const std::string& imu_topic,  const int use_imu
                            )
{
	if(scan_num)
        LaunchLaserScanSub(scan_topic, scan_num);
    if(use_odom)
        LaunchOdomSub(odom_topic, use_odom);
    if(use_imu)
        LaunchImuSub(imu_topic, use_imu);
}

void Node::LaunchLaserScanSub(const std::string& scan_topic, const int scan_num)
{
    subscribers_.push_back(
      { SubscribeWithHandler<sensor_msgs::LaserScan>(
           &Node::HandleLaserScanMessage, 
           scan_num, scan_topic, &node_handle_, this),
       scan_topic}  );
}

// 这里也可以把 int 改为 bool
template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(
    void (Node::*handler)(int, const std::string&,
                          const typename MessageType::ConstPtr&),
    const int num, const std::string& topic,
    ::ros::NodeHandle* const node_handle, Node* const node)
{
  return node_handle->subscribe<MessageType>(
      topic, 1,
      boost::function<void(const typename MessageType::ConstPtr&)>(
          [node, handler, num,
           topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(num, topic, msg);
          })  );
}

// 根据上面的void (Node::*handler)部分，参数依次为int,  string&,  MessageType::ConstPtr&
// scan_num 就是main函数里的1
void Node::HandleLaserScanMessage(int scan_num, const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg)
{
    ROS_INFO("range_min: %f, scan_num: %d", msg->range_min, scan_num );
}
Husky和Gazebo仿真平台|ROSROS机器人

Gazebo对电脑显卡有一定要求，不能太差了。另外对Nvidia显卡支持较好，对AMD的显卡支持较差，如果是AMD的显卡，复杂的世界模型一般加载不出来。

根据官方文档配置
husky_empty_world.launch
最好添加一行remap: < remap from

这里的报错很正常，不用管

view_robot.launch
不知道为什么Rviz里有两个很大的圆，不是话题，应该是Rviz的配置

参考：官方文档

(七) R树-空间索引|C++Boost

R树是为了加快地理方面的计算，构建一颗R树，然后非常方便的往R树里添加矩形索引。

namespace bgi = bg::index;

using RTree = bgi::rtree>;

(六) geometry|C++Boost
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
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
#include <ros/ros.h>
#include <boost/assign.hpp>
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <list>
using namespace std;
namespace bg = boost::geometry;
namespace bgi = bg::index;

using Point = bg::model::point<double, 2, bg::cs::cartesian>;
using Box = bg::model::box<Point>;
using Line = bg::model::linestring<Point>;
// 线段
using Segment = bg::model::segment<Point>;


int main(int argc, char** argv)
{
    ros::init(argc, argv, "test_boost");
    ros::NodeHandle nh;
    Point pt0(100, 100);
    Point pt1(200, 200);
    // 点的间距
    cout << "distance from pt0 to pt1: "<< bg::distance(pt0, pt1) << endl;

    Segment s1(Point(0, 100), Point(100, 0));
    Segment s2(Point(0, 0), Point(120, 120));
    // 点到线段的距离，垂足不一定在线段上
    cout << "distance from pt0 to s1: " << bg::distance(pt0, s1) << endl;
    // 两线段是否相交
    cout << "Is segment1 and segment2 intersect:  " << bg::intersects(s1, s2) << endl;

    std::list<Point> points;
    // 两线段的交点
    bg::intersection(s1, s2, points);
    for(auto p : points)
	cout << bg::dsv(p) << "  ";
    cout << endl;

    // 判断box是否相交
    Box rc(Point(0, 0), Point(200, 200));
    Box rc0(Point(250, 250), Point(450, 450));
    Box rc1(Point(100, 100), Point(300, 300));

    cout << "rc and rc0 intersect:  " << bg::intersects(rc, rc0) <<endl;
    cout << "rc and rc1 intersect:  " << bg::intersects(rc, rc1) <<endl;
    
    // 求外接矩形Bounding Box
    Line line = { {1,1},{2,0},{3,2} };
	Box box;
	bg::envelope(line, box);
	std::cout << bg::dsv(line) << "'s AABB is " << bg::dsv(box) << std::endl;

    return 0;
}

注意直线的初始化和线段是不一样的

参考:
SLAM本质剖析-Boost
boost::geometry简介

find 和 find_if, find_if_not|C++C++ 模板与STL

头文件： #include <algorithm>

返回范围 [first, last) 中满足特定判别标准的首个元素:

  • find: 搜索等于 value 的元素

  • find_if: 根据指定的查找规则，在指定区域内查找第一个符合该函数要求（使函数返回 true）的元素。如果查找成功，该迭代器指向的是查找到的那个元素；反之，如果查找失败，该迭代器的指向和 last 迭代器相同

  • find_if_not: 与find_if相反

示例代码：

1
2
3
4
5
6
7
8
9
10
11
std::vector<int> score{ 10, 20, 30, 40 };

auto it_1 = std::find(score.begin(), score.end(), 30 );
if(it_1 != score.end() )
    qDebug()<< "found !";
else qDebug()<< "no found !";

auto it_2 = std::find_if(score.begin(), score.end(), [](int& m){return m>20;}  );
if(it_2 != score.end() )
    qDebug()<< "found: " << *it_2;
else qDebug()<< "no found !";

  • 若作为算法一部分调用的函数的执行抛出异常，且 ExecutionPolicy 为标准策略之一，则调用 std::terminate 。对于任何其他 ExecutionPolicy ，行为是实现定义的。
  • 若算法无法分配内存，则抛出 std::bad_alloc