From 751f6ce0105324d9628d8944b75327f28bba68eb Mon Sep 17 00:00:00 2001
From: Derek Witcpalek <dwitcpa@umich.edu>
Date: Sun, 19 Apr 2020 13:06:04 -0400
Subject: [PATCH] Added insert object test script

---
 .../src/rrrobot/test/test_insert_object.cpp   | 34 +++++++++++++++++++
 1 file changed, 34 insertions(+)
 create mode 100644 rrrobot_ws/src/rrrobot/test/test_insert_object.cpp

diff --git a/rrrobot_ws/src/rrrobot/test/test_insert_object.cpp b/rrrobot_ws/src/rrrobot/test/test_insert_object.cpp
new file mode 100644
index 0000000..5ae50e2
--- /dev/null
+++ b/rrrobot_ws/src/rrrobot/test/test_insert_object.cpp
@@ -0,0 +1,34 @@
+#include "rrrobot/model_insertion.h"
+#include "ros/ros.h"
+
+#include <iostream>
+
+using std::cin;
+using std::cout;
+using std::endl;
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "insert_models");
+
+    ros::NodeHandle nh;
+
+    ros::Publisher pub = nh.advertise<rrrobot::model_insertion>("/object_to_load", 1000);
+
+    while (ros::ok())
+    {
+        rrrobot::model_insertion msg;
+        cout << "Enter the model name: ";
+        cin >> msg.model_name;
+        cout << "Enter the pose: " << endl;
+        cout << "x: ";
+        cin >> msg.pose.position.x;
+        cout << "y: ";
+        cin >> msg.pose.position.y;
+        cout << "z: ";
+        cin >> msg.pose.position.z;
+
+        pub.publish(msg);
+        ros::spinOnce();
+    }
+}
\ No newline at end of file