当前位置：网站首页>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 .

Interesting practice of robot programming 15- Remote control to automatic （AutoAvoidObstacles）_ Force plan

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 ^_^

Interesting practice of robot programming 15- Remote control to automatic （AutoAvoidObstacles）_ Interesting practice _02

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 .

Interesting practice of robot programming 15- Remote control to automatic （AutoAvoidObstacles）_ Robot programming _03

2 rqt Tools

Use rqt_graph Etc , Node information flow .

Interesting practice of robot programming 15- Remote control to automatic （AutoAvoidObstacles）_ Robot programming _04

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.