void MotionPlanningFrame::snapStateToFrame(robot_state::RobotState& state,
                                               const Eigen::Affine3d& T_frame_world,
                                               const std::string& link,
                                               const Eigen::Affine3d& T_frame_link,
                                               const std::string& group)
    {
        ROS_DEBUG_FUNCTION;

        // std::cout << "T_frame_world:\n" << T_frame_world.matrix() << std::endl;
        // std::cout << "T_frame_link:\n" << T_frame_link.matrix() << std::endl;

        // Back out the desired link transform in global coordinates.
        Eigen::Affine3d T_link_world = T_frame_world * T_frame_link.inverse();
        // Snap to the frame using IK.
        const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group);

        // std::cout << "group: " << group << std::endl;

        state.update();
        trajectory_msgs::JointTrajectoryPoint point;
        state.copyJointGroupPositions(jmg, point.positions);
        ROS_DEBUG_STREAM("pre-ik positions:\n" << point);

        geometry_msgs::Pose pose;
        tf::poseEigenToMsg(T_link_world, pose);
        state.setFromIK(jmg, pose, link);
        state.update();

        state.copyJointGroupPositions(jmg, point.positions);
        ROS_DEBUG_STREAM("post-ik positions:\n" << point);

        ROS_DEBUG_FUNCTION;
    }
//this is a pretty general function:
// goal contains a desired tool pose;
// path is planned from current joint state to some joint state that achieves desired tool pose
bool ArmMotionInterface::rt_arm_plan_path_current_to_goal_pose() {
    ROS_INFO("computing a cartesian trajectory to right-arm tool goal pose");
    //unpack the goal pose:
    goal_gripper_pose_right_ = cart_goal_.des_pose_gripper_right;
    
    goal_gripper_affine_right_= transformPoseToEigenAffine3d(goal_gripper_pose_right_.pose);
    ROS_INFO("tool frame goal: ");
    display_affine(goal_gripper_affine_right_);    
    goal_flange_affine_right_ = goal_gripper_affine_right_ * A_tool_wrt_flange_.inverse();
    ROS_INFO("flange goal");
    display_affine(goal_flange_affine_right_);
    Vectorq7x1 q_start;
    q_start = get_jspace_start_right_arm_(); // choose last cmd, or current joint angles
    path_is_valid_ = cartTrajPlanner_.cartesian_path_planner(q_start, goal_flange_affine_right_, optimal_path_);

    if (path_is_valid_) {
        baxter_traj_streamer_.stuff_trajectory(optimal_path_, des_trajectory_); //convert from vector of poses to trajectory message   
        computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec();
        cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::SUCCESS;
        cart_result_.computed_arrival_time = computed_arrival_time_;
        cart_move_as_.setSucceeded(cart_result_); 
    }
    else {
        cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID;
        cart_result_.computed_arrival_time = -1.0; //impossible arrival time        
        cart_move_as_.setSucceeded(cart_result_); //the communication was a success, but not the computation 
    }   

    return path_is_valid_;    
}
예제 #3
0
void Irp6pInverseKinematic::updateHook() {

  if (port_input_wrist_pose_.read(wrist_pose_) == RTT::NewData) {

    Eigen::Affine3d trans;
    tf::poseMsgToEigen(wrist_pose_, trans);

    port_current_joint_position_.read(local_current_joints_);

    inverse_kinematics_single_iteration(local_current_joints_, trans,
                                        &local_desired_joints_);

    port_output_joint_position_.write(local_desired_joints_);

  } else if (port_input_end_effector_pose_.read(end_effector_pose_)
      == RTT::NewData) {

    port_tool_.read(tool_msgs_);

    Eigen::Affine3d tool;
    Eigen::Affine3d trans;
    Eigen::Affine3d wrist_pose;
    tf::poseMsgToEigen(end_effector_pose_, trans);
    tf::poseMsgToEigen(tool_msgs_, tool);

    wrist_pose = trans * tool.inverse();

    port_current_joint_position_.read(local_current_joints_);

    inverse_kinematics_single_iteration(local_current_joints_, wrist_pose,
                                        &local_desired_joints_);

    port_output_joint_position_.write(local_desired_joints_);
  }
}
예제 #4
0
template <typename PointT, typename NormalT> bool
cpu_tsdf::TSDFVolumeOctree::integrateCloud (
  const pcl::PointCloud<PointT> &cloud, 
  const pcl::PointCloud<NormalT> &normals,
  const Eigen::Affine3d &trans)
{
  Eigen::Affine3f trans_inv = trans.inverse ().cast<float> ();

  // First, sample a few points and force their containing voxels to split
  int px_step = 1;
  int nsplit = 0;
  for (size_t u = 0; u < cloud.width; u += px_step)
  {
    for (size_t v = 0; v < cloud.height; v += px_step)
    {
      const PointT &pt_surface_orig = cloud (u, v);
      if (pcl_isnan (pt_surface_orig.z))
        continue;
      // Look at surroundings
      int nstep = 0;
      Eigen::Vector3f ray = pt_surface_orig.getVector3fMap ().normalized ();
      for (int perm = 0; perm < num_random_splits_; perm++)
      {
        // Get containing voxels
        PointT pt_trans; 
        float scale = (float)rand () / (float)RAND_MAX * 0.03;
        Eigen::Vector3f noise = Eigen::Vector3f::Random ().normalized () * scale;;
        if (perm == 0) noise *= 0;
        pt_trans.getVector3fMap () = trans.cast<float> () * (pt_surface_orig.getVector3fMap ()+ noise);
        OctreeNode* voxel = octree_->getContainingVoxel (pt_trans.x, pt_trans.y, pt_trans.z);
        if (voxel != NULL)
        {
          while (voxel->getMinSize () > xsize_ / xres_)
          {
            nsplit++;
            voxel->split ();
            voxel = voxel->getContainingVoxel (pt_trans.x, pt_trans.y, pt_trans.z);
            
          }
        }
      }
    }
  }
  
  // Do Frustum Culling to get rid of unseen voxels
  std::vector<cpu_tsdf::OctreeNode::Ptr> voxels_culled;
  getFrustumCulledVoxels(trans, voxels_culled);
#pragma omp parallel for
  for (size_t i = 0; i < voxels_culled.size (); i++)
  {
    updateVoxel (voxels_culled[i], cloud, normals, trans_inv);
  }
  // Cloud is no longer empty
  is_empty_ = false;
  return (true);
}
예제 #5
0
void ICPLocalization::addScanLineToPointCloud(Eigen::Affine3d body2Odo, Eigen::Affine3d body2World, Eigen::Affine3d laser2Body, const ::base::samples::LaserScan &scan_reading)
{

    if(scansWithTransforms.size() == 0) 
    {
	addLaserScan(body2Odo,body2World,laser2Body,scan_reading); 
	return;
    }
    
    /*
    double max_rotation =  1.5 * conf_point_cloud.lines_per_point_cloud * conf_point_cloud.min_rotation_for_new_line;
    double max_translation = 1.5 * conf_point_cloud.lines_per_point_cloud * conf_point_cloud.min_distance_travelled_for_new_line;
    double max_head_movement = 1.5 * conf_point_cloud.lines_per_point_cloud * conf_point_cloud.min_rotation_head_for_new_line; 
    */
    bool add_laser_scan = true; 
    for( uint i = 0; i < scansWithTransforms.size(); i++) 
    {
	Eigen::Affine3d diference( body2Odo.inverse() * scansWithTransforms.at(i).body2Odo );

	Vector3d Ylaser2Body = laser2Body * Vector3d::UnitY() - laser2Body.translation();
	Ylaser2Body.normalize();
	Vector3d YlastLaser2Body = scansWithTransforms.back().laser2Body * Vector3d::UnitY() - scansWithTransforms.at(i).laser2Body.translation();
	YlastLaser2Body.normalize();
	
	double laserChange = acos(Ylaser2Body.dot(YlastLaser2Body));
	double translation =  diference.translation().norm(); 
	double rotation = fabs(Eigen::AngleAxisd( diference.rotation() ).angle()) ; 
	
	add_laser_scan = add_laser_scan && ( rotation > conf_point_cloud.min_rotation_for_new_line || translation > conf_point_cloud.min_distance_travelled_for_new_line || laserChange >  conf_point_cloud.min_rotation_head_for_new_line);
	 
	//if  the distance is to big means the old laser scan is not consistent anymore. 
// 	if( rotation > max_rotation|| translation > max_translation || laserChange > max_head_movement)
// 	{
// 	    scansWithTransforms.erase(scansWithTransforms.begin() + i); 
// 	    scanCount--; 
// 	    std::cout << " IcpLocalization.cpp erasing old laser scan " << std::endl; 
// 	}

/*	std::cout <<" add new scan " << add_laser_scan << std::endl; 
	std::cout << "\t translation " << (translation > conf_point_cloud.min_distance_travelled_for_new_line)<< " "  << translation << " > " << conf_point_cloud.min_distance_travelled_for_new_line << std::endl;
	std::cout << "\t rotation " << (rotation > conf_point_cloud.min_rotation_for_new_line) << " "  << rotation * 180 / M_PI << " > " << conf_point_cloud.min_rotation_for_new_line * 180 / M_PI << std::endl;
	std::cout << "\t head " << (laserChange >  conf_point_cloud.min_rotation_head_for_new_line) << "  "<< laserChange * 180 / M_PI << " > " << conf_point_cloud.min_rotation_head_for_new_line * 180 / M_PI<< std::endl; */
	
	if (!add_laser_scan) 
	    break; 
    }
    
    if ( add_laser_scan )
    {
	addLaserScan(body2Odo,body2World,laser2Body,scan_reading); 
    }
    
    return; 
  
}
void PolygonArrayTransformer::transformPolygon(const Eigen::Affine3d& transform,
        const geometry_msgs::PolygonStamped& polygon,
        geometry_msgs::PolygonStamped& result)
{
    result.header = polygon.header;
    result.header.frame_id = frame_id_;
    for (size_t i = 0; i < polygon.polygon.points.size(); i++) {
        Eigen::Vector4d point;
        point[0] = polygon.polygon.points[i].x;
        point[1] = polygon.polygon.points[i].y;
        point[2] = polygon.polygon.points[i].z;
        point[3] = 1;             // homogenious
        Eigen::Vector4d transformed_point_eigen = transform.inverse() * point;
        geometry_msgs::Point32 transformed_point;
        transformed_point.x = transformed_point_eigen[0];
        transformed_point.y = transformed_point_eigen[1];
        transformed_point.z = transformed_point_eigen[2];
        result.polygon.points.push_back(transformed_point);
    }
}
bool ArmMotionInterface::unpack_goal_pose(void) {
    geometry_msgs::PoseStamped poseStamped_goal = request_.poseStamped_goal;
    Eigen::Vector3d goal_origin;
    goal_origin[0] = poseStamped_goal.pose.position.x;
    goal_origin[1] = poseStamped_goal.pose.position.y;
    goal_origin[2] = poseStamped_goal.pose.position.z;
    a_tool_end_.translation() = goal_origin;

    Eigen::Quaterniond quat;
    quat.x() = poseStamped_goal.pose.orientation.x;
    quat.y() = poseStamped_goal.pose.orientation.y;
    quat.z() = poseStamped_goal.pose.orientation.z;
    quat.w() = poseStamped_goal.pose.orientation.w;
    Eigen::Matrix3d R_goal(quat);
    a_tool_end_.linear() = R_goal;
    a_flange_end_ = a_tool_end_ * A_tool_wrt_flange_.inverse();


    return true;
}
 bool CaptureStereoSynchronizer::checkNearPose(
   const geometry_msgs::Pose& new_pose)
 {
   Eigen::Affine3d new_affine;
   tf::poseMsgToEigen(new_pose, new_affine);
   for (size_t i = 0; i < poses_.size(); i++) {
     // convert pose into eigen
     Eigen::Affine3d affine;
     tf::poseMsgToEigen(poses_[i], affine);
     // compute difference
     Eigen::Affine3d diff = affine.inverse() * new_affine;
     double positional_difference = diff.translation().norm();
     if (positional_difference < positional_bin_size_) {
       Eigen::AngleAxisd rotational_difference(diff.rotation());
       if (rotational_difference.angle() < rotational_bin_size_) {
         return true;
       }
     }
   }
   return false;
 }
