Esempio n. 1
0
    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);
    }
Esempio n. 2
0
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");
}