Ask Your Question
1

RViz Panel: How not to freeze RViz?

asked 2020-05-21 06:08:16 -0500

xkaraman gravatar image

updated 2020-05-21 07:14:37 -0500

Hey there,

I have written a C++ RViz custom panel. The implementation roughly follows this example link.

The panel subscribe to a specific topic PointStamped type, and fills a list that gets displayed inside the panel as x and y coordinates.

Now, I've created a push button and implemented an actionlib move_base goal function. The purpose is, the robot to go from one point to another. The problem is while i succesfully send the goals, RViz Visualization is blocked/freezed until the move is complete probably because ac.waitForResult() is a blocking function.

How can i overcome this and let RViz keep visualizing robot's position while the robot moves to a point, get the result and then move to another point in the list. What i was thinking is probably about blocking GUI thread, so i good idead proably is to spin a new thread, but how can i accomplish this? Correct me. if I am wrong, and please if you need more info just let me know.

Here is a block of code that i am using right now and is blocking:

void Waypoints::moveTo() {
//tell the action client that we want to spin a thread by default
MoveBaseClient ac("move_base",true);
//wait for the action server to come up
while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
}
ROS_INFO("Move_base action server has come up");

move_base_msgs::MoveBaseGoal goal;

for (size_t x = 0; x < mBestPathNodes.size() - 1; x++) {
   // we'll send a goal to the robot to move 1 meter forward
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();

    goal.target_pose.pose.position.x = mWaypoints[mBestPathNodes[x]].point.x;
    goal.target_pose.pose.position.y = mWaypoints[mBestPathNodes[x]].point.y;
    goal.target_pose.pose.orientation.w = 1.0;

    ROS_INFO("Sending goal");

    ac.sendGoal(goal);
    ac.waitForResult();
    bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));

      if (finished_before_timeout)
      {
        actionlib::SimpleClientGoalState state = ac.getState();
        ROS_INFO("Action finished: %s",state.toString().c_str());
      }
      else
        ROS_INFO("Action did not finish before the time out.");

}
edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
0

answered 2020-05-21 15:03:23 -0500

updated 2020-05-21 15:04:19 -0500

You need to spin up another thread to wait and throw back a result once its complete. Or optionally not wait for a result since all you're doing is printing to screen. Since you're not using the result or tracking progress, there's not a bunch of value in waiting around, unless that print statement is really important to you. In that case, you should just run another thread that is waiting for the result and printing the completion status.

If you want to thread it, std::threads is what you want for the most basic implementation https://en.cppreference.com/w/cpp/thr...

edit flag offensive delete link more

Comments

I' am actually printing to see it's working because RViz freezes and can't see my robot moving around. Even so, i would need the result in order to see what's happening, and if it's a point is a reached THEN move to another one. If i have 3 points to go through, if i don't have the waitForResult, the for loop will just start for a moment the movement to 1st point, then stop it almost immediately, begin course for 2nd point, stop and then go for the 3rd point. I could implement another way, if you have anything in mind but the waitForResult i think it should be there.

xkaraman gravatar image xkaraman  ( 2020-05-22 04:29:32 -0500 )edit

Answer still stands- you need to spin up a background thread to process it.

stevemacenski gravatar image stevemacenski  ( 2020-05-22 11:05:55 -0500 )edit

OK i understand that. How can i spin a background thread then? The link is not that helpful without an example unfortunately.

xkaraman gravatar image xkaraman  ( 2020-05-23 09:57:20 -0500 )edit

I think that's something you should look up yourself. That's not really a ROS question but a basic C++ question. That documentation link should be sufficient to use, but there are other C++ threading tutorials that may be better suited to introducing the concept to you.

stevemacenski gravatar image stevemacenski  ( 2020-05-24 22:13:57 -0500 )edit

OK thanks for your answer man! :) Have a good day!

xkaraman gravatar image xkaraman  ( 2020-05-25 04:11:58 -0500 )edit
0

answered 2020-05-21 18:18:52 -0500

AlexisGaziello gravatar image

I have a similar issue. I haven't resolved it completly yet, but to put you on track, I basically went though the moveit code to try to understand how the team does it. I have managed to make it work with:

this header:

#include moveit/background_processing/background_processing.h>

declaring the following object in MyPanel class:

moveit::tools::BackgroundProcessing background_process_;

After, I put all the moveit code in a function

void MyPanel::plan(){ /* code*/}

and call the function with:

background_process_.addJob(boost::bind(&MyPanel::plan, this), "plan");

May be necessary to add this to the MyPanel destructor:

background_process_.clear();

edit flag offensive delete link more

Comments

This would require MoveIt framework if I'm not mistaken right? I would like not to involve yet other libraries rather than ROS and maybe boost!

xkaraman gravatar image xkaraman  ( 2020-05-22 04:25:05 -0500 )edit

Its just spinning up a background worker like my answer suggested. They just wrote a nice tool for it. I didn’t want to overcomplicate it for you - if you’re not familiar just using std::thread is fine.

stevemacenski gravatar image stevemacenski  ( 2020-05-22 11:05:29 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-05-21 06:08:16 -0500

Seen: 25 times

Last updated: May 21