Use plugin after the pr2 has spawned

asked 2018-08-14 12:22:19 -0600

guyhaume25 gravatar image

Hi. I have a plugin I need to use after a specific event. I want to stick objects to the pr2 grippers, so I need to call the plugin to stick the objects to the gripper after the pr2 has spawned. I didn't find a way to start the pr2 before the plugin. Unless there is a way to spawn the pr2 from the world file ? (I need to call pr2.launch from pr2_gazebo, not like this

<uri>model://pr2</uri>

Can I use the get_model_state gazebo message ? Should I do all of this in a world plugin ? Here is what I've tried, to try to understand how it should work, but this is executed before the pr2 spawns. I know this is wrong, but how can I change it so it works ?

#include "GripTest.hh"
#include <gazebo_msgs/GetModelState.h>
#include <ros/ros.h>
#include <thread>
#include <chrono>
#include <string>

using namespace gazebo;
GZ_REGISTER_WORLD_PLUGIN(GripTest);

void GripTest::Load(physics::WorldPtr world, sdf::ElementPtr /*sdf*/)
{
    ros::NodeHandle n;
    ros::ServiceClient client =
         n.serviceClient<gazebo_msgs::GetModelState>("get_model_state", "{model_name = pr2}");
    gazebo_msgs::GetModelState srv;

    for (int i = 0; i < 10; i++)
    {
        if (client.call(srv))
        {
            ROS_INFO("Success : %d", srv.response.success);
        }
        else
        {
            ROS_ERROR("Failed to call service get_model_state of pr2");
        }
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

    int i(0);
    gazebo_msgs::GetModelState::Request req;
    gazebo_msgs::GetModelState::Response res;

    do
    {
        i++;
        req.model_name = "pr2";
        ROS_INFO("Not yet...");
    } while (i < 10000 && res.success == 0);

 // Then, when out of the loop (== "pr2" has spawn) ?
    ROS_INFO("PR2 has spawned ! i : %d; res : %d", i, res.success);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "model_state");
    ros::spin();
}

here is the GripTest.hh file :

#ifndef PLUGINS_GRIPTEST_HH
#define PLUGINS_GRIPTEST_HH
#include <gazebo/gazebo.hh>

namespace gazebo {
    class GripTest : public WorldPlugin {
        public: void Load(physics::WorldPtr world, sdf::ElementPtr /* _sdf */) override;
    };
}

#endif //PLUGINS_GRIPPLUGIN_HH

And I just call it at the end of the world file :

<plugin name="grip_test" filename="libGripTest.so"></plugin>
edit retag flag offensive close merge delete