源码学习：obstacle-detector 01

本文最后更新于：2020年8月13日 下午

项目地址：https://github.com/tysik/obstacle_detector

1. launch

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
<node name="scans_merger" pkg="obstacle_detector" type="scans_merger_node">
	<param name="active"            value="true"/>
    <param name="publish_scan"      value="false"/>
    <param name="publish_pcl"       value="true"/>

    <param name="ranges_num"        value="1000"/>

    <param name="min_scanner_range" value="0.05"/>
    <param name="max_scanner_range" value="10.0"/>

    <param name="min_x_range"       value="-10.0"/>
    <param name="max_x_range"       value="10.0"/>
    <param name="min_y_range"       value="-10.0"/>
    <param name="max_y_range"       value="10.0"/>

    <param name="fixed_frame_id"   value="map"/>
    <param name="target_frame_id"   value="robot"/>
</node>

2. src

2.1 scans_merger_node.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
int main(int argc, char** argv) {
  ros::init(argc, argv, "scans_merger", ros::init_options::NoRosout);
  ros::NodeHandle nh("");
  ros::NodeHandle nh_local("~");

  try {
    ROS_INFO("[Scans Merger]: Initializing node");
    ScansMerger sm(nh, nh_local);
    ros::spin();
  }
  catch (const char* s) {
    ROS_FATAL_STREAM("[Scans Merger]: " << s);
  }
  catch (...) {
    ROS_FATAL_STREAM("[Scans Merger]: Unexpected error");
  }

  return 0;
}

2.2 scans_merger.cpp

2.2.1 ScansMerger::ScansMerger()

1
ScansMerger::ScansMerger(ros::NodeHandle& nh, ros::NodeHandle& nh_local) : nh_(nh), nh_local_(nh_local) {}

首先设置标志位

1
2
3
4
5
6
7
p_active_ = false;

front_scan_received_ = false;
rear_scan_received_ = false;

front_scan_error_ = false;
rear_scan_error_ = false;

初始化参数服务并调用成员函数initialize()

1
2
params_srv_ = nh_local_.advertiseService("params", &ScansMerger::updateParams, this);
initialize();
1
void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); }

2.2.2 ScansMerger::updateParam()

1
bool ScansMerger::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res){}

首先获取活动状态标志位，并加载参数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
bool prev_active = p_active_;
nh_local_.param<bool>("active", p_active_, true);
nh_local_.param<bool>("publish_scan", p_publish_scan_, false);
nh_local_.param<bool>("publish_pcl", p_publish_pcl_, true);

nh_local_.param<int>("ranges_num", p_ranges_num_, 1000);

nh_local_.param<double>("min_scanner_range", p_min_scanner_range_, 0.05);
nh_local_.param<double>("max_scanner_range", p_max_scanner_range_, 10.0);

nh_local_.param<double>("min_x_range", p_min_x_range_, -10.0);
nh_local_.param<double>("max_x_range", p_max_x_range_,  10.0);
nh_local_.param<double>("min_y_range", p_min_y_range_, -10.0);
nh_local_.param<double>("max_y_range", p_max_y_range_,  10.0);

nh_local_.param<string>("fixed_frame_id", p_fixed_frame_id_, "map");
nh_local_.param<string>("target_frame_id", p_target_frame_id_, "robot");

判断活动状态是否发生改变

1
if (p_active_ != prev_active) {}

如果状态为active，则订阅和发布话题

订阅的话题：

  • front_scan
  • rear_scan

发布的话题：

  • scan
  • pcl
1
2
3
4
5
6
if (p_active_) {
	front_scan_sub_ = nh_.subscribe("front_scan", 10, &ScansMerger::frontScanCallback, this);
	rear_scan_sub_ = nh_.subscribe("rear_scan", 10, &ScansMerger::rearScanCallback, this);
	scan_pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 10);
	pcl_pub_ = nh_.advertise<sensor_msgs::PointCloud>("pcl", 10);
}

否则，关闭所有的话题

1
2
3
4
5
6
else {
	front_scan_sub_.shutdown();
	rear_scan_sub_.shutdown();
	scan_pub_.shutdown();
	pcl_pub_.shutdown();
}

2.2.3 ScansMerger::frontScanCallback()

1
void ScansMerger::frontScanCallback(const sensor_msgs::LaserScan::ConstPtr front_scan) {}

首先进行 tf变换：laser ->fixed_frame_id，并将LaserScan转换为PointCloud

1
2
3
4
5
6
7
8
9
try {
    tf_ls_.waitForTransform(front_scan->header.frame_id, p_fixed_frame_id_,
                            front_scan->header.stamp + ros::Duration().fromSec(front_scan->ranges.size() * front_scan->time_increment), ros::Duration(0.05));
    projector_.transformLaserScanToPointCloud(p_fixed_frame_id_, *front_scan, front_pcl_, tf_ls_);
  }
  catch (tf::TransformException& ex) {
    front_scan_error_ = true;
    return;
  }

设置标志位

1
2
front_scan_received_ = true;
front_scan_error_ = false;

在rearScan成功接收到信息或者出现错误时发送信息

1
2
3
4
if (rear_scan_received_ || rear_scan_error_)
	publishMessages();
else
	rear_scan_error_ = true;

2.2.4 ScansMerger::frontScanCallback()

1
void ScansMerger::rearScanCallback(const sensor_msgs::LaserScan::ConstPtr rear_scan){}

与2.2.3 ScansMerger::frontScanCallback()大同小异

2.2.5 ScansMerger::publishMessages()

1
void ScansMerger::publishMessages() {}

首先一些临时变量的初始化

1
2
3
4
5
6
7
ros::Time now = ros::Time::now();

vector<float> ranges;
vector<geometry_msgs::Point32> points;
sensor_msgs::PointCloud new_front_pcl, new_rear_pcl;

ranges.assign(p_ranges_num_, nanf("")); // Assign nan values

如果front_scan没有出现错误

1
if (!front_scan_error_) {}

将数据进行 tf变换：map -> robot

1
2
3
4
5
6
7
try {
	tf_ls_.waitForTransform(p_target_frame_id_, now, front_pcl_.header.frame_id, front_pcl_.header.stamp, p_fixed_frame_id_, ros::Duration(0.05));
	tf_ls_.transformPointCloud(p_target_frame_id_, now, front_pcl_, p_fixed_frame_id_, new_front_pcl);
}
catch (tf::TransformException& ex) {
	return;
}

如果数据在设置的范围内

1
2
3
4
5
if (point.x > p_min_x_range_ && point.x < p_max_x_range_ &&
          point.y > p_min_y_range_ && point.y < p_max_y_range_) {
	double range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0));
    if (range > p_min_scanner_range_ && range < p_max_scanner_range_) {}
}

则根据标志位的信息将数据进行保存

1
2
3
4
5
6
7
8
9
10
11
if (p_publish_pcl_) {
	points.push_back(point);
}

if (p_publish_scan_) {
	double angle = atan2(point.y, point.x);

	size_t idx = static_cast<int>(p_ranges_num_ * (angle + M_PI) / (2.0 * M_PI));
	if (isnan(ranges[idx]) || range < ranges[idx])
		ranges[idx] = range;
}

发布LaserScan数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
if (p_publish_scan_) {
	sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan);

	scan_msg->header.frame_id = p_target_frame_id_;
	scan_msg->header.stamp = now;
	scan_msg->angle_min = -M_PI;
	scan_msg->angle_max = M_PI;
	scan_msg->angle_increment = 2.0 * M_PI / (p_ranges_num_ - 1);
	scan_msg->time_increment = 0.0;
	scan_msg->scan_time = 0.1;
	scan_msg->range_min = p_min_scanner_range_;
	scan_msg->range_max = p_max_scanner_range_;
	scan_msg->ranges.assign(ranges.begin(), ranges.end());

	scan_pub_.publish(scan_msg);
}

发布PointCloud数据

1
2
3
4
5
6
7
8
9
if (p_publish_pcl_) {
	sensor_msgs::PointCloudPtr pcl_msg(new sensor_msgs::PointCloud);

	pcl_msg->header.frame_id = p_target_frame_id_;
	pcl_msg->header.stamp = now;
	pcl_msg->points.assign(points.begin(), points.end());

	pcl_pub_.publish(pcl_msg);
}

设置标志位，为下一次接收做准备

1
2
front_scan_received_ = false;
rear_scan_received_ = false;
编程
ROS 源码学习

本博客所有文章除特别声明外，均采用 CC BY-SA 3.0协议 。转载请注明出处！