예제 #9
0
void inPointCallback(const geometry_msgs::Point& pt_msg) {
    Eigen::Affine3d des_gripper1_wrt_base;

    g_des_point(0) = pt_msg.x;
    g_des_point(1) = pt_msg.y;
    g_des_point(2) = pt_msg.z;
    cout << "received des point = " << g_des_point.transpose() << endl;
    g_des_gripper_affine1.translation() = g_des_point;
    //convert this to base coords:
    g_des_gripper1_wrt_base = g_affine_lcamera_to_psm_one.inverse() * g_des_gripper_affine1;
    //try computing IK:
    if (g_ik_solver.ik_solve(g_des_gripper1_wrt_base)) {
        ROS_INFO("found IK soln");
        g_got_new_pose = true;
        g_q_vec1_start = g_q_vec1_goal;
        g_q_vec1_goal = g_ik_solver.get_soln();
        cout << g_q_vec1_goal.transpose() << endl;
        g_trajectory_point1 = g_trajectory_point2; //former goal is new start

        for (int i = 0; i < 6; i++) {
            g_trajectory_point2.positions[i] = g_q_vec1_goal[i]; //leave psm2 angles alone; just update psm1 goal angles
        }
        //gripper_affines_wrt_camera.push_back(affine_gripper_frame_wrt_camera_frame_);
        //    des_trajectory.joint_names.push_back("right_s0"); //yada-yada should fill in names
        g_des_trajectory.header.stamp = ros::Time::now();

        g_des_trajectory.points[0] = g_des_trajectory.points[1]; //former goal is new start
        g_des_trajectory.points[1] = g_trajectory_point2;



        // copy traj to goal:
        g_goal.trajectory = g_des_trajectory;
    } else {
        ROS_WARN("NO IK SOLN FOUND");
    }

}
// static BITBOTS_INLINE Eigen::Matrix4d inverse_chain_matrix(const Eigen::Matrix4d chain_matrix) {
//     Eigen::Matrix4d inverse;
//     inverse.block<3, 3>(0, 0) = chain_matrix.block<3, 3>(0, 0).inverse();
//     inverse.col(3).head<3>() = -(inverse.block<3, 3>(0, 0) * chain_matrix.col(3).head<3>());
//     inverse.row(3) = Eigen::RowVector4d(0, 0, 0, 1);
//     return inverse;
// }
static BITBOTS_INLINE Eigen::Affine3d inverse_chain_matrix(const Eigen::Affine3d& chain_matrix) {
    return chain_matrix.inverse();
}
/**
 * This is where the localization is done... not really callback in the offline version
 */ 
