Boolean
find_best_corresponding_pads_or_pins(ElementPadPinData* ppd_a, int len_a,
int index1_a, int index2_a,
Boolean reflect,
ElementPadPinData* ppd_b, int len_b,
int* index1_b_ptr, int* index2_b_ptr)
{
int block1_start = 0;
int block1_end = 0;
int block2_start = 0;
int block2_end = 0;
if (! find_number_block(ppd_b, len_b, &ppd_a[index1_a].pp,
&block1_start, &block1_end)) {
return False;
}
if (! find_number_block(ppd_b, len_b, &ppd_a[index2_a].pp,
&block2_start, &block2_end)) {
return False;
}
Boolean min_set = False;
double min_sum_of_distances = 0;
int min_index1_b = 0;
int min_index2_b = 0;
int i;
for (i = block1_start; i < block1_end; i++) {
int j;
for (j = block2_start; j < block2_end; j++) {
/* Block1 and block2 ranges may coincide. */
if (i != j) {
int k;
for (k = 0; k < 4; k++) {
/* Find transformation */
double angle = k * M_PI / 2;
Matrix3x3 trans;
if (calculate_transformation(ppd_a[index1_a].center,
ppd_a[index2_a].center,
ppd_b[i].center,
ppd_b[j].center,
reflect, angle,
trans)) {
/* Transform ppd_a */
transform_pad_pin_data(ppd_a, len_a, trans);
/* Sum of distances */
double sd = calculate_sum_of_distances(ppd_a, len_a,
ppd_b, len_b);
if (! min_set || sd < min_sum_of_distances) {
min_set = True;
min_sum_of_distances = sd;
min_index1_b = i;
min_index2_b = j;
}
}
}
}
}
}
if (min_set) {
debug_log("Best corresponding pads/pins found.\n");
*index1_b_ptr = min_index1_b;
*index2_b_ptr = min_index2_b;
} else {
debug_log("No best corresponding pads/pins found.\n");
}
return min_set;
}
bool send2(brio_assembly_vision_new::TrasformStampedRequest &req, brio_assembly_vision_new::TrasformStampedResponse &res)
{
request_a_new_cluster =true;
if(find_cloud_from_kinect_head==true && final ==false)
{
brio_assembly_vision_new::Container * cont = new brio_assembly_vision_new::Container();
while((cont=client_call())==NULL)
{
std::this_thread::sleep_for (std::chrono::seconds(1)); //wait for a good response
}
if(first_time==true)
{
initial_cluster_size=cont->date_container.size();
}
first_time=false;
if(cont->date_container.size()>0) //after moving objects size is decreased
{
int i=0; //start with the lowest index
std::string object_with_shape_requested = model[model_step];
while(cont->date_container[i].piece_type!=object_with_shape_requested && i<cont->date_container.size()-1) //
{
i++;
}
//end while when cluster_vector[i] has the requested shape or when the search index is bigger then cluster_vector
if(i<=cont->date_container.size()-1) // if i<=cluster_vector.size() then we did find requested object at indices i
{
Eigen::Vector3d z_axe,x_or_y_axe;
Eigen::Vector4d translation;
z_axe=estimate_plane_normals(cloud);
int i_center = cont->date_container[i].center_index;
int i_head = cont->date_container[i].head_conn_index;
x_or_y_axe= obj_axe_2_points_next(i_head,i_center);
pcl::PointXYZRGB center = cloud->at(cont->date_container[i].center_index);
translation[0] = center.x;
translation[1] = center.y;
translation[2] = center.z;
calculate_transformation(x_or_y_axe,z_axe ,translation);
ROS_INFO("Send TransformedPose");
Eigen::Quaternionf quat;
quat = createQuaternion();
res.msg.child_frame_id = "brio_piece_frame";
res.msg.header.frame_id = "head_mount_kinect_rgb_optical_frame";
res.msg.transform.translation.x = transformata_finala(0,3);
res.msg.transform.translation.y = transformata_finala(1,3);
res.msg.transform.translation.z = transformata_finala(2,3);
res.msg.transform.rotation.w = (double)quat.w();
res.msg.transform.rotation.x = (double)quat.x();
res.msg.transform.rotation.y = (double)quat.y();
res.msg.transform.rotation.z = (double)quat.z();
if(model_step<initial_cluster_size-1)
model_step++;
else
{ final=true;
//return false;
}
return true;
}
