前往小程序,Get更优阅读体验!
立即前往
首页
学习
活动
专区
工具
TVP
发布
社区首页 >专栏 >ROS2机器人编程简述humble-第四章-BASIC DETECTOR .3

ROS2机器人编程简述humble-第四章-BASIC DETECTOR .3

作者头像
zhangrelay
发布2023-02-26 18:55:28
6120
发布2023-02-26 18:55:28
举报

书中程序适用于turtlebot、husky等多种机器人,配置相似都可以用的。

支持ROS2版本foxy、humble。

基础检测效果如下:

由于缺¥,所有设备都非常老旧,都是其他实验室淘汰或者拼凑出来的设备。机器人控制笔记本是2010年版本。

但是依然可以跑ROS1、ROS2。

book_ros2/br2_tf2_detector目录:

代码语言:javascript
复制
.
├── 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):

代码语言:javascript
复制
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()

障碍物识别节点

代码语言:javascript
复制
// 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

主要就是回调函数完成大部分功能。具体参考源代码即可。

障碍物监控节点:

代码语言:javascript
复制
// 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

代码和原始版本稍微有些不同。

重要部分:

代码语言:javascript
复制
  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没有

代码语言:javascript
复制
[detector-1] [WARN] [1676266943.177279939] [obstacle_monitor]: Obstacle transform not found: "odom" passed to lookupTransform argument target_frame does not exist.

例如detected_obstacle没有

代码语言:javascript
复制
[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是由激光传感器测距给出的。

节点主题图:

这个代码主程序!

代码语言:javascript
复制
// 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;
}

这里需要注意!

代码语言:javascript
复制
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(obstacle_detector->get_node_base_interface());
  executor.add_node(obstacle_monitor->get_node_base_interface());

如果C++掌握一般推荐看一看:

蓝桥ROS机器人之现代C++学习笔记7.1 并行基础

多线程是如何实现的。

整个程序要跑起来:

终端1-gazebo仿真:ros2 launch turtlebot3_gazebo empty_world.launch.py

代码语言:javascript
复制
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处理标记的数据类型。

本文参与 腾讯云自媒体同步曝光计划,分享自作者个人站点/博客。
原始发表:2023-02-13,如有侵权请联系 cloudcommunity@tencent.com 删除

本文分享自 作者个人站点/博客 前往查看

如有侵权,请联系 cloudcommunity@tencent.com 删除。

本文参与 腾讯云自媒体同步曝光计划  ,欢迎热爱写作的你一起参与!

评论
登录后参与评论
0 条评论
热度
最新
推荐阅读
领券
问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档