mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
798f9c0839
commit
c3c6d899cb
@ -62,15 +62,16 @@ moveit::task_constructor::subtasks::GenerateGraspPose::canCompute(){
|
|||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
bool isValid(planning_scene::PlanningSceneConstPtr scene,
|
bool isValid(planning_scene::PlanningSceneConstPtr scene,
|
||||||
std::vector< std::vector<double> >& old_solutions,
|
std::vector< std::vector<double> >* old_solutions,
|
||||||
robot_state::RobotState* state,
|
robot_state::RobotState* state,
|
||||||
const robot_model::JointModelGroup* jmg,
|
const robot_model::JointModelGroup* jmg,
|
||||||
const double* joint_positions){
|
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 ){
|
if( jmg->distance(joint_positions, sol.data()) < 0.1 ){
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
state->setJointGroupPositions(jmg, joint_positions);
|
state->setJointGroupPositions(jmg, joint_positions);
|
||||||
state->update();
|
state->update();
|
||||||
return scene->isStateColliding(*state, jmg->getName());
|
return scene->isStateColliding(*state, jmg->getName());
|
||||||
@ -91,12 +92,11 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
|||||||
std::bind(
|
std::bind(
|
||||||
&isValid,
|
&isValid,
|
||||||
scene_,
|
scene_,
|
||||||
previous_solutions_,
|
&previous_solutions_,
|
||||||
std::placeholders::_1,
|
std::placeholders::_1,
|
||||||
std::placeholders::_2,
|
std::placeholders::_2,
|
||||||
std::placeholders::_3);
|
std::placeholders::_3);
|
||||||
|
|
||||||
|
|
||||||
geometry_msgs::Pose pose;
|
geometry_msgs::Pose pose;
|
||||||
const Eigen::Affine3d object_pose= scene_->getFrameTransform(object_);
|
const Eigen::Affine3d object_pose= scene_->getFrameTransform(object_);
|
||||||
if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user