Merge branch 'master' into ros2

Author: rhaschke

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.

core/src/solvers/cartesian_path.cpp

@@ -89,7 +89,7 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
 	Eigen::Isometry3d ik_pose_world;
 
 	if (!utils::getRobotTipForFrame(props.property("ik_frame"), *from, jmg, error_msg, link, ik_pose_world))
-		return { false, error_msg };
+		return { false, "CartesianPath: " + error_msg };
 
 	Eigen::Isometry3d offset = from->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
 
@@ -136,7 +136,7 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
 		                          props.get<double>("max_acceleration_scaling_factor"));
 
 	if (achieved_fraction < props.get<double>("min_fraction")) {
-		return { false, "min_fraction not met. Achieved: " + std::to_string(achieved_fraction) };
+		return { false, "CartesianPath: min_fraction not met. Achieved: " + std::to_string(achieved_fraction) };
 	}
 	return { true, "achieved fraction: " + std::to_string(achieved_fraction) };
 }