void wrenchCallback(geometry_msgs::WrenchStamped::ConstPtr wrench_stamped)
{
double cur_time = wrench_stamped->header.stamp.toSec();
if(start_time == -1)
start_time = cur_time;
CartVec w;
wrenchMsgToEigen(wrench_stamped->wrench, w);
double force_mag = NORM(w(0, 0), w(1, 0), w(2, 0));
tf::StampedTransform tool_loc_tf;
try {
tf_list.waitForTransform("/head_center", "/wipe_finger", wrench_stamped->header.stamp, ros::Duration(0.1));
tf_list.lookupTransform("/head_center", "/wipe_finger", wrench_stamped->header.stamp, tool_loc_tf);
}
catch (tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
return;
}
tool_loc_tf.mult(registration_tf, tool_loc_tf);
btVector3 tool_loc = tool_loc_tf.getOrigin();
PRGB query_pt;
query_pt.x = tool_loc.x(); query_pt.y = tool_loc.y(); query_pt.z = tool_loc.z();
vector<int> nk_inds(1);
vector<float> nk_dists(1);
kd_tree->nearestKSearch(query_pt, 1, nk_inds, nk_dists);
int closest_ind = nk_inds[0];
float closest_dist = nk_dists[0];
hrl_phri_2011::ForceProcessed fp;
fp.time_offset = cur_time - start_time;
tf::transformStampedTFToMsg(tool_loc_tf, fp.tool_frame);
fp.header = wrench_stamped->header;
fp.header.frame_id = "/head_center";
fp.wrench = wrench_stamped->wrench;
fp.pc_ind = closest_ind;
fp.pc_dist = closest_dist;
fp.pc_pt.x = pc_head->points[closest_ind].x;
fp.pc_pt.y = pc_head->points[closest_ind].y;
fp.pc_pt.z = pc_head->points[closest_ind].z;
fp.force_magnitude = force_mag;
if(compute_norms) {
fp.pc_normal.x = normals->points[closest_ind].normal[0];
fp.pc_normal.y = normals->points[closest_ind].normal[1];
fp.pc_normal.z = normals->points[closest_ind].normal[2];
}
// do ellipsoidal processing
tf::Transform tool_loc_ell = ell_reg_tf * tool_loc_tf;
tf::transformTFToMsg(tool_loc_ell, fp.tool_ell_frame);
btVector3 tloce_pos = tool_loc_ell.getOrigin();
ell.cartToEllipsoidal(tloce_pos.x(), tloce_pos.y(), tloce_pos.z(),
fp.ell_coords.x, fp.ell_coords.y, fp.ell_coords.z);
fp_list.push_back(fp);
pub_ind++;
if(pub_ind % 100 == 0)
ROS_INFO("Recorded %d samples", pub_ind);
}
void projectEllipsoid(Ellipsoid& ell, double ell_height, const PCRGB& pc, PCRGB& pc_ell)
{
double lat, lon, height, x, y, z, h, s, l;
BOOST_FOREACH(PRGB const pt, pc.points) {
PRGB npt;
extractHSL(pt.rgb, h, s, l);
ell.cartToEllipsoidal(pt.x, pt.y, pt.z, lat, lon, height);
ell.ellipsoidalToCart(lat, lon, ell_height, x, y, z);
npt.x = x; npt.y = y; npt.z = z;
writeHSL(0, 0, l, npt.rgb);
pc_ell.points.push_back(npt);
}
pc_ell.header.frame_id = "base_link";
//pubLoop(pc_ell, "/proj_head");
}