void cmd_vx_bcaps_get(xmlrpc_env *env, int argc, char **argv)
{
xmlrpc_value *response, *result;
char *name, *bcap;
int len, i;
if (argc < 1)
usage(EXIT_FAILURE);
name = argv[0];
response = client_call("vxdb.vx.bcaps.get",
"{s:s}",
"name", name);
return_if_fault(env);
len = xmlrpc_array_size(env, response);
return_if_fault(env);
for (i = 0; i < len; i++) {
xmlrpc_array_read_item(env, response, i, &result);
return_if_fault(env);
xmlrpc_decompose_value(env, result, "s", &bcap);
return_if_fault(env);
xmlrpc_DECREF(result);
printf("%s\n", bcap);
}
xmlrpc_DECREF(response);
}
void cmd_list(xmlrpc_env *env, int argc, char **argv)
{
xmlrpc_value *response, *result;
char *username;
int len, i;
if (argc > 0)
username = argv[0];
else
username = "";
response = client_call("vxdb.list",
"{s:s}",
"username", username);
return_if_fault(env);
len = xmlrpc_array_size(env, response);
return_if_fault(env);
for (i = 0; i < len; i++) {
xmlrpc_array_read_item(env, response, i, &result);
return_if_fault(env);
xmlrpc_decompose_value(env, result, "s", &username);
return_if_fault(env);
xmlrpc_DECREF(result);
printf("%s\n", username);
}
xmlrpc_DECREF(response);
}
void cmd_vx_stop(xmlrpc_env *env, int argc, char **argv)
{
char *group;
if (argc < 1)
usage(EXIT_FAILURE);
group = argv[0];
client_call("vg.vx.stop",
"{s:s}",
"group", group);
}
void cmd_vx_bcaps_add(xmlrpc_env *env, int argc, char **argv)
{
char *name, *bcap;
if (argc < 2)
usage(EXIT_FAILURE);
name = argv[0];
bcap = argv[1];
client_call("vxdb.vx.bcaps.add",
"{s:s,s:s}",
"name", name,
"bcap", bcap);
}
void cmd_user_caps_add(xmlrpc_env *env, int argc, char **argv)
{
char *username, *cap;
if (argc < 2)
usage(EXIT_FAILURE);
username = argv[0];
cap = argv[1];
client_call("vcd.user.caps.add",
"{s:s,s:s}",
"username", username,
"cap", cap);
}
void cmd_vx_bcaps_remove(xmlrpc_env *env, int argc, char **argv)
{
char *name, *bcap;
if (argc < 1)
usage(EXIT_FAILURE);
name = argv[0];
if (argc > 1)
bcap = argv[1];
else
bcap = "";
client_call("vxdb.vx.bcaps.remove",
"{s:s,s:s}",
"name", name,
"bcap", bcap);
}
void cmd_user_caps_remove(xmlrpc_env *env, int argc, char **argv)
{
char *username, *cap;
if (argc < 1)
usage(EXIT_FAILURE);
username = argv[0];
if (argc > 1)
cap = argv[1];
else
cap = "";
client_call("vcd.user.caps.remove",
"{s:s,s:s}",
"username", username,
"cap", cap);
}
void call_add(client_t *cli, int k) {
struct_add sa;
sa.a = k;
sa.b = 0;
client_call(cli, ADD, (uint8_t *)&sa, sizeof(struct_add));
}
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;
}
