Add support for GenerateRandomPose

Author: JafarAbdi

core/include/moveit/task_constructor/stages.h

@@ -47,6 +47,7 @@
 #include "stages/generate_grasp_pose.h"
 #include "stages/generate_place_pose.h"
 #include "stages/generate_pose.h"
+#include "stages/generate_random_pose.h"
 #include "stages/modify_planning_scene.h"
 #include "stages/move_relative.h"
 #include "stages/move_to.h"

core/python/bindings/src/stages.cpp

@@ -57,6 +57,7 @@ PYBIND11_SMART_HOLDER_TYPE_CASTERS(Connect)
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(FixCollisionObjects)
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(GenerateGraspPose)
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePlacePose)
+PYBIND11_SMART_HOLDER_TYPE_CASTERS(GenerateRandomPose)
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePose)
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(Pick)
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(Place)
@@ -405,6 +406,26 @@ void export_stages(pybind11::module& m) {
 		)")
 	    .def(py::init<const std::string&>(), "name"_a);
 
+	py::enum_<GenerateRandomPose::PoseDimension>(m, "PoseDimension",
+		R"(Define the dimensions of a pose that can be randomized.)")
+	   .value("X", GenerateRandomPose::PoseDimension::X, "X dimension")
+	   .value("Y", GenerateRandomPose::PoseDimension::Y, "Y dimension")
+	   .value("Z", GenerateRandomPose::PoseDimension::Z, "Z dimension")
+	   .value("ROLL", GenerateRandomPose::PoseDimension::ROLL, "Roll dimension")
+	   .value("PITCH", GenerateRandomPose::PoseDimension::PITCH, "Pitch dimension")
+	   .value("YAW", GenerateRandomPose::PoseDimension::YAW, "Yaw dimension");
+
+	properties::class_<GenerateRandomPose, GeneratePose>(m, "GenerateRandomPose", R"(
+			Monitoring generator stage which can be used to generate random poses, based on solutions provided
+			by the monitored stage and the specified pose dimension samplers.
+		)")
+	    .def(py::init<const std::string&>(), "name"_a)
+	    .def("set_max_solutions", &GenerateRandomPose::setMaxSolutions, "max_solutions"_a)
+	    .def("sample_dimension", [](GenerateRandomPose& self, const GenerateRandomPose::PoseDimension pose_dimension,
+	                              const double width) {
+	     self.sampleDimension<std::uniform_real_distribution>(pose_dimension, width);
+	   }, "pose_dimension"_a, "width"_a);
+
 	properties::class_<Pick, SerialContainer>(m, "Pick", R"(
 			The Pick stage is a specialization of the PickPlaceBase class, which
 			wraps the pipeline to pick or place an object with a given end effector.