From 188215b604c2dba0e12f3443557f766e66f3164b Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 17 Oct 2023 20:47:30 -0700 Subject: [PATCH] Allow mismatched plan frames since we coerce anyway Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 21 +++++++------------ 1 file changed, 7 insertions(+), 14 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 0f57deb..e412812 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -1054,16 +1054,6 @@ MotionPlanCache::put_cartesian_plan( node_->get_logger(), "Skipping insert: Frame IDs cannot be empty."); return false; } - if (plan_request.header.frame_id != plan.joint_trajectory.header.frame_id) - { - RCLCPP_ERROR( - node_->get_logger(), - "Skipping insert: " - "Plan request frame (%s) does not match plan frame (%s).", - plan_request.header.frame_id.c_str(), - plan.joint_trajectory.header.frame_id.c_str()); - return false; - } auto coll = db_->openCollection( "move_group_cartesian_plan_cache", move_group_namespace); @@ -1125,9 +1115,11 @@ MotionPlanCache::put_cartesian_plan( { Metadata::Ptr insert_metadata = coll.createMetadata(); - bool start_meta_ok = this->extract_and_append_cartesian_plan_start_to_metadata( + bool start_meta_ok = + this->extract_and_append_cartesian_plan_start_to_metadata( *insert_metadata, move_group, plan_request); - bool goal_meta_ok = this->extract_and_append_cartesian_plan_goal_to_metadata( + bool goal_meta_ok = + this->extract_and_append_cartesian_plan_goal_to_metadata( *insert_metadata, move_group, plan_request); insert_metadata->append("execution_time_s", execution_time_s); insert_metadata->append("planning_time_s", planning_time_s); @@ -1537,8 +1529,8 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_metadata( // Apply offsets // Position metadata.append(meta_name + ".position.x", x_offset + waypoint.position.x); - metadata.append(meta_name + ".position.y", y_offset+ waypoint.position.y); - metadata.append(meta_name + ".position.z", z_offset+waypoint.position.z); + metadata.append(meta_name + ".position.y", y_offset + waypoint.position.y); + metadata.append(meta_name + ".position.z", z_offset + waypoint.position.z); // Orientation tf2::Quaternion tf2_quat_goal_offset( @@ -1558,6 +1550,7 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_metadata( } metadata.append("link_name", plan_request.link_name); + metadata.append("header.frame_id", base_frame); return true; }