当前位置：网站首页>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 .
边栏推荐
- Poap: the adoption entrance of NFT?
- Application and Optimization Practice of redis in vivo push platform
- Call Huawei order service to verify the purchase token interface and return connection reset
- Take you ten days to easily complete the go micro service series (IX. link tracking)
- 流批一体在京东的探索与实践
- Four pits in reentrantlock!
- Incremental backup? db full
- How to use words to describe breaking change in Spartacus UI of SAP e-commerce cloud
- 【LeetCode】88. Merge two ordered arrays
- Global and Chinese markets for stratospheric UAV payloads 2022-2028: Research Report on technology, participants, trends, market size and share
猜你喜欢
Blue Bridge Cup Square filling (DFS backtracking)
Yyds dry inventory swagger positioning problem ⽅ formula
Is there a sudden failure on the line? How to make emergency diagnosis, troubleshooting and recovery
Basic operations of database and table ----- delete index
实战模拟│JWT 登录认证
A simple SSO unified login design
Wechat applet: independent background with distribution function, Yuelao office blind box for making friends
Database performance optimization tool
Call Huawei order service to verify the purchase token interface and return connection reset
PHP wechat official account development
随机推荐
Nebula Importer 数据导入实践
JS implementation determines whether the point is within the polygon range
Redis master-slave replication cluster and recovery ideas for abnormal data loss ＃ yyds dry goods inventory ＃
[wave modeling 2] three dimensional wave modeling and wave generator modeling matlab simulation
Yyds dry inventory swagger positioning problem ⽅ formula
整理混乱的头文件，我用include what you use
phpstrom设置函数注释说明
Introduction to the gtid mode of MySQL master-slave replication
pytorch fine-tuning (funtune) : 镂空设计or 偷梁换柱
Great God developed the new H5 version of arXiv, saying goodbye to formula typography errors in one step, and mobile phones can also easily read literature
实战模拟│JWT 登录认证
Poap: the adoption entrance of NFT?
微信小程序：星宿UI V1.5 wordpress系统资讯资源博客下载小程序微信QQ双端源码支持wordpress二级分类 加载动画优化
[wave modeling 3] three dimensional random real wave modeling and wave generator modeling matlab simulation
Can financial products be redeemed in advance?
Robley's global and Chinese markets 2022-2028: technology, participants, trends, market size and share Research Report
DOM basic syntax
Kibana installation and configuration
【微处理器】基于FPGA的微处理器VHDL开发
Basic operations of database and table ----- delete index