peek - Finding location of models after specified time
I want to find the estimated position of a model after a time X. So, I basically want gazebo to run the simulation for time X and give back the ModelState after it runs it for time X.
I am currently using the ROS Service /gazebo/get_model_state and /gazebo/pause_physics with /gazebo/unpause_physics to:
unpause -> wait for time X -> pause -> query model state
But this is giving inaccurate results. I guess because the "wait for time X" is bad. I need it to give the same positions repeatably.
How do I do something like what I want ?
EDIT: This is the algorithm I wish to run:
- Launch file is run, Node1 and gazebo started. Gazebo is paused initially.
- Node1 sets some motor velocities
- Gazebo is unpaused. As soon as 1 sec passes in simulation time, gazebo pauses and give back the state of the model
- Node1 finds the new motor velocities to set, and it publishes that.
- Goto step 3
Asked by AbdealiJK on 2015-09-02 02:02:47 UTC
Answers
I expect when you say you're using ROS Service that you're calling it from the command line? If so then yes it will always be a little off.
You could write a ROS node that checks the rostopic /clock and if the time is right, calls the service get_model_state to get your model position. You have to enable use_sime_time when starting Gazebo though to make sure the clocks are synchronized (http://answers.gazebosim.org/question/3172/timing-synchronisation-between-gazebo-and-ros/). The node can run as fast as you want so it will check the time every Gazebo iteration making it fast enough for your needs I expect. Good luck!
Asked by niall on 2015-09-02 15:33:25 UTC
Comments
When I said I was using the ros service, I did it through a C code (where I used the usleep function for the time gap). I'll try the /clock you mentioned and see if that works out. I hope the simulation does not become considerably slower because of the topic to /clock
Asked by AbdealiJK on 2015-09-02 22:40:13 UTC
niall, I tried subscribing to /clock, but I had to set my queueDepth to 10000, as with queueDepth=1, it skipped a lot of clock ticks. I found that the rosnode was lagging behind the actual gazebo simulation. I need to set a new velocity every time I query the current state, and as the rosnode and gazebo are running at different speeds, I the action is being set in the future according to gazebo. You mentioned I can make the rosnode "can run as fast as I want" - what did you mean by that ?
Asked by AbdealiJK on 2015-09-02 23:08:50 UTC
Did you try use_sim_time? This will sync Gazebo and ROS clock. With run as fast as you want I mean it can be set to run every time a callback is received from clock, that is a lot faster then you want to do anything.
Asked by niall on 2015-09-03 09:54:12 UTC
Comments