Visualizing in Rviz
This tutorial will introduce you to a tool that can help you more easily understand what your MoveIt application is doing by rendering visualizations in RViz.
Follow along as we do the tutorial found here: Visualizing in rviz
Transcript: Hello. This is Tyler Weaver, and I am welcoming you to the second part of a three part series on building your first project with MoveIt. And in this one, I’m going to show you something that’s really cool. It’s a set of tools that live technically outside of MoveIt, but are really critical to developing robot applications.
One of the things that’s really hard to do when working with a serial robot arm is to understand how the planning is working or how the executing is working without visualizations. And the reason is because all the planning and stuff is happening in, like, six or seven dimensional space, and it’s really hard to wrap your head around it. Even if you print stuff, even if you’re really familiar with the math, visualizations can really just make things easier for you.
In this one, I’m going to introduce you to a series of tools that was created by Dave called MoveIt. Visual Tools. It builds on this other package called RVs Visual Tools. What it does is it provides hooks that you can write in C to cause RVs to visualize things.
So first thing we got to do is add it as a dependency. It’s already built as part of the tutorials workspace. We just have to add it to our package and move it that we created. So I grabbed this. I’m going over here. I go to the package XML, and I’m going to add it here.
All right, the next thing I need to do is is add the fine package statement into C make. So go over to C make and add the fine package statement. And then lastly, we need to update this event target dependencies. So that includes moving the visual tools, which is pretty easy. We got that. Lastly, I’m going to add it as a header and build it to verify that all that went correctly.
It’s really important when you’re developing software to work incrementally and verify your assumptions. Makes it much easier to catch things than like, programming a whole bunch of stuff and then testing and finding out it doesn’t build for 47 different reasons. You have to separate all those reasons to debug it incrementally. It makes your life a little bit easier. So build and we’ll verify that it builds.
Here we go. There we are. We’re building the hello movement package. And it worked. All right, so on to the next step. So the first thing that we got to do is we have to spin the node in an executor. Now, if you did a bunch of the ROS 2 tutorials, you know about this ROS has one of the big things that’s really powerful about ROS is you have these services and. Publishers and subscribers and things like that. It all has to happen asynchronously, which means that you have to have some other thread to service those callbacks. Because of how visual tools work. We need to get that started.
So we got to add this little bit of code. We need to add include thread at the top of our program that’s from the standard library. And then the next thing is after we do the logger, then we have to add the single threaded executor spinning. It’s going to spin the node that we created earlier. It’s going to create a thread name thread spinner. And then lastly at the end, after the Rclcpv shut down, we’re going to join. The reason we do it afterwards, after that is because by calling this it will cancel all the executors. And so this executor right here will return from the spin function and so we can join the thread.
Okay, so there’s a little bit of boilerplate raw stuff that we had to get out of the way. Now we can move onto the meat of it. We’re going to create this thing called Vigil tools. Oh yeah, we already included the header file so we’re good to go. We got to create one of these movement visual tool objects. It takes a handful of parameters and I’ll talk you through what those are. I’m going to copy, copy this, this blocker code and we’re going to paste it right after the move group interface.
Again, there’s an indentation issue that I should figure out some way to fix. But anyway, we create this new thing. It’s constructor takes the node that it uses for these services, the base link of the robot that is needed for when we give it positions and space and stuff. We want it to be able to not plan but be able to draw things relative to that base.
So we need a base link that we’re going to use relative for drawing things. This is the default topic, our vismarker topic. I’ll show you what that is later. And then lastly, we have to give it a robot model. Move group interface loads the robot model. How it works is a little complicated but here’s an easy way to get a copy of it. So we got to give it a robot model. It has to know about the model of robot for some of these visualizations. And then I called delete all markers.
And really what that’s for is like if you run this program multiple times that keeps it from drawing over itself basically. And then I’m going to load this remote control. Remote control is something really cool and I’m going to show you in a bit what’s it is. What it does is it provides a really simple way for you to.
Add these prompt statements to your program that let you step through it. So say you have some complicated problem or program that like did different things and you want to pause the program in a certain place and be able to step through it. It’s sort of bugging tool.
If you’re working on figuring out how to do some complicated motion and move it, this can be really helpful. So we’re going to load remote control that sets up a service for it. All right, the next thing we have to do is write some closures for visualizations.
And I want to talk to you about closures. So closures, all I closure is is a function essentially that keeps access to something in the local scope. So this first one draw a title. We use lambdas to create closures in code. Plus it takes a reference to move itvigital tools. This allows it to call moveitvigital tools. And then it takes a parameter called text. And what it does is it creates here’s another one of these lambdas that create a constant. This one, you’ll notice it has a translation z of 1 meter. What this does is gives us a pose in space that is a meter above the base link of the robot.
Remember, we gave it the basement of the robot here, so it uses that as the relative position. And then it’s going to call this thing called publish text, which takes the pose that we created, the text that we passed in, and then some stuff about the pose. So we’re going to use white and extra large font. And it’s really useful to have just a way to draw text in our visit so you can show sort of what your programming is doing while people are looking, while you’re looking at the visualization, so you don’t have to trace through the logs and stuff. It’s just easy to have stuff in the same place.
All right. The second thing is that prompt command that I told you about that pauses it. This one is pretty basic sick. We’re going to create a closure called prompt and we’re going. Move a visual tools prompt. And the last one is Draw Trajectory Path. This one is really cool because what this one does is it will draw a line. We’re going to call this published trajectory reliant. It’s going to draw a line that the end effector will trace as you execute the trajectory. It’s really helpful for visualizing, sort of like how the robot is going to move roughly and how efficiently the end effector is moving and stuff. And it’s just a useful tool.
So anyway, we have these three closures. The last one, we need this joint model group pointer thing. And we can get the robot model. Again, we need the robot model for something and we can get the joint model group. And the joint model group, it’s an object that represents data about the group of things. Like in this case, the panda arm. So that’s what we’re doing. And we pass in a trajectory trajectory. In this case, it’s just a message. And you’ll see how we get that out of the plan later.
Anyway, let’s go back to the website and get more code. All right. So the next thing we’re going to do is before we do this plan, remember this code right here, which does the plan. We’re going to have a prompt to have us press next. And then we’re going to draw a title saying we’re planning after you press next. And then there’s this trigger thing. And that’s because with visual tools, it could send a ton of messages. And this is more important. Once you. Start using it more heavily. But what it does is it batches up the message it’s just going to send to RVs and does them all at once. This triggers. This tells it’s sort of like the like a render command. It’s like winning in our business is going to do all the things you asked it to do.
So I’m going to grab that code and we’ll go back over to our editor and we’ll put it before this plan. All right? Now the next thing is under execute plan. The plan is a message and one of the fields in it, it’s called trajectory. It’s technical trajectory underscore. And that’s a trajectory message we need. So if we succeed, I’m going to draw the path and then ask you to press next. And when you press X, I’m going to switch to the title to executing. I’m going to trigger that and then execute the plan. And if it failed, I want to draw a planning failed trigger in visualization. So you don’t have to look at the air in the log. Just make it a little bit easier.
So I copy that. We’ll come over here. And edit the execute the Plan section of the code. Alright, now we just need to build it and run it, the whole thing. So here we go. I’m going to build it. Oh. The next thing is we need to change our Arvis set up. So while it’s building, there’s a series of screenshots here to talk through it. But I’m just going to show you how to do it. We come over here to Arvis. We already turned off motion planning.
The next thing we need to do is we need to add the Arvis Visual Tools thing that has this thing down here. That’s the next button that we’re going to push later. The next thing we have to do is add a marker array. This is the topic that’s going to use to draw visualizations on here. We don’t want this topic. We actually want a different one. I have to go back to the instructions to find find it. That’s the one we added that constant for. We added a marker array we clicked.
Okay. It’s called our Visual Tools. It’s. One where we’re using the topic we want it to use. Alright, we’re good to go. So if I come over here and I run the program again, you’ll notice it says, waiting to continue. Continue. Press next to continue. So we’ll come back over here, nothing has changed. I’m going to press Next. And now, you see, it says planning. Planning failed. Well, run again. Sometimes plans fail, and there are a lot of reasons for that, and it’s way more complicated than the scope of this video. But I go back and we can try executing it again, and it should ask us to press next again.
All right, it’s planning. All right, it planned successfully. And here’s the tool path that I told you about. It shows what technically the end of the last link, not the end of the tool. It’s going to trace. If I press next, it’ll execute the path and you see the robot moving along the path that we planned. And that’s the whole thing.
I hope you’ve enjoyed this. I hope you’ll join me for the third and final episode of building your hello movie project. In that one, we will add a collision object to the environment plan around it. Thank you for watching.
If you enjoyed this video be sure to check out our other series here: Getting Started With MoveIt 2 and ROS Developer Notes
