# Threading Across Multiple Nodes ROS1 C++

**URL:** <https://get-help.theconstruct.ai/t/threading-across-multiple-nodes-ros1-c/22359>\
**Category:** ROS Basics In 5 Days (C++)\
**Tags:** threading\
**Created:** [February 13, 2023, 6:35pm UTC](https://get-help.theconstruct.ai/t/threading-across-multiple-nodes-ros1-c/22359 "2023-02-13T18:35:42Z")\
**Posts on this page:** 7\
**Page:** 1

<div class="post-metadata">

**Author:** ![IMSETraining](https://get-help.theconstruct.ai/letter_avatar_proxy/v4/letter/i/9de053/32.png) [@IMSETraining](https://get-help.theconstruct.ai/u/IMSETraining)\
**Post date:** [February 13, 2023, 6:35pm UTC](https://get-help.theconstruct.ai/t/threading-across-multiple-nodes-ros1-c/22359/1 "2023-02-13T18:35:43Z")

</div>

I am currently working on the finishing touches of the rosject associated with the ROS Basics in 5 Days (C++). However, my action server client callbacks are not executing as intended. I believe it is a threading issue, but my research and debugging attempts have not yielded results.

I have 3 nodes with a launch file that launches all 3.

- find\_wall.cpp (service server)
- follow\_wall.cpp (service client, action client, basic subscriber/publisher)
- record\_odom.cpp (action server)

follow\_wall should wait for the find\_wall service to finish, then call the record\_odom server, then perform a subscriber/publisher callback that navigates the robot around the room. As the robot is traveling around the room (laser\_Callback), feedbackCb() should also be printing the total distance traveled and doneCb() should execute at the end of the lap when a result is returned.

I have tried creating asynchronous spinners and multithreaded spinners to no avail.

Please let me know how to coordinate callbacks between follow\_wall.cpp and record\_odom.cpp. Thank you!

**find\_wall.cpp** :

```auto
#include "ros/duration.h"
#include "ros/init.h"
#include "ros/rate.h"
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "rosject/FindWall.h"
#include <bits/stdc++.h>
#include <sensor_msgs/LaserScan.h>

class controller {

    public:
        controller(){
            pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);
            sub = nh.subscribe("/scan", 10, &controller::laserCallback, this);
            my_service = nh.advertiseService("/find_wall", &controller::find_Callback, this); 
        }

        void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
        {
            // Identify which laser ray is the shortest one. Assume that this is the one pointing to a wall.
            laser_readings.clear();

            // creating vector of laser distances
            for (int i = 0; i < msg -> ranges.size(); i++) {
                laser_readings.push_back(msg -> ranges[i]);
            }
        }

        bool find_Callback(rosject::FindWall::Request &req,
                           rosject::FindWall::Response &res)
        {  
            ROS_INFO("find_Callback has been called"); // We print an string whenever the Service gets called

            // 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.
            status_update = "Rotating to face wall.";
            ROS_INFO("%s\n", status_update.c_str());

            while((laser_min < 359) || (laser_min > 361)){

                // finding index of min value in vector
                laser_min = distance(laser_readings.begin(), min_element(laser_readings.begin(),laser_readings.end()));

                if (laser_min > 360){
                    // if wall on the left, turn left
                    vel.angular.z = 0.1;
                } else if (laser_min < 360){
                    // if wall on the right, turn right
                    vel.angular.z = -0.1;
                } else {
                    // if facing wall, stop
                    vel.angular.z = 0;
                }
                pub.publish(vel);
                ros::spinOnce();
            }

            // Move the robot forward until the front ray is smaller than 30cm.
            vel.angular.z = 0;
            vel.linear.x = 0.1;
            pub.publish(vel);

            ROS_INFO("\n Min Element = %d", laser_min);
            status_update = "Moving toward wall.";
            ROS_INFO("%s\n", status_update.c_str());

            while(laser_readings[360] > 0.3){
                ros::spinOnce();
            }
        
            // Now rotate the robot again until ray number 270 of the laser ranges is pointing to the wall.
            vel.angular.z = 0.1;
            vel.linear.x = 0;
            pub.publish(vel);

            ROS_INFO("\n Min Element = %d", laser_min);
            status_update = "Turning parallel with wall.";
            ROS_INFO("%s\n", status_update.c_str());

            while((laser_min < 269) || (laser_min > 271)){
                laser_min = distance(laser_readings.begin(), min_element(laser_readings.begin(),laser_readings.end()));
                ros::spinOnce();
            }

            // stop robot, ready for follow_wall 
            vel.angular.z = 0;
            vel.linear.x = 0;
            pub.publish(vel);

            ROS_INFO("\n Min Element = %d", laser_min);
            status_update = "In position for follow_wall.";
            ROS_INFO("%s\n", status_update.c_str());

            // At this point, consider that the robot is ready to start following the wall.
            // Return the service message with True.
            ROS_INFO("find_Callback completed");
            res.wallfound = true;
            return true;
        }

    private:
        ros::Subscriber sub;
        ros::Publisher pub;
        ros::NodeHandle nh;
        ros::ServiceServer my_service;

        std::string status_update;

        geometry_msgs::Twist vel;

        std::vector<float> laser_readings;
        int laser_min;
        
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "find_wall");
  controller ctrl;

  ros::spin();

  return 0;
}

```

**follow\_wall.cpp**

```auto
#include "ros/publisher.h"
#include <ros/ros.h>
#include <iostream> 
#include <string>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>
#include <stdlib.h> 
#include "rosject/FindWall.h"
#include <rosject/OdomRecordAction.h>
#include <actionlib/client/simple_action_client.h>

class controller {

    public:
        controller(): nh(), spinner(0){

        spinner.start();

        ros::service::waitForService("/find_wall"); // wait for service to be running
        ros::ServiceClient find_wall_service = nh.serviceClient<rosject::FindWall>("/find_wall");
        rosject::FindWall srv; // Create an object of type srv
        
        while(find_wall_service.call(srv) == false){
            ROS_INFO("Failure calling service /find_wall");
        }
        ROS_INFO("Successfully called service /find_wall");

        // call action server to start recording odom values
        actionlib::SimpleActionClient<rosject::OdomRecordAction> client("record_odom", true);
        client.waitForServer(); // Waits until the action server is up and running 

        rosject::OdomRecordGoal goal; // Creates a goal to send to the action server
        goal.success = false; // Fills the goal. Indicates, take pictures along 10 seconds                                                                                                                     
        client.sendGoal(goal, // sends the goal to the action server  
                boost::bind(&controller::doneCb, this, _1, _2),
                boost::bind(&controller::activeCb, this),
                boost::bind(&controller::feedbackCb, this, _1));

        sub = nh.subscribe("/scan", 1000, &controller::laserCallback, this);
        pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
        vel.angular.z = 0;
        }

        void doneCb(const actionlib::SimpleClientGoalState& state,
                    const rosject::OdomRecordResultConstPtr& result)
        {
            ROS_INFO("The robot has completed a lap! Great job, robot.");
            ROS_INFO("The robot started at x = %f, y = %f, and theta = %f", result->list_of_odoms[0].x, result->list_of_odoms[0].y, result->list_of_odoms[0].z);

            int final = result -> list_of_odoms.size();
            //using final as index, so need to reduce by 1 since c++ indexing starts at 0.
            final -= 1;

            ROS_INFO("The robot started at x = %f, y = %f, and theta = %f", result->list_of_odoms[final].x, result->list_of_odoms[final].y, result->list_of_odoms[final].z);         
            ros::shutdown();
        }

        void activeCb()
        {
            ROS_INFO("Odom values are now being recorded.");
        }

        void feedbackCb(const rosject::OdomRecordFeedbackConstPtr& feedback)
        {
            ROS_INFO("[Feedback] The robot has traveled %f meters.", feedback->current_total);
        }

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

            mylocation = msg -> ranges[180];
            vel.linear.x = 0.1;

            //continue straight
            if (mylocation > 0.2 and mylocation < 0.3) {
                vel.angular.z = 0;
                status_update = "straight";
            } 

            //turn left to move away from the wall
            if (mylocation < 0.2 ) {
                vel.angular.z = 0.14;
                status_update = "away_from";
            } 

            //turn right to move closer to the wall
            if (mylocation > 0.3 ) {
                vel.angular.z = -0.1;
                status_update = "to";
            } 

            // turn left if wall ahead
            if (msg -> ranges[360] < 0.7 ) {
                vel.angular.z = 0.4;
                status_update = "corner";
            }
            
            ROS_INFO("%s\n", status_update.c_str());
            //ROS_INFO("%f\n", mylocation);            

            pub.publish(vel);
            //ros::spinOnce();
        }

    private:
        ros::Subscriber sub;
        ros::Publisher pub;
        ros::NodeHandle nh;

        geometry_msgs::Twist vel;
        std::string status_update;

        double mylocation; 

        ros::AsyncSpinner spinner;
        
};

int main(int argc, char** argv) {

    ros::init(argc, argv, "follow_wall");

    controller ctrl;
    
    ros::waitForShutdown();
    //spinner.spin();
    //ros::spin();
    
    return 0;
}

```

**record\_odom.cpp**

```auto
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <std_msgs/Empty.h>
#include <rosject/OdomRecordAction.h>
#include <string>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Point32.h>

class recordOdom
{
protected:

  ros::NodeHandle nh_;
  // NodeHandle instance must be created before this line. Otherwise strange error occurs.
  actionlib::SimpleActionServer<rosject::OdomRecordAction> as_; 
  std::string action_name_;
  // create messages that are used to publish feedback and result
  rosject::OdomRecordFeedback feedback_;
  rosject::OdomRecordResult result_;

  ros::Subscriber odom;

  float calculatedDistance;
  float totalDistance = 0;
  float startDistance;
  int i = 0;

  geometry_msgs::Point32 coords;

  bool exit;
  bool enter;

  ros::AsyncSpinner spinner;

public:

  recordOdom(std::string name): 
    nh_(),
    spinner(0),
    as_(nh_, name, boost::bind(&recordOdom::executeCB,this,_1), false),
    action_name_(name)
  {
    spinner.start();
    odom = nh_.subscribe("/odom", 10, &recordOdom::odomCallback, this);
    as_.start();
  }

  ~recordOdom(void)
  {
  }

  float calculateDistance(float x1, float y1, float x2, float y2){
    return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
  }
  
  void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
  {
    coords.x = msg -> pose.pose.position.x;
    coords.y = msg -> pose.pose.position.y;
    coords.z = msg -> pose.pose.orientation.w;
  }

  void executeCB(const rosject::OdomRecordGoalConstPtr &goal)
  {
    // helper variables
    ros::Rate r(1);

    bool success_;

    exit = false;
    enter = false;

    while(!enter){

        // check that preempt has not been requested by the client
        if (as_.isPreemptRequested() || !ros::ok())
        {
            ROS_INFO("%s: Preempted", action_name_.c_str());
            // set the action state to preempted
            as_.setPreempted();
            success_ = false;
            break;
        }

        result_.list_of_odoms.push_back(coords);

        // new point - previous point = distance traveled since last sub call
        // increment onto totalDistance
        if(i > 0){
            totalDistance += calculateDistance(result_.list_of_odoms[i-1].x,result_.list_of_odoms[i-1].y,result_.list_of_odoms[i].x, result_.list_of_odoms[i].y);
        }

        // distance from the starting point
        startDistance = calculateDistance(result_.list_of_odoms[0].x,result_.list_of_odoms[0].y,result_.list_of_odoms[i].x, result_.list_of_odoms[i].y);

        // has the robot left the starting zone?
        if(startDistance > 0.5){
            // leaves the starting zone
            exit = true;
        }

        // has the robot left the starting zone before and is now back within the starting zone?
        if((exit == true) && (startDistance < 0.5)){
            // reenters the starting zone - lap complete!
            enter = true;
        }

        
        feedback_.current_total = totalDistance;
        as_.publishFeedback(feedback_);
        i += 1;
        ros::spinOnce();
        r.sleep();
    }

    if(success_)
    {
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      as_.setSucceeded(result_);
    }

  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "record_odom");

  recordOdom rOdom("record_odom");

  ros::waitForShutdown();

  return 0;
}

```

---

<div class="post-metadata">

**Author:** ![Voltedge](https://get-help.theconstruct.ai/user_avatar/get-help.theconstruct.ai/voltedge/32/7586_2.png) [@Voltedge](https://get-help.theconstruct.ai/u/Voltedge)\
**Post date:** [February 13, 2023, 7:01pm UTC](https://get-help.theconstruct.ai/t/threading-across-multiple-nodes-ros1-c/22359/2 "2023-02-13T19:01:33Z")

</div>

Hi @IMSETraining

- When you compile server action and action client, what appears on shells?

Remember that when you use multithreading it is not for controlling multiple nodes, but for multiple callbacks

```
 ros::AsyncSpinner spinner(0); //number of threads
 spinner.start();
 //callbacks u want to control 
 ros::waitForShutdown();

```

---

<div class="post-metadata">

**Author:** ![IMSETraining](https://get-help.theconstruct.ai/letter_avatar_proxy/v4/letter/i/9de053/32.png) [@IMSETraining](https://get-help.theconstruct.ai/u/IMSETraining)\
**Post date:** [February 13, 2023, 7:07pm UTC](https://get-help.theconstruct.ai/t/threading-across-multiple-nodes-ros1-c/22359/3 "2023-02-13T19:07:53Z")

</div>

@Voltedge thank you for your response. The shell response upon compilation is below. I understand multithreading is utilized within each node. Am I utilizing it incorrectly within follow\_wall.cpp?

```auto
user:~/catkin_ws$ catkin_make
Base path: /home/user/catkin_ws
Source space: /home/user/catkin_ws/src
Build space: /home/user/catkin_ws/build
Devel space: /home/user/catkin_ws/devel
Install space: /home/user/catkin_ws/install
####
#### Running command: "make cmake_check_build_system" in "/home/user/catkin_ws/build"
####
####
#### Running command: "make -j8 -l8" in "/home/user/catkin_ws/build"
####
[0%] Built target geometry_msgs_generate_messages_eus
[0%] Built target geometry_msgs_generate_messages_py
[0%] Built target geometry_msgs_generate_messages_cpp
[0%] Built target rosgraph_msgs_generate_messages_py
[0%] Built target geometry_msgs_generate_messages_nodejs
[0%] Built target rosgraph_msgs_generate_messages_lisp
[0%] Built target rosgraph_msgs_generate_messages_cpp
[0%] Built target rosgraph_msgs_generate_messages_eus
[0%] Built target std_msgs_generate_messages_cpp
[0%] Built target actionlib_msgs_generate_messages_eus
[0%] Built target std_msgs_generate_messages_eus
[0%] Built target roscpp_generate_messages_nodejs
[0%] Built target actionlib_generate_messages_py
[0%] Built target actionlib_msgs_generate_messages_nodejs
[0%] Built target geometry_msgs_generate_messages_lisp
[0%] Built target actionlib_generate_messages_lisp
[0%] Built target roscpp_generate_messages_py
[0%] Built target std_msgs_generate_messages_py
[0%] Built target std_msgs_generate_messages_lisp
[0%] Built target roscpp_generate_messages_eus
[0%] Built target roscpp_generate_messages_cpp
[0%] Built target roscpp_generate_messages_lisp
[0%] Built target actionlib_generate_messages_eus
[0%] Built target actionlib_generate_messages_cpp
[0%] Built target actionlib_msgs_generate_messages_cpp
[0%] Built target rosgraph_msgs_generate_messages_nodejs
[0%] Built target actionlib_msgs_generate_messages_py
[0%] Built target actionlib_msgs_generate_messages_lisp
[0%] Built target actionlib_generate_messages_nodejs
[0%] Built target std_msgs_generate_messages_nodejs
[0%] Built target _rosject_generate_messages_check_deps_OdomRecordActionResult
[0%] Built target _rosject_generate_messages_check_deps_FindWall
[0%] Built target _rosject_generate_messages_check_deps_OdomRecordFeedback
Scanning dependencies of target follow_wall
[2%] Building CXX object rosject/CMakeFiles/follow_wall.dir/src/follow_wall.cpp.o
[2%] Built target _rosject_generate_messages_check_deps_OdomRecordResult
[6%] Built target find_wall
[6%] Built target _rosject_generate_messages_check_deps_OdomRecordActionGoal
[6%] Built target _rosject_generate_messages_check_deps_OdomRecordGoal
[10%] Built target record_odom
[10%] Built target _rosject_generate_messages_check_deps_OdomRecordActionFeedback
[10%] Built target _rosject_generate_messages_check_deps_OdomRecordAction
[28%] Built target rosject_generate_messages_eus
[44%] Built target rosject_generate_messages_lisp
[61%] Built target rosject_generate_messages_cpp
[77%] Built target rosject_generate_messages_nodejs
[97%] Built target rosject_generate_messages_py
[97%] Built target rosject_generate_messages
[100%] Linking CXX executable /home/user/catkin_ws/devel/lib/rosject/follow_wall
[100%] Built target follow_wall

```

---

<div class="post-metadata">

**Author:** ![IMSETraining](https://get-help.theconstruct.ai/letter_avatar_proxy/v4/letter/i/9de053/32.png) [@IMSETraining](https://get-help.theconstruct.ai/u/IMSETraining)\
**Post date:** [February 13, 2023, 7:38pm UTC](https://get-help.theconstruct.ai/t/threading-across-multiple-nodes-ros1-c/22359/4 "2023-02-13T19:38:38Z")

</div>

I experimented with the location of ros::waitForShutdown() and the program started working as intended. I moved it within the class at the end of the constructor. This makes sense because the spinner is initialized at the beginning of the constructor and not the main function.

**Updated follow\_wall.cpp:**

```auto
#include "ros/publisher.h"
#include <ros/ros.h>
#include <iostream> 
#include <string>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>
#include <stdlib.h> 
#include "rosject/FindWall.h"
#include <rosject/OdomRecordAction.h>
#include <actionlib/client/simple_action_client.h>

class controller {

    public:
        controller(): 
        nh(), 
        spinner(0){

        spinner.start();

        ros::service::waitForService("/find_wall"); // wait for service to be running
        ros::ServiceClient find_wall_service = nh.serviceClient<rosject::FindWall>("/find_wall");
        rosject::FindWall srv; // Create an object of type srv
        
        while(find_wall_service.call(srv) == false){
            ROS_INFO("Failure calling service /find_wall");
        }
        ROS_INFO("Successfully called service /find_wall");

        // call action server to start recording odom values
        actionlib::SimpleActionClient<rosject::OdomRecordAction> client("record_odom", true);
        client.waitForServer(); // Waits until the action server is up and running 

        rosject::OdomRecordGoal goal; // Creates a goal to send to the action server
        goal.success = false; // Fills the goal.                                                                                                              
        client.sendGoal(goal, // sends the goal to the action server  
                boost::bind(&controller::doneCb, this, _1, _2),
                boost::bind(&controller::activeCb, this),
                boost::bind(&controller::feedbackCb, this, _1));

        vel.angular.z = 0;
        pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
        sub = nh.subscribe("/scan", 1000, &controller::laserCallback, this);

        ros::waitForShutdown(); //<--- Moved waitForShutdown to here.
        }

        void doneCb(const actionlib::SimpleClientGoalState& state,
                    const rosject::OdomRecordResultConstPtr& result)
        {
            ROS_INFO("The robot has completed a lap! Great job, robot.");
            ROS_INFO("The robot started at x = %f, y = %f, and theta = %f", result->list_of_odoms[0].x, result->list_of_odoms[0].y, result->list_of_odoms[0].z);

            int final = result -> list_of_odoms.size();
            //using final as index, so need to reduce by 1 since c++ indexing starts at 0.
            final -= 1;

            ROS_INFO("The robot finished at x = %f, y = %f, and theta = %f", result->list_of_odoms[final].x, result->list_of_odoms[final].y, result->list_of_odoms[final].z); 

            vel.linear.x = 0;
            vel.angular.z = 0;
            pub.publish(vel);

            ros::shutdown();
        }

        void activeCb()
        {
            ROS_INFO("Odom values are now being recorded.");
        }

        void feedbackCb(const rosject::OdomRecordFeedbackConstPtr& feedback)
        {
            ROS_INFO("[Feedback] The robot has traveled %f meters.", feedback->current_total);
        }

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

            mylocation = msg -> ranges[180];
            vel.linear.x = 0.1;

            //continue straight
            if (mylocation > 0.2 and mylocation < 0.3) {
                vel.angular.z = 0;
                status_update = "Moving straight forward.";
            } 

            //turn left to move away from the wall
            if (mylocation < 0.2 ) {
                vel.angular.z = 0.14;
                status_update = "Moving away from the wall.";
            } 

            //turn right to move closer to the wall
            if (mylocation > 0.3 ) {
                vel.angular.z = -0.1;
                status_update = "Moving toward the wall.";
            } 

            // turn left if wall ahead
            if (msg -> ranges[360] < 0.7 ) {
                vel.angular.z = 0.4;
                status_update = "Corner ahead...turning...";
            }
            
            ROS_INFO("%s\n", status_update.c_str());
            //ROS_INFO("%f\n", mylocation);            

            pub.publish(vel);
            ros::spinOnce();
        }

    private:
        ros::Subscriber sub;
        ros::Publisher pub;
        ros::NodeHandle nh;

        geometry_msgs::Twist vel;
        std::string status_update;

        double mylocation; 

        ros::AsyncSpinner spinner;
        
};

int main(int argc, char** argv) {

    ros::init(argc, argv, "follow_wall");

    controller ctrl;
    
    return 0;
}

```

---

<div class="post-metadata">

**Author:** ![Voltedge](https://get-help.theconstruct.ai/user_avatar/get-help.theconstruct.ai/voltedge/32/7586_2.png) [@Voltedge](https://get-help.theconstruct.ai/u/Voltedge)\
**Post date:** [February 13, 2023, 7:40pm UTC](https://get-help.theconstruct.ai/t/threading-across-multiple-nodes-ros1-c/22359/5 "2023-02-13T19:40:58Z")

</div>

Hi @IMSETraining

Exactly!

I’m glad I helped you

---

<div class="post-metadata">

**Author:** ![Voltedge](https://get-help.theconstruct.ai/user_avatar/get-help.theconstruct.ai/voltedge/32/7586_2.png) [@Voltedge](https://get-help.theconstruct.ai/u/Voltedge)\
**Post date:** [February 13, 2023, 8:08pm UTC](https://get-help.theconstruct.ai/t/threading-across-multiple-nodes-ros1-c/22359/6 "2023-02-13T20:08:14Z")

</div>

Oh and I think that the solution is this:  
[Threading Across Multiple Nodes ROS1 C++ - #2 by Voltedge](https://get-help.robotigniteacademy.com/t/threading-across-multiple-nodes-ros1-c/22359/2) 👀

---

<div class="post-metadata">

**Author:** ![bayodesegun](https://get-help.theconstruct.ai/user_avatar/get-help.theconstruct.ai/bayodesegun/32/10684_2.png) [@bayodesegun](https://get-help.theconstruct.ai/u/bayodesegun)\
**Post date:** [February 16, 2023, 8:09pm UTC](https://get-help.theconstruct.ai/t/threading-across-multiple-nodes-ros1-c/22359/7 "2023-02-16T20:09:06Z")

</div>

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