Merge pull request #363 from ros-planning/fix-dirty-transforms

fixed dirty transform bugs
moveit · Nov 5, 2013 · cd1e916 · cd1e916
2 parents 6692777 + 0562376
commit cd1e916
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 0 deletions.
diff --git a/benchmarks_gui/src/tab_states_and_goals.cpp b/benchmarks_gui/src/tab_states_and_goals.cpp
@@ -796,6 +796,7 @@ void MainWindow::copySelectedGoalPoses(void)
     std::stringstream ss;
     ss << scene_name.c_str() << "_pose_" << std::setfill('0') << std::setw(4) << goal_poses_.size();
 
+    scene_display_->getPlanningSceneRW()->getCurrentStateNonConst().update();
     Eigen::Affine3d tip_pose = scene_display_->getPlanningSceneRO()->getCurrentState().getGlobalLinkTransform(robot_interaction_->getActiveEndEffectors()[0].parent_link);
     geometry_msgs::Pose marker_pose;
     marker_pose.position.x = goal_poses_[name]->imarker->getPosition().x;

diff --git a/benchmarks_gui/src/tab_trajectories.cpp b/benchmarks_gui/src/tab_trajectories.cpp
@@ -82,6 +82,7 @@ void MainWindow::createTrajectoryButtonClicked(void)
       else
       {
         //Create the new trajectory starting point at the current eef pose, and attach an interactive marker to it
+        scene_display_->getPlanningSceneRW()->getCurrentStateNonConst().update();
         Eigen::Affine3d tip_pose = scene_display_->getPlanningSceneRO()->getCurrentState().getGlobalLinkTransform(robot_interaction_->getActiveEndEffectors()[0].parent_link);
         geometry_msgs::Pose marker_pose;
         tf::poseEigenToMsg(tip_pose, marker_pose);
@@ -337,6 +338,7 @@ void MainWindow::trajectoryExecuteButtonClicked()
     if (waypoint_poses.size() > 0)
     {
       robot_state::RobotState &rstate = scene_display_->getPlanningSceneRW()->getCurrentStateNonConst();
+      rstate.update();
       const robot_model::JointModelGroup *jmg = rstate.getJointModelGroup(ui_.planning_group_combo->currentText().toStdString());
 
       std::vector<robot_state::RobotStatePtr> traj;