书中程序适用于turtlebot、husky等多种机器人，配置相似都可以用的。
支持ROS2版本foxy、humble。
基础检测效果如下：
由于缺￥，所有设备都非常老旧，都是其他实验室淘汰或者拼凑出来的设备。机器人控制笔记本是2010年版本。
但是依然可以跑ROS1、ROS2。
book_ros2/br2_tf2_detector目录：
-
.
-
├── CMakeLists.txt
-
├── include
-
│ └── br2_tf2_detector
-
│ ├── ObstacleDetectorImprovedNode.hpp
-
│ ├── ObstacleDetectorNode.hpp
-
│ └── ObstacleMonitorNode.hpp
-
├── launch
-
│ ├── detector_basic.launch.py
-
│ ├── detector_improved.launch.py
-
│ ├── turtlebot_detector_basic.launch.py
-
│ └── turtlebot_detector_improved.launch.py
-
├── package.xml
-
└── src
-
├── br2_tf2_detector
-
│ ├── ObstacleDetectorImprovedNode.cpp
-
│ ├── ObstacleDetectorNode.cpp
-
│ ├── ObstacleMonitorNode (copy).cpp
-
│ └── ObstacleMonitorNode.cpp
-
├── detector_improved_main.cpp
-
└── detector_main.cpp
-
-
5 directories, 15 files
里面有两个部分basic和improved。
CMakelist（lib）：
-
cmake_minimum_required(VERSION 3.5)
-
project(br2_tf2_detector)
-
-
set(CMAKE_CXX_STANDARD 17)
-
-
# find dependencies
-
find_package(ament_cmake REQUIRED)
-
find_package(rclcpp REQUIRED)
-
find_package(tf2_ros REQUIRED)
-
find_package(geometry_msgs REQUIRED)
-
find_package(sensor_msgs REQUIRED)
-
find_package(visualization_msgs REQUIRED)
-
-
set(dependencies
-
rclcpp
-
tf2_ros
-
geometry_msgs
-
sensor_msgs
-
visualization_msgs
-
)
-
-
include_directories(include)
-
-
add_library(
${PROJECT_NAME} SHARED
-
src/br2_tf2_detector/ObstacleDetectorNode.cpp
-
src/br2_tf2_detector/ObstacleMonitorNode.cpp
-
src/br2_tf2_detector/ObstacleDetectorImprovedNode.cpp
-
)
-
ament_target_dependencies(
${PROJECT_NAME}
${dependencies})
-
-
add_executable(detector src/detector_main.cpp)
-
ament_target_dependencies(detector
${dependencies})
-
target_link_libraries(detector
${PROJECT_NAME})
-
-
add_executable(detector_improved src/detector_improved_main.cpp)
-
ament_target_dependencies(detector_improved
${dependencies})
-
target_link_libraries(detector_improved
${PROJECT_NAME})
-
-
install(TARGETS
-
${PROJECT_NAME}
-
detector
-
detector_improved
-
ARCHIVE DESTINATION lib
-
LIBRARY DESTINATION lib
-
RUNTIME DESTINATION lib/
${PROJECT_NAME}
-
)
-
-
install(DIRECTORY launch DESTINATION share/
${PROJECT_NAME})
-
-
if(BUILD_TESTING)
-
find_package(ament_lint_auto REQUIRED)
-
ament_lint_auto_find_test_dependencies()
-
endif()
-
-
ament_package()
障碍物识别节点
-
// Copyright 2021 Intelligent Robotics Lab
-
//
-
// Licensed under the Apache License, Version 2.0 (the "License");
-
// you may not use this file except in compliance with the License.
-
// You may obtain a copy of the License at
-
//
-
// http://www.apache.org/licenses/LICENSE-2.0
-
//
-
// Unless required by applicable law or agreed to in writing, software
-
// distributed under the License is distributed on an "AS IS" BASIS,
-
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-
// See the License for the specific language governing permissions and
-
// limitations under the License.
-
-
#include <memory>
-
-
#include "br2_tf2_detector/ObstacleDetectorNode.hpp"
-
-
#include "sensor_msgs/msg/laser_scan.hpp"
-
#include "geometry_msgs/msg/transform_stamped.hpp"
-
-
#include "rclcpp/rclcpp.hpp"
-
-
namespace br2_tf2_detector
-
{
-
-
using std::placeholders::_1;
-
-
ObstacleDetectorNode::
ObstacleDetectorNode()
-
:
Node(
"obstacle_detector")
-
{
-
scan_sub_ =
create_subscription<sensor_msgs::msg::LaserScan>(
-
"input_scan", rclcpp::
SensorDataQoS(),
-
std::
bind(&ObstacleDetectorNode::scan_callback,
this, _1));
-
-
tf_broadcaster_ = std::
make_shared<tf2_ros::StaticTransformBroadcaster>(*
this);
-
}
-
-
void
-
ObstacleDetectorNode::scan_callback
(sensor_msgs::msg::LaserScan::UniquePtr msg)
-
{
-
double dist = msg->ranges[msg->ranges.
size() /
2];
-
-
if (!std::
isinf(dist)) {
-
geometry_msgs::msg::TransformStamped detection_tf;
-
-
detection_tf.header = msg->header;
-
detection_tf.child_frame_id =
"detected_obstacle";
-
detection_tf.transform.translation.x = msg->ranges[msg->ranges.
size() /
2];
-
-
tf_broadcaster_->
sendTransform(detection_tf);
-
}
-
}
-
-
}
// namespace br2_tf2_detector
主要就是回调函数完成大部分功能。具体参考源代码即可。
障碍物监控节点：
-
// Copyright 2021 Intelligent Robotics Lab
-
//
-
// Licensed under the Apache License, Version 2.0 (the "License");
-
// you may not use this file except in compliance with the License.
-
// You may obtain a copy of the License at
-
//
-
// http://www.apache.org/licenses/LICENSE-2.0
-
//
-
// Unless required by applicable law or agreed to in writing, software
-
// distributed under the License is distributed on an "AS IS" BASIS,
-
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-
// See the License for the specific language governing permissions and
-
// limitations under the License.
-
-
#include <tf2/transform_datatypes.h>
-
#include <tf2/LinearMath/Quaternion.h>
-
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-
-
#include <memory>
-
-
#include "br2_tf2_detector/ObstacleMonitorNode.hpp"
-
-
#include "geometry_msgs/msg/transform_stamped.hpp"
-
-
#include "rclcpp/rclcpp.hpp"
-
-
namespace br2_tf2_detector
-
{
-
-
using
namespace std::chrono_literals;
-
-
ObstacleMonitorNode::
ObstacleMonitorNode()
-
:
Node(
"obstacle_monitor"),
-
tf_buffer_(),
-
tf_listener_(tf_buffer_)
-
{
-
marker_pub_ =
create_publisher<visualization_msgs::msg::Marker>(
"obstacle_marker",
1);
-
-
timer_ =
create_wall_timer(
-
500ms, std::
bind(&ObstacleMonitorNode::control_cycle,
this));
-
}
-
-
void
-
ObstacleMonitorNode::control_cycle
()
-
{
-
geometry_msgs::msg::TransformStamped robot2obstacle;
-
-
try {
-
robot2obstacle = tf_buffer_.
lookupTransform(
-
"odom",
"detected_obstacle", tf2::TimePointZero);
-
}
catch (tf2::TransformException & ex) {
-
RCLCPP_WARN(
get_logger(),
"Obstacle transform not found: %s", ex.
what());
-
return;
-
}
-
-
double x = robot2obstacle.transform.translation.x;
-
double y = robot2obstacle.transform.translation.y;
-
double z = robot2obstacle.transform.translation.z;
-
double theta =
atan2(y, x);
-
-
RCLCPP_INFO(
-
get_logger(),
"Obstacle detected at (%lf m, %lf m, , %lf m) = %lf rads",
-
x, y, z, theta);
-
-
visualization_msgs::msg::Marker obstacle_arrow;
-
obstacle_arrow.header.frame_id =
"odom";
-
obstacle_arrow.header.stamp =
now();
-
obstacle_arrow.type = visualization_msgs::msg::Marker::ARROW;
-
obstacle_arrow.action = visualization_msgs::msg::Marker::ADD;
-
obstacle_arrow.lifetime = rclcpp::
Duration(
1s);
-
-
geometry_msgs::msg::Point start;
-
start.x =
0.0;
-
start.y =
0.0;
-
start.z =
0.0;
-
geometry_msgs::msg::Point end;
-
end.x = x;
-
end.y = y;
-
end.z = z;
-
obstacle_arrow.points = {start, end};
-
-
obstacle_arrow.color.r =
1.0;
-
obstacle_arrow.color.g =
0.0;
-
obstacle_arrow.color.b =
0.0;
-
obstacle_arrow.color.a =
1.0;
-
-
obstacle_arrow.scale.x =
0.02;
-
obstacle_arrow.scale.y =
0.1;
-
obstacle_arrow.scale.z =
0.1;
-
-
-
marker_pub_->
publish(obstacle_arrow);
-
}
-
-
}
// namespace br2_tf2_detector
代码和原始版本稍微有些不同。
重要部分：
-
try {
-
robot2obstacle = tf_buffer_.
lookupTransform(
-
"odom",
"detected_obstacle", tf2::TimePointZero);
-
}
catch (tf2::TransformException & ex) {
-
RCLCPP_WARN(
get_logger(),
"Obstacle transform not found: %s", ex.
what());
-
return;
-
}
-
-
double x = robot2obstacle.transform.translation.x;
-
double y = robot2obstacle.transform.translation.y;
-
double z = robot2obstacle.transform.translation.z;
-
double theta =
atan2(y, x);
-
-
RCLCPP_INFO(
-
get_logger(),
"Obstacle detected at (%lf m, %lf m, , %lf m) = %lf rads",
-
x, y, z, theta);
如果tf不能正常工作，会报错Obstacle transform not found：
例如odom没有
[detector-1] [WARN] [1676266943.177279939] [obstacle_monitor]: Obstacle transform not found: "odom" passed to lookupTransform argument target_frame does not exist.
例如detected_obstacle没有
[detector-1] [WARN] [1676267019.166991316] [obstacle_monitor]: Obstacle transform not found: "detected_obstacle" passed to lookupTransform argument source_frame does not exist. 
需要思考并解决问题哦^_^
如果都ok！那么"Obstacle detected at (%lf m, %lf m, , %lf m) = %lf rads"：
机器人在运动中所以角度和距离会不断变化。
此时如果查看：
rqt
其中检测tf是由激光传感器测距给出的。
节点主题图：
这个代码主程序！
-
// Copyright 2021 Intelligent Robotics Lab
-
//
-
// Licensed under the Apache License, Version 2.0 (the "License");
-
// you may not use this file except in compliance with the License.
-
// You may obtain a copy of the License at
-
//
-
// http://www.apache.org/licenses/LICENSE-2.0
-
//
-
// Unless required by applicable law or agreed to in writing, software
-
// distributed under the License is distributed on an "AS IS" BASIS,
-
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-
// See the License for the specific language governing permissions and
-
// limitations under the License.
-
-
#include <memory>
-
-
#include "br2_tf2_detector/ObstacleDetectorNode.hpp"
-
#include "br2_tf2_detector/ObstacleMonitorNode.hpp"
-
-
#include "rclcpp/rclcpp.hpp"
-
-
int main(int argc, char * argv[])
-
{
-
rclcpp::
init(argc, argv);
-
-
auto obstacle_detector = std::
make_shared<br2_tf2_detector::ObstacleDetectorNode>();
-
auto obstacle_monitor = std::
make_shared<br2_tf2_detector::ObstacleMonitorNode>();
-
-
rclcpp::executors::SingleThreadedExecutor executor;
-
executor.
add_node(obstacle_detector->
get_node_base_interface());
-
executor.
add_node(obstacle_monitor->
get_node_base_interface());
-
-
executor.
spin();
-
-
rclcpp::
shutdown();
-
return
0;
-
}
这里需要注意！
-
rclcpp::executors::SingleThreadedExecutor executor;
-
executor.
add_node(obstacle_detector->
get_node_base_interface());
-
executor.
add_node(obstacle_monitor->
get_node_base_interface());
如果C++掌握一般推荐看一看：
多线程是如何实现的。
整个程序要跑起来：
终端1-gazebo仿真：ros2 launch turtlebot3_gazebo empty_world.launch.py
-
ros2 launch turtlebot3_gazebo empty_world.launch.py
-
[INFO] [launch]: All
log files can be found below /home/zhangrelay/.ros/log/2023-02-13-13-43-10-244500-Aspire4741-10860
-
[INFO] [launch]: Default logging verbosity is
set to INFO
-
urdf_file_name : turtlebot3_burger.urdf
-
[INFO] [gzserver-1]: process started with pid [10862]
-
[INFO] [gzclient -2]: process started with pid [10864]
-
[INFO] [ros2-3]: process started with pid [10868]
-
[INFO] [robot_state_publisher-4]: process started with pid [10870]
-
[robot_state_publisher-4] [WARN] [1676266991.467830827] [robot_state_publisher]: No robot_description parameter, but command-line argument available. Assuming argument is name of URDF file. This backwards compatibility fallback will be removed
in the future.
-
[robot_state_publisher-4] Parsing robot urdf xml string.
-
[robot_state_publisher-4] Link base_link had 5 children
-
[robot_state_publisher-4] Link caster_back_link had 0 children
-
[robot_state_publisher-4] Link imu_link had 0 children
-
[robot_state_publisher-4] Link base_scan had 0 children
-
[robot_state_publisher-4] Link wheel_left_link had 0 children
-
[robot_state_publisher-4] Link wheel_right_link had 0 children
-
[robot_state_publisher-4] [INFO] [1676266991.472337172] [robot_state_publisher]: got segment base_footprint
-
[robot_state_publisher-4] [INFO] [1676266991.472419811] [robot_state_publisher]: got segment base_link
-
[robot_state_publisher-4] [INFO] [1676266991.472444636] [robot_state_publisher]: got segment base_scan
-
[robot_state_publisher-4] [INFO] [1676266991.472465018] [robot_state_publisher]: got segment caster_back_link
-
[robot_state_publisher-4] [INFO] [1676266991.472485972] [robot_state_publisher]: got segment imu_link
-
[robot_state_publisher-4] [INFO] [1676266991.472505808] [robot_state_publisher]: got segment wheel_left_link
-
[robot_state_publisher-4] [INFO] [1676266991.472525491] [robot_state_publisher]: got segment wheel_right_link
-
[ros2-3] Set parameter successful
-
[INFO] [ros2-3]: process has finished cleanly [pid 10868]
-
[gzserver-1] [INFO] [1676266994.292818234] [turtlebot3_imu]: <initial_orientation_as_reference> is
unset, using default value of
false to comply with REP 145 (world as orientation reference)
-
[gzserver-1] [INFO] [1676266994.417396256] [turtlebot3_diff_drive]: Wheel pair 1 separation
set to [0.160000m]
-
[gzserver-1] [INFO] [1676266994.417528534] [turtlebot3_diff_drive]: Wheel pair 1 diameter
set to [0.066000m]
-
[gzserver-1] [INFO] [1676266994.420616206] [turtlebot3_diff_drive]: Subscribed to [/cmd_vel]
-
[gzserver-1] [INFO] [1676266994.425994254] [turtlebot3_diff_drive]: Advertise odometry on [/odom]
-
[gzserver-1] [INFO] [1676266994.428920116] [turtlebot3_diff_drive]: Publishing odom transforms between [odom] and [base_footprint]
-
[gzserver-1] [INFO] [1676266994.460852885] [turtlebot3_joint_state]: Going to publish joint [wheel_left_joint]
-
[gzserver-1] [INFO] [1676266994.461009035] [turtlebot3_joint_state]: Going to publish joint [wheel_right_joint]
-
终端2-障碍物检测：
ros2 launch br2_tf2_detector turtlebot_detector_basic.launch.py
终端3-rqt：rqt
终端4-rviz2：rviz2
windows端也可以获取信息。
补充：
四元数是方向的4元组表示，它比旋转矩阵更简洁。四元数对于分析涉及三维旋转的情况非常有效。四元数广泛应用于机器人、量子力学、计算机视觉和3D动画。
可以在维基百科上了解更多关于基础数学概念的信息。还可以观看一个可探索的视频系列，将3blue1brown制作的四元数可视化。
官方教程将指导完成调试典型tf2问题的步骤。它还将使用许多tf2调试工具，如tf2_echo、tf2_monitor和view_frames。
TF2完整教程提纲：
tf2
许多tf2教程都适用于C++和Python。这些教程经过简化，可以完成C++曲目或Python曲目。如果想同时学习C++和Python，应该学习一次C++教程和一次Python教程。
目录
工作区设置
学习tf2
调试tf2
将传感器消息与tf2一起使用
工作区设置
如果尚未创建完成教程的工作空间，请遵循本教程。
学习tf2
tf2简介。
本教程将让了解tf2可以为您做什么。它在一个多机器人的例子中展示了一些tf2的力量，该例子使用了turtlesim。这还介绍了使用tf2_echo、view_frames和rviz。
编写静态广播（Python）（C++）。
本教程教如何向tf2广播静态坐标帧。
编写广播（Python）（C++）。
本教程教如何向tf2广播机器人的状态。
编写监听器（Python）（C++）。
本教程教如何使用tf2访问帧变换。
添加框架（Python）（C++）。
本教程教如何向tf2添加额外的固定帧。
使用时间（Python）（C++）。
本教程教使用lookup_transform函数中的超时来等待tf2树上的转换可用。
时间旅行（Python）（C++）。
本教程向介绍tf2的高级时间旅行功能。
调试tf2
四元数基本原理。
本教程介绍ROS 2中四元数的基本用法。
调试tf2问题。
本教程向介绍调试tf2相关问题的系统方法。
将传感器消息与tf2一起使用
对tf2_ros：：MessageFilter使用标记数据类型。
本教程教您如何使用tf2_ros：：MessageFilter处理标记的数据类型。
转载：https://blog.csdn.net/ZhangRelay/article/details/129007417
查看评论