From d39d7d93f8b1dd9d6862a50ce87914b6677941bb Mon Sep 17 00:00:00 2001 From: Abishalini Date: Fri, 7 Jul 2023 08:54:52 -0600 Subject: [PATCH 1/7] Mark Incomplete trajectories as failure --- .../moveit/task_constructor/stages/connect.h | 2 ++ core/src/stages/connect.cpp | 16 +++++++++++----- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index c8cf883d5..d4f3aca9c 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -97,6 +97,8 @@ class Connect : public Connecting moveit::core::JointModelGroupPtr merged_jmg_; std::list subsolutions_; std::list states_; + + std::string name_; }; } // namespace stages } // namespace task_constructor diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 0a51b84e3..b455284d7 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -61,6 +61,8 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : "constraints to maintain during trajectory"); properties().declare("merge_time_parameterization", std::make_shared()); + + name_ = name; } void Connect::reset() { @@ -149,6 +151,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { bool success = false; std::vector positions; + for (const GroupPlannerVector::value_type& pair : planner_) { // set intermediate goal state planning_scene::PlanningScenePtr end = start->diff(); @@ -161,13 +164,16 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { robot_trajectory::RobotTrajectoryPtr trajectory; success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); + sub_trajectories.push_back(trajectory); - // Do not push partial solutions + // Check if the end goal and the last waypoint are close. The trajectory should be marked as failure otherwise. if (success) { - sub_trajectories.push_back(trajectory); - } else { - // Pushing a nullptr instead of a failed trajectory. - sub_trajectories.push_back(nullptr); + const auto distance = end->getCurrentState().distance(trajectory->getLastWayPoint()); + if (distance > 0.05) { + RCLCPP_INFO_STREAM(LOGGER, "Stage Name : " << name_); + RCLCPP_INFO_STREAM(LOGGER, "The trajectory given by the plan function does not satisfy the goal state. Marking it as a failure"); + success = false; + } } if (!success) From 2adb19b8176e65e02935745fb4b59eb41db593af Mon Sep 17 00:00:00 2001 From: Abishalini Date: Fri, 7 Jul 2023 09:42:56 -0600 Subject: [PATCH 2/7] Test CI passes --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index e336a093b..e4b456f51 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -43,7 +43,7 @@ jobs: # TODO: Port to ROS2 # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master" # Silent googletest warnings: https://github.com/ament/ament_cmake/pull/408#issuecomment-1306004975 - UPSTREAM_WORKSPACE: github:ament/googletest#clalancette/update-to-gtest-1.11 + # UPSTREAM_WORKSPACE: github:ament/googletest#clalancette/update-to-gtest-1.11 AFTER_SETUP_UPSTREAM_WORKSPACE: dpkg --purge --force-all ros-rolling-gtest-vendor ros-rolling-gmock-vendor TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release CCACHE_DIR: ${{ github.workspace }}/.ccache From 86f5c30bec82217f401dbeeb3fe17a4080c8366c Mon Sep 17 00:00:00 2001 From: Abishalini Date: Fri, 7 Jul 2023 09:44:55 -0600 Subject: [PATCH 3/7] Fix formatting --- core/src/stages/connect.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index b455284d7..892a0056f 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -171,7 +171,8 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { const auto distance = end->getCurrentState().distance(trajectory->getLastWayPoint()); if (distance > 0.05) { RCLCPP_INFO_STREAM(LOGGER, "Stage Name : " << name_); - RCLCPP_INFO_STREAM(LOGGER, "The trajectory given by the plan function does not satisfy the goal state. Marking it as a failure"); + RCLCPP_INFO_STREAM(LOGGER, "The trajectory given by the plan function does not satisfy the goal state. " + + "Marking it as a failure"); success = false; } } From 80e2d6cc44a1294bec8724625cf932cab45327eb Mon Sep 17 00:00:00 2001 From: Abishalini Date: Fri, 7 Jul 2023 09:57:24 -0600 Subject: [PATCH 4/7] Fix build --- core/src/stages/connect.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 892a0056f..20cb6e356 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -171,8 +171,8 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { const auto distance = end->getCurrentState().distance(trajectory->getLastWayPoint()); if (distance > 0.05) { RCLCPP_INFO_STREAM(LOGGER, "Stage Name : " << name_); - RCLCPP_INFO_STREAM(LOGGER, "The trajectory given by the plan function does not satisfy the goal state. " + - "Marking it as a failure"); + RCLCPP_INFO_STREAM(LOGGER, "The trajectory given by the plan function does not satisfy the goal state. " + << "Marking it as a failure"); success = false; } } From 17cb0518c91ac3f2f4ebccc1fcc92196a793cfbf Mon Sep 17 00:00:00 2001 From: Abishalini Date: Fri, 7 Jul 2023 10:46:13 -0600 Subject: [PATCH 5/7] Fix CI upstream workspace --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index e4b456f51..ff5a75e41 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -43,7 +43,7 @@ jobs: # TODO: Port to ROS2 # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master" # Silent googletest warnings: https://github.com/ament/ament_cmake/pull/408#issuecomment-1306004975 - # UPSTREAM_WORKSPACE: github:ament/googletest#clalancette/update-to-gtest-1.11 + UPSTREAM_WORKSPACE: github:ament/googletest/rolling AFTER_SETUP_UPSTREAM_WORKSPACE: dpkg --purge --force-all ros-rolling-gtest-vendor ros-rolling-gmock-vendor TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release CCACHE_DIR: ${{ github.workspace }}/.ccache From b573b3980074f2668cf8434b4ebf83185c8d6706 Mon Sep 17 00:00:00 2001 From: Abishalini Date: Fri, 7 Jul 2023 11:06:26 -0600 Subject: [PATCH 6/7] Add goal tolerance as a property --- .github/workflows/ci.yaml | 2 +- core/include/moveit/task_constructor/stages/connect.h | 4 ++++ core/src/stages/connect.cpp | 5 ++++- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index ff5a75e41..1ffadfb3d 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -43,7 +43,7 @@ jobs: # TODO: Port to ROS2 # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master" # Silent googletest warnings: https://github.com/ament/ament_cmake/pull/408#issuecomment-1306004975 - UPSTREAM_WORKSPACE: github:ament/googletest/rolling + UPSTREAM_WORKSPACE: github:ament/googletest#rolling AFTER_SETUP_UPSTREAM_WORKSPACE: dpkg --purge --force-all ros-rolling-gtest-vendor ros-rolling-gmock-vendor TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release CCACHE_DIR: ${{ github.workspace }}/.ccache diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index d4f3aca9c..5a3868e8d 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -80,6 +80,10 @@ class Connect : public Connecting setProperty("path_constraints", std::move(path_constraints)); } + // To make sure the L1 distance between the goal state and the last waypoint in the planned trajectory + // are close enough. + void setGoalTolerance(float goal_tolerance) { setProperty("goal_tolerance", std::move(goal_tolerance)); } + void reset() override; void init(const moveit::core::RobotModelConstPtr& robot_model) override; void compute(const InterfaceState& from, const InterfaceState& to) override; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 20cb6e356..1ecd5f681 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -59,6 +59,8 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : p.declare("merge_mode", WAYPOINTS, "merge mode"); p.declare("path_constraints", moveit_msgs::msg::Constraints(), "constraints to maintain during trajectory"); + p.declare("goal_tolerance", 0.02, + "L1 distance between goal state and end of trajectory state should be within this tolerance"); properties().declare("merge_time_parameterization", std::make_shared()); @@ -169,7 +171,8 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { // Check if the end goal and the last waypoint are close. The trajectory should be marked as failure otherwise. if (success) { const auto distance = end->getCurrentState().distance(trajectory->getLastWayPoint()); - if (distance > 0.05) { + const auto& goal_tolerance = props.get("goal_tolerance"); + if (distance > goal_tolerance) { RCLCPP_INFO_STREAM(LOGGER, "Stage Name : " << name_); RCLCPP_INFO_STREAM(LOGGER, "The trajectory given by the plan function does not satisfy the goal state. " << "Marking it as a failure"); From 2c9afca815b6e71fb3260fa9cc9b13b64131d865 Mon Sep 17 00:00:00 2001 From: Abishalini Date: Fri, 7 Jul 2023 11:41:07 -0600 Subject: [PATCH 7/7] Remove L1 --- core/src/stages/connect.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 1ecd5f681..0628dc514 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -60,7 +60,7 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : p.declare("path_constraints", moveit_msgs::msg::Constraints(), "constraints to maintain during trajectory"); p.declare("goal_tolerance", 0.02, - "L1 distance between goal state and end of trajectory state should be within this tolerance"); + "Distance between goal state and end of trajectory state should be within this tolerance"); properties().declare("merge_time_parameterization", std::make_shared());