Home | Tutorials | Wiki | Issues
Ask Your Question
0

WorldPlugin. Process has died. Exit code 134

asked 2018-04-20 04:55:15 -0600

iAba gravatar image

updated 2018-04-20 05:15:44 -0600

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/iss...

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=gu... . The plugin code is the following:

#include .....

namespace gazebo
{

class StepWorldPlugin : public WorldPlugin
{

public: StepWorldPlugin() : WorldPlugin()
{
}

private:
    physics::WorldPtr world;
    std::unique_ptr<ros::NodeHandle> 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<std_msgs::Bool>("/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<t>::type boost::shared_ptr<t>::operator->() const [with T = gazebo::physics::World; typename boost::detail::sp_member_access<t>::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 ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-04-20 05:22:04 -0600

You never assign a value to this->world, so you're trying to access an uninitialized variable.

You should add this line somewhere in the StepWorldPlugin::Load function:

this->world = _world;

edit flag offensive delete link more

Comments

Stupid mistake... Thanks a lot!!

iAba gravatar imageiAba ( 2018-04-20 05:45:47 -0600 )edit
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2018-04-20 04:55:15 -0600

Seen: 544 times

Last updated: Apr 20 '18