Allows models to be placed dynamically at any location and orientation

rrrobot_ws/src/simulation_env/CMakeLists.txt

@@ -10,6 +10,7 @@ add_compile_options(-std=c++11)
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
+  geometry_msgs
   gazebo_ros
   message_generation
 )
@@ -53,6 +54,7 @@ add_message_files(
   FILES
   arm_angles.msg
   arm_command.msg
+  model_insertion.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -73,6 +75,7 @@ add_message_files(
 generate_messages(
   DEPENDENCIES
   std_msgs
+  geometry_msgs
 )
 
 ################################################
@@ -107,7 +110,7 @@ generate_messages(
 catkin_package(
   INCLUDE_DIRS include
 #  LIBRARIES simulation_env
-  CATKIN_DEPENDS roscpp std_msgs gazebo_ros
+  CATKIN_DEPENDS roscpp std_msgs geometry_msgs gazebo_ros
 #  DEPENDS system_lib
 )
 
@@ -122,6 +125,7 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
   ${roscpp_INCLUDE_DIRS}
   ${std_msgs_INCLUDE_DIRS}
+  ${geometry_msgs_INCLUDE_DIRS}
   ${GAZEBO_INCLUDE_DIRS}
 )
 
@@ -149,6 +153,7 @@ add_dependencies(insert_model ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPOR
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(test_move_arm test/move_arm.cpp)
+add_executable(test_insert_object test/test_insert_object.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -159,6 +164,7 @@ add_executable(test_move_arm test/move_arm.cpp)
 ## Add cmake target dependencies of the executable
 ## same as for the library above
 add_dependencies(test_move_arm arm_angles arm_motors)
+add_dependencies(test_insert_object insert_model)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(arm_angles
@@ -180,6 +186,10 @@ target_link_libraries(test_move_arm
   ${catkin_LIBRARIES}
 )
 
+target_link_libraries(test_insert_object
+  ${catkin_LIBRARIES}
+)
+
 #############
 ## Install ##
 #############

rrrobot_ws/src/simulation_env/msg/model_insertion.msg

@@ -0,0 +1,2 @@
+string model_name
+geometry_msgs/Pose pose

rrrobot_ws/src/simulation_env/package.xml

@@ -51,13 +51,16 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_depend>gazebo_ros</build_depend>
   <build_depend>message_generation</build_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
   <build_export_depend>gazebo_ros</build_export_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>gazebo_ros</exec_depend>
   <exec_depend>message_runtime</exec_depend>
 

rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp

@@ -5,6 +5,7 @@
 
 #include "ros/ros.h"
 #include <std_msgs/String.h>
+#include "simulation_env/model_insertion.h"
 
 #include <string>
 #include <memory>
@@ -34,9 +35,9 @@ public:
         subscriber = nh->subscribe("object_to_load", 1000, &ModelInsertion::insertModel, this);
     }
 
-    void insertModel(const std_msgs::String &msg)
+    void insertModel(const simulation_env::model_insertion &msg)
     {
-        std::cout << "Received message to load model: " << msg.data << std::endl;
+        std::cout << "Received message to load model: " << msg.model_name << std::endl;
 
         // Create a new transport node
         transport::NodePtr node(new transport::Node());
@@ -52,13 +53,13 @@ public:
         msgs::Factory to_pub;
 
         // Model file to load
-        to_pub.set_sdf_filename(std::string("model://") + msg.data);
+        to_pub.set_sdf_filename(std::string("model://") + msg.model_name);
 
         // Pose to initialize the model to
         msgs::Set(to_pub.mutable_pose(),
                   ignition::math::Pose3d(
-                      ignition::math::Vector3d(2, 0, 0),
+                      ignition::math::Vector3d(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z),
-                      ignition::math::Quaterniond(0, 0, 0)));
+                      ignition::math::Quaterniond(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)));
 
         // Send the message
         factoryPub->Publish(to_pub);

rrrobot_ws/src/simulation_env/test/test_insert_object.cpp

@@ -0,0 +1,34 @@
+#include "simulation_env/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, "inert_models");
+
+    ros::NodeHandle nh;
+
+    ros::Publisher pub = nh.advertise<simulation_env::model_insertion>("/object_to_load", 1000);
+
+    while (ros::ok())
+    {
+        simulation_env::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();
+    }
+}