当前位置：网站首页>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 .
边栏推荐
- 【海浪建模2】三维海浪建模以及海浪发电机建模matlab仿真
- Jcenter () cannot find Alibaba cloud proxy address
- La jeunesse sans rancune de Xi Murong
- PHP wechat official account development
- Basic operation of database and table ----- the concept of index
- Global and Chinese markets for industrial X-ray testing equipment 2022-2028: Research Report on technology, participants, trends, market size and share
- C语音常用的位运算技巧
- [microprocessor] VHDL development of microprocessor based on FPGA
- 整理混乱的头文件，我用include what you use
- Roads and routes -- dfs+topsort+dijkstra+ mapping
猜你喜欢
![[flutter topic] 64 illustration basic textfield text input box (I) ＃ yyds dry goods inventory ＃](/img/1c/deaf20d46e172af4d5e11c28c254cf.jpg)
[flutter topic] 64 illustration basic textfield text input box (I) ＃ yyds dry goods inventory ＃
Logstash、Fluentd、Fluent Bit、Vector？ How to choose the appropriate open source log collector
![[wave modeling 2] three dimensional wave modeling and wave generator modeling matlab simulation](/img/50/b6cecc95e46fe1e445eb00ca415669.png)
[wave modeling 2] three dimensional wave modeling and wave generator modeling matlab simulation
Wechat applet: new independent backstage Yuelao office one yuan dating blind box
DOM basic syntax
Is there a sudden failure on the line? How to make emergency diagnosis, troubleshooting and recovery
BGP comprehensive experiment
微信小程序：微群人脉微信小程序源码下载全新社群系统优化版支持代理会员系统功能超高收益
How to use words to describe breaking change in Spartacus UI of SAP e-commerce cloud
Application and Optimization Practice of redis in vivo push platform
随机推荐
Wechat applet: exclusive applet version of the whole network, independent wechat community contacts
Are you still writing the TS type code
PHP Joseph Ring problem
Grabbing and sorting out external articles -- status bar [4]
Common bit operation skills of C speech
Es uses collapsebuilder to de duplicate and return only a certain field
MySQL regexp: Regular Expression Query
【LeetCode】88. Merge two ordered arrays
Compare whether two lists are equal
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
Phpstrom setting function annotation description
Arbitrum: two-dimensional cost
Wechat applet: wechat applet source code download new community system optimized version support agent member system function super high income
What sparks can applet container technology collide with IOT
增量备份 ?db full
Wechat applet: the latest WordPress black gold wallpaper wechat applet two open repair version source code download support traffic main revenue
Exploration and practice of integration of streaming and wholesale in jd.com
微信小程序；胡言乱语生成器
Global and Chinese market of portable CNC cutting machines 2022-2028: Research Report on technology, participants, trends, market size and share
MySQL backup and recovery + experiment