show boxes in gazebo

CMakeLists.txt

@@ -32,6 +32,8 @@ find_package(Boost REQUIRED COMPONENTS filesystem)
 
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(JSONCPP jsoncpp)
+pkg_check_modules(SDF sdformat REQUIRED)
+
 message(${JSONCPP_LIBRARIES})
 
 # ################################################################################################################################
@@ -54,14 +56,14 @@ catkin_package(CATKIN_DEPENDS
 # ################################################################################################################################
 
 # Specify additional locations of header files Your package locations should be listed before other locations
-include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${catkin_INCLUDE_DIRS} ${SDF_INCLUDE_DIRS})
 
 add_executable(robot_control_node src/robot_control_node.cpp)
 add_executable(box_publisher_node src/box_publisher.cpp)
 add_executable(robot_state_initializer_node src/robot_state_initializer.cpp)
 add_executable(move_group_interface_tutorial src/move_group_interface_tutorial.cpp)
 
-target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
+target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES} ${SDF_LIBRARIES})
 target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
 target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})
 target_link_libraries(move_group_interface_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})

models/box.sdf

+<?xml version="1.0" ?>
+<sdf version="1.5">
+    <model name="box">
+        <static>true</static>
+        <link name="link">
+            <visual name="visual">
+                <material>
+                    <script>
+                        <uri>/usr/share/gazebo-9/media/materials/scripts/gazebo.material</uri>
+                        <name>Gazebo/DarkMagentaTransparent</name>
+                    </script>
+                </material>
+                <pose>2 3 0.5 0 0 0</pose>
+                <geometry>
+                    <box>
+                        <size>1 1 1</size>
+                    </box>
+                </geometry>
+            </visual>
+        </link>
+    </model>
+</sdf>
\ No newline at end of file

package.xml

@@ -20,7 +20,8 @@
 
   <depend>controller_manager</depend>
   <depend>effort_controllers</depend>
-  <depend>gazebo_ros</depend>
+<!--  <depend>gazebo_ros</depend>-->
+  <depend>gazebo_plugins</depend>
   <depend>gazebo_ros_control</depend>
   <depend>joint_state_controller</depend>
   <depend>joint_state_publisher</depend>
@@ -41,10 +42,17 @@
   <depend>controller_interface</depend>
   <depend>hardware_interface</depend>
 
+
+  <build_depend>sdformat</build_depend>
+  <exec_depend>sdformat</exec_depend>
+
+  <exec_depend>gazebo_ros</exec_depend>
+
   <exec_depend>pluginlib</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <controller_interface plugin="${prefix}/controller_plugins.xml" />
+    <gazebo_ros gazebo_media_path="/usr/share/gazebo-9/media/materials/scripts"/>
   </export>
 </package>

src/robot_control_node.cpp

@@ -6,9 +6,63 @@
 #include <ros/ros.h>
 #include <boost/filesystem.hpp>
 
+#include <sdformat-6.0/sdf/sdf.hh>  // for sdf model parsing
+#include <gazebo_msgs/SpawnModel.h> // for spawning stuff in gazebo
+
 static const std::string PLANNING_GROUP_ARM = "panda_arm";
 static const std::string APP_DIRECTORY_NAME = ".panda_simulation";
 
+
+void spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
+  if (shape.type != shape.BOX) {
+    ROS_ERROR("Safety-zone could not be created due to wrong shape-type");
+    return;
+  }
+
+  sdf::SDFPtr sdf(new sdf::SDF());
+  sdf::init(sdf);
+
+  auto sdfFileName = ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.sdf";
+
+  assert(sdf::readFile(sdfFileName, sdf));
+
+  ROS_INFO_NAMED("model-loader", "successfully read SDF file: %s", sdf->ToString().c_str());
+
+  auto visualElement = sdf->Root()->GetElement("model")->GetElement("link")->GetElement("visual");
+  auto poseElement = visualElement->GetElement("pose");
+  auto sizeElement = visualElement->GetElement("geometry")->GetElement("box")->GetElement("size");
+
+  std::ostringstream poseStream;
+  poseStream << pose.position.x << " " << pose.position.y << " " << pose.position.z << " 0 0 0";
+  poseElement->Set(poseStream.str());
+
+  ROS_INFO_NAMED("model-loader", "successfully changed SDF file, pose is: %s", poseElement->GetValue()->GetAsString().c_str());
+
+  std::ostringstream sizeStream;
+  sizeStream << shape.dimensions[0] << " " << shape.dimensions[1] << " " << shape.dimensions[2];
+  sizeElement->Set(sizeStream.str());
+
+  ROS_INFO_NAMED("model-loader", "successfully changed SDF file, size is: '%s', should be '%s'", sizeElement->GetValue()->GetAsString().c_str(), sizeStream.str().c_str());
+
+  ros::NodeHandle n;
+  ros::ServiceClient client = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_sdf_model");
+  gazebo_msgs::SpawnModel spawnModelService;
+  spawnModelService.request.robot_namespace = "box space";
+  // spawnModelService.request.initial_pose = pose; // not required here
+  spawnModelService.request.model_name = sdf->Root()->GetElement("model")->GetAttribute("name")->GetAsString();
+  spawnModelService.request.model_xml = sdf->ToString().c_str();
+  // spawnModelService.request.reference_frame = ; // if left empty, world is used
+  if (client.call(spawnModelService))
+  {
+    ROS_INFO("result: %s", spawnModelService.response.success ? "success!" : "failed!");
+    ROS_INFO("result: %s", spawnModelService.response.status_message.c_str());
+  }
+  else
+  {
+    ROS_ERROR("Failed to call service '/gazebo/spawn_sdf_model'");
+  }
+}
+
 moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::string name)
 {
   moveit_msgs::CollisionObject collision_object;
@@ -47,6 +101,8 @@ moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::str
   collision_object.primitive_poses.push_back(box_pose);
   collision_object.operation = collision_object.ADD;
 
+  spawnCollisionBox(primitive, box_pose);
+
   return std::move(collision_object);
 }