当前位置：网站首页>Interesting practice of robot programming 15- autoavoidobstacles
Interesting practice of robot programming 15- autoavoidobstacles
2022-07-05 01:32:00 【zhangrelay】
Before , Whether it's a two-dimensional platform , Or 3D platform , It's all controlled by keyboard , about turtlebot3 The robot is equipped with laser sensors , You can measure around 360 The distance between obstacles , This makes it very convenient to use it for autonomous driving to avoid obstacles .
Here the autonomous exercise is the most basic function, that is, to avoid obstacles in the environment, driving randomly in the open space .
Robot selection ：
- export TURTLEBOT3_MODEL=burger
- export TURTLEBOT3_MODEL=waffle_pi
One out of two , Then you don't have to start the keyboard remote control node , Change to the following node ：
- ros2 run turtlebot3_gazebo turtlebot3_drive
next , Continue to turn on 3D Visualization ：
- ros2 launch turtlebot3_bringup rviz2.launch.py
The simulation software is Gazebo, The visualization tool is rviz, Don't use it wrong ^_^
get set
need ROS2+TurtleBot3 Simulation package .
practice
1 Basic commands
Need to master ：
- ros2 run
- ros2 launch
Turn on the simulation environment and obstacle avoidance nodes .
2 rqt Tools
Use rqt_graph Etc , Node information flow .
3 Source code reading
launch
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
world_file_name = 'turtlebot3_worlds/' + TURTLEBOT3_MODEL + '.model'
world = os.path.join(get_package_share_directory('turtlebot3_gazebo'),
'worlds', world_file_name)
launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
return LaunchDescription([
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
),
launch_arguments={'world': world}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource([launch_file_dir, '/robot_state_publisher.launch.py']),
launch_arguments={'use_sim_time': use_sim_time}.items(),
),
])
- 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.
drive
#include "turtlebot3_gazebo/turtlebot3_drive.hpp"
#include <memory>
using namespace std::chrono_literals;
Turtlebot3Drive::Turtlebot3Drive()
: Node("turtlebot3_drive_node")
{
/************************************************************
** Initialise variables
************************************************************/
scan_data_[0] = 0.0;
scan_data_[1] = 0.0;
scan_data_[2] = 0.0;
robot_pose_ = 0.0;
prev_robot_pose_ = 0.0;
/************************************************************
** Initialise ROS publishers and subscribers
************************************************************/
auto qos = rclcpp::QoS(rclcpp::KeepLast(10));
// Initialise publishers
cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", qos);
// Initialise subscribers
scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
"scan", \
rclcpp::SensorDataQoS(), \
std::bind(
&Turtlebot3Drive::scan_callback, \
this, \
std::placeholders::_1));
odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
"odom", qos, std::bind(&Turtlebot3Drive::odom_callback, this, std::placeholders::_1));
/************************************************************
** Initialise ROS timers
************************************************************/
update_timer_ = this->create_wall_timer(10ms, std::bind(&Turtlebot3Drive::update_callback, this));
RCLCPP_INFO(this->get_logger(), "Turtlebot3 simulation node has been initialised");
}
Turtlebot3Drive::~Turtlebot3Drive()
{
RCLCPP_INFO(this->get_logger(), "Turtlebot3 simulation node has been terminated");
}
/********************************************************************************
** Callback functions for ROS subscribers
********************************************************************************/
void Turtlebot3Drive::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
tf2::Quaternion q(
msg->pose.pose.orientation.x,
msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z,
msg->pose.pose.orientation.w);
tf2::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
robot_pose_ = yaw;
}
void Turtlebot3Drive::scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
uint16_t scan_angle[3] = {0, 30, 330};
for (int num = 0; num < 3; num++) {
if (std::isinf(msg->ranges.at(scan_angle[num]))) {
scan_data_[num] = msg->range_max;
} else {
scan_data_[num] = msg->ranges.at(scan_angle[num]);
}
}
}
void Turtlebot3Drive::update_cmd_vel(double linear, double angular)
{
geometry_msgs::msg::Twist cmd_vel;
cmd_vel.linear.x = linear;
cmd_vel.angular.z = angular;
cmd_vel_pub_->publish(cmd_vel);
}
/********************************************************************************
** Update functions
********************************************************************************/
void Turtlebot3Drive::update_callback()
{
static uint8_t turtlebot3_state_num = 0;
double escape_range = 30.0 * DEG2RAD;
double check_forward_dist = 0.7;
double check_side_dist = 0.6;
switch (turtlebot3_state_num) {
case GET_TB3_DIRECTION:
if (scan_data_[CENTER] > check_forward_dist) {
if (scan_data_[LEFT] < check_side_dist) {
prev_robot_pose_ = robot_pose_;
turtlebot3_state_num = TB3_RIGHT_TURN;
} else if (scan_data_[RIGHT] < check_side_dist) {
prev_robot_pose_ = robot_pose_;
turtlebot3_state_num = TB3_LEFT_TURN;
} else {
turtlebot3_state_num = TB3_DRIVE_FORWARD;
}
}
if (scan_data_[CENTER] < check_forward_dist) {
prev_robot_pose_ = robot_pose_;
turtlebot3_state_num = TB3_RIGHT_TURN;
}
break;
case TB3_DRIVE_FORWARD:
update_cmd_vel(LINEAR_VELOCITY, 0.0);
turtlebot3_state_num = GET_TB3_DIRECTION;
break;
case TB3_RIGHT_TURN:
if (fabs(prev_robot_pose_ - robot_pose_) >= escape_range) {
turtlebot3_state_num = GET_TB3_DIRECTION;
} else {
update_cmd_vel(0.0, -1 * ANGULAR_VELOCITY);
}
break;
case TB3_LEFT_TURN:
if (fabs(prev_robot_pose_ - robot_pose_) >= escape_range) {
turtlebot3_state_num = GET_TB3_DIRECTION;
} else {
update_cmd_vel(0.0, ANGULAR_VELOCITY);
}
break;
default:
turtlebot3_state_num = GET_TB3_DIRECTION;
break;
}
}
/*******************************************************************************
** Main
*******************************************************************************/
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Turtlebot3Drive>());
rclcpp::shutdown();
return 0;
}
- 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.
- 61.
- 62.
- 63.
- 64.
- 65.
- 66.
- 67.
- 68.
- 69.
- 70.
- 71.
- 72.
- 73.
- 74.
- 75.
- 76.
- 77.
- 78.
- 79.
- 80.
- 81.
- 82.
- 83.
- 84.
- 85.
- 86.
- 87.
- 88.
- 89.
- 90.
- 91.
- 92.
- 93.
- 94.
- 95.
- 96.
- 97.
- 98.
- 99.
- 100.
- 101.
- 102.
- 103.
- 104.
- 105.
- 106.
- 107.
- 108.
- 109.
- 110.
- 111.
- 112.
- 113.
- 114.
- 115.
- 116.
- 117.
- 118.
- 119.
- 120.
- 121.
- 122.
- 123.
- 124.
- 125.
- 126.
- 127.
- 128.
- 129.
- 130.
- 131.
- 132.
- 133.
- 134.
- 135.
- 136.
- 137.
- 138.
- 139.
- 140.
- 141.
- 142.
- 143.
- 144.
- 145.
- 146.
- 147.
- 148.
- 149.
- 150.
- 151.
- 152.
- 153.
- 154.
- 155.
- 156.
- 157.
- 158.
This involves some key reading sensors ,0 degree ,30 degree ,-30 Distance in degrees .
Then there are some initial values to look at the header file ：
#ifndef TURTLEBOT3_GAZEBO__TURTLEBOT3_DRIVE_HPP_
#define TURTLEBOT3_GAZEBO__TURTLEBOT3_DRIVE_HPP_
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#define DEG2RAD (M_PI / 180.0)
#define RAD2DEG (180.0 / M_PI)
#define CENTER 0
#define LEFT 1
#define RIGHT 2
#define LINEAR_VELOCITY 0.3
#define ANGULAR_VELOCITY 1.5
#define GET_TB3_DIRECTION 0
#define TB3_DRIVE_FORWARD 1
#define TB3_RIGHT_TURN 2
#define TB3_LEFT_TURN 3
class Turtlebot3Drive : public rclcpp::Node
{
public:
Turtlebot3Drive();
~Turtlebot3Drive();
private:
// ROS topic publishers
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
// ROS topic subscribers
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
// Variables
double robot_pose_;
double prev_robot_pose_;
double scan_data_[3];
// ROS timer
rclcpp::TimerBase::SharedPtr update_timer_;
// Function prototypes
void update_callback();
void update_cmd_vel(double linear, double angular);
void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
};
#endif // TURTLEBOT3_GAZEBO__TURTLEBOT3_DRIVE_HPP_
- 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.
This is a general and easy obstacle avoidance code , As long as it is a laser sensor, two wheel differential car is applicable .
边栏推荐
- Wechat applet: the latest WordPress black gold wallpaper wechat applet two open repair version source code download support traffic main revenue
- Wechat applet: Xingxiu UI v1.5 WordPress system information resources blog download applet wechat QQ dual end source code support WordPress secondary classification loading animation optimization
- Check if this is null - checking if this is null
- phpstrom设置函数注释说明
- [wave modeling 1] theoretical analysis and MATLAB simulation of wave modeling
- Main window in QT application
- Kibana installation and configuration
- [pure tone hearing test] pure tone hearing test system based on MATLAB
- Lsblk command - check the disk of the system. I don't often use this command, but it's still very easy to use. Onion duck, like, collect, pay attention, wait for your arrival!
- 微信小程序:全网独家小程序版本独立微信社群人脉
猜你喜欢
![[pure tone hearing test] pure tone hearing test system based on MATLAB](/img/1c/62ed6b3eb27a4dff976c4a2700a850.png)
[pure tone hearing test] pure tone hearing test system based on MATLAB
【海浪建模3】三维随机真实海浪建模以及海浪发电机建模matlab仿真
Basic operations of database and table ----- create index
Actual combat simulation │ JWT login authentication
Nebula Importer 数据导入实践
Call Huawei order service to verify the purchase token interface and return connection reset
Lsblk command - check the disk of the system. I don't often use this command, but it's still very easy to use. Onion duck, like, collect, pay attention, wait for your arrival!
流批一體在京東的探索與實踐
Take you ten days to easily complete the go micro service series (IX. link tracking)
[wave modeling 3] three dimensional random real wave modeling and wave generator modeling matlab simulation
随机推荐
DOM basic syntax
Application and Optimization Practice of redis in vivo push platform
[FPGA tutorial case 10] design and implementation of complex multiplier based on Verilog
微信小程序：微群人脉微信小程序源码下载全新社群系统优化版支持代理会员系统功能超高收益
[development of large e-commerce projects] performance pressure test - Performance Monitoring - heap memory and garbage collection -39
[wave modeling 1] theoretical analysis and MATLAB simulation of wave modeling
【FPGA教程案例9】基于vivado核的时钟管理器设计与实现
如果消费互联网比喻成「湖泊」的话，产业互联网则是广阔的「海洋」
[OpenGL learning notes 8] texture
C语音常用的位运算技巧
To sort out messy header files, I use include what you use
BGP comprehensive experiment
MySQL backup and recovery + experiment
视频网站手绘
Talking about JVM 4: class loading mechanism
实战模拟│JWT 登录认证
How to use words to describe breaking change in Spartacus UI of SAP e-commerce cloud
Analysis and comparison of leetcode weekly race + acwing weekly race (t4/t3)
微信小程序：星宿UI V1.5 wordpress系统资讯资源博客下载小程序微信QQ双端源码支持wordpress二级分类 加载动画优化
Research Report on the overall scale, major producers, major regions, products and application segmentation of agricultural automatic steering system in the global market in 2022