void callback(const sensor_msgs::LaserScan &scan, tf::Transform odo_pose, tf::Transform state_pose)
{
	static int counter = 0;
	counter++;
	
	static tf::TransformListener tf_listener;
	
	double looptime = TT.Tac();
	TT.Tic();
	fprintf(stderr,"Lt( %.1lfms %.1lfHz seq:%d) -",looptime*1000,1.0/looptime,scan.header.seq);
	
	if(has_sensor_offset_set == false) return;
	
	double gx,gy,gyaw,x,y,yaw;
	
	///Get the ground truth
	gyaw = tf::getYaw(state_pose.getRotation());  
	gx = state_pose.getOrigin().x();
	gy = state_pose.getOrigin().y();
	
	///Get the odometry
	yaw = tf::getYaw(odo_pose.getRotation());  
	x = odo_pose.getOrigin().x();
	y = odo_pose.getOrigin().y();
		
	mrpt::utils::CTicTac	tictac;
	tictac.Tic();

	int N =(scan.angle_max - scan.angle_min)/scan.angle_increment; ///< number of scan lines
	
	/////
	/// Pose conversions
	////
	
	Eigen::Affine3d T = getAsAffine(x,y,yaw);
	Eigen::Affine3d Tgt = getAsAffine(gx,gy,gyaw);
	
	/**
	 * We are now using the information from the ground truth to initialize the filter
	 */ 
	if(isFirstLoad){
		fprintf(stderr,"Initializing to (%lf, %lf, %lf)\n",gx,gy,gyaw);
		///Initialize the filter with 1m^2 variance in position and 20deg in heading
		ndtmcl->initializeFilter(gx, gy,gyaw,1.0, 1.0, 20.0*M_PI/180.0, 450);
		Told = T;
		Todo = Tgt;
	}
	
	///Compute the differential motion between two time steps 
	Eigen::Affine3d Tmotion = Told.inverse() * T;
	Todo = Todo*Tmotion;
	Told = T;
	
	///Get the laser pose with respect to the base
	float dy =offy;
	float dx = offx;
	float alpha = atan2(dy,dx);
	float L = sqrt(dx*dx+dy*dy);
	
	///Laser pose in base frame
	float lpx = L * cos(alpha);
	float lpy = L * sin(alpha);
	float lpa = offa;
	
	
	
	///Laser scan to PointCloud transformed with respect to the base
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);	
	for (int j=0;j<N;j++){
		double r  = scan.ranges[j];
		if(r>=scan.range_min && r<scan.range_max && r>0.3 && r<20.0){
			double a  = scan.angle_min + j*scan.angle_increment;
			pcl::PointXYZ pt;
			pt.x = r*cos(a+lpa)+lpx;
			pt.y = r*sin(a+lpa)+lpy;
			pt.z = 0.1+0.02 * (double)rand()/(double)RAND_MAX;
			cloud->push_back(pt);
		}
	}
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	/// Now we have the differential motion and pointcloud -- Lets do MCL
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	ndtmcl->updateAndPredict(Tmotion, *cloud); ///< does the prediction, update and resampling steps (see ndt_mcl.hpp)
	
	Eigen::Vector3d dm = ndtmcl->getMean(); ///<Maximum aposteriori estimate of the pose
	Eigen::Matrix3d cov = ndtmcl->pf.getDistributionVariances(); ///distribution variances
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	double Time = tictac.Tac();
	fprintf(stderr,"Time elapsed %.1lfms (%lf %lf %lf) \n",Time*1000,dm[0],dm[1],dm[2]);
	isFirstLoad = false;
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	
	//If visualization desired
#ifdef USE_VISUALIZATION_DEBUG
	///The sensor pose in the global frame for visualization
	Eigen::Vector3d origin(dm[0] + L * cos(dm[2]+alpha),dm[1] + L * sin(dm[2]+alpha),0.1);

	Eigen::Affine3d ppos = getAsAffine(dm[0],dm[1],dm[2]);
	
	//lslgeneric::transformPointCloudInPlace(Tgt, *cloud);
	lslgeneric::transformPointCloudInPlace(ppos, *cloud);
	mrpt::opengl::COpenGLScenePtr &scene = win3D.get3DSceneAndLock();	
	win3D.setCameraPointingToPoint(gx,gy,1);
	if(counter%2000==0) gl_points->clear();
	scene->clear();
	scene->insert(plane);
	
	addMap2Scene(ndtmcl->map, origin, scene);
	addPoseCovariance(dm[0],dm[1],cov,scene);
	addScanToScene(scene, origin, cloud);
	addParticlesToWorld(ndtmcl->pf,Tgt.translation(),dm, Todo.translation());
	scene->insert(gl_points);
	scene->insert(gl_particles);
	win3D.unlockAccess3DScene();
	win3D.repaint();

	if (win3D.keyHit())
	{
		mrpt::gui::mrptKeyModifier kmods;
		int key = win3D.getPushedKey(&kmods);
	}
