ROS2 cpp rosject part 2 help

[reallen]

I’m currently stuck on the second part of the rosject for the ros2 c++ in 5 days course. I’m subscribing to the /scan topic and have the state variables updated in the callback. I would like to use while loops to get the robot to reach a certain state depending on the front, right, and closest distances, but I would need those values to be continuously updating, which I haven’t been able to do for a few days now. I guess my question is, how can I execute the while loop code while still updating the state variables the callback is responsible for? Thanks

#include "rclcpp/executors.hpp"
#include <ros2_project/find_wall_move.hpp>

using namespace std::chrono_literals;
using std::placeholders::_1;

FindWall::FindWall(std::shared_ptr<rclcpp::Node> node) {
  // Other variables
  this->running_ = true;
  this->state_ = 0;
//   this->front_distance_ = 0;
//   this->closest_distance_ = 0;
//   this->right_distance_ = 0;

  node_ = node;
  RCLCPP_INFO(this->node_->get_logger(), "Starting ");

  //  Rate
  rclcpp::WallRate loop_rate(500ms);

  RCLCPP_INFO(node_->get_logger(), "Creating publisher ");
  // ROS Publishers
  pub_cmd_vel_ =
      node_->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);

  RCLCPP_INFO(node_->get_logger(), "Creating subscriber ");
  // ROS Subscribers
  sub_laser_scan = node_->create_subscription<sensor_msgs::msg::LaserScan>(
      "/scan", 10, std::bind(&FindWall::topic_callback, this, _1));
}

FindWall::~FindWall(void) {}

void FindWall::rateSleep(void) {}

// return the appropriate velocity commands based on the state the vehicle is in
geometry_msgs::msg::Twist FindWall::getStateVelocity() {
  geometry_msgs::msg::Twist vel;
  switch (state_) {
  case 0: // vehicle is stopped
    vel.linear.x = 0;
    vel.angular.z = 0;
    break;
  case 1: // turning to the right
    vel.linear.x = 0;
    vel.angular.z = -.3;
    break;
  case 2: // turning to the left
    vel.linear.x = 0;
    vel.angular.z = .3;
    break;
  case 3: // move forward
    vel.linear.x = .3;
    vel.angular.z = 0;
    break;
  case 4: // move backward
    vel.linear.x = -.3;
    vel.angular.z = 0;
    break;
  }

  return vel;
}

bool call_back_hit = false;

// returns True if the vehicle has found the nearest wall and positioned itself,
// false otherwise
bool FindWall::runTimeStateMachine(void) {
 
  if(!call_back_hit){
    return false;
  }
  
  geometry_msgs::msg::Twist vel;

  // If the statemachine is no longer running, stop movement and return true
  if (!running_) {
    vel.linear.x = 0;
    vel.angular.z = 0;
    pub_cmd_vel_->publish(vel);
    return true;
  }

  // otherwise, get the velocity for the current state and move robot
  vel = this->getStateVelocity();
  this->pub_cmd_vel_->publish(vel);

  RCLCPP_INFO(this->node_->get_logger(), "State [%d], Vel [%.2f, %.2f]", state_,
              vel.linear.x, vel.angular.z);
  RCLCPP_INFO(this->node_->get_logger(),
              "Front distance: %f, Closest Distance: %f", "Right Distance %f",
              this->front_distance_, this->closest_distance_,
              this->right_distance_);

  rclcpp::Rate loop_rate(100);
  if (front_distance_ == closest_distance_) {
    if (front_distance_ == 0 && closest_distance_ == 0) {
      RCLCPP_INFO(this->node_->get_logger(), "stuck...");
      loop_rate.sleep();
    } else { // if the front is the closest and is not 0, then proceed to move
             // forward
      if (front_distance_ > .3) { // if it's too far, move forward
        RCLCPP_INFO(this->node_->get_logger(), "Too Far...");
        while (front_distance_ > .3) {
          RCLCPP_INFO(this->node_->get_logger(), "Moving forward...");
          this->changeState(3);
          vel = this->getStateVelocity();
          this->pub_cmd_vel_->publish(vel);
        }
        stopRobot();
      } else if (front_distance_ < .3) { // if its too close, move backward
        while (front_distance_ < .3) {
          RCLCPP_INFO(this->node_->get_logger(), "Too close...");
          RCLCPP_INFO(this->node_->get_logger(), "Moving backward...");
          this->changeState(4);
          vel = this->getStateVelocity();
          this->pub_cmd_vel_->publish(vel);
        }
        stopRobot();
      }
    }
  } else {
    while (front_distance_ != closest_distance_) {
      RCLCPP_INFO(this->node_->get_logger(), "Front not aligned");
      RCLCPP_INFO(this->node_->get_logger(), "aligning front...");
      this->changeState(1);
      vel = getStateVelocity();
      this->pub_cmd_vel_->publish(vel);
    }
    stopRobot();
  }

  if (closest_distance_ == .3 && right_distance_ != closest_distance_) {
    while (right_distance_ != closest_distance_) {
      RCLCPP_INFO(this->node_->get_logger(), "Aligning rightwith Wall..");
      this->changeState(2);
      vel = getStateVelocity();
      this->pub_cmd_vel_->publish(vel);
    }
    stopRobot();
  }

  if (right_distance_ == closest_distance_ && closest_distance_ == .3) {
    this->running_ = false;
    return false;
  }

  this->running_ = true;

  return false;
}

