Problem with the real robot, while simulation works fine

Course Support ROS2 in 5 Days C++
1

Hi everyone, I’m currently working on the Rosject referring to the course ROS2 C++ Humble. The whole code works perfectly fine in the simulation, but with the actual robot I’m having a problem. When I launch the nodes, the robot should first use service I created to find the wall, but for some reason it can’t function properly and keeps rotating. Moreover, even when I kill the nodes and try to use the keyboard to control it, the robot still keeps rotating for a while. You can see the problem in the video linked below.

Here below I will let the codes I’m using.

The move_robot_service_action_v1.cpp, this script is responsable for calling the client and the action and moving the robot near the wall.

#include "custom_messages/action/odom_record.hpp"
#include "custom_messages/srv/detail/find_wall__struct.hpp"
#include "custom_messages/srv/find_wall.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "rcl/node_options.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "sensor_msgs/msg/detail/laser_scan__struct.hpp"
#include <chrono>
#include <sensor_msgs/msg/laser_scan.hpp>

#include <chrono>
#include <cstdlib>
#include <future>
#include <inttypes.h>
#include <iostream>
#include <memory>
#include <thread>

using namespace std::chrono_literals;
using FindWall = custom_messages::srv::FindWall;

class MoveRobot : public rclcpp::Node {
public:
  float vx;
  float vz;

  using OdomRecord = custom_messages::action::OdomRecord;
  using GoalHandleOdomRecord = rclcpp_action::ClientGoalHandle<OdomRecord>;

  explicit MoveRobot(
      const rclcpp::NodeOptions &node_options = rclcpp::NodeOptions())
      : Node("move_robot_service_action_node_v1", node_options),
        goal_done_(false) {

    // Initialize the MutuallyExclusive callback group object
    callback_publisher_moverobot_group_ = this->create_callback_group(
        rclcpp::CallbackGroupType::MutuallyExclusive);

    callback_action_moverobot_group_ = this->create_callback_group(
        rclcpp::CallbackGroupType::MutuallyExclusive);

    callback_subscriber_moverobot_group_ = this->create_callback_group(
        rclcpp::CallbackGroupType::MutuallyExclusive);

    rclcpp::SubscriptionOptions options1;
    options1.callback_group = callback_subscriber_moverobot_group_;

    publisher_ =
        this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);

    timer_ = this->create_wall_timer(
        500ms, std::bind(&MoveRobot::timer_callback, this),
        callback_publisher_moverobot_group_);

    subscription_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
        "scan", 10,
        std::bind(&MoveRobot::topic_callback, this, std::placeholders::_1),
        options1);

    this->client_ptr_ = rclcpp_action::create_client<OdomRecord>(
        this->get_node_base_interface(), this->get_node_graph_interface(),
        this->get_node_logging_interface(),
        this->get_node_waitables_interface(), "record");

    this->timer_action_ = this->create_wall_timer(
        std::chrono::milliseconds(500), std::bind(&MoveRobot::send_goal, this),
        callback_action_moverobot_group_);
  }

  bool is_goal_done() const { return this->goal_done_; }

  void send_goal() {
    using namespace std::placeholders;

    this->timer_action_->cancel();

    this->goal_done_ = false;

    if (!this->client_ptr_) {
      RCLCPP_ERROR(this->get_logger(), "Action client not initialized");
    }

    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(),
                   "Action server not available after waiting");
      this->goal_done_ = true;
      return;
    }

    auto goal_msg = OdomRecord::Goal();

    RCLCPP_INFO(this->get_logger(), "Sending goal");

    auto send_goal_options =
        rclcpp_action::Client<OdomRecord>::SendGoalOptions();

    send_goal_options.goal_response_callback =
        std::bind(&MoveRobot::goal_response_callback, this, _1);

    send_goal_options.feedback_callback =
        std::bind(&MoveRobot::feedback_callback, this, _1, _2);

    send_goal_options.result_callback =
        std::bind(&MoveRobot::result_callback, this, _1);

    auto goal_handle_future =
        this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