#endif
	
	

}
예제 #12
0
FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess) {

    unsigned int s_nr_data = src->data.cols();//std::min(int(src->data.cols()),int(500000));
    unsigned int d_nr_data = dst->data.cols();
    refinement->allow_regularization = true;
    //printf("s_nr_data: %i d_nr_data: %i\n",s_nr_data,d_nr_data);

    int stepy = std::max(1,int(d_nr_data)/100000);

    Eigen::Matrix<double, 3, Eigen::Dynamic> Y;
    Eigen::Matrix<double, 3, Eigen::Dynamic> N;
    Y.resize(Eigen::NoChange,d_nr_data/stepy);
    N.resize(Eigen::NoChange,d_nr_data/stepy);
    unsigned int ycols = Y.cols();

    for(unsigned int i = 0; i < d_nr_data/stepy; i++) {
        Y(0,i)	= dst->data(0,i*stepy);
        Y(1,i)	= dst->data(1,i*stepy);
        Y(2,i)	= dst->data(2,i*stepy);
        N(0,i)	= dst->normals(0,i*stepy);
        N(1,i)	= dst->normals(1,i*stepy);
        N(2,i)	= dst->normals(2,i*stepy);
    }

    /// Build kd-tree
    //nanoflann::KDTreeAdaptor<Eigen::Matrix3Xd, 3, nanoflann::metric_L2_Simple>                  kdtree(Y);

    Eigen::VectorXd DST_INORMATION = Eigen::VectorXd::Zero(Y.cols());
    for(unsigned int i = 0; i < d_nr_data/stepy; i++) {
        DST_INORMATION(i) = dst->information(0,i*stepy);
    }

    double s_mean_x = 0;
    double s_mean_y = 0;
    double s_mean_z = 0;
    for(unsigned int i = 0; i < s_nr_data; i++) {
        s_mean_x += src->data(0,i);
        s_mean_y += src->data(1,i);
        s_mean_z += src->data(2,i);
    }
    s_mean_x /= double(s_nr_data);
    s_mean_y /= double(s_nr_data);
    s_mean_z /= double(s_nr_data);

    double d_mean_x = 0;
    double d_mean_y = 0;
    double d_mean_z = 0;
    for(unsigned int i = 0; i < d_nr_data; i++) {
        d_mean_x += dst->data(0,i);
        d_mean_y += dst->data(1,i);
        d_mean_z += dst->data(2,i);
    }
    d_mean_x /= double(d_nr_data);
    d_mean_y /= double(d_nr_data);
    d_mean_z /= double(d_nr_data);

    double stop		= 0.00001;

    Eigen::Affine3d Ymean = Eigen::Affine3d::Identity();
    Ymean(0,3) = d_mean_x;
    Ymean(1,3) = d_mean_y;
    Ymean(2,3) = d_mean_z;

    Eigen::Affine3d Xmean = Eigen::Affine3d::Identity();
    Xmean(0,3) = s_mean_x;
    Xmean(1,3) = s_mean_y;
    Xmean(2,3) = s_mean_z;

    std::vector< Eigen::Matrix<double, 3, Eigen::Dynamic> > all_X;
    std::vector< Eigen::Affine3d > all_res;
    std::vector< int > count_X;
    std::vector< float > score_X;
    std::vector< std::vector< Eigen::VectorXd > > all_starts;
    int stepxsmall = std::max(1,int(s_nr_data)/250);
    Eigen::VectorXd Wsmall (s_nr_data/stepxsmall);
    for(unsigned int i = 0; i < s_nr_data/stepxsmall; i++) {
        Wsmall(i) = src->information(0,i*stepxsmall);
    }


    double sumtime = 0;
    double sumtimeSum = 0;
    double sumtimeOK = 0;
    int r = 0;

    refinement->viewer = viewer;
    refinement->visualizationLvl = 0;
    //for(int r = 0; r < 1000; r++){
//	while(true){
//		double rx = 2.0*M_PI*0.0001*double(rand()%10000);
//		double ry = 2.0*M_PI*0.0001*double(rand()%10000);
//		double rz = 2.0*M_PI*0.0001*double(rand()%10000);
    //double stop = 0;
    double step = 0.1+2.0*M_PI/5;
    for(double rx = 0; rx < 2.0*M_PI; rx += step) {
        for(double ry = 0; ry < 2.0*M_PI; ry += step)
            for(double rz = 0; rz < 2.0*M_PI; rz += step) {
                printf("rx: %f ry: %f rz: %f\n",rx,ry,rz);

                double start = getTime();

                double meantime = 999999999999;
                if(r != 0) {
                    meantime = sumtimeSum/double(sumtimeOK+1.0);
                }
                refinement->maxtime = std::min(0.5,3*meantime);

                Eigen::VectorXd startparam = Eigen::VectorXd(3);
                startparam(0) = rx;
                startparam(1) = ry;
                startparam(2) = rz;

                Eigen::Affine3d randomrot = Eigen::Affine3d::Identity();

                randomrot =	Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) *
                            Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) *
                            Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ());

                Eigen::Affine3d current_guess = Ymean*randomrot*Xmean.inverse();//*Ymean;
                refinement->target_points = 250;
                FusionResults fr = refinement->getTransform(current_guess.matrix());
