From f0e3eef22a4c0c0e67fc464ef3e19308a03acdd1 Mon Sep 17 00:00:00 2001
From: Derek Witcpalek <dwitcpa@umich.edu>
Date: Sat, 18 Apr 2020 00:37:32 -0400
Subject: [PATCH] Allows models to be inserted at a specific location

---
 .../src/model_insertion_plugin.cpp            | 27 ++++++++++++++++++-
 1 file changed, 26 insertions(+), 1 deletion(-)

diff --git a/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp b/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp
index 2a9dfd5..844fa26 100644
--- a/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp
+++ b/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp
@@ -38,7 +38,32 @@ public:
     {
         std::cout << "Received message to load model: " << msg.data << std::endl;
 
-        parent->InsertModelFile(std::string("model://") + msg.data);
+        // Create a new transport node
+        transport::NodePtr node(new transport::Node());
+
+        // Initialize the node with the world name
+        node->Init(parent->Name());
+
+        // Create a publisher on the ~/factory topic
+        transport::PublisherPtr factoryPub =
+            node->Advertise<msgs::Factory>("~/factory");
+
+        // Create the message
+        msgs::Factory to_pub;
+
+        // Model file to load
+        to_pub.set_sdf_filename(std::string("model://") + msg.data);
+
+        // Pose to initialize the model to
+        msgs::Set(to_pub.mutable_pose(),
+                  ignition::math::Pose3d(
+                      ignition::math::Vector3d(2, 0, 0),
+                      ignition::math::Quaterniond(0, 0, 0)));
+
+        // Send the message
+        factoryPub->Publish(to_pub);
+
+        // parent->InsertModelFile(std::string("model://") + msg.data);
     }
 
 private: