Use plugin after the pr2 has spawned
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>