ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Creating pub/sub to ROS2 node spinning in a thread

asked 2022-04-30 09:40:05 -0500

Speedox gravatar image

Hi everyone I'm trying to migrate this project I had from ROS to ROS2 and I'm having a problem creating pub/sub to my node spinning a thread, I have this function runRosnode() where I create the node then rosMain() will spin the node on a thread so it doesn't stop the code from executing, getROSNode() returns the node so I can eventually call it and create pub/sub in other cpp files. My problem is that I can't create any pub/sub outside of the rosMain() function where before (in ROS) this approach was working, I don't get any compilation errors but the pub/sub created outside of rosMain() don't show with

ros2 topic list

while the ones created inside rosMain() work as intended, anyone has any idea on why this is the case and how I can solve it ? Thanks in advance

     rclcpp::Node::SharedPtr & ROSOMNeT::getROSNode() {

       return rosNode;
     }

    void ROSOMNeT::runROSNode() {

      cout << "Running ROS2 node" << endl;

      rclcpp::init(0, nullptr);
      rosNode = std::make_shared<rclcpp::Node>("ROS2OMNeT");

       //these do  not work
       rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher__ =
                 getROSNode()->create_publisher<std_msgs::msg::String>("topic2", 10);

        auto subscription_ = getROSNode()->create_subscription<std_msgs::msg::String>(
                 "topic2", 10, std::bind(&ROSOMNeT::topic_callback, this, _1));

       rosThread = new thread(&ROSOMNeT::rosMain, this);


   /* old ROS code that used to work
        cout << "Running ROS node" << endl;

       init(M_string(), "ROSOMNeT");

       rosNode = new NodeHandle();

      rosThread = new thread(&ROSOMNeT::rosMain, this);*/
  }

 void ROSOMNeT::topic_callback(const std_msgs::msg::String::SharedPtr msg)
 {
  cout<<( "I heard: '%s'", msg->data.c_str());

 }

 void ROSOMNeT::rosMain() {
        cout << "ROS Main spinning on ROS" << endl;

//these work
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_ =
             getROSNode()->create_publisher<std_msgs::msg::String>("topic1", 10);

       auto subscription_ = getROSNode()->create_subscription<std_msgs::msg::String>(
              "topic", 10, std::bind(&ROSOMNeT::topic_callback, this, _1));


   rclcpp::spin(getROSNode());

  }
edit retag flag offensive close merge delete

Comments

Maybe not a relevant question, but why do you not let ROSOMNeT inherit from Node, but instead create a ros1 nodehandler workaround?

Joe28965 gravatar image Joe28965  ( 2022-05-03 10:14:25 -0500 )edit

As I mentioned the goal is to able to create pub/sub in other files (so I can use their methods as callbacks) so this approach was perfect as I could just create a ROSOMNeT obj and call getROSNode() in the other files something like this

auto subscription_ = rosomnet.getROSNode()->create_subscription<std_msgs::msg::String>( "topic", 10, std::bind(&MyOtherClass::MyOtherClassMethod, this, _1)); and this used to work with ROS but now that I'm trying to switch to ROS2 it doesn't.

now if I was to try and accomplish this by making my node through a class that inherits from Node how would I use my other class method as CallBacks for the subs and how would I create pubs in other classes (outside of the node one ?)

Speedox gravatar image Speedox  ( 2022-05-03 23:08:57 -0500 )edit

2 Answers

Sort by » oldest newest most voted
0

answered 2022-05-05 04:10:24 -0500

Speedox gravatar image

updated 2022-05-05 15:28:35 -0500

First of all thank you for ur help ^_^

I've just attached this snippet because I thought it would be the important part as the rest isn't relevant to the problem of the sub/pub only being created inside the thread function, here's the ROSOMNeT.hfile where I declare everything if it helps

class ROSOMNeT
 {
public:

     ROSOMNeT();
    ~ROSOMNeT();

    static ROSOMNeT &getInstance();
    void topic_callback(const std_msgs::msg::String::SharedPtr msg);
    void runROSNode();
    void runSimulation(string configFileName);
    void stopROS();
    rclcpp::Node::SharedPtr &getROSNode();


private:
    static ROSOMNeT instance;

    std::string configFileName;

    rclcpp::Node::SharedPtr rosNode ;

    thread* rosThread;
    void rosMain();
};

