Interesting Practice of Robot Programming 15-Remote Control to Automatic (AutoAvoidObstacles)

Previously, whether it was a two-dimensional or three-dimensional platform, it was remotely controlled by a keyboard. The turtlebot3 robot is equipped with a laser sensor, which can measure the distance of 360-degree obstacles around it, which is very convenient to use it for autonomous driving to avoid obstacles. .

The autonomous exercise here is the most basic function, which is to avoid obstacles in the environment and drive randomly in the open.

Robot selection:

  1. export TURTLEBOT3_MODEL=burger
  2. export TURTLEBOT3_MODEL=waffle_pi

Just choose one of the two, and then do not need to start the keyboard remote control node, change to the following node:

  • ros2 run turtlebot3_gazebo turtlebot3_drive

Then, continue to open the 3D visualization:

  • ros2 launch turtlebot3_bringup rviz2.launch.py

The simulation software is Gazebo, the visualization tool is rviz, don’t use it wrong ^_^

preparation

ROS2+TurtleBot3 simulation package is required.

practice

1 Basic commands

Need to master:

  1. ros2 run
  2. ros2 launch

Turn on the simulation environment and obstacle avoidance driving node.

2 rqt tools

Use rqt_graph to view, node information flow.

3 Source code reading

launch

import os from ament_index_python.packages import get_package_share_directoryfrom launch import LaunchDescriptionfrom launch.actions import IncludeLaunchDescriptionfrom launch.launch_description_sources import PythonLaunchDescriptionSourcefrom 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(),        ),    ])

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;}

This involves some key reading sensor, 0 degree, 30 degree, -30 degree distance.

Then there are some initial values ​​that need to be viewed in 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_

This is a universal code for simple obstacle avoidance driving, as long as it is a two-wheel differential car with a laser sensor.