From 0562376dd77e05edbe57634cba27be8f4f49d75e Mon Sep 17 00:00:00 2001 From: Dave Hershberger Date: Mon, 21 Oct 2013 11:33:29 -0700 Subject: [PATCH] fixed dirty transform bugs --- benchmarks_gui/src/tab_states_and_goals.cpp | 1 + benchmarks_gui/src/tab_trajectories.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/benchmarks_gui/src/tab_states_and_goals.cpp b/benchmarks_gui/src/tab_states_and_goals.cpp index 8e3278012d..f7af250cc6 100644 --- a/benchmarks_gui/src/tab_states_and_goals.cpp +++ b/benchmarks_gui/src/tab_states_and_goals.cpp @@ -778,6 +778,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 index 3d63f52634..bb5f8877d8 100644 --- 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 traj;