Hello everybody. I have been stucked for a day on that and i do not find the solution to this problem.

I am just trying to do a mix between the clone simulation tutorial (http://gazebosim.org/tutorials?tut=clone_simulation&cat=) and a empty_world.launch simulation.

First of all, I am using ROS Kinetic and Gazebo 7.0

in clone.cpp I have commented the server run lines:

std::thread serverThread(RunServer);

and

serverThread.join();

Instead of running like this way the server, i just added the cpp code inside a launch copy of empty_world. adding the following line:

  <node name="clone_node" pkg="ros_trial" type="cloner" output="screen" />

(just to be clear, CMakeLists.txt and package.xml are correctly configured

When I do a roslaunch, everyting seems to work, but the api message /gazebo/server/control is not published.

I have check if the channel exists with gz topic -l (and exists) but if i do gz topic -e /gazebo/server/control nothing appears.

The reason why i want to do this is because I want to clone a simulation several time and run them simoultaneously in order to reduce training time. The simulation which i want to run uses gazebomsgs topics like gazebomsgs::SpawnModel which does not exists woithout ROS (and I did not found any substitute).

I do not know if I have been clear, (sorry if not!) in which case just ask me. Any help will be appreciated.

Thanks!

EDIT: I add the code to be more clear.

cloner.cpp:

#include <cstdlib>

#include <iostream>
#include <memory>
#include <thread>
#include <gazebo/gazebo.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>

#include <fstream>
#include <string>

#include <ros/ros.h>
#include <ros/package.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Inertia.h>
#include <std_srvs/Empty.h>
#include <std_srvs/Trigger.h>

#include <urdf/model.h>
#include <urdf_parser/urdf_parser.h>
#include <collada_urdf/collada_urdf.h>

#include <gazebo_msgs/SpawnModel.h>
#include <gazebo_msgs/DeleteModel.h>
#include <gazebo_msgs/SetModelState.h>
#include <gazebo_msgs/GetModelState.h>

#include <actionlib/server/simple_action_server.h>

std::mutex feedback_mutex__;
std::vector<std::shared_ptr<std::thread>> clone_thread_ptr_vector__;
std::vector<char*> __environments;
/////////////////////////////////////////////////
void OnWorldModify(ConstWorldModifyPtr &_msg)
{
    std::cout << "inside on world modify function" << std::endl;
  if (_msg->has_cloned() && _msg->cloned() && _msg->has_cloned_uri())
  {
    std::cout << "World cloned. You can connect a client by typing\n"
              << "\tGAZEBO_MASTER_URI=" << _msg->cloned_uri()
              << " gzclient" << std::endl;
    char uri = *_msg->cloned_uri().c_str();
    __environments.push_back(&uri);
  }
  else{
      std::cout << "ERROR 1" << std::endl;
}
}

/////////////////////////////////////////////////
void RunServer()
{
  // Initialize gazebo server.
    std::cout << "inside run server function" << std::endl;
  std::unique_ptr<gazebo::Server> server(new gazebo::Server());
  try
  {
    if (!server->ParseArgs(0, NULL))
      return;

    // Initialize the informational logger. This will log warnings, and errors.
    gzLogInit("server-", "gzserver.log");

    server->Run();
    server->Fini();
  }
  catch(gazebo::common::Exception &_e)
  {
      std::cout << "ERROR 2" << std::endl;
    _e.Print();
    server->Fini();
  }
}


int port_ = 11346;
char* __master = getenv("ROS_MASTER_URI");




void GenerateClone(gazebo::transport::PublisherPtr serverControlPub )
{
  std::cout << "inside GenerateClone" << std::endl;
  gazebo::msgs::ServerControl msg;
  msg.set_save_world_name("");
  msg.set_clone(true);
  msg.set_new_port(port_);

  std::cout << "publishing " << std::endl;
  serverControlPub->Publish(msg);
  std::cout << "published" << std::endl;

  port_++;

  return;

}

/////////////////////////////////////////////////
int main(int argc, char **argv)
{

ros::init(argc, argv, "talker");

ros::NodeHandle n;
__environments.push_back(__master);

 //std::thread serverThread(RunServer);
//setenv("GAZEBO_MASTER_URI", __master, 1);

  // Create a node for communication.
  gazebo::transport::NodePtr node(new gazebo::transport::Node());
  node->Init();

  // Publisher to the server control.
  gazebo::transport::PublisherPtr serverControlPub =
    node->Advertise<gazebo::msgs::ServerControl>("/gazebo/server/control");
//  ros::Publisher serverControlPub = n.advertise<gazebo_msgs::ServerControl>("/gazebo/server/control", 1000);


  // Subscriber to receive world updates (e.g.: a notification after a cloning).
  gazebo::transport::SubscriberPtr worldModSub =
    node->Subscribe("/gazebo/world/modify", &OnWorldModify);
    std::cout << "master: "<< __master << std::endl;
  std::cout << "\nPress [ENTER] to clone the current simulation\n" << std::endl;
  getchar();

  // Clone the server programmatically.
  for (int i = 0; i<2 ; i++){

    if (feedback_mutex__.try_lock()){
        clone_thread_ptr_vector__.push_back(std::make_shared<std::thread>(GenerateClone, serverControlPub ) );
        feedback_mutex__.unlock();

        //sleep(5);


    }
  }
  //__environments.push_back("http://localhost:11346");

  //__environments.push_back("http://localhost:11347");

   sleep(1);
  // modify the different worlds
  //master world
  std::cout << "\nPress [ENTER] to send a 301 to the master world\n" << std::endl;
  getchar();

  {
      //setenv("GAZEBO_MASTER_URI", __master, 1);
    std::string package_path = ros::package::getPath("ros_trial");
    std::string filename = package_path + "/urdf/part_301.urdf";
    urdf::ModelInterfaceSharedPtr part_model_ptr = urdf::parseURDFFile(filename);

    geometry_msgs::Inertia inertial__;
    inertial__.m =  0.05; 
    inertial__.com.x =  0.0; 
    inertial__.com.y =  0.0; 
    inertial__.com.z = 0.014879; 
    inertial__.ixx = 0.0006158372000000;
    inertial__.ixy = -0.0000000010145837;
    inertial__.ixz = -0.0000000054554562;
    inertial__.iyy = 0.0006157460000000;
    inertial__.iyz = 0.0000000212208923;
    inertial__.izz = 0.0003144807500000;

    part_model_ptr->getRoot()->inertial->mass = inertial__.m;
    part_model_ptr->getRoot()->inertial->origin.position.x = inertial__.com.x;
    part_model_ptr->getRoot()->inertial->origin.position.y = inertial__.com.y;
    part_model_ptr->getRoot()->inertial->origin.position.z = inertial__.com.z;
    part_model_ptr->getRoot()->inertial->ixx = inertial__.ixx;
    part_model_ptr->getRoot()->inertial->ixy = inertial__.ixy;
    part_model_ptr->getRoot()->inertial->ixz = inertial__.ixz;
    part_model_ptr->getRoot()->inertial->iyy = inertial__.iyy;
    part_model_ptr->getRoot()->inertial->iyz = inertial__.iyz;
    part_model_ptr->getRoot()->inertial->izz = inertial__.izz;


    TiXmlDocument* urdf_doc_ptr;
    urdf_doc_ptr = urdf::exportURDF( part_model_ptr );

    TiXmlPrinter printer;
    urdf_doc_ptr->Accept( &printer );
    std::string urdf_str = printer.CStr();

    sdf::ElementPtr __sdf;

    ros::ServiceClient spawn_srv = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_urdf_model");

    gazebo_msgs::SpawnModel spawn_service;
    spawn_service.request.initial_pose.position.x = 0.0;
    spawn_service.request.initial_pose.position.y = 0.0;
    spawn_service.request.initial_pose.position.z = 20.0;
    spawn_service.request.initial_pose.orientation.w = 1.0;
    spawn_service.request.model_name = "part";
    spawn_service.request.model_xml = urdf_str;
    spawn_srv.call(spawn_service);

  } 


//cloned 1
  std::cout << "\nPress [ENTER] to send a 021 to the first cloned world\n" << std::endl;
  getchar();
  setenv("GAZEBO_MASTER_URI", __environments[1], 1);

    {
    std::string package_path = ros::package::getPath("ros_trial");
    std::string filename = package_path + "/urdf/part_02.urdf";
    urdf::ModelInterfaceSharedPtr part_model_ptr = urdf::parseURDFFile(filename);

    geometry_msgs::Inertia inertial__;
    inertial__.m =  0.05; 
    inertial__.com.x =  0.0; 
    inertial__.com.y =  0.0; 
    inertial__.com.z = 0.014879; 
    inertial__.ixx = 0.0006158372000000;
    inertial__.ixy = -0.0000000010145837;
    inertial__.ixz = -0.0000000054554562;
    inertial__.iyy = 0.0006157460000000;
    inertial__.iyz = 0.0000000212208923;
    inertial__.izz = 0.0003144807500000;

    part_model_ptr->getRoot()->inertial->mass = inertial__.m;
    part_model_ptr->getRoot()->inertial->origin.position.x = inertial__.com.x;
    part_model_ptr->getRoot()->inertial->origin.position.y = inertial__.com.y;
    part_model_ptr->getRoot()->inertial->origin.position.z = inertial__.com.z;
    part_model_ptr->getRoot()->inertial->ixx = inertial__.ixx;
    part_model_ptr->getRoot()->inertial->ixy = inertial__.ixy;
    part_model_ptr->getRoot()->inertial->ixz = inertial__.ixz;
    part_model_ptr->getRoot()->inertial->iyy = inertial__.iyy;
    part_model_ptr->getRoot()->inertial->iyz = inertial__.iyz;
    part_model_ptr->getRoot()->inertial->izz = inertial__.izz;


    TiXmlDocument* urdf_doc_ptr;
    urdf_doc_ptr = urdf::exportURDF( part_model_ptr );

    TiXmlPrinter printer;
    urdf_doc_ptr->Accept( &printer );
    std::string urdf_str = printer.CStr();

    ros::ServiceClient spawn_srv = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_urdf_model");

    gazebo_msgs::SpawnModel spawn_service;
    spawn_service.request.initial_pose.position.x = 0.0;
    spawn_service.request.initial_pose.position.y = 0.0;
    spawn_service.request.initial_pose.position.z = 20.0;
    spawn_service.request.initial_pose.orientation.w = 1.0;
    spawn_service.request.model_name = "part";
    spawn_service.request.model_xml = urdf_str;
    spawn_srv.call(spawn_service);

  } 

  //cloned 2
  std::cout << "\nPress [ENTER] to send a 301 to the second cloned world\n" << std::endl;
  getchar();
  setenv("GAZEBO_MASTER_URI", __environments[2], 1);

    {
    std::string package_path = ros::package::getPath("ros_trial");
    std::string filename = package_path + "/urdf/part_301.urdf";
    urdf::ModelInterfaceSharedPtr part_model_ptr = urdf::parseURDFFile(filename);

    geometry_msgs::Inertia inertial__;
    inertial__.m =  0.05; 
    inertial__.com.x =  0.0; 
    inertial__.com.y =  0.0; 
    inertial__.com.z = 0.014879; 
    inertial__.ixx = 0.0006158372000000;
    inertial__.ixy = -0.0000000010145837;
    inertial__.ixz = -0.0000000054554562;
    inertial__.iyy = 0.0006157460000000;
    inertial__.iyz = 0.0000000212208923;
    inertial__.izz = 0.0003144807500000;

    part_model_ptr->getRoot()->inertial->mass = inertial__.m;
    part_model_ptr->getRoot()->inertial->origin.position.x = inertial__.com.x;
    part_model_ptr->getRoot()->inertial->origin.position.y = inertial__.com.y;
    part_model_ptr->getRoot()->inertial->origin.position.z = inertial__.com.z;
    part_model_ptr->getRoot()->inertial->ixx = inertial__.ixx;
    part_model_ptr->getRoot()->inertial->ixy = inertial__.ixy;
    part_model_ptr->getRoot()->inertial->ixz = inertial__.ixz;
    part_model_ptr->getRoot()->inertial->iyy = inertial__.iyy;
    part_model_ptr->getRoot()->inertial->iyz = inertial__.iyz;
    part_model_ptr->getRoot()->inertial->izz = inertial__.izz;


    TiXmlDocument* urdf_doc_ptr;
    urdf_doc_ptr = urdf::exportURDF( part_model_ptr );

    TiXmlPrinter printer;
    urdf_doc_ptr->Accept( &printer );
    std::string urdf_str = printer.CStr();

    ros::ServiceClient spawn_srv = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_urdf_model");

    gazebo_msgs::SpawnModel spawn_service;
    spawn_service.request.initial_pose.position.x = 0.0;
    spawn_service.request.initial_pose.position.y = 0.0;
    spawn_service.request.initial_pose.position.z = 20.0;
    spawn_service.request.initial_pose.orientation.w = 1.0;
    spawn_service.request.model_name = "part";
    spawn_service.request.model_xml = urdf_str;
    spawn_srv.call(spawn_service);

  } 


  // Wait for the simulation clone before showing the next message.
  gazebo::common::Time::MSleep(200);

  std::cout << "\nPress [ENTER] to exit and kill all the servers." << std::endl;
  getchar();

  // Make sure to shut everything down.
  std::string cmd = "kill -15 `ps -A | grep -m1 gzserver | awk '{print $1}'`";
  int ret = std::system(cmd.c_str());
  if (ret != 0)
    std::cerr << "kill gzserver returned a non zero value:" << ret << std::endl;

  gazebo::shutdown();

  //serverThread.join();

  ros::spin();
  return 0;
}

clone.launch:

<?xml version="1.0"?>
<launch>
  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="extra_gazebo_args" default=""/>
  <arg name="gui" default="true"/>
  <arg name="recording" default="false"/>
  <!-- Note that 'headless' is currently non-functional.  See gazebo_ros_pkgs issue #491 (-r arg does not disable
       rendering, but instead enables recording). The arg definition has been left here to prevent breaking downstream
       launch files, but it does nothing. -->
  <arg name="headless" default="false"/>
  <arg name="debug" default="true"/>
  <arg name="physics" default="ode"/>
  <arg name="verbose" default="false"/>
  <arg name="world_name" default="worlds/empty.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
  <arg name="respawn_gazebo" default="false"/>
  <arg name="use_clock_frequency" default="false"/>
  <arg name="pub_clock_frequency" default="100"/>
  <arg name="required" default="false"/>

  <!-- set use_sim_time flag -->
  <group if="$(arg use_sim_time)">
    <param name="/use_sim_time" value="true" />
  </group>

  <!-- set command arguments -->
  <arg unless="$(arg paused)" name="command_arg1" value=""/>
  <arg     if="$(arg paused)" name="command_arg1" value="-u"/>
  <arg unless="$(arg recording)" name="command_arg2" value=""/>
  <arg     if="$(arg recording)" name="command_arg2" value="-r"/>
  <arg unless="$(arg verbose)" name="command_arg3" value=""/>
  <arg     if="$(arg verbose)" name="command_arg3" value="--verbose"/>
  <arg unless="$(arg debug)" name="script_type" value="gzserver"/>
  <arg     if="$(arg debug)" name="script_type" value="debug"/>

  <!-- start gazebo server-->



   <group if="$(arg use_clock_frequency)">
    <param name="gazebo/pub_clock_frequency" value="$(arg pub_clock_frequency)" />
  </group>
  <node name="clone_node" pkg="ros_trial" type="cloner" output="screen" />
  <node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="$(arg respawn_gazebo)" output="screen"
    args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) -e $(arg physics) $(arg extra_gazebo_args) $(arg world_name)" />


  <!-- start gazebo client -->
  <group if="$(arg gui)">
    <node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen" required="$(arg required)"/>
  </group>



</launch>