//fr.timeout = timestopped;
                double stoptime = getTime();
                sumtime += stoptime-start;

                if(!fr.timeout) {
                    sumtimeSum += stoptime-start;
                    sumtimeOK++;
                }

                //printf("sumtime: %f\n",sumtime);
                stop = fr.score;
                Eigen::Matrix4d m = fr.guess;
                current_guess = m;//fr.guess;
                float m00 = current_guess(0,0);
                float m01 = current_guess(0,1);
                float m02 = current_guess(0,2);
                float m03 = current_guess(0,3);
                float m10 = current_guess(1,0);
                float m11 = current_guess(1,1);
                float m12 = current_guess(1,2);
                float m13 = current_guess(1,3);
                float m20 = current_guess(2,0);
                float m21 = current_guess(2,1);
                float m22 = current_guess(2,2);
                float m23 = current_guess(2,3);

                Eigen::Matrix<double, 3, Eigen::Dynamic> Xsmall;
                Xsmall.resize(Eigen::NoChange,s_nr_data/stepxsmall);
                for(unsigned int i = 0; i < s_nr_data/stepxsmall; i++) {
                    float x		= src->data(0,i*stepxsmall);
                    float y		= src->data(1,i*stepxsmall);
                    float z		= src->data(2,i*stepxsmall);
                    Xsmall(0,i)	= m00*x + m01*y + m02*z + m03;
                    Xsmall(1,i)	= m10*x + m11*y + m12*z + m13;
                    Xsmall(2,i)	= m20*x + m21*y + m22*z + m23;
                }

                bool exists = false;
                for(unsigned int ax = 0; ax < all_X.size(); ax++) {
                    Eigen::Matrix<double, 3, Eigen::Dynamic> axX = all_X[ax];
                    Eigen::Affine3d axT = all_res[ax];
                    double diff = (Xsmall-axX).colwise().norm().mean();
                    if(diff < 20*stop) {
                        count_X[ax]++;
                        all_starts[ax].push_back(startparam);
                        int count = count_X[ax];
                        float score = score_X[ax];
                        std::vector< Eigen::VectorXd > starts = all_starts[ax];
                        for(int bx = ax-1; bx >= 0; bx--) {
                            if(count_X[bx] < count_X[bx+1]) {
                                count_X[bx+1]		= count_X[bx];
                                score_X[bx+1]		= score_X[bx];
                                all_X[bx+1]			= all_X[bx];
                                all_starts[bx+1]	= all_starts[bx];
                                all_res[bx+1]		= all_res[bx];


                                all_X[bx] = axX;
                                count_X[bx] = count;
                                score_X[bx] = score;
                                all_starts[bx] = starts;
                                all_res[bx] = axT;
                            } else {
                                break;
                            }
                        }
                        exists = true;
                        break;
                    }
                }



                if(!exists) {
                    all_X.push_back(Xsmall);
                    count_X.push_back(1);
                    score_X.push_back(stop);
                    all_starts.push_back(std::vector< Eigen::VectorXd >());
                    all_starts.back().push_back(startparam);
                    all_res.push_back(current_guess);
                }
                r++;
            }
    }


    FusionResults fr = FusionResults();
    refinement->allow_regularization = false;

    int tpbef = refinement->target_points;
    refinement->target_points = 2000;
    for(unsigned int ax = 0; ax < all_X.size(); ax++) {
        Eigen::Matrix4d np = all_res[ax].matrix();
        refinement->visualizationLvl = visualizationLvl;
        if(ax < 25) {
            double start = getTime();
            FusionResults fr1 = refinement->getTransform(np);
            double stop = getTime();
            np = fr1.guess;
        }
        refinement->visualizationLvl = 0;
        fr.candidates.push_back(np);
        fr.counts.push_back(count_X[ax]);
        fr.scores.push_back(1.0/score_X[ax]);
    }
    refinement->target_points = tpbef;
    return fr;
}
예제 #13
0
double transformationdiff(Eigen::Affine3d & A, Eigen::Affine3d & B, double rotationweight) {
    Eigen::Affine3d C = A.inverse()*B;
    double r = fabs(1-C(0,0))+fabs(C(0,1))+fabs(C(0,2))  +  fabs(C(1,0))+fabs(1-C(1,1))+fabs(C(1,2))  +  fabs(C(2,0))+fabs(C(2,1))+fabs(1-C(2,2));
    double t = sqrt(C(0,3)*C(0,3)+C(1,3)*C(1,3)+C(2,3)*C(2,3));
    return r*rotationweight+t;
}
    void callback(const PointPlusConstPtr& robot_point, const PointPlusConstPtr& mass_point)
	{
	    ROS_DEBUG("Synchronized Callback triggered");
	    puppeteer_msgs::PointPlus r_pt, m_pt;

	    // if not in run or calibrate, just exit
	    if (run_system_logic())
		return;

	    // let's first correct for the radii of the objects:
	    r_pt = *robot_point;
	    r_pt = correct_vals(r_pt, DEFAULT_ROBOT_RADIUS);
	    m_pt = *mass_point;
	    m_pt = correct_vals(m_pt, DEFAULT_MASS_RADIUS);

	    if (calibrated_flag == false)
	    {
		if (calibrate_count == 0)
		{
		    calibrate_count++;
		    robot_cal_pos << 0, 0, 0;
		    mass_cal_pos << 0, 0, 0;
		    robot_start_pos << 0, 0, 0;
		    mass_start_pos << 0, 0, 0;
		    return;
		}
		else if (calibrate_count <= NUM_CALIBRATES)
		{
		    ROS_INFO_THROTTLE(1, "Calibrating...");
		    if (!m_pt.error && !r_pt.error)
		    {
			// then we got both objects successfully:
			robot_cal_pos(0) += r_pt.x;
			robot_cal_pos(1) += r_pt.y;
			robot_cal_pos(2) += r_pt.z;
			mass_cal_pos(0) += m_pt.x;
			mass_cal_pos(1) += m_pt.y;
			mass_cal_pos(2) += m_pt.z;
			calibrate_count++;
		    }
		    return;
		}
		else
		{
		    ROS_INFO("Calibration completed successfully!");
		    // if here, calibration is done!
		    // first find averages:
		    robot_cal_pos = (robot_cal_pos/((double) NUM_CALIBRATES));
		    mass_cal_pos = (mass_cal_pos/((double) NUM_CALIBRATES));
		    // now set start positions:
		    mass_start_pos << q0.xm, q0.ym, 0;
		    robot_start_pos << q0.xr, H0, 0;
		    // now get the transform:
		    mass_cal_pos -= mass_start_pos;
		    robot_cal_pos -= robot_start_pos;
		    // let's check if there is an error here:
		    ////////////////////////
                    // ERROR CHECK	  //
                    ////////////////////////
		    // if no error:
		    // cal_pos = (mass_cal_pos + robot_cal_pos)/2.0;
		    cal_pos = mass_cal_pos;

		    // reset vars:
		    calibrate_count = 0;
		    calibrated_flag = true;
		}
	    }
	    
	    // send global frame transforms:
	    send_global_transforms(m_pt.header.stamp);

	    // transform both of the points into the
	    // optimization_frame, build the output message, and
	    // publish
	    tf::Transform transform;
	    PlanarSystemConfig qmeas;
	    Eigen::Affine3d gwo;
	    Eigen::Vector3d robot_trans, mass_trans;
	    transform.setOrigin(tf::Vector3(cal_pos(0),
					    cal_pos(1), cal_pos(2)));
	    transform.setRotation(tf::Quaternion(0,0,0,1));
	    tf::TransformTFToEigen(transform, gwo);
	    gwo = gwo.inverse();
	    // transform robot:
	    robot_trans << r_pt.x, r_pt.y, r_pt.z;
	    robot_trans = gwo*robot_trans;
	    // transform mass:
	    mass_trans << m_pt.x, m_pt.y, m_pt.z;
	    mass_trans = gwo*mass_trans;
	    // calculate string length:
	    mass_trans(2) = 0; robot_trans(2) = 0;
	    double rad = (mass_trans - robot_trans).norm();
	    qmeas.xm = mass_trans(0);
	    qmeas.ym = mass_trans(1);
	    qmeas.xr = robot_trans(0);
	    qmeas.r = rad;
	    qmeas.mass_err = m_pt.error;
	    qmeas.robot_err = r_pt.error;
	    qmeas.header.stamp = m_pt.header.stamp;
	    qmeas.header.frame_id = "optimization_frame";
	    config_pub.publish(qmeas);
		    
	    return;
	}
