Ask Your Question
0

Get current position of a robot from a world file (Stage)

asked 2012-08-09 03:48:15 -0600

Clefairy gravatar image

updated 2012-08-09 03:48:49 -0600

I have C++ code where I am defining the behaviour of a group of robots. However right now the initial positions of each robots are defined in a .world file for the Stage simulation. Is there a way of possibly getting the position and theta defined in the .world file and referring to it in the C++ code?

I need some sort of way to set the following in the C++ code:

x = theXNumberDefinedInWorldFile
y = theYNumberDefinedinWorldFile
z = 0
theta = theAngleNumberDefinedInWorldFile

Sorry if this is hard to understand but I couldn't think of any other way of putting it as I'm still new to ROS. Any help would be much appreciated.

Thanks.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2012-08-09 05:20:05 -0600

mcevoyandy gravatar image

updated 2012-08-09 05:21:52 -0600

you should be able to use:

math::Pose currentPose;
currentPose=world->GetModel(i)->GetLink("link_name_from_urdf")->GetWorldPose();

since i can change even when loading the same urdf file, you can find i by something like:

if (world->GetModel(i)->GetName().find("model_name_in_gazebo")!=std::string::npos) { }

check out the manipulating models world plugin example and the math page for some more info.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

Stats

Asked: 2012-08-09 03:48:15 -0600

Seen: 515 times

Last updated: Aug 09 '12