avoid deprecation warnings of MoveIt master branch

Use new methods - moveit::core::isEmpty - moveit::core::CartesianInterpolator::computeCartesianPath
moveit · Dec 13, 2019 · 233d63a · 233d63a
1 parent e4920ce
commit 233d63a
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 1 deletion.
diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp
@@ -41,6 +41,9 @@
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit/move_group/capability_names.h>
 #include <moveit/robot_state/conversions.h>
+#if MOVEIT_MASTER
+#include <moveit/utils/message_checks.h>
+#endif
 
 namespace {
 
@@ -167,14 +170,22 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 
 		/* TODO add action feedback and markers */
 		exec_traj.effect_on_success_ = [this, sub_traj, description](const plan_execution::ExecutableMotionPlan*) {
+#if MOVEIT_MASTER
+			if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
+#else
 			if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)) {
+#endif
 				ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
 				return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
 			}
 			return true;
 		};
 
+#if MOVEIT_MASTER
+		if (!moveit::core::isEmpty(sub_traj.scene_diff.robot_state) &&
+#else
 		if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&
+#endif
 		    !moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)) {
 			ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution",
 			                       "invalid intermediate robot state in scene diff of SubTrajectory " << description);

diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h
@@ -46,7 +46,7 @@ namespace solvers {
 
 MOVEIT_CLASS_FORWARD(CartesianPath)
 
-/** Use RobotState's computeCartesianPath() to generate a straigh-line path between to scenes */
+/** Use MoveIt's computeCartesianPath() to generate a straigh-line path between to scenes */
 class CartesianPath : public PlannerInterface
 {
 public:

diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp
@@ -39,6 +39,9 @@
 #include <moveit/task_constructor/solvers/cartesian_path.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
+#if MOVEIT_MASTER
+#include <moveit/robot_state/cartesian_interpolator.h>
+#endif
 
 namespace moveit {
 namespace task_constructor {
@@ -86,9 +89,16 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
 	};
 
 	std::vector<moveit::core::RobotStatePtr> trajectory;
+#if MOVEIT_MASTER
+	double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
+	    &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
+	    moveit::core::MaxEEFStep(props.get<double>("step_size")),
+	    moveit::core::JumpThreshold(props.get<double>("jump_threshold")), isValid);
+#else
 	double achieved_fraction = sandbox_scene->getCurrentStateNonConst().computeCartesianPath(
 	    jmg, trajectory, &link, target, true, props.get<double>("step_size"), props.get<double>("jump_threshold"),
 	    isValid);
+#endif
 
 	if (!trajectory.empty()) {
 		result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg));