Skip to content

Commit a5c6b19

Browse files
rhaschkesjahr
authored andcommitted
Connect: Relax validity check of reached end state
Looks like ROS2 uses a more relaxed goal constraint threshold than ROS1. For this reason, all end states reached by Connect solutions of pick+place demo are rejected. This commit relaxes the max_distance threshold of Connect as well.
1 parent e1b891b commit a5c6b19

File tree

1 file changed

+1
-1
lines changed

1 file changed

+1
-1
lines changed

core/src/stages/connect.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) :
5757

5858
auto& p = properties();
5959
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
60-
p.declare<double>("max_distance", 1e-4, "maximally accepted distance between end and goal sate");
60+
p.declare<double>("max_distance", 1e-2, "maximally accepted distance between end and goal sate");
6161
p.declare<moveit_msgs::msg::Constraints>("path_constraints", moveit_msgs::msg::Constraints(),
6262
"constraints to maintain during trajectory");
6363
properties().declare<TimeParameterizationPtr>("merge_time_parameterization",

0 commit comments

Comments
 (0)