private:
  void timer_callback() {
    auto message = geometry_msgs::msg::Twist();
    message.linear.x = 0.1;
    message.angular.z = vz;
    publisher_->publish(message);
  }

  void topic_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
    std::this_thread::sleep_for(250ms);
    float Foward1 = msg->ranges[360];
    float Right = msg->ranges[180]; // 180
    float Left = msg->ranges[520];  // 520
    vx = 0.2;

    if (Right > 0.3 && Foward1 > 0.5) {
      vz = -0.2;
    } else if (Right < 0.2 && Foward1 > 0.5) {
      vz = 0.2;
    } else if (Right < 0.3 && Right > 0.2 && Foward1 > 0.5) {
      vz = 0;
    } else if (Foward1 < 0.5) {
      vz = 1;
    }

    /*RCLCPP_INFO(this->get_logger(),
                "Foward: '%f'. Right: '%f'. Left:'%f'. Vz '%f'", Foward1, Right,
                Left, vz);*/
  }

  // Action client functions

  void goal_response_callback(
      std::shared_future<GoalHandleOdomRecord::SharedPtr> future) {
    // this is necessary because the system uses ros2 foxy e not humble
    auto goal_handle = future.get();
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    } else {
      RCLCPP_INFO(this->get_logger(),
                  "Goal accepted by server, waiting for result");
    }
  }

  void feedback_callback(
      GoalHandleOdomRecord::SharedPtr,
      const std::shared_ptr<const OdomRecord::Feedback> feedback) {
    RCLCPP_INFO(this->get_logger(), "Feedback received: %f",
                feedback->current_total);
  }

  void result_callback(const GoalHandleOdomRecord::WrappedResult &result) {
    this->goal_done_ = true;
    switch (result.code) {
    case rclcpp_action::ResultCode::SUCCEEDED:
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
      return;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
      return;
    default:
      RCLCPP_ERROR(this->get_logger(), "Unknown result code");
      return;
    }
    // possivelmente vou printar um a um

    for (int i = 0; i < result.result->list_of_odoms.size(); ++i) {
      /*RCLCPP_INFO(this->get_logger(), "X: %f. Y: %f. Theta: %f",
                  result.result->list_of_odoms[i].x,
                  result.result->list_of_odoms[i].y,
                  result.result->list_of_odoms[i].z);*/

      std::cout << "Dado salvo: " << i + 1 << std::endl;
      std::cout << "X: . " << result.result->list_of_odoms[i].x << std::endl;
      std::cout << "Y: . " << result.result->list_of_odoms[i].y << std::endl;
      std::cout << "THETA: . " << result.result->list_of_odoms[i].z
                << std::endl;
    }
  }

  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::TimerBase::SharedPtr timer_action_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
  rclcpp_action::Client<OdomRecord>::SharedPtr client_ptr_;
  bool goal_done_;
  rclcpp::CallbackGroup::SharedPtr callback_action_moverobot_group_;
  rclcpp::CallbackGroup::SharedPtr callback_subscriber_moverobot_group_;
  rclcpp::CallbackGroup::SharedPtr callback_publisher_moverobot_group_;
};

class ServiceClient : public rclcpp::Node {
private:
  rclcpp::Client<FindWall>::SharedPtr client_;
  rclcpp::TimerBase::SharedPtr timer_client_;
  bool service_done_ = false;

  void timer_client_callback() {
    while (!client_->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(
            this->get_logger(),
            "Client interrupted while waiting for service. Terminating...");
        return;
      }
      RCLCPP_INFO(this->get_logger(),
                  "Service Unavailable. Waiting for Service...");
    }

    auto request = std::make_shared<FindWall::Request>();

    service_done_ = false;
    auto result_future = client_->async_send_request(
        request, std::bind(&ServiceClient::response_client_callback, this,
                           std::placeholders::_1));
  }

  void response_client_callback(rclcpp::Client<FindWall>::SharedFuture future) {
    auto status = future.wait_for(1s);
    if (status == std::future_status::ready) {
      RCLCPP_INFO(this->get_logger(), "Result: success");
      service_done_ = true;
    } else {
      RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
    }
  }

public:
  ServiceClient() : Node("service_client") {
    client_ = this->create_client<custom_messages::srv::FindWall>("findwall");
    timer_client_ = this->create_wall_timer(
        1s, std::bind(&ServiceClient::timer_client_callback, this));
  }

  bool is_service_done() const { return this->service_done_; }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);

  auto service_client = std::make_shared<ServiceClient>();
  while (!service_client->is_service_done()) {
    rclcpp::spin_some(service_client);
  }

  // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "ele chegou ate aqui");
  // After the robot found the wall it will start moving and call the service

  std::shared_ptr<MoveRobot> move_robot_service_action_node =
      std::make_shared<MoveRobot>();

  // Initialize one MultiThreadedExecutor object
  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(move_robot_service_action_node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

This is the service server: find_wall_server.cpp

#include "custom_messages/srv/find_wall.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/executor.hpp"
#include "rclcpp/executors.hpp"
#include "rclcpp/parameter_service.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/utilities.hpp"
#include "sensor_msgs/msg/detail/laser_scan__struct.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include <chrono>
#include <unistd.h>

#include <algorithm>
#include <functional>
#include <iostream>
#include <memory>
#include <thread>

using namespace std::chrono_literals;
using namespace std;
using FindWall = custom_messages::srv::FindWall;
using std::placeholders::_1;
using std::placeholders::_2;

class ServerNode : public rclcpp::Node {
public:
  ServerNode() : Node("find_wall_server_node") {

    callback_service_group_ = this->create_callback_group(
        rclcpp::CallbackGroupType::MutuallyExclusive);

    callback_subscriber_group_ = this->create_callback_group(
        rclcpp::CallbackGroupType::MutuallyExclusive);

    rclcpp::SubscriptionOptions options_sub;
    options_sub.callback_group = callback_subscriber_group_;

    srv_ = create_service<FindWall>(
        "findwall", std::bind(&ServerNode::findwall_callback, this, _1, _2),
        rmw_qos_profile_services_default, callback_service_group_);

    publisher_ =
        this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);

    subscription_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
        "scan", 10,
        std::bind(&ServerNode::topic_callback, this, std::placeholders::_1),
        options_sub);
  }