void CartMoveActionServer::executeCB(const actionlib::SimpleActionServer<cwru_action::cart_moveAction>::GoalConstPtr& goal) {

    ROS_INFO("in executeCB of CartMoveActionServer");
   cart_result_.err_code=0;
   cart_move_as_.isActive();
   //unpack the necessary info:
   gripper_ang1_ = goal->gripper_jaw_angle1;
   gripper_ang2_ = goal->gripper_jaw_angle2;
   arrival_time_ = goal->move_time;
   // interpret the desired gripper poses:
   geometry_msgs::PoseStamped des_pose_gripper1 = goal->des_pose_gripper1;
   geometry_msgs::PoseStamped des_pose_gripper2 = goal->des_pose_gripper2;
   // convert the above to affine objects:
   des_gripper1_affine_wrt_lcamera_ = transformPoseToEigenAffine3d(des_pose_gripper1.pose);
   cout<<"gripper1 desired pose;  "<<endl;
   cout<<des_gripper1_affine_wrt_lcamera_.linear()<<endl;
   cout<<"origin: "<<des_gripper1_affine_wrt_lcamera_.translation().transpose()<<endl;

   des_gripper2_affine_wrt_lcamera_ = transformPoseToEigenAffine3d(des_pose_gripper2.pose);
   cout<<"gripper2 desired pose;  "<<endl;
   cout<<des_gripper2_affine_wrt_lcamera_.linear()<<endl;
   cout<<"origin: "<<des_gripper2_affine_wrt_lcamera_.translation().transpose()<<endl;

   //do IK to convert these to joint angles:
    //Eigen::VectorXd q_vec1,q_vec2;
    Vectorq7x1 q_vec1,q_vec2;
    q_vec1.resize(7);
    q_vec2.resize(7);

    
    trajectory_msgs::JointTrajectory des_trajectory; // an empty trajectory 
    des_trajectory.points.clear(); // can clear components, but not entire trajectory_msgs
    des_trajectory.joint_names.clear(); //could put joint names in...but I assume a fixed order and fixed size, so this is unnecessary
    // if using wsn's trajectory streamer action server
    des_trajectory.header.stamp = ros::Time::now();

    trajectory_msgs::JointTrajectoryPoint trajectory_point; //,trajectory_point2; 
    trajectory_point.positions.resize(14);
    
    ROS_INFO("\n");
    ROS_INFO("stored previous command to gripper one: ");
    cout<<gripper1_affine_last_commanded_pose_.linear()<<endl;
    cout<<"origin: "<<gripper1_affine_last_commanded_pose_.translation().transpose()<<endl;
   

    // first, reiterate previous command:
    // this could be easier, if saved previous joint-space trajectory point...
    des_gripper_affine1_ = affine_lcamera_to_psm_one_.inverse()*gripper1_affine_last_commanded_pose_; //previous pose
    ik_solver_.ik_solve(des_gripper_affine1_); //convert desired pose into equiv joint displacements
    q_vec1 = ik_solver_.get_soln(); 
    q_vec1(6) = last_gripper_ang1_; // include desired gripper opening angle
    
    
    des_gripper_affine2_ = affine_lcamera_to_psm_two_.inverse()*gripper2_affine_last_commanded_pose_; //previous pose
    ik_solver_.ik_solve(des_gripper_affine2_); //convert desired pose into equiv joint displacements
    q_vec2 = ik_solver_.get_soln(); 
    cout<<"q_vec1 of stored pose: "<<endl;
    for (int i=0;i<6;i++) {
        cout<<q_vec1[i]<<", ";
    }
    cout<<endl;
    q_vec2(6) = last_gripper_ang2_; // include desired gripper opening angle
    
       for (int i=0;i<7;i++) {
            trajectory_point.positions[i] = q_vec1(i);
            trajectory_point.positions[i+7] = q_vec2(i);  
        }
    cout<<"start traj pt: "<<endl;
    for (int i=0;i<14;i++) {
        cout<<trajectory_point.positions[i]<<", ";
    }
    cout<<endl;
      trajectory_point.time_from_start = ros::Duration(0.0); // start time set to 0
    // PUSH IN THE START POINT:
      des_trajectory.points.push_back(trajectory_point);            

    // compute and append the goal point, in joint space trajectory:
    des_gripper_affine1_ = affine_lcamera_to_psm_one_.inverse()*des_gripper1_affine_wrt_lcamera_;
    ROS_INFO("desired gripper one location in base frame: ");
    cout<<"gripper1 desired pose;  "<<endl;
    cout<<des_gripper_affine1_.linear()<<endl;
    cout<<"origin: "<<des_gripper_affine1_.translation().transpose()<<endl;

    ik_solver_.ik_solve(des_gripper_affine1_); //convert desired pose into equiv joint displacements
    q_vec1 = ik_solver_.get_soln(); 
    q_vec1(6) = gripper_ang1_; // include desired gripper opening angle

    des_gripper_affine2_ = affine_lcamera_to_psm_two_.inverse()*des_gripper2_affine_wrt_lcamera_;
    ik_solver_.ik_solve(des_gripper_affine2_); //convert desired pose into equiv joint displacements
    q_vec2 = ik_solver_.get_soln();  
    cout<<"q_vec1 of goal pose: "<<endl;
    for (int i=0;i<6;i++) {
        cout<<q_vec1[i]<<", ";
    }
    cout<<endl;
    q_vec2(6) = gripper_ang2_;
        for (int i=0;i<7;i++) {
            trajectory_point.positions[i] = q_vec1(i);
            trajectory_point.positions[i+7] = q_vec2(i);  
        }
      trajectory_point.time_from_start = ros::Duration(arrival_time_);
   cout<<"goal traj pt: "<<endl;
    for (int i=0;i<14;i++) {
        cout<<trajectory_point.positions[i]<<", ";
    }
    cout<<endl;
      des_trajectory.points.push_back(trajectory_point);

    js_goal_.trajectory = des_trajectory;

    // Need boost::bind to pass in the 'this' pointer
  // see example: http://library.isr.ist.utl.pt/docs/roswiki/actionlib_tutorials%282f%29Tutorials%282f%29Writing%2820%29a%2820%29Callback%2820%29Based%2820%29Simple%2820%29Action%2820%29Client.html
  //  ac.sendGoal(goal,
  //              boost::bind(&MyNode::doneCb, this, _1, _2),
  //              Client::SimpleActiveCallback(),
  //              Client::SimpleFeedbackCallback());

    js_action_client_.sendGoal(js_goal_, boost::bind(&CartMoveActionServer::js_doneCb_,this,_1,_2)); // we could also name additional callback functions here, if desired
    //    action_client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb); //alt--more callback funcs possible
    
    double t_timeout=arrival_time_+2.0; //wait 2 sec longer than expected duration of move
    
    bool finished_before_timeout = js_action_client_.waitForResult(ros::Duration(t_timeout));
    //bool finished_before_timeout = action_client.waitForResult(); // wait forever...
    if (!finished_before_timeout) {
        ROS_WARN("joint-space interpolation result is overdue ");
    } else {
        ROS_INFO("finished before timeout");
    }

    ROS_INFO("completed callback" );
    cart_move_as_.setSucceeded(cart_result_); // tell the client that we were successful acting on the request, and return the "result" message 
    
    //let's remember the last pose commanded, so we can use it as start pose for next move
    gripper1_affine_last_commanded_pose_ = des_gripper1_affine_wrt_lcamera_;
    gripper2_affine_last_commanded_pose_ = des_gripper2_affine_wrt_lcamera_;    
    //and the jaw opening angles:
    last_gripper_ang1_=gripper_ang1_;
    last_gripper_ang2_=gripper_ang2_;
}
예제 #16
0
	/** 
	 * Test if the delta of the provided transformations is greater in 
	 * either distance or angle than the threshold.
	 *
	 * @param a2b the initial transformation from A to B
	 * @param aprime2b the next transformation from A' to B
	 *
	 * @result true if the transformation A' to A is greater than the stored thresholds
	 */
	bool test( const Eigen::Affine3d& a2b, const Eigen::Affine3d& aprime2b )
	{
	    return test( a2b.inverse() * aprime2b );
	}
