How can I combine the task from section 1 and section 2 in ROS Basics in 5 Days: Course Project?

ROSDS Support
, ,
1

I have complete the task from section 2 which is to find the nearest wall and rotate the robot until the front of the robot facing the wall. The code is below:

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

ros::Publisher pub;
geometry_msgs::Twist vel;
using namespace std;

void counterCallback (const sensor_msgs::LaserScan::ConstPtr& msg)
{

float min_range = std::numeric_limits<float>::infinity();
float min_angle = 0.0;

for (int i = 0; i < msg->ranges.size(); ++i) 
{
    if (msg->ranges[i] < min_range) 
    {
    min_range = msg->ranges[i];
    }
}
ROS_INFO("min_range %f", min_range);

if (msg->ranges[360] > min_range + 0.05) 
{
vel.linear.x = 0.0;
vel.angular.z = 0.2;
pub.publish(vel);
}

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

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

res.wallfound = true;
return true;
}

int main (int argc, char** argv)
{
ros::init(argc, argv, “services_quiz_node”);
ros::NodeHandle nh;
ros::ServiceServer my_service = nh.advertiseService(“/find_wall”, my_callback);
ros::Subscriber sub = nh.subscribe (“scan”, 1000, counterCallback);
pub = nh.advertise<geometry_msgs::Twist>(“cmd_vel”, 1000);
ros::spin();

return 0;

}

For section 1, the code is below:

#include “ros/node_handle.h”
#include “ros/publisher.h”
#include “ros/subscriber.h”
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>

ros::Publisher pub;

void counterCallback (const sensor_msgs::LaserScan::ConstPtr& msg)
{
ROS_INFO(“360 %f”, msg->ranges[360]);
ROS_INFO(“270 %f”, msg->ranges[270]);
ROS_INFO(“450 %f”, msg->ranges[450]);

geometry_msgs::Twist vel;

if (msg->ranges[360] > 0.3 && msg->ranges[270] > 0.3 && msg->ranges[450] > 0.3) // 0.3 is distance in meter & ranges[NUMBER] identify the robot view angle
{
    vel.linear.x = 0.2; //Adjust the robot speed
    vel.angular.z = 0; //Adjust the robot angle view
    pub.publish(vel); //Execute command to robot 
}
else if (msg->ranges[360] <= 0.3 || msg->ranges[450] <= 0.3) {
vel.angular.z = -0.2;
vel.linear.x = 0.0;
pub.publish(vel);
}

else if (msg->ranges[270] <= 0.3) {
vel.angular.z = 0.2;
vel.linear.x = 0.0;
pub.publish(vel);
}

}

int main (int argc, char** argv)
{
ros::init(argc, argv, “topics_quiz_node”);
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe (“scan”, 1000, counterCallback);
pub = nh.advertise<geometry_msgs::Twist>(“cmd_vel”, 1000);
ros::spin();

return 0;

}

How can I combine this code by using services? I am a beginner in c++ and ROS.

2

hi @AmirulJ

You can create a service client inside the task 1 or create a new file called main.cpp and put the first task and service client.

Remember, service client is synchronous, so, when u call the service server(task 2) the program waits until response has recieved, after this you can start with the task 1.

Let me know if this solved your question

3

Hi @Voltedge

Thank you for your response. I would like to know how can I implement bool my_callback(services_quiz::FindWall::Request &req, services_quiz::FindWall::Response &res) in the void counterCallback (const sensor_msgs::LaserScan::ConstPtr& msg) ? Because when I put bool function in the void, there is an error in coding.

4

Hi @AmirulJ ,

Callbacks are always defined as void since they do not return any value to the main program. Therefore, defining a callback as bool will only throw errors.

Try to think of some other intuitive way to solve your problem.

This is just my quick response to your previous post.

Also, the best way to integrate task 1 and task 2 would be what @Voltedge mentioned.

Regards,
Girish

5

Hi @girishkumar.kannan @Voltedge

I completed the second task 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 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.

6

Hi @AmirulJ

It’s pretty simple!

one important tool when you use a while inside a callback and you have another callbacks. You have to remember that multiple callbacks create a callback queue, so in order to actualize nums yo have to put

        ros::spinOnce();

at the bottom of the while

Let me know if this solved your question

7

Hi @girishkumar.kannan @Voltedge

Thank you very much for your suggestion @Voltedge . I have succeeded to actualize nums in my code. Howerver, I have some problem to rotate the robot again until ray number 270 of the laser ranges is pointing to the wall.

My full coding is 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");
    float min_range = std::numeric_limits<float>::infinity();
    label1: while (nums[270] > 0.3) 
    {
        label2: while (nums[360] > 0.3)
        {
            for (int i = 0; i < nums.size(); ++i) 
            {
                ROS_INFO("min_range %f", min_range);
                if (nums[i] < min_range) 
                {
                min_range = nums[i];
                }
            }

            ROS_INFO("min_range %f", min_range);

            if (nums[360] > min_range + 0.03) 
            {
                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);

                if (nums[360] < 0.3)
                {
                    vel.linear.x = 0.0;
                    vel.angular.z = 0.0;
                    pub.publish(vel);
                    goto label1;
                }
            }
            ros::spinOnce();
        }
        if (nums[360] < 0.3)
        {
            vel.linear.x = 0.0;
            vel.angular.z = 0.2;
            pub.publish(vel);
            ROS_INFO("270 %f", nums[270]);    
        }
        ros::spinOnce();
    }
    vel.linear.x = 0.0;
    vel.angular.z = 0.0;
    pub.publish(vel);

    res.wallfound = true;
    return true;
}

};

int main(int argc, char **argv)

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

From the task, there are 6 step to complete the task:

  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. Now rotate the robot again until ray number 270 of the laser ranges is pointing to the wall.
  5. At this point, consider that the robot is ready to start following the wall.
  6. Return the service message with True.

In this code, I use label 2 which is while loop in order to execute step 1 to step 3. after step 1 to step 3 have been executed, the label 2 will terminate and go to label 1 to execute step 4. But, when I run this code, step 4 which is label 1 does not run smoothly.

After moving forward, the robot should stop when the front ray is smaller than 30cm and then rotate until 270 degree facing the wall. But, what I see is, the robot stop, move forward and rotate at the same time. I suspect that, while loop in label 2 does not terminate completely.

What I mean is, when label 2 meet the condition again while label 1 is running, the while loop in label 2 will be running again. Hence, the robot does not move smoothly in step 4. May I know how to solve this problem? Thank you very much.

8

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