private:
  void findwall_callback(const std::shared_ptr<FindWall::Request> request,
                         const std::shared_ptr<FindWall::Response> response) {
    // response->wallfound = false;
    final_position = false;
    // RCLCPP_INFO(this->get_logger(), "Final_Position: %d", final_position);

    while (final_position != true) {
      if (Foward > min_distance + 0.05 && min_distance >= 0.30) {
        // Send velocity to move the robot to the right
        vx = 0;
        vz = -0.2;
      } else if (Foward <= min_distance + 0.05 && Foward >= 0.25) {
        // Send velocity to move the robot foward
        vx = 0.1;
        vz = 0;
      } else {
        vx = 0;
        vz = -0.2;
        if (Right <= min_distance + 0.05 && min_distance <= 0.25) {
          vx = 0;
          vz = 0;
          this->twist_msg.linear.x = vx;
          this->twist_msg.angular.z = vz;
          this->publisher_->publish(this->twist_msg);

          response->wallfound = true;
          // break;
          final_position = true;
          // RCLCPP_INFO(this->get_logger(), "Ele entrou aqui");
        }
      }

      this->twist_msg.linear.x = vx;
      this->twist_msg.angular.z = vz;
      this->publisher_->publish(this->twist_msg);
      std::this_thread::sleep_for(250ms);
    }
    // final_position = false;
    // RCLCPP_INFO(this->get_logger(), "Final_Position: %d", final_position);
  }

  void topic_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {

    std::this_thread::sleep_for(250ms);
    Foward = msg->ranges[360];
    Right = msg->ranges[180];

    min_distance = 100;
    // finding the minimal value
    for (int i = 0; i < 720; i++) {
      if ((msg->ranges[i] <= min_distance) &&
          (msg->ranges[i] >= msg->range_min) &&
          (msg->ranges[i] <= msg->range_max)) {
        min_distance = msg->ranges[i];
        position = i;
      }
    }

    /*RCLCPP_INFO(this->get_logger(),
                "Foward: '%f'. Right: '%f'. Min: '%f'. position: '%d'", Foward,
                Right, min_distance, position);*/
  }

  float Foward;
  float Right;
  float min_distance;
  float vx;
  float vz;
  bool final_position;
  int position;
  rclcpp::Service<FindWall>::SharedPtr srv_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
  geometry_msgs::msg::Twist twist_msg;
  rclcpp::CallbackGroup::SharedPtr callback_service_group_;
  rclcpp::CallbackGroup::SharedPtr callback_subscriber_group_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  std::shared_ptr<ServerNode> server_node = std::make_shared<ServerNode>();

  // Initialize one MultiThreadedExecutor object
  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(server_node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

And the launch file:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='wall_service_pkg',
            executable='find_wall_server',
            output='screen'),
        Node(
            package='action_service',
            executable='action_server_node',
            output='screen'),
        Node(
            package='topic_publisher_pkg',
            executable='move_robot_service_action_node_v1',
            output='screen')
    ])

I didn’t put the other scripts like the action server and the CMakeLists because there are already a lot of information, and I think that if the problem was in these files, the simulation wouldn’t work properly, but it does. But if anyone thinks it is important, I can add them.

2

Hi @wenis_svtrp ,

Welcome to this Community!

After looking at the video, I was sure that your find_wall logic has some issues. After looking at your find_wall_server.cpp I was able to confirm that issue.

The logic that you have in place for finding the final_position will not work properly. The reason being the robot’s speed of rotation. Your robot can overshoot the final_position when it is rotating at a fast angular speed. The best approach to solve this is to implement a rotate->scan->stop->repeat method instead of rotate->scan->repeat method (which you have in your code) which will not stop the robot rotation.

Try this change and let me know if it worked for you.

Here is a quick pseudocode (just the rotation logic - not the entire program):

loop over:
1. rotate the robot (in some direction)
2. wait for few seconds (0.5 / 1.0 / 1.5)
3. stop the robot
4. scan for laser readings
5. check if this is the final position
6. if final_position then break else continue

Regards,
Girish

3

Thanks a lot, @girishkumar.kannan ! I implemented your suggestions and the code worked perfectly.

4

@wenis_svtrp ,

Awesome! Glad to hear that! Please mark the post as “Solution” so this issue can be closed.

Regards,
Girish

5

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.