Του Marco Arruda

Σε αυτήν την ανάρτηση θα μάθετε πώς να προγραμματίζετε ένα ρομπότ για να αποφεύγετε εμπόδια χρησιμοποιώντας ROS2 και C++. Μέχρι το τέλος της ανάρτησης, το ρομπότ Dolly κινείται αυτόνομα σε μια σκηνή με πολλά εμπόδια, που προσομοιώνεται με το Gazebo 11.

Θα μάθετε:

• Πώς να δημοσιεύσετε ΚΑΙ να εγγραφείτε θέματα στον ίδιο κόμβο ROS2
• Πώς να αποφύγετε τα εμπόδια
• Πώς να εφαρμόσετε τον δικό σας αλγόριθμο σε ROS2 και C++

1 – Περιβάλλον εγκατάστασης – Εκκίνηση προσομοίωσης

Πριν οτιδήποτε άλλο φρόντισε να έχεις το rosject από την προηγούμενη ανάρτηση, μπορείς να το αντιγράψεις από εδώ.

Εκκινήστε την προσομοίωση σε ένα κέλυφος ιστού και σε διαφορετική καρτέλα, ελέγξτε τα θέματα που έχουμε διαθέσιμα. Πρέπει να πάρετε κάτι παρόμοιο με την παρακάτω εικόνα:

2 – Δημιουργήστε τον κόμβο

Για να έχουμε τον αλγόριθμο αποφυγής εμποδίων, ας δημιουργήσουμε ένα νέο εκτελέσιμο αρχείο στο αρχείο ~/ros2_ws/src/my_package/obstacle_avoidance.cpp:

#include "geometry_msgs/msg/twist.hpp"    // Twist
#include "rclcpp/rclcpp.hpp"              // ROS Core Libraries
#include "sensor_msgs/msg/laser_scan.hpp" // Laser Scan

using std::placeholders::_1;

class ObstacleAvoidance : public rclcpp::Node 
public:
  ObstacleAvoidance() : Node("ObstacleAvoidance") 

    auto default_qos = rclcpp::QoS(rclcpp::SystemDefaultsQoS());
    subscription_ = this->create_subscription(
        "laser_scan", default_qos,
        std::bind(&ObstacleAvoidance::topic_callback, this, _1));
    publisher_ =
        this->create_publisher("cmd_vel", 10);
  

private:
  void topic_callback(const sensor_msgs::msg::LaserScan::SharedPtr _msg) 
    // 200 readings, from right to left, from -57 to 57 degress
    // calculate new velocity cmd
    float min = 10;
    for (int i = 0; i < 200; i++)  float current = _msg->ranges[i];
      if (current < min)  min = current;   
    auto message = this->calculateVelMsg(min);
    publisher_->publish(message);
  
  geometry_msgs::msg::Twist calculateVelMsg(float distance) 
    auto msg = geometry_msgs::msg::Twist();
    // logic
    RCLCPP_INFO(this->get_logger(), "Distance is: '%f'", distance);
    if (distance < 1) 
      // turn around
      msg.linear.x = 0;
      msg.angular.z = 0.3;
     else 
      // go straight ahead
      msg.linear.x = 0.3;
      msg.angular.z = 0;
    
    return msg;
  
  rclcpp::Publisher::SharedPtr publisher_;
  rclcpp::Subscription::SharedPtr subscription_;
;

int main(int argc, char *argv[]) 
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared());
  rclcpp::shutdown();
  return 0;

Στο κύριος λειτουργία έχουμε:

• Αρχικοποίηση κόμβου rclcpp::init
• Συνεχίστε να λειτουργεί rclcpp::spin

Μέσα στον κατασκευαστή κλάσης:

• Εγγραφείτε στα μηνύματα σάρωσης λέιζερ: συνδρομή_
• Δημοσίευση στο πρόγραμμα οδήγησης διαφοράς ρομπότ: εκδότης_

Η νοημοσύνη αποφυγής εμποδίων εντάσσεται στη μέθοδο υπολογισμόςVelMsg. Εδώ λαμβάνονται αποφάσεις με βάση τις μετρήσεις λέιζερ. Σημειώστε ότι εξαρτάται καθαρά από την ελάχιστη απόσταση που διαβάζεται από το μήνυμα.

Εάν θέλετε να το προσαρμόσετε, για παράδειγμα, λάβετε υπόψη μόνο τις ενδείξεις μπροστά από το ρομπότ ή ακόμα και ελέγξτε αν είναι καλύτερο να στρίψετε αριστερά ή δεξιά, αυτό είναι το μέρος που πρέπει να δουλέψετε! Θυμηθείτε να προσαρμόσετε τις παραμέτρους, γιατί έτσι όπως είναι, μόνο η ελάχιστη τιμή έρχεται σε αυτήν τη μέθοδο.

3 – Μεταγλώττιση του κόμβου

Αυτό το εκτελέσιμο εξαρτάται και από τα δύο geometry_msgs και sensor_msgs, που έχουμε προσθέσει στις δύο προηγούμενες αναρτήσεις αυτής της σειράς. Φροντίστε να τα έχετε στην αρχή του ~/ros2_ws/src/my_package/CMakeLists.txt αρχείο:

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)

Και τέλος, προσθέστε το εκτελέσιμο αρχείο και εγκαταστήστε το:

# obstacle avoidance
add_executable(obstacle_avoidance src/obstacle_avoidance.cpp)
ament_target_dependencies(obstacle_avoidance rclcpp sensor_msgs geometry_msgs)

...

install(TARGETS
  reading_laser
  moving_robot
  obstacle_avoidance
  DESTINATION lib/$PROJECT_NAME/
)

Συγκεντρώστε το πακέτο:
colcon build --symlink-install --packages-select my_package

4 – Εκτελέστε τον κόμβο

Για να τρέξετε, χρησιμοποιήστε την ακόλουθη εντολή:
ros2 run my_package obstacle_avoidance

Δεν θα λειτουργήσει για αυτό το ρομπότ! Γιατί αυτό? Εγγραφόμαστε και δημοσιεύουμε γενικά θέματα: cmd_vel και laser_scan.

Χρειαζόμαστε ένα αρχείο εκκίνησης για να αντιστοιχίσουμε ξανά αυτά τα θέματα, ας δημιουργήσουμε ένα στο ~/ros2_ws/src/my_package/launch/obstacle_avoidance.launch.py:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

    obstacle_avoidance = Node(
        package="my_package",
        executable="obstacle_avoidance",
        output="screen",
        remappings=[
            ('laser_scan', '/dolly/laser_scan'),
            ('cmd_vel', '/dolly/cmd_vel'),
        ]
    )

    return LaunchDescription([obstacle_avoidance])

Κάντε εκ νέου μεταγλώττιση του πακέτου, πηγή του χώρου εργασίας για άλλη μια φορά και εκκινήστε το:
colcon build --symlink-install --packages-select my_package
source ~/ros2_ws/install/setup.bash
ros2 launch my_package obstacle_avoidance.launch.py

Σχετικά μαθήματα και επιπλέον σύνδεσμοι:

Η δημοσίευση Εξερευνώντας το ROS2 με ένα τροχοφόρο ρομπότ – #4 – Αποφυγή εμποδίων εμφανίστηκε πρώτα στις Η Κατασκευή.