As for init ROS in my main I've tried doing that but it didn't change anything so I figured that's not the problem ? (still kept the spin in the thread because I don't want the rest of the code to stop excusing like in the Pub/Sub tutorial) here's how the main look :

    cout << "Starting ROS node" << endl;
    rclcpp::init(argc, argv);
    rosomnet.runROSNode();

the imu sensor looks interesting as it seems like they are creating pub/sub to their node in different classes (which is basically what I'm trying to accomplish) but I still can't figure out how their implementation works impl_ sdf::ElementPtr _sdf ? (seems a bit complicated at my lvl tbh)

Any extra help will be appreciated

EDIT: to address your update:

the way the object was created in main (this already existed on the project so I didn't modify it ) :

cout << "ROSOMNeT++ starting up" << endl;
ROSOMNeT &rosomnet = ROSOMNeT::getInstance()

the getInstance is declared in the ROSOMNeT class :

ROSOMNeT ROSOMNeT::instance;
ROSOMNeT& ROSOMNeT::getInstance() {
   return instance;
  }

The reason why I don't spin the object as they did in the tutorial is because that way the code will stop executing and stay on the spin() command, while I want the node to spin and the rest of the code keep executing so ideally I would call runROSNode() that creates my node, then creates a new thread, that thread does the spin and the rest of the code where I have my pub/sub, callbacks & other code to keep working (I added the creating of pub/sub in rosMain() and runROSNode() to highlight the problem of the pub/sub outside of rosMain() not working ... so if I can eventually create them in runROSNode() I can create them in the other cpp files too )

I was also thinking that the problem comes from the way the node is spinning but I can't figure out why/how to fix it, as this same approach was working with ROS 🤔🤔

edit flag offensive delete link more

Comments

In the future please edit your post to add extra information, not as an answer (makes it more confusing). I've updated my answer since I hate adding too much code etc to comments.

Joe28965 gravatar image Joe28965  ( 2022-05-05 04:52:43 -0500 )edit
0

answered 2022-05-04 01:55:19 -0500

Joe28965 gravatar image

updated 2022-05-05 04:52:21 -0500

So I actually remembered that gazebo_ros does something similar. They can't actually inherit Node due to needing to inherit gazebo::SensorPlugin. Anyway, long story short, have you looked at the implementation of (for instance) their imu sensor? They have a class called GazeboRosImuSensorPrivate that has the following public variable gazebo_ros::Node::SharedPtr ros_node_;.

For help on your actual code, it's a bit difficult, due to it being a snippet, not your full implementation. (also, note that I assume rosNode is a member variable, but custom is to give it a trailing underscore like rosNode_).

I would also not init ROS in your runROSNode but in your main and let the object ROSOMNeT spin in the same way they do in the Pub/Sub tutorial? That has become the more default way of doing this in ROS 2.

EDIT: to address your update:

cout << "Starting ROS node" << endl;
rclcpp::init(argc, argv);
rosomnet.runROSNode();

Where do you create the object rosomnet?

In the Pub/Sub tutorial they do the following:

rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalPublisher>());
rclcpp::shutdown();
return 0;

You, on the other hand, call runROSNode(), which does some stuff, then creates a new thread, that thread does some stuff and adds a spin at the end.

Why not spin the object, as done in the tutorial? Also, isn't runROSNode() something that would be better done in the constructor?

I'm wondering if your problem stems from the way you spin your node. You first create the publisher/subscriber, then create a separate thread and that one ends up spinning your node. That might be the reason why your pub/subs in runROSNode() do not work.

I think the problem you are having is unrelated to your wish to share the rosnode. You could create the shared pointer rclcpp::Node::SharedPtr rosNode_; (please start using trailing underscores for member variables, it's good cpp practice) and in the constructor have rosNode_ = std::make_shared<rclcpp::Node>("ROS2OMNeT"); and use that way to share your node.

I think the problem comes from how you spin the object.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2022-04-30 09:40:05 -0500

Seen: 1,058 times

Last updated: May 05 '22