Implement cylinder ground bumps

MRPT · Oct 10, 2024 · 338edfe · 338edfe
1 parent 52edec8
commit 338edfe
Show file tree

Hide file tree

Showing 3 changed files with 47 additions and 3 deletions.
diff --git a/modules/simulator/src/Block.cpp b/modules/simulator/src/Block.cpp
@@ -440,6 +440,8 @@ void DummyInvisibleBlock::internalGuiUpdate(
 
 void Block::internal_parseGeometry(const rapidxml::xml_node<char>& xml_geom_node)
 {
+	using namespace mrpt::literals;	 // _deg
+
 	auto& _ = geomParams_;
 
 	parse_xmlnode_attribs(
@@ -498,6 +500,28 @@ void Block::internal_parseGeometry(const rapidxml::xml_node<char>& xml_geom_node
 		}
 		break;
 
+		case mvsim::GeometryType::SemiCylinderBump:
+		{
+			ASSERTMSG_(_.lx > 0, "Missing 'lx' attribute for semi_cylinder_bump geometry");
+			ASSERTMSG_(_.ly > 0, "Missing 'ly' attribute for semi_cylinder_bump geometry");
+			ASSERTMSG_(_.lz > 0, "Missing 'lz' attribute for semi_cylinder_bump geometry");
+
+			if (_.vertex_count == 0) _.vertex_count = 10;  // default
+
+			auto glCyl = mrpt::opengl::CCylinder::Create();
+			glCyl->setHeight(_.lx);
+			glCyl->setRadius(_.ly * 0.5);
+			glCyl->setScale(2 * _.lz / _.ly, 1.0, 1.0);
+
+			glCyl->setSlicesCount(_.vertex_count);
+			glCyl->setColor_u8(block_color_);
+
+			addCustomVisualization(
+				glCyl, mrpt::poses::CPose3D::FromXYZYawPitchRoll(
+						   -0.5 * _.lx, .0, .0, .0_deg, 90.0_deg, .0_deg));
+		}
+		break;
+
 		case mvsim::GeometryType::Ramp:
 		{
 			ASSERTMSG_(_.lx > 0, "Missing 'lx' attribute for ramp geometry");
@@ -595,5 +619,12 @@ std::optional<float> mvsim::Block::getElevationAt(const mrpt::math::TPoint2Df& w
 
 		case GeometryType::Ramp:
 			return myPose.z() + _.lz * localPt.x / _.lx;
+
+		case GeometryType::SemiCylinderBump:
+		{
+			const auto f =
+				std::sqrt(std::max<double>(.0, 1.0 - mrpt::square(2.0 * localPt.y / _.ly)));
+			return myPose.z() + _.lz * f;
+		}
 	};
 }
diff --git a/modules/simulator/src/VisualObject.cpp b/modules/simulator/src/VisualObject.cpp
@@ -83,6 +83,7 @@ void VisualObject::guiUpdate(
 			const auto& cs = collisionShape_.value();
 
 			const double height = cs.zMax() - cs.zMin();
+			ASSERT_(height == height);
 			ASSERT_(height > 0);
 
 			const auto c = cs.getContour();

diff --git a/mvsim_tutorial/demo_warehouse.world.xml b/mvsim_tutorial/demo_warehouse.world.xml
@@ -191,8 +191,18 @@
 		<intangible>true</intangible> <!-- Does not generate collisions -->
 		<color>#707070</color>
 		<shape_from_visual/> <!-- automatic collision shape from 3D model -->
-		<geometry type="ramp" lx="2.0" ly="1.0" lz="0.25" /> -->
+		<geometry type="ramp" lx="2.0" ly="1.0" lz="0.25" />
 	</block:class>
+
+	<block:class name="my_bump">
+		<static>true</static> <!-- Does not move -->
+		<intangible>true</intangible> <!-- Does not generate collisions -->
+		<color>#707070</color>
+		<zmin>0</zmin> <zmax>0.02</zmax>
+		<shape_from_visual/> <!-- automatic collision shape from 3D model -->
+		<geometry type="semi_cylinder_bump" lx="1.0" ly="0.20" lz="0.02" />
+	</block:class>
+
 
 	<!-- ======================================
 	      Object instances
@@ -218,8 +228,10 @@
 	<block class="pallet_jack"> <init_pose>-11 12 80</init_pose> </block>
 
 
-	<block class="my_ramp"> <init_pose>2 2 0</init_pose> </block>
-	<block class="my_ramp"> <init_pose>6 2 180</init_pose> </block>
+<!--	<block class="my_ramp"> <init_pose>2 2 0</init_pose> </block>
+	<block class="my_ramp"> <init_pose>6 2 180</init_pose> </block> -->
+
+<!--	<block class="my_bump"> <init_pose>4 0 90</init_pose> </block> -->
 
 	<block class="pallet_jack"> 
 		<init_pose>0 0 0</init_pose>