Hi,
I am trying to write a World Plugin so I can control the simulation and run it step by step using function world->Step(1) as described in https://bitbucket.org/osrf/gazebo/issues/2385/step-by-step-simulation-c-with-gazebo
The idea is to start the simulation paused and to synchronise Gazebo's simulation with an external processing, so what I want to do is to have a ROS node publishing on a topic and every time a message is published on that topic the plugin will advance the simulation one step.
I have created the plugin following tutorial http://gazebosim.org/tutorials?cat=guided_i&tut=guided_i6 . The plugin code is the following:
#include .....
namespace gazebo
{
class StepWorldPlugin : public WorldPlugin
{
public: StepWorldPlugin() : WorldPlugin()
{
}
private:
physics::WorldPtr world;
std::unique_ptr rosNode;
ros::Subscriber rosSub;
ros::CallbackQueue rosQueue;
std::thread rosQueueThread;
public:
void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
{
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
{
int argc = 0;
char **argv = NULL;
ros::init(argc, argv, "gazebo_client",
ros::init_options::NoSigintHandler);
}
// Create our ROS node.
this->rosNode.reset(new ros::NodeHandle("gazebo_client"));
// Subscribe to topic "/step_cmd"
this->rosSub = this->rosNode->subscribe("/step_cmd", 1000, &StepWorldPlugin::OnMsg, this);
ROS_INFO("Subscribed");
// Spin up the queue helper thread.
this->rosQueueThread = std::thread(std::bind(&StepWorldPlugin::QueueThread, this));
{
//Step simulation after a message is received
public: void OnMsg(const std_msgs::BoolConstPtr &_msg)
{
ROS_INFO("GZ Plugin: OnMsg");
this->world->Step(1);
}
//ROS helper function that processes messages
private: void QueueThread()
{
static const double timeout = 0.001;
while (this->rosNode->ok())
{
this->rosQueue.callAvailable(ros::WallDuration(timeout));
}
}
};
GZ_REGISTER_WORLD_PLUGIN(StepWorldPlugin)
}
Then I have created a simple ROS node to publish on the topic "/step_cmd" as follows:
#include ....
int main(int argc, char **argv)
{
ros::init(argc, argv, "step_node");
ros::NodeHandle nh;
// Create publisher
ros::Publisher step_pub = nh.advertise("/step_cmd", 1000);
ros::WallDuration loop_rate(5);
// Wait for subscribers
do{
ROS_INFO("Waiting for connection");
}while (step_pub.getNumSubscribers() == 0);
// Message to publish
std_msgs::Bool msg;
msg.data = true;
while (ros::ok())
{
//publish message
step_pub.publish(msg);
ROS_INFO("Message sent");
loop_rate.sleep();
++count;
}
return 0;
}
The problem comes when the first message is published on topic "/step_cmd", gazebo crashes with the following error:
> gzserver:
> /usr/include/boost/smart_ptr/shared_ptr.hpp:648:
> typename
> boost::detail::sp_member_access::type
> boost::shared_ptr::operator->()
> const [with T =
> gazebo::physics::World; typename
> boost::detail::sp_member_access::type
> = gazebo::physics::World*]: Assertion `px != 0' failed. Aborted (core
> dumped)
>> [gazebo-2] process has died [pid 6414,
> exit code 134, cmd
> /opt/ros/kinetic/lib/gazebo_ros/gzserver
> -u -e ode /home/usuario/ros_ws/src/baxter_simulator/baxter_gazebo/worlds/baxter_sync.world
> __name:=gazebo __log:=/home/usuario/.ros/log/6ade6288-447f-11e8-ac66-9cb6d01fa38b/gazebo-2.log]. log file:
> /home/usuario/.ros/log/6ade6288-447f-11e8-ac66-9cb6d01fa38b/gazebo-2*.log
The OnMsg method of the Plugin is runned, (the ROS_INFO message is published) but right after that Gazebo crashes.
Any ideas why this is happening? Maybe I'm not using the Plugin in a right way, or it is a problem with the function world->Step()...? I don't know...
Thanks for the help!
↧