fix implicit copy

It seems (not verified) the vector is copied here by bind.
Instead, let's pass a pointer.
This commit is contained in:
v4hn 2017-03-02 17:23:13 +01:00
parent 798f9c0839
commit c3c6d899cb

View File

@ -62,15 +62,16 @@ moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){
namespace {
bool isValid(planning_scene::PlanningSceneConstPtr scene,
std::vector< std::vector<double> >& old_solutions,
std::vector< std::vector<double> >* old_solutions,
robot_state::RobotState* state,
const robot_model::JointModelGroup* jmg,
const double* joint_positions){
for( std::vector<double> sol : old_solutions ){
for( std::vector<double> sol : *old_solutions ){
if( jmg->distance(joint_positions, sol.data()) < 0.1 ){
return false;
}
}
state->setJointGroupPositions(jmg, joint_positions);
state->update();
return scene->isStateColliding(*state, jmg->getName());
@ -91,12 +92,11 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
std::bind(
&isValid,
scene_,
previous_solutions_,
&previous_solutions_,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3);
geometry_msgs::Pose pose;
const Eigen::Affine3d object_pose= scene_->getFrameTransform(object_);
if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())