void FindWall::stopRobot(void) {
  geometry_msgs::msg::Twist vel;
  changeState(0);
  vel = getStateVelocity();
  this->pub_cmd_vel_->publish(vel);
}

// this function changes the current state based on laser readings
void FindWall::changeState(int state) {
  state_ = state;
  RCLCPP_INFO(this->node_->get_logger(), "Change to state [%d]", state_);
}

// read the laser scan from the scan topic and set the distance variables equal
// to this value
void FindWall::topic_callback(
    const sensor_msgs::msg::LaserScan::SharedPtr msg) {
  call_back_hit = true;
  RCLCPP_INFO(this->node_->get_logger(), "callback");
  front_distance_ = msg->ranges[180];
  right_distance_ = msg->ranges[90];
  closest_distance_ = msg->range_min;
}

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

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("FindWall");
  RCLCPP_INFO(node->get_logger(), "Start Init FindWall");
  FindWall FindWall(node);
  rclcpp::spin(node);
  
  rclcpp::Rate loop_rate(20);

  bool finished_statemachine = false;

  while (rclcpp::ok() && !finished_statemachine) {
    finished_statemachine = FindWall.runTimeStateMachine();
    loop_rate.sleep();
  }

  rclcpp::shutdown();
  return 0;
}

[rzegers]

Hi @reallen,
this answer is not meant to give you a solution to the exercise, I am just giving you two small hints as to where to look at, and what to look for. Also remember that there can be many different ways to solve this problem.

My first hint is to look at what the message field msg->range_min contain. As per the documentation it contains information about the minimum range at which the laser is able to detect objects. If an object is located closer than the value indicated by msg->range_min the laser will just not detect that object. Is this what you meant to keep in the variable closest_distance_ ??

Also within many of the if statements in your code you add a while loop, just remember that inside main you are already running a loop using rclcpp::spin() which keeps the ros node running and check for events on subscribed topics and service calls. So basically the code has nested loops, and when inside an inner loops, it will not check for callback events, only when inside an outer loop, the one that is kept by running rclcpp::spin().

Hope this can I hope I’ve managed to shed some light on this.

Regards,

Roberto

[reallen]

Hi Roberto,

Thanks for the reply. Appreciate you taking the time to help. On the topic of your hints, I discovered what you mentioned about the msg->range_min value through some trial and error. Also, after alot of googling, I thought that rclcpp::spin is essentially just looping the code. Thanks for confirming that. I think I figured it out now. thank you!

Allen

[rzegers]

You are very welcome. I am glad you found it helpful.

Roberto