diff --git a/include/GazeboZoneSpawner.h b/include/GazeboZoneSpawner.h index 7e8645d90812056e8a57020d4123bc45d02f6de5..a108d50cbd3bb13a39c0f08dafcea77d00fcb32f 100644 --- a/include/GazeboZoneSpawner.h +++ b/include/GazeboZoneSpawner.h @@ -59,6 +59,8 @@ public: */ static void spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose); + static void spawnPrimitive(const std::string &name, shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose, double mass = 1.0, bool is_static = false); + }; diff --git a/models/hardbox.sdf b/models/hardbox.sdf new file mode 100644 index 0000000000000000000000000000000000000000..51dbb750b9874bd705e704bdf5466f4bab5b22af --- /dev/null +++ b/models/hardbox.sdf @@ -0,0 +1,41 @@ +<?xml version="1.0" ?> +<sdf version="1.5"> + <model name="box"> + <static>false</static> + <link name="link"> + <pose>2 3 0.5 0 0 0</pose> + <inertial> + <mass>1.0</mass> + <inertia> <!-- inertias are tricky to compute --> + <!-- http://gazebosim.org/tutorials?tut=inertia&cat=build_robot --> + <ixx>0.083</ixx> <!-- for a box: ixx = 0.083 * mass * (y*y + z*z) --> + <ixy>0.0</ixy> <!-- for a box: ixy = 0 --> + <ixz>0.0</ixz> <!-- for a box: ixz = 0 --> + <iyy>0.083</iyy> <!-- for a box: iyy = 0.083 * mass * (x*x + z*z) --> + <iyz>0.0</iyz> <!-- for a box: iyz = 0 --> + <izz>0.083</izz> <!-- for a box: izz = 0.083 * mass * (x*x + y*y) --> + </inertia> + </inertial> + <visual name="visual"> + <material> + <script> + <uri>box.material</uri> + <name>Zone/DarkMagentaTransparent</name> + </script> + </material> + <geometry> + <box> + <size>1 1 1</size> + </box> + </geometry> + </visual> + <collision name="collision"> + <geometry> + <box> + <size>1 1 1</size> + </box> + </geometry> + </collision> + </link> + </model> +</sdf> \ No newline at end of file diff --git a/src/GazeboZoneSpawner.cpp b/src/GazeboZoneSpawner.cpp index 7f84c3f941ba019334f9f7bcfaffe661ced5cc3e..265a5d8c7fc74c59363e3f0b014a0ecb39e24569 100644 --- a/src/GazeboZoneSpawner.cpp +++ b/src/GazeboZoneSpawner.cpp @@ -69,3 +69,77 @@ void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geom ROS_ERROR("Failed to call service '/gazebo/spawn_sdf_model'"); } } + +void GazeboZoneSpawner::spawnPrimitive(const std::string &name, shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose, double mass, bool is_static) { + if (shape.type != shape.BOX) { + ROS_ERROR_STREAM("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/hardbox.sdf"; + auto materialFileName = "file://" + ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.material"; + + assert(sdf::readFile(sdfFileName, sdf)); + + auto modelElement = sdf->Root()->GetElement("model"); + auto linkElement = modelElement->GetElement("link"); + auto poseElement = linkElement->GetElement("pose"); + + auto visualElement = linkElement->GetElement("visual"); + auto visualMaterialUriElement = visualElement->GetElement("material")->GetElement("script")->GetElement("uri"); + auto visualSizeElement = visualElement->GetElement("geometry")->GetElement("box")->GetElement("size"); + + auto collisionElement = linkElement->GetElement("collision"); + auto collisionSizeElement = collisionElement->GetElement("geometry")->GetElement("box")->GetElement("size"); + + tf::Quaternion rot; + rot.setValue(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + double roll, pitch, yaw; + tf::Matrix3x3(rot).getRPY(roll, pitch, yaw); + + std::ostringstream poseStream; + poseStream << pose.position.x << " " << pose.position.y << " " << pose.position.z << " " + << roll << " " << pitch << " " << yaw; + poseElement->Set(poseStream.str()); + + // set the element to static + modelElement->GetElement("static")->Set(is_static); + + double sx = shape.dimensions[0]; + double sy = shape.dimensions[1]; + double sz = shape.dimensions[2]; + + // set the size both for visual and collision objects + std::ostringstream sizeStream; + sizeStream << sx << " " << sy << " " << sz; + visualSizeElement->Set(sizeStream.str()); + collisionSizeElement->Set(sizeStream.str()); + + // set inertia matrix + auto inertialElement = sdf->Root()->GetElement("model")->GetElement("link")->GetElement("inertial"); + inertialElement->GetElement("mass")->Set(mass); + inertialElement->GetElement("inertia")->GetElement("ixx")->Set(0.083 * mass * sy*sy + sz*sz); + inertialElement->GetElement("inertia")->GetElement("iyy")->Set(0.083 * mass * sx*sx + sz*sz); + inertialElement->GetElement("inertia")->GetElement("izz")->Set(0.083 * mass * sx*sx + sy*sy); + + ros::NodeHandle n; + ros::ServiceClient client = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_sdf_model"); + gazebo_msgs::SpawnModel srv; + srv.request.robot_namespace = "box space"; + // srv.request.initial_pose = pose; // not required here + srv.request.model_name = name; + srv.request.model_xml = sdf->ToString(); + // srv.request.reference_frame = ; // if left empty, world is used + if (client.call(srv)) { + if (srv.response.success) { + ROS_INFO_STREAM("Spawned box '" << srv.request.model_name << "'. " << srv.response.status_message); + } else { + ROS_ERROR_STREAM("Failed to spawn box '" << srv.request.model_name << "'. " << srv.response.status_message); + } + } else { + ROS_ERROR("Failed to call service '/gazebo/spawn_sdf_model'"); + } +} \ No newline at end of file