Set planning frame correctly in evaluation of reachable and valid pos…

…e filter (moveit#476) The current behavior is that the evaluation sets the frame id of the goal_pose_ to the planning frame of the planning_scene_ when they are the same. This also applys for the pose. This should be done when the frames are different.
KaihongYang · Mar 26, 2017 · 7d98c34 · 7d98c34
1 parent 312b2cd
commit 7d98c34
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp
@@ -114,7 +114,7 @@ bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr
     // pose
     // can be that of objects in the collision world but most components are unaware of those transforms,
     // so we convert to a frame that is certainly known
-    if (robot_state::Transforms::sameFrame(planning_scene_->getPlanningFrame(), plan->goal_pose_.header.frame_id))
+    if (!robot_state::Transforms::sameFrame(planning_scene_->getPlanningFrame(), plan->goal_pose_.header.frame_id))
     {
       tf::poseEigenToMsg(plan->transformed_goal_pose_, plan->goal_pose_.pose);
       plan->goal_pose_.header.frame_id = planning_scene_->getPlanningFrame();