예제 #17
0
int do_inits() {
    Eigen::Matrix3d R;
    Eigen::Vector3d nvec, tvec, bvec, tip_pos;
    bvec << 0, 0, 1; //default for testing: gripper points "down"
    nvec << 1, 0, 0;
    tvec = bvec.cross(nvec);
    R.col(0) = nvec;
    R.col(1) = tvec;
    R.col(2) = bvec;
    g_des_gripper_affine1.linear() = R;
    tip_pos << -0.15, -0.03, 0.07;
    g_des_gripper_affine1.translation() = tip_pos; //will change this, but start w/ something legal

    //hard-coded camera-to-base transform, useful for simple testing/debugging
    g_affine_lcamera_to_psm_one.translation() << -0.155, -0.03265, 0.0;
    nvec << -1, 0, 0;
    tvec << 0, 1, 0;
    bvec << 0, 0, -1;
    //Eigen::Matrix3d R;
    R.col(0) = nvec;
    R.col(1) = tvec;
    R.col(2) = bvec;
    g_affine_lcamera_to_psm_one.linear() = R;

    g_q_vec1_start.resize(7);
    g_q_vec1_goal.resize(7);
    g_q_vec2_start.resize(7);
    g_q_vec2_goal.resize(7);

    g_des_gripper1_wrt_base = g_affine_lcamera_to_psm_one.inverse() * g_des_gripper_affine1;
    g_ik_solver.ik_solve(g_des_gripper1_wrt_base); // compute IK:
    g_q_vec1_goal = g_ik_solver.get_soln();
    g_q_vec1_start = g_q_vec1_goal;
    g_q_vec2_start << 0, 0, 0, 0, 0, 0, 0; //just put gripper 2 at home position
    g_q_vec2_goal << 0, 0, 0, 0, 0, 0, 0;
    //repackage q's into a trajectory;
    //populate a goal message for action server; 
    // initialize with 2 poses that are identical
    g_trajectory_point1.positions.clear();
    g_trajectory_point2.positions.clear();
    //resize these:
    for (int i=0;i<14;i++)  {
        g_trajectory_point1.positions.push_back(0.0); 
        g_trajectory_point2.positions.push_back(0.0); 
    }
        
    for (int i = 0; i < 7; i++) { 
        g_trajectory_point1.positions[i] = g_q_vec1_start[i];
        g_trajectory_point1.positions[i + 7] = g_q_vec2_start[i];
        //should fix up jaw-opening values...do this later
    }

    g_trajectory_point1.time_from_start = ros::Duration(0.0);
    g_trajectory_point2.time_from_start = ros::Duration(2.0);


    g_des_trajectory.points.clear(); // can clear components, but not entire trajectory_msgs
    g_des_trajectory.joint_names.clear();
    //    des_trajectory.joint_names.push_back("right_s0"); //yada-yada should fill in names
    g_des_trajectory.header.stamp = ros::Time::now();

    g_des_trajectory.points.push_back(g_trajectory_point1); // first point of the trajectory 
    g_des_trajectory.points.push_back(g_trajectory_point2);

    // copy traj to goal:
    g_goal.trajectory = g_des_trajectory;
    g_got_new_pose = true; //send robot to start pose
    
     ROS_INFO("getting transforms from camera to PSMs");
    tf::TransformListener tfListener;
    tf::StampedTransform tfResult_one, tfResult_two;
    bool tferr = true;
    int ntries = 0;
    ROS_INFO("waiting for tf between base and right_hand...");
    while (tferr) {
        if (ntries > 5) break; //give up and accept default after this many tries
        tferr = false;
        try {
            //The direction of the transform returned will be from the target_frame to the source_frame. 
            //Which if applied to data, will transform data in the source_frame into the target_frame. See tf/CoordinateFrameConventions#Transform_Direction
            tfListener.lookupTransform("left_camera_optical_frame", "one_psm_base_link", ros::Time(0), tfResult_one);
            //tfListener.lookupTransform("left_camera_optical_frame", "two_psm_base_link", ros::Time(0), tfResult_two);
        } catch (tf::TransformException &exception) {
            ROS_WARN("%s", exception.what());
            tferr = true;
            ros::Duration(0.5).sleep(); // sleep for half a second
            ros::spinOnce();
            ntries++;
        }
    }
    //default transform: need to match this up to camera calibration!
    if (tferr) {
        g_affine_lcamera_to_psm_one.translation() << -0.155, -0.03265, 0.0;
        Eigen::Vector3d nvec, tvec, bvec;
        nvec << -1, 0, 0;
        tvec << 0, 1, 0;
        bvec << 0, 0, -1;
        Eigen::Matrix3d R;
        R.col(0) = nvec;
        R.col(1) = tvec;
        R.col(2) = bvec;
        g_affine_lcamera_to_psm_one.linear() = R;
        g_affine_lcamera_to_psm_one.linear() = R;
        g_affine_lcamera_to_psm_one.translation() << 0.145, -0.03265, 0.0;
        ROS_WARN("using default transform");
    } else {

        ROS_INFO("tf is good");

        //affine_lcamera_to_psm_one is the position/orientation of psm1 base frame w/rt left camera link frame
        // need to extend this to camera optical frame
        g_affine_lcamera_to_psm_one = transformTFToEigen(tfResult_one);
        //affine_lcamera_to_psm_two = transformTFToEigen(tfResult_two);
    }
    ROS_INFO("transform from left camera to psm one:");
    cout << g_affine_lcamera_to_psm_one.linear() << endl;
    cout << g_affine_lcamera_to_psm_one.translation().transpose() << endl;
    //ROS_INFO("transform from left camera to psm two:");
    //cout << affine_lcamera_to_psm_two.linear() << endl;
    //cout << affine_lcamera_to_psm_two.translation().transpose() << endl;    
    
    
    ROS_INFO("done w/ inits");

    return 0;
    
}
    // This function is responsible for converting the data from the
    // kinect into an odometry message, and tranforming it to the same
    // frame that the ekf uses    
    void send_kinect_estimate(geometry_msgs::PointStamped pt,
			      geometry_msgs::PointStamped ptlast,
			      int index, int op)
	{
	    ROS_DEBUG("send_kinect_estimate triggered");

	    Eigen::Affine3d gwo;
	    Eigen::Vector3d tmp_point; 
	    tf::Transform transform;
	    geometry_msgs::PointStamped transpt, transptlast;
	    ros::Time tstamp = pt.header.stamp;
	    

	    // set transform parameters
	    transform.setOrigin(tf::Vector3(cal_pos(0),
					    cal_pos(1), cal_pos(2)));
	    transform.setRotation(tf::Quaternion(0,0,0,1));

	    // convert to Eigen:
	    tf::TransformTFToEigen(transform, gwo);
	    gwo = gwo.inverse();

	    tmp_point << pt.point.x, pt.point.y, pt.point.z;
	    tmp_point = gwo*tmp_point;
	    transpt.header.frame_id = "optimization_frame";
	    transpt.header.stamp = tstamp;
	    transpt.point.x = tmp_point(0);
	    transpt.point.y = tmp_point(1);
	    transpt.point.z = tmp_point(2);

	    tmp_point << ptlast.point.x, ptlast.point.y, ptlast.point.z;
	    tmp_point = gwo*tmp_point;
	    transptlast.header.frame_id = "optimization_frame";
	    transptlast.header.stamp = tstamp;
	    transptlast.point.x = tmp_point(0);
	    transptlast.point.y = tmp_point(1);
	    transptlast.point.z = tmp_point(2);
	    
	    ROS_DEBUG("Done transforming PointStamped messages");
	    // Let's first get the transform from /optimization_frame
	    // to /map
	    tf::StampedTransform trans_stamped;
	    geometry_msgs::PointStamped tmp;
	    try{
	    	tf.lookupTransform(
	    	    "map", "optimization_frame",
	    	    tstamp, trans_stamped);
	    	tf.transformPoint("map", transpt, tmp);

	    }
	    catch(tf::TransformException& ex){
	    	ROS_ERROR(
	    	    "Error trying to lookupTransform from /map "
	    	    "to /optimization_frame: %s", ex.what());
	    	return;
	    }
	    
	    // Now we can publish the Kinect's estimate of the robot's
	    // pose
	    std::stringstream ss;
	    ss << "base_footprint_kinect_robot_" << index;
	    kin_pose[index].header.stamp = ros::Time::now();
	    kin_pose[index].header.frame_id = "map";
	    kin_pose[index].child_frame_id = ss.str();
	    tmp.point.z = 0.0;
	    kin_pose[index].pose.pose.position = tmp.point;
	    double theta = 0.0;
	    if (op == 2)
	    {
	    	theta = atan2(transpt.point.x-
	    			     transptlast.point.x,
	    			     transpt.point.z-
	    			     transptlast.point.z);
	    	theta = clamp_angle(theta-M_PI/2.0);
		ROS_DEBUG("Calculated angle = %f",theta);					  
	    }
	    else
	    {
	    	theta = robot_start_ori[index];
	    	theta = clamp_angle(-theta);
		ROS_DEBUG("predetermined angle = %f",theta);
	    }
	    geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(theta);
	    kin_pose[index].pose.pose.orientation = quat;

	    // Let's check if this is a "bad" point
	    if (bad_array[index])
	    {
		boost::array<double,36ul> tmpcov;
		for (int i=0; i<36; i++)
		    tmpcov[i] = kincov[i]*1000.0;
		kin_pose[index].pose.covariance = tmpcov;
	    }
	    else
		kin_pose[index].pose.covariance = kincov;
	    
	    ROS_DEBUG("Done filling in Odometry message");

	    // Now let's publish the estimated pose as a
	    // nav_msgs/Odometry message on a topic called /vo
	    ROS_DEBUG("publishing /vo for robot %d", index+1);
	    robots_pub[index].publish(kin_pose[index]);

	    // now, let's publish the transforms that goes along with it
	    geometry_msgs::TransformStamped kin_trans;
	    tf::Quaternion q1, q2;
	    q1 = tf::createQuaternionFromYaw(theta);
	    q2 = tf::Quaternion(1.0,0,0,0);
	    q1 = q1*q2;
	    tf::quaternionTFToMsg(q1, quat);

	    kin_trans.header.stamp = tstamp;
	    kin_trans.header.frame_id = kin_pose[index].header.frame_id;
	    kin_trans.child_frame_id = kin_pose[index].child_frame_id;
	    kin_trans.transform.translation.x = kin_pose[index].pose.pose.position.x;
	    kin_trans.transform.translation.y = kin_pose[index].pose.pose.position.y;
	    kin_trans.transform.translation.z = kin_pose[index].pose.pose.position.z;
	    kin_trans.transform.rotation = quat;

	    ROS_DEBUG("Sending transform for output of estimator node");
	    br.sendTransform(kin_trans);
	    
	    return;
	}
예제 #19
0
    pcl::PointCloud<pcl::PointNormal>::Ptr TSDFVolumeOctree::renderView (const Eigen::Affine3d& trans, int downsampleBy) const {
        int new_width = image_width_ / downsampleBy;
        int new_height = image_height_ / downsampleBy;
        double new_fx = focal_length_x_ / downsampleBy;
        double new_fy = focal_length_y_ / downsampleBy;
        double new_cx = principal_point_x_ / downsampleBy;
        double new_cy = principal_point_y_ / downsampleBy;
        pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal> (new_width, new_height));
        cloud->is_dense = false;

        float min_step = max_dist_neg_ * 3/4.;
    #pragma omp parallel for
        for (size_t i = 0; i < cloud->size (); ++i) {
            size_t x = i % new_width;
            size_t y = i / new_width;
            //Raytrace
            pcl::PointNormal& pt = cloud->operator() (x,y);
            bool found_crossing = false;
            Eigen::Vector3f du ( (x - new_cx)/new_fx,
                                 (y - new_cy)/new_fy,
                                 1);
            du.normalize ();
            //Apply transform -- rotate the ray, start at the offset
            du = trans.rotation ().cast<float> () * du;
            Eigen::Vector3f p = trans.translation ().cast<float> ();
            //Preallocate
            int x_i, y_i, z_i;
            float d;
            float w;
            float last_w = 0;
            float last_d = 0;
            float t = min_sensor_dist_;
            p += t*du;
            float step = min_step;
            //Check if ray intersects cube
            //Initially we haven't ever hit a voxel
            bool hit_voxel = false;
            int niter = 0;
            while (t < max_sensor_dist_) {
                const OctreeNode* voxel = octree_->getContainingVoxel (p (0), p(1), p(2));
                if (voxel) {
                    hit_voxel = true;
                    voxel->getData (d, w);
                    if (((d < 0 && last_d > 0) || (d > 0 && last_d < 0)) && last_w && w) {
                        found_crossing = true;
                        float old_t = t - step;
                        step = (zsize_/zres_)/2;
                        float new_d;
                        float new_w;
                        float last_new_d = d;
                        float last_new_w = w;
                        while (t >= old_t) {
                            t -= step;
                            p -= step*du;
                            voxel = octree_->getContainingVoxel (p (0), p(1), p(2));
                            if (!voxel)
                                break;
                            voxel->getData (new_d, new_w);
                            if ((last_d > 0 && new_d > 0) || (last_d < 0 && new_d < 0)) {
                                last_d = new_d;
                                last_w = new_w;
                                d = last_new_d;
                                w = last_new_w;
                                t += step;
                                p += step*du;
                                break;
                            }
                            last_new_d = d;
                            last_new_w = w;
                        }
                        break;
                    }
                    last_d = d;
                    last_w = w;
                    //Update step
                    step = std::max (voxel->getMinSize () / 4., fabs (d)*max_dist_neg_);
                }
                else {
                    if (hit_voxel)
                        break;
                }
                t += step;
                p += step*du;
                niter++;
            }
            if (!found_crossing) {
                pt.x = pt.y = pt.z =std::numeric_limits<float>::quiet_NaN();
            }
            else {
                //Get optimal t
                bool has_data = true;
                float tcurr = t;
                float tprev = t-step;
                last_d = getTSDFValue (trans.translation ().cast<float> () + (tprev) * du,
                                       &has_data);
                d = getTSDFValue (trans.translation ().cast<float> () + tcurr * du, &has_data);
                if (!has_data || pcl_isnan (d) || pcl_isnan (last_d)) {
                    pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
                }
                float t_star = t + step * (-1 + fabs (last_d / (last_d -d)));
                pt.getVector3fMap () = trans.translation().cast<float> () + tcurr * du;
                //Get normals by looking at adjacent voxels
                const OctreeNode* voxel = octree_->getContainingVoxel (pt.x, pt.y, pt.z);
                if (!voxel) {
                    pt.normal_x = pt.normal_y = pt.normal_z = std::numeric_limits<float>::quiet_NaN();
                    continue;
                }
                float xsize, ysize, zsize;
                voxel->getSize (xsize, ysize, zsize);
                bool valid = true;
                float d_xm = getTSDFValue (pt.x-xsize, pt.y, pt.z, &valid);
                float d_xp = getTSDFValue (pt.x+xsize, pt.y, pt.z, &valid);
                float d_ym = getTSDFValue (pt.x, pt.y-ysize, pt.z, &valid);
                float d_yp = getTSDFValue (pt.x, pt.y+ysize, pt.z, &valid);
                float d_zm = getTSDFValue (pt.x, pt.y, pt.z-zsize, &valid);
                float d_zp = getTSDFValue (pt.x, pt.y, pt.z+zsize, &valid);
                if (!valid) {
                    pt.normal_x = pt.normal_y = pt.normal_z = std::numeric_limits<float>::quiet_NaN();
                    continue;
                }
                Eigen::Vector3f dF;
                dF (0) = (d_xp - d_xm)*max_dist_neg_/ (2*xsize);
                dF (1) = (d_yp - d_ym)*max_dist_neg_/ (2*ysize);
                dF (2) = (d_zp - d_zm)*max_dist_neg_/ (2*zsize);
                dF.normalize();
                pt.normal_x = dF(0);
                pt.normal_y = dF(1);
                pt.normal_z = dF(2);
            }
        }
        pcl::transformPointCloudWithNormals (*cloud, *cloud, trans.inverse());
        return (cloud);
    }