0

I have a task to complete using ros service. The task is when the service is called, the robot must perform the following behavior:

  1. Identify which laser ray is the shortest one. Assume that this is the one pointing to a wall.
  2. Rotate the robot until the front of it is facing the wall. This can be done by publishing an angular velocity until front ray is the smallest one.
  3. Move the robot forward until the front ray is smaller than 30cm.
  4. Then, the robot stop moving. Return the service message with True.

From this task I have completed the code as below

#include “ros/node_handle.h”
#include “ros/publisher.h”
#include “ros/subscriber.h”
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <services_quiz/FindWall.h>
#include <unistd.h>
#include
using namespace std;

class TurtleFindWall
{
public:
ros::Publisher pub;
geometry_msgs::Twist vel;
vector nums;
ros::NodeHandle nh;
ros::ServiceServer my_service;
ros::Subscriber sub;

TurtleFindWall()
{
    my_service = nh.advertiseService("/find_wall", &TurtleFindWall::my_callback, this);
    sub = nh.subscribe("scan", 1000, &TurtleFindWall::counterCallback, this);
    pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
    ROS_INFO("Service /find_wall Ready");
}

void counterCallback(const sensor_msgs::LaserScan::ConstPtr &msg) 
{
    nums = msg->ranges;
    ROS_INFO("list = %f", nums[360]);
}

bool my_callback(services_quiz::FindWall::Request &req,
                services_quiz::FindWall::Response &res) 
{
    ROS_INFO("The Service find_wall has been called");

    while (nums[360] > 0.3) 
    {
        float min_range = std::numeric_limits<float>::infinity();
        for (int i = 0; i < nums.size(); ++i) 
        {
            if (nums[i] < min_range) 
            {
            min_range = nums[i];
            }
        }

        ROS_INFO("min_range %f", min_range);

        if (nums[360] > min_range + 0.05) 
        {
            vel.linear.x = 0.0;
            vel.angular.z = 0.2;
            pub.publish(vel);
            ROS_INFO("360 %f", nums[360]);
        }

        else 
        {
            vel.linear.x = 0.2;
            vel.angular.z = 0.0;
            pub.publish(vel);
        }
    }
    vel.linear.x = 0.0;
    vel.angular.z = 0.0;
    pub.publish(vel);

    ROS_INFO("360 %f", nums[360]);
    res.wallfound = true;
    return true;
} 
};

int main(int argc, char **argv)

{
ros::init(argc, argv, “services_quiz_node”);
TurtleFindWall turtlefindwall;
ros::spin();
return 0;
}

But I’m having some trouble reading the laserscan data. If you run this code, the min_range and nums[360]value in while (nums[360] > 0.3) loop taking only the initial laserscan data before the client calling the server. So when the server is called and the robot is rotated, the min_range and nums[360] value is not updated to the latest value. How can I solve this problem? Thank you in advance.

AmirulJ
  • 118
  • 1
  • 11
  • why are you using `nums[360]`? you should be using the smallest distance measurement – ignacio Mar 25 '23 at 11:03
  • you should create a thread safe vector `num` to avoid racing condition, because right now you are reading and writing to the `nums` from different threads – ignacio Mar 25 '23 at 11:05
  • I think the easiest approach for you would be to calculate the minimum distance and the angle inside the scan callback, use `std::atomic` for those variables. And use those values inside the service call as well – ignacio Mar 25 '23 at 11:20

0 Answers0