Пример #1
1
visualization_msgs::Marker SatDetector3DMonitor::getMarker(int marker_id, Eigen::Vector4f centroid, Eigen::Matrix3f covariance_matrix){

  //------- Compute Principal Componets for Marker publishing


  //Get the principal components and the quaternion
  Eigen::Matrix3f evecs;
  Eigen::Vector3f evals;
  //pcl::eigen33 (covariance_matrix, evecs, evals);
  Eigen::EigenSolver<Eigen::Matrix3f> es(covariance_matrix);
	
  evecs = es.eigenvectors().real();
  evals = es.eigenvalues().real();
	    
  Eigen::Matrix3f rotation;
  rotation.row (0) = evecs.col (0);
  rotation.row (1) = evecs.col (1);
  rotation.row (2) = rotation.row (0).cross (rotation.row (1));
	    
  rotation.transposeInPlace ();
  Eigen::Quaternion<float> qt (rotation);
  qt.normalize ();
	    
  //Publish Marker for cluster
  visualization_msgs::Marker marker;	
		
  marker.header.frame_id = base_frame_;
  marker.header.stamp = ros::Time().now();
  marker.ns = "Perception";
  marker.action = visualization_msgs::Marker::ADD;
  marker.id = marker_id;	
  marker.lifetime = ros::Duration(1);	
		
  //centroid position
  marker.type = visualization_msgs::Marker::SPHERE;
  marker.pose.position.x = centroid[0];
  marker.pose.position.y = centroid[1];
  marker.pose.position.z = centroid[2];	
  marker.pose.orientation.x = qt.x();
  marker.pose.orientation.y = qt.y();
  marker.pose.orientation.z = qt.z();
  marker.pose.orientation.w = qt.w();			

  //std::cout << "e1: " << evals(0) << " e2: " << evals(1) << " e3: " << evals(2) << std::endl;

  marker.scale.x = sqrt(evals(0)) * 4;
  marker.scale.y = sqrt(evals(1)) * 4;
  marker.scale.z = sqrt(evals(2)) * 4;
	

  //give it some color!
  marker.color.a = 0.75;
  satToRGB(marker.color.r, marker.color.g, marker.color.b);

  //std::cout << "marker being published" << std::endl;

  return marker;
}
Пример #2
1
 // returns the local R,t in nd0 that produces nd1
 // NOTE: returns a postfix rotation; should return a prefix
 void transformN2N(Eigen::Matrix<double,4,1> &trans, 
                   Eigen::Quaternion<double> &qr,
                   Node &nd0, Node &nd1)
 {
   Matrix<double,3,4> tfm;
   Quaterniond q0,q1;
   q0 = nd0.qrot;
   transformW2F(tfm,nd0.trans,q0);
   trans.head(3) = tfm*nd1.trans;
   trans(3) = 1.0;
   q1 = nd1.qrot;
   qr = q0.inverse()*q1;
   qr.normalize();
   if (qr.w() < 0)
     qr.coeffs() = -qr.coeffs();
 }
Пример #3
0
bool checkRange(const Eigen::Quaternion<DerivedA>& qq,
  double minVal, double maxVal)
{
    assert(minVal < maxVal);

    if (std::isnan(qq.w()) || std::isnan(qq.x()) || std::isnan(qq.y()) || std::isnan(qq.z()))
    {
        std::stringstream ss;
        ss << "Quaternion [" << qq.w() << ", " << qq.x() << ", " << qq.y() << ", " << qq.z()  << "] contains NaN values!\n";
        ROS_WARN_STREAM(ss.str());
        return false;
    }

    if (qq.w() > maxVal || qq.w() < minVal ||
        qq.x() > maxVal || qq.x() < minVal ||
        qq.y() > maxVal || qq.y() < minVal ||
        qq.z() > maxVal || qq.z() < minVal)
    {
        std::stringstream ss;
        ss << "Quaternion contains terms out of range:\n"
          << " - w: [" << qq.w() << ", " << qq.x() << ", " << qq.y() << ", " << qq.z()  << "]\n"
          << " - minVal: " << minVal << "\n"
          << " - maxVel: " << maxVal;
        ROS_WARN_STREAM(ss.str());
    }

    return true;
}
Пример #4
0
 void computeEr(const Eigen::Quaternion<double>& q, Eigen::MatrixXd& Er)
 {
   Er.resize(4, 3);
   Er << -q.x(), -q.y(), -q.z(),
          q.w(),  q.z(), -q.y(),
         -q.z(),  q.w(),  q.x(),
          q.y(), -q.x(),  q.w();
   Er = 0.5 * Er;
 }
Eigen::Matrix3f
motion_controller::qToRotation(Eigen::Quaternion<float> q)
{
    Eigen::Matrix3f temp;

    temp << 1-2*(q.y()*q.y()+q.z()*q.z()), 2*(q.x()*q.y()-q.w()*q.z())  , 2*(q.w()*q.y()+q.x()*q.z())  ,
            2*(q.x()*q.y()+q.w()*q.z())  , 1-2*(q.x()*q.x()+q.z()*q.z()), 2*(q.y()*q.z()-q.w()*q.x())  ,
            2*(q.x()*q.z()-q.w()*q.y())  , 2*(q.y()*q.z()+q.w()*q.x())  , 1-2*(q.x()*q.x()+q.y()*q.y());
    return temp;
}
Пример #6
0
void HLManager::start()
{
	ros::Rate rate(10);

	while (nh.ok())
	{
		if (fixValidated)
		{
			tf::Transform transform;
			transform.setOrigin(tf::Vector3(originLon, originLat, 0));
			transform.setRotation(tf::createQuaternionFromRPY(0,0,0));
			broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/worldLatLon", "/world"));
			transform.setOrigin(tf::Vector3(0, 0, 0));
			Eigen::Quaternion<float> q;
			labust::tools::quaternionFromEulerZYX(M_PI,0,M_PI/2,q);
			transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
			broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "local"));
		}
		this->safetyTest();
		this->step();
		hlMessagePub.publish(hlDiagnostics);
		rate.sleep();
		ros::spinOnce();
	}
}
Пример #7
0
void HLManager::onVTTwist(const geometry_msgs::TwistStamped::ConstPtr& twist)
{
	if (mode == circle || mode == vtManual)
	{
		//\todo Generalize this 0.1 with Ts.
		s +=twist->twist.linear.x*0.1;

		//Circle
		if (s>=2*circleRadius*M_PI) s=s-2*circleRadius*M_PI;
		else if (s<0) s=2*circleRadius*M_PI-s;

		double xRabbit = point.point.x + circleRadius*cos(s/circleRadius);
		double yRabbit = point.point.y + circleRadius*sin(s/circleRadius);
		double gammaRabbit=labust::math::wrapRad(s/circleRadius)+M_PI/2;

		tf::Transform transform;
		transform.setOrigin(tf::Vector3(xRabbit, yRabbit, 0));
		Eigen::Quaternion<float> q;
		labust::tools::quaternionFromEulerZYX(0,0,gammaRabbit, q);
		transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
		broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local", "serret_frenet_frame"));

		auv_msgs::NavStsPtr msg(new auv_msgs::NavSts());
		msg->position.north = xRabbit;
		msg->position.east = yRabbit;
		msg->body_velocity.x = twist->twist.linear.x;
		msg->orientation.yaw = gammaRabbit;
		sfPub.publish(msg);
	}
}
Пример #8
0
    SE3<> IMU2camWorldfromQuat(Eigen::Quaternion<double> atti){// use ini attitude info from EKF node to ini ptam pose
        Vector<4> attivec = makeVector(atti.w(), atti.x(), atti.y(), atti.z());//Rw1i
        Matrix<3> iniOrientationEKF;
        iniOrientationEKF = tag::quaternionToMatrix(attivec);

        Matrix<3> roll = TooN::Data(1.0, 0, 0,//Rww1, because the roll and pitch angles are in
                              0, -1, 0, // a world frame which pointing downward.
                              0, 0, -1);

        SE3<> camWorld = SE3<>();
        Matrix<3> rotation;
        if (tracker_->attitude_get)
            rotation = iniOrientationEKF; //
        else
            rotation = roll * iniOrientationEKF;//Rwi = Rww1*Rw1i
        camWorld.get_rotation() = rotation*se3IMUfromcam.get_rotation().get_matrix();//Rwc = (Rwi * Ric)

        Vector<3> twr = makeVector(0.0, 0.0, 0.198);// twc = twr + Rwr * trc
        Vector<3> twc = twr + rotation * se3IMUfromcam.get_translation();
        camWorld.get_translation()[0] = 0.0;//twc[0]; //twc
        camWorld.get_translation()[1] = 0.0;//twc[1];
        camWorld.get_translation()[2] = twc[2];

        camWorld = camWorld.inverse();//Tcw

        cout<< "TCW INITIALIZED. TWC: " << twc[0]<< ", " << twc[1]<< ", " << twc[2]<<endl;
//        cout<< camWorld.get_rotation().get_matrix()(2,2)<<endl;
        return camWorld;
    }
bool CloudMatcher::match(modular_cloud_matcher::MatchClouds::Request& req, modular_cloud_matcher::MatchClouds::Response& res)
{
	// get and check reference
	size_t referenceGoodCount;
	DP referenceCloud(rosMsgToPointMatcherCloud(req.reference, referenceGoodCount));
	const unsigned referencePointCount(req.reference.width * req.reference.height);
	const double referenceGoodRatio(double(referenceGoodCount) / double(referencePointCount));
	
	if (referenceGoodCount == 0)
	{
		ROS_ERROR("I found no good points in the reference cloud");
		return false;
	}
	if (referenceGoodRatio < 0.5)
	{
		ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - referenceGoodRatio*100.0 << "% of the cloud (received " << referenceGoodCount << ")");
	}
	
	// get and check reading
	size_t readingGoodCount;
	DP readingCloud(rosMsgToPointMatcherCloud(req.readings, readingGoodCount));
	const unsigned readingPointCount(req.readings.width * req.readings.height);
	const double readingGoodRatio(double(readingGoodCount) / double(readingPointCount));
	
	if (readingGoodCount == 0)
	{
		ROS_ERROR("I found no good points in the reading cloud");
		return false;
	}
	if (readingGoodRatio < 0.5)
	{
		ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - readingGoodRatio*100.0 << "% of the cloud (received " << readingGoodCount << ")");
	}
	
	// call icp
	TP transform;
	try 
	{
		transform = icp(readingCloud, referenceCloud);
		ROS_INFO_STREAM("match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl);
	}
	catch (PM::ConvergenceError error)
	{
		ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
		return false;
	}
	
	// fill return value
	res.transform.translation.x = transform.coeff(0,3);
	res.transform.translation.y = transform.coeff(1,3);
	res.transform.translation.z = transform.coeff(2,3);
	const Eigen::Quaternion<Scalar> quat(Matrix3(transform.block(0,0,3,3)));
	res.transform.rotation.x = quat.x();
	res.transform.rotation.y = quat.y();
	res.transform.rotation.z = quat.z();
	res.transform.rotation.w = quat.w();
	
	return true;
}
Пример #10
0
void quaternion2vector(const Eigen::Quaternion<T_qt> &quat_In, std::vector<T_vec> &quat_Vec )
{
    quat_Vec.resize(4,T_vec(0.0));
    quat_Vec[0] = T_vec( quat_In.w() );
    quat_Vec[1] = T_vec( quat_In.x() );
    quat_Vec[2] = T_vec( quat_In.y() );
    quat_Vec[3] = T_vec( quat_In.z() );
};
Пример #11
0
void YouBot::updateWheels(const ros::TimerEvent& e)
{
  try
  {
    if(q_base_.rows() != robot_->getMacroManipulatorDOF()) return;
    if(tau_base_.rows() != robot_->getMacroManipulatorDOF()) return;
    if(!gazebo_interface_wheel_->subscribed()) return;

    Eigen::VectorXd q = gazebo_interface_wheel_->getJointStates();
    robot_->updateWheel(q);

    Eigen::Vector3d base_pos;
    base_pos << q_base_[0], q_base_[1], 0.0;
    Eigen::Quaternion<double> base_ori;
    base_ori.x() = 0.0;
    base_ori.y() = 0.0;
    base_ori.z() = sin(0.5 * q_base_[2]);
    base_ori.w() = cos(0.5 * q_base_[2]);

    robot_->updateBase(base_pos, base_ori);

    Eigen::VectorXd v_base;
    controller_->computeBaseVelocityFromTorque(tau_base_, v_base, 3);

    Eigen::VectorXd v_wheel;
    controller_->computeWheelVelocityFromBaseVelocity(v_base, v_wheel);

    Eigen::VectorXd q_wheel_d = robot_->getMobility()->update_rate * v_wheel;

    std::vector<Eigen::Quaternion<double> > quat_d;
    for(unsigned int i = 0; i < q_wheel_d.rows(); ++i)
    {
      double rad = q_wheel_d[i];

      Eigen::Quaternion<double> quat;
      quat.x() = 0.0;
      quat.y() = sin(0.5 * rad);
      quat.z() = 0.0;
      quat.w() = cos(0.5 * rad);

      quat_d.push_back(quat);
    }

    gazebo_interface_wheel_->rotateLink(quat_d);
  }
  catch(ahl_robot::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
  catch(ahl_ctrl::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
}
Пример #12
0
    void rpyToQuaternion(const Eigen::Vector3d& rpy, Eigen::Quaternion<double>& q)
    {
      double a = rpy.coeff(0);
      double b = rpy.coeff(1);
      double g = rpy.coeff(2);

      double sin_b_half = sin(0.5 * b);
      double cos_b_half = cos(0.5 * b);
      double diff_a_g_half = 0.5 * (a - g);
      double sum_a_g_half = 0.5 * (a + g);

      q.x() = sin_b_half * cos(diff_a_g_half);
      q.y() = sin_b_half * sin(diff_a_g_half);
      q.z() = cos_b_half * sin(sum_a_g_half);
      q.w() = cos_b_half * cos(sum_a_g_half);
    }
Пример #13
0
base::Vector6d AuvMotion::gravity_buoyancy(const Eigen::Quaternion<double> q)
{
    base::Vector6d gravitybuoyancy;
  
    double mass;
    control->nodes->getNodeMass(vehicle_id ,&mass);
    double buoyancy = _buoyancy_force;
    base::Vector3d pos = control->nodes->getPosition(vehicle_id);
  
    // In Quaternion form
    float e1 = q.x();
    float e2 = q.y();
    float e3 = q.z();
    float e4 = q.w();
    float xg = 0.0;
    float yg = 0.0;
    float zg = 0.0;
    float xb = centerOfBuoyancy[0];
    float yb = centerOfBuoyancy[1];
    float zb = centerOfBuoyancy[2];;

    gravitybuoyancy(0) = 0.0;
    gravitybuoyancy(1) = 0.0;
    
    if(pos[2] + centerOfBuoyancy[2] < 0.0){ //The vehicle is completly under water
      gravitybuoyancy(2) = buoyancy - mass;
    }else if(pos[2] < 0.0 && centerOfBuoyancy[2] != 0){ //The vehicle is partwise underwater -> apply buoyancy partwise
      gravitybuoyancy(2) = (buoyancy * (-pos[2]/ centerOfBuoyancy[2] ) ) - mass;
    }else{ //The vehicle is over the surface -> no buoyancy
      gravitybuoyancy(2) = -mass;
    }
    
    //Angular buoancy forces
    gravitybuoyancy(3) = ((-(e4 * e4)+(e1 * e1)+(e2 * e2)-(e3 * e3))*((yg*mass)-(yb*buoyancy)))+(2*((e4*e1)+(e2*e3))*((zg*mass)-(zb*buoyancy)));
    gravitybuoyancy(4) =-((-(e4 * e4)+(e1 * e1)+(e2 * e2)-(e3 * e3))*((xg*mass)-(xb*buoyancy)))+(2*((e4*e2)-(e1*e3))*((zg*mass)-(zb*buoyancy)));
    gravitybuoyancy(5) =-(2*((e4*e1)+(e2*e3))*((xg*mass)-(xb*buoyancy)))-(2*((e4*e2)-(e1*e3))*((yg*mass)-(yb*buoyancy)));
    
    //Convert angular forces to world frame
    gravitybuoyancy.block<3,1>(3,0) = q * gravitybuoyancy.block<3,1>(3,0);  
    
    return gravitybuoyancy;
}
Пример #14
0
void publishNode(unsigned int index, Eigen::Matrix<double,4,1> trans, 
                Eigen::Quaternion<double> fq,
                const frame_common::CamParams &cp,
                bool isFixed, sba::CameraNode &msg)
{ 
    msg.index = index;
    
    msg.transform.translation.x = trans.x();
    msg.transform.translation.y = trans.y();
    msg.transform.translation.z = trans.z();
    msg.transform.rotation.x = fq.x();
    msg.transform.rotation.y = fq.y();
    msg.transform.rotation.z = fq.z();
    msg.transform.rotation.w = fq.w();
    msg.fx = cp.fx;
    msg.fy = cp.fy;
    msg.cx = cp.cx;
    msg.cy = cp.cy;
    msg.baseline = cp.tx;
    msg.fixed = isFixed;
}
Пример #15
0
    bool operator()(T const* const* parameters, T* residuals) const {
        // Map spline
        UniformSpline<T> spline(spline_dt_, spline_offset_);
        for (size_t i=0; i < num_knots_; ++i) {
            spline.add_knot((T*) &parameters[i][0]);
        }

        Eigen::Matrix<T, 3, 1> landmark(T(5.0), T(1.0), T(3.0));
        Eigen::Matrix<T, 3, 1> expected(T(0.0), T(0.0), T(1.0));


        // Distance to center
        Sophus::SE3Group<T> P;
        Eigen::Matrix<T, 4, 4> dP, d2P;
        for (size_t i=0; i < eval_times_.size(); ++i) {
            spline.evaluate(T(eval_times_[i]), P, dP, d2P);
            Eigen::Matrix<T, 3, 1> X_spline = P.inverse() * landmark;
            Eigen::Matrix<T, 3, 1> diff = X_spline - expected;
            residuals[3*i + 0] = diff(0);
            residuals[3*i + 1] = diff(1);
            residuals[3*i + 2] = diff(2);
        }

        const size_t poff = 3 * eval_times_.size();

        // Constant angular difference
        for (size_t i=1; i < eval_times_.size(); ++i) {
            SE3Group<T> P0, P1, delta;
            spline.evaluate(T(eval_times_[i-1]), P0, dP, d2P);
            spline.evaluate(T(eval_times_[i]), P1, dP, d2P);
            delta = P0.inverse() * P1;
            Eigen::Quaternion<T> q = delta.unit_quaternion();
            residuals[poff + 4*(i - 1) + 0] = T(5.0) * (q.w() - T(cos(0.5 * expected_angular_difference_)));
            residuals[poff + 4*(i - 1) + 1] = T(5.0) * (q.x() - T(sin(0.5 * expected_angular_difference_)));
            residuals[poff + 4*(i - 1) + 2] = T(5.0) * q.y(); //- 0;
            residuals[poff + 4*(i - 1) + 3] = T(5.0) * q.z(); //- 0;
        }

        return true;
    }
      bool base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf)
      {
        assert(q_urdf.size()==7);
        assert(q_sot.size()==6);
        // *********  RPY to Quat *********
        const double r = q_sot[3];
        const double p = q_sot[4];
        const double y = q_sot[5];
        const Eigen::AngleAxisd  rollAngle(r, Eigen::Vector3d::UnitX());
        const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
        const Eigen::AngleAxisd   yawAngle(y, Eigen::Vector3d::UnitZ());
        const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;

        q_urdf[0 ]=q_sot[0 ]; //BASE
        q_urdf[1 ]=q_sot[1 ];
        q_urdf[2 ]=q_sot[2 ];
        q_urdf[3 ]=quat.x();
        q_urdf[4 ]=quat.y();
        q_urdf[5 ]=quat.z();
        q_urdf[6 ]=quat.w();

        return true;
      }
  lwr_impedance_controller::CartImpTrajectoryPoint sampleInterpolation() {
    lwr_impedance_controller::CartImpTrajectoryPoint next_point;

    double timeFromStart =
        (double) (now() - trajectory_.header.stamp).toSec();
    double segStartTime = last_point_.time_from_start.toSec();
    double segEndTime =
        trajectory_.trajectory[trajectory_index_].time_from_start.toSec();

    next_point = setpoint_;

    // interpolate position
    // x
    next_point.pose.position.x = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.pose.position.x,
        trajectory_.trajectory[trajectory_index_].pose.position.x);
    // y
    next_point.pose.position.y = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.pose.position.y,
        trajectory_.trajectory[trajectory_index_].pose.position.y);
    // z
    next_point.pose.position.z = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.pose.position.z,
        trajectory_.trajectory[trajectory_index_].pose.position.z);

    // interpolate orientation

    Eigen::Quaternion<double> start = Eigen::Quaternion<double>(last_point_.pose.orientation.w,
        last_point_.pose.orientation.x, last_point_.pose.orientation.y,
        last_point_.pose.orientation.z);
    Eigen::Quaternion<double> end = Eigen::Quaternion<double>(
        trajectory_.trajectory[trajectory_index_].pose.orientation.w,
        trajectory_.trajectory[trajectory_index_].pose.orientation.x,
        trajectory_.trajectory[trajectory_index_].pose.orientation.y,
        trajectory_.trajectory[trajectory_index_].pose.orientation.z);

    double t = linearlyInterpolate(timeFromStart, segStartTime, segEndTime, 0,
        1);

    Eigen::Quaternion<double> rot = start.slerp(t, end);

    next_point.pose.orientation.x = rot.x();
    next_point.pose.orientation.y = rot.y();
    next_point.pose.orientation.z = rot.z();
    next_point.pose.orientation.w = rot.w();


    /*
     // x
     next_point.pose.orientation.x = linearlyInterpolate(timeFromStart,
     segStartTime, segEndTime, last_point_.pose.orientation.x,
     trajectory_.trajectory[trajectory_index_].pose.orientation.x);
     // y
     next_point.pose.orientation.y = linearlyInterpolate(timeFromStart,
     segStartTime, segEndTime, last_point_.pose.orientation.y,
     trajectory_.trajectory[trajectory_index_].pose.orientation.y);
     // z
     next_point.pose.orientation.z = linearlyInterpolate(timeFromStart,
     segStartTime, segEndTime, last_point_.pose.orientation.z,
     trajectory_.trajectory[trajectory_index_].pose.orientation.z);
     // w
     next_point.pose.orientation.w = linearlyInterpolate(timeFromStart,
     segStartTime, segEndTime, last_point_.pose.orientation.w,
     trajectory_.trajectory[trajectory_index_].pose.orientation.w);
     */
    //interpolate stiffness
    // x
    next_point.impedance.stiffness.force.x = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.impedance.stiffness.force.x,
        trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.x);

    next_point.impedance.stiffness.force.y = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.impedance.stiffness.force.y,
        trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.y);

    next_point.impedance.stiffness.force.z = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.impedance.stiffness.force.z,
        trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.z);

    next_point.impedance.stiffness.torque.x = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.impedance.stiffness.torque.x,
        trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.x);

    next_point.impedance.stiffness.torque.y = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.impedance.stiffness.torque.y,
        trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.y);

    next_point.impedance.stiffness.torque.z = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.impedance.stiffness.torque.z,
        trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.z);

    next_point.impedance.damping =
        trajectory_.trajectory[trajectory_index_].impedance.damping;
    next_point.wrench = trajectory_.trajectory[trajectory_index_].wrench;

    return next_point;
  }
Пример #18
0
  void get3DMoments(vector<Point> feature_points, int feat_index, int index)
  {
    //    for(int i=0; i<feature_points.size(); i++)
    //   ROS_INFO("%d --> %d,%d",i,feature_points[i].x,feature_points[i].y);
    //ROS_INFO("Getting 3D Moments : %d --> %d,%d", feature_points.size(), width, height);
    
    //Extract the indices for the points in the point cloud data
    pcl::PointIndices point_indices;
     
    for(int i=0; i<feature_points.size(); i++)
      {
	//ROS_INFO("Feature Index : %d, %d",feature_points[i].x, feature_points[i].y);
	point_indices.indices.push_back(feature_points[i].y * width + feature_points[i].x);
      }
    
    //ROS_INFO("Computing 3D Centroid : %d",point_indices.indices.size());
    Eigen::Vector4f centroid;
    Eigen::Matrix3f covariance_matrix;
    
    // Estimate the XYZ centroid
    pcl::compute3DCentroid (pcl_in, point_indices, centroid); 
#ifdef DEBUG
    ROS_INFO("Centroid %d: %f, %f, %f, %f",index,centroid(0),centroid(1),centroid(2),centroid(3));
#endif

    //ROS_INFO("Computing Covariance ");
    //Compute the centroid and the covariance of the points
    computeCovarianceMatrix(pcl_in, point_indices.indices, centroid, covariance_matrix);
    
    //Print the 3D Moments
    //ROS_INFO("Centroid : %f, %f, %f, %f",centroid(0),centroid(1),centroid(2),centroid(3));
#ifdef DEBUG
    std::cout<<"Covariance : "<<std::endl<<covariance_matrix <<std::endl;
#endif

    for(int i=0; i<3; i++)
      {
	feedback_.features[feat_index].moments[index].mean[i] = centroid(i);
	for(int j=0; j<3; j++)
	  {
	    feedback_.features[feat_index].moments[index].covariance[i*3+j] = covariance_matrix(i,j);
	  }
      }

    //Get the principal components and the quaternion
    Eigen::Matrix3f evecs;
    Eigen::Vector3f evals;
    pcl::eigen33 (covariance_matrix, evecs, evals);
    
    Eigen::Matrix3f rotation;
    rotation.row (0) = evecs.col (0);
    rotation.row (1) = evecs.col (1);
    //rotation.row (2) = evecs.col (2);
    rotation.row (2) = rotation.row (0).cross (rotation.row (1));
    //rotation.transposeInPlace ();
#ifdef DEBUG
    std::cerr << "Rotation matrix: " << endl;
    std::cerr << rotation << endl;
    std::cout<<"Eigen vals : "<<evals<<std::endl;
#endif

    rotation.transposeInPlace ();
    Eigen::Quaternion<float> qt (rotation);
    qt.normalize ();

    //Publish Marker
    visualization_msgs::Marker marker;	
    
    marker.header.frame_id = "/openni_rgb_optical_frame";
    marker.header.stamp = ros::Time().now();
    marker.ns = "Triangulation";
    marker.id = index+1;	
    marker.action = visualization_msgs::Marker::ADD;
    marker.lifetime = ros::Duration(5);		
    
    //centroid position
    marker.type = visualization_msgs::Marker::SPHERE;
    
    
    marker.pose.position.x = centroid(0);
    marker.pose.position.y = centroid(1);
    marker.pose.position.z = centroid(2);	
    marker.pose.orientation.x = qt.x();
    marker.pose.orientation.y = qt.y();
    marker.pose.orientation.z = qt.z();
    marker.pose.orientation.w = qt.w();			
    
    marker.scale.x = sqrt(evals(0)) *2;
    marker.scale.y =  sqrt(evals(1)) *2;
    marker.scale.z =  sqrt(evals(2)) *2;
    
    //red
    marker.color.a = 0.5;
    marker.color.r = rand()/((double)RAND_MAX + 1);
    marker.color.g = rand()/((double)RAND_MAX + 1);
    marker.color.b = rand()/((double)RAND_MAX + 1);
    object_pose_marker_pub_.publish(marker);	
    
  }
void
motion_controller::running(void)
{
    cycle_start = ros::Time::now();

//  receive path and when received block path receiver until mission completed
    if (m_path && !block_path_receiver)
    {
        nav_msgs::Path temp_path = *m_path;
        std::vector<geometry_msgs::PoseStamped> temp_path_vector;
        extracted_path.clear();
        for( std::vector<geometry_msgs::PoseStamped>::iterator itr = temp_path.poses.begin() ; itr != temp_path.poses.end(); ++itr)
        {
            temp_path_vector.push_back(*itr);
        }

        if(!temp_path_vector.empty())
        {
             ROS_INFO("Path Received!");
        }
        p_count++;

        while(!temp_path_vector.empty())
        {
            extracted_path.push_back(temp_path_vector.back());
            temp_path_vector.pop_back();
        }
        block_path_receiver = true;
    }

//  switch status according to the command
    switch (m_move)
    {
        // move forward 
        case FORWARD:
        {
            // v is linear speed and gain is angular velocity
            v =  sqrt(dist)*v_max/(1 + gamma*k*k)*10000/PI;

            gain = 0.3265*v*k;
            if (fabs(gain) > 600)
            {
                gain = boost::math::sign(gain) * 600;
            }

            if(fabs(v)>2000)
            {
                v = boost::math::sign(v)*2000;
            }
            // set motor rotation velocity
            locomotor->set_vel(floor(v)+floor(gain),-floor(v)+floor(gain));

            // when the euler distance is less than 100mm, achieved waypoint
            if (distance2d(m_state, m_cmd) < 0.09 && indicator(m_state[2],m_cmd[2],0.5))
            {
                m_move = IDLE;
            }
            break;
        }

        case BACKWARD:
        {
            v =  sqrt(dist) * 0.75/(1+0.2*k_back*k_back) *10000/3.1415926;

            gain = 0.3265*v*k_back;

            if (fabs(gain) > 600)
            {
                gain = boost::math::sign(gain) * 600;
            }

            if(fabs(v)>2000)
            {
                v = boost::math::sign(v)*2000;
            }
            
            locomotor->set_vel(-floor(v)+floor(gain),floor(v)+floor(gain));
            if (distance2d(m_state, m_cmd) < 0.09 && indicator(m_state[2],m_cmd[2],0.5))
            {
                m_move = IDLE;
            }
            break;
        }

        case LIFTFORK:
        {
            // TODO: Lift height should be determined, fork stroke should be determined
            printf("Lift and fork!\n");

            locomotor->set_vel(0, 0);
            sleep(3);
//            locomotor->shut_down();
            lift_motor->power_on();
            if ((fork_count%2) == 1)
//          change -243720 to be -223720, wants to liftfork a little bit lower
                lift_motor->set_pos(-223720);
            else
                lift_motor->set_pos(-293720);

            sleep(10);

           printf("Start Test\n");

            // pose correction code inserted here  first make sure tag is attached vertically, second camera has no pitch angle relative to the vehicle
            

            if ((fork_count%2) == 1)
            {
                printf("Start Correction!\n");
                int count_detect = 0;
                while(ros::ok())
                {
                    if(abs_pose1)
                    {
                        Eigen::Quaternion<float> quat;
                        quat.w() = abs_pose1->pose.pose.orientation.w;
                        quat.x() = abs_pose1->pose.pose.orientation.x;
                        quat.y() = abs_pose1->pose.pose.orientation.y;
                        quat.z() = abs_pose1->pose.pose.orientation.z;

                        Eigen::Matrix3f Rotation;

                        Rotation = qToRotation(quat);

                        Eigen::Matrix3f FixTF;
                        FixTF << 1, 0,  0,
                                 0, 0, -1,
                                 0, 1,  0;

                        Rotation = FixTF * Rotation.inverse();

                        float yaw_error;
                    
                        yaw_error = getYawFromMat(Rotation);
                    
                        gain = -3265*(k_angle*yaw_error)/3.1415926;
                        if(fabs(gain)>150)
                        {
                            gain = boost::math::sign(gain) * 150;
                        }
                        locomotor->set_vel(floor(gain), floor(gain));
                        if (fabs(yaw_error*180/3.1415926) < 0.5)
                        {
                            locomotor->set_vel(0,0);
                            printf("Yaw %f corrected!\n", yaw_error*180/3.1415926);
                            break;
                        }
                    }
                    else
                    {
                        locomotor->set_vel(0, 0);
                        usleep(10000);
                        count_detect++;
                    }
                    ros::spinOnce();

                    if (count_detect>1000)
                    {
                        ROS_WARN("No Tag detected when stoped");
                        ros::shutdown();
                        exit(1);
                    }
                }
 
                count_detect = 0;
                while(ros::ok())
                {
                    if(abs_pose1)
                    {

                        Eigen::Quaternion<float> quat;
                        quat.w() = abs_pose1->pose.pose.orientation.w;
                        quat.x() = abs_pose1->pose.pose.orientation.x;
                        quat.y() = abs_pose1->pose.pose.orientation.y;
                        quat.z() = abs_pose1->pose.pose.orientation.z;

                        Eigen::Matrix3f Rotation;
                        Eigen::Vector3f translation;
                        translation << abs_pose1->pose.pose.position.x,
                                       abs_pose1->pose.pose.position.y,
                                       abs_pose1->pose.pose.position.z;

                        Rotation = qToRotation(quat);

                        translation = translation;

                        float x_error;
                    
                        x_error = translation[0];
                    
                        v = 0.25*(x_error-0.003)*10000/3.1415926;
//                        printf("x is %f\n", x_error);
//                        printf("v is %f\n\n", floor(v));
                        if(fabs(v)>150)
                        {
                            v = boost::math::sign(v) * 150;
                        }
                        locomotor->set_vel(floor(v), -floor(v));
                        if (fabs(x_error-0.003) < 0.003)
                        {
                            locomotor->set_vel(0, 0);
                            printf("x %f corrected!\n", (x_error-0.006));
                            break;
                        }
                    }
                    else
                    {
                        locomotor->set_vel(0, 0);
                        usleep(10000);
                        count_detect++;
                    }
                    ros::spinOnce();

                    if (count_detect>1000)
                    {
                        ROS_WARN("No Tag detected when stoped");
                        ros::shutdown();
                        exit(1);
                    }
                }
            }

            if ((fork_count%2) == 0)
            {
                int count_detect = 0;

                while(ros::ok())
                {
                    if (abs_pose)
                    {
                        Eigen::Quaternion<float> quat;
                        quat.w() = abs_pose->pose.pose.orientation.w;
                        quat.x() = abs_pose->pose.pose.orientation.x;
                        quat.y() = abs_pose->pose.pose.orientation.y;
                        quat.z() = abs_pose->pose.pose.orientation.z;

                        Eigen::Matrix3f Rotation;

                        Rotation = qToRotation(quat);

                        Eigen::Matrix3f fixTF;
                        fixTF << 1, 0,  0,
                                 0, -1, 0,
                                 0, 0, -1;

                        Rotation = Rotation.inverse()*fixTF;

                        m_state[2] = getYawFromMat(Rotation);
                        gain = 3265*(k_angle*angle(m_cmd[2],m_state[2]))/PI;
                        if (fabs(gain) > 100)
                        {
                            gain = boost::math::sign(gain) * 100;
                        }
                        
                        if (fabs(gain) >100)
                        {
                            ros::shutdown();
                            exit(1);
                        }

                        locomotor->set_vel(floor(gain),floor(gain));
                        if (indicator(m_cmd[2], m_state[2], 0.008))
                        {
                            locomotor->set_vel(0, 0);
                            printf("Corrected!\n");
                            break;
                        }
                    }
                    else
                    {
                        locomotor->set_vel(0, 0);
                        usleep(10000);
                        count_detect++;
                    }
                    ros::spinOnce();

                    if (count_detect>1000)
                    {
                        locomotor->set_vel(0, 0);
                        ROS_WARN("No Tag detected when stoped");
                        //ros::shutdown();
                        //exit(1);
                    }
                }
            }

            // if we carry out hand hold barcode test, after correction, stop
//               ros::shutdown();
//               exit(1);

            locomotor->shut_down();
            sleep(0.5);

// comment following two lines can set make telefork not strech out
            telefork_motor->set_pos( boost::math::sign(lift_param)*(-386000) );
            sleep(15);

            if ((fork_count%2) == 1)
                 lift_motor->set_pos(-293720);
            else
                 lift_motor->set_pos(-223720);
            sleep(3);

            telefork_motor->set_pos(0);
            sleep(15);            

//            ros::shutdown();
//            exit(1);
            lift_motor->shut_down();
            locomotor->power_on();
            sleep(0.5);
            m_move = IDLE;

            break;
        }

        case TURN:
        {
            v = cos(alpha) * dist* k_dist *10000/PI;
            if(fabs(v)>300)
            {
                v = boost::math::sign(v)*300;
            }

            gain = 3265*(k_angle*angle(m_cmd[2],m_state[2]))/PI;
            if (fabs(gain) > 400)
            {
                gain = boost::math::sign(gain) * 400;
            }
            locomotor->set_vel(floor(v)+floor(gain),-floor(v)+floor(gain));

            if (indicator(m_state[2],m_cmd[2],0.01))
            {
                m_move = IDLE;
            }
            break;
        }
  
        case IDLE:
        {
           locomotor->set_vel(0,0);
           if (extracted_path.size()!=0)
           {
               geometry_msgs::PoseStamped temp_pose = extracted_path.back();
               float yaw_ = 2*atan2(temp_pose.pose.orientation.z,temp_pose.pose.orientation.w);
               m_cmd << temp_pose.pose.position.x * m_resolution,
                        temp_pose.pose.position.y * m_resolution,
                        angle(yaw_,0);

               printf("Next Commanded Pose is (%f, %f, %f)\n", m_cmd[0], m_cmd[1], m_cmd[2]);
               if ( (fabs(m_cmd[0] - m_state[0])>0.5) && (fabs(m_cmd[1] - m_state[1])>0.5) )
               {
                   printf("Invalid commanded position received!\n");
                   locomotor->shut_down();
                   ros::shutdown();
                   exit(1);
               }
               if ( (fabs(m_cmd[0] - m_state[0])>0.5) || (fabs(m_cmd[1] - m_state[1])>0.5) )
               {
                   if (fabs(angle(m_cmd[2],m_state[2]))>0.5)
                   {
                       printf("Invalid commanded pose orientation received!\n");
                       locomotor->shut_down();
                       ros::shutdown();
                       exit(1);
                   }
                   if (fabs(m_cmd[0] - m_state[0])>0.5)
                   {
                       if (cos(m_state[2]) *  (m_cmd[0] - m_state[0]) > 0)
                           m_move = FORWARD;
                       else
                           m_move = BACKWARD;
                   }
                   else
                   {
                       if (sin(m_state[2]) *  (m_cmd[1] - m_state[1]) > 0)
                           m_move = FORWARD;
                       else
                           m_move = BACKWARD;
                   }
                   if (m_move == FORWARD)
                       printf("Move Forward!\n");
                   else
                       printf("Move Backward!\n");
               }
               else if (fabs(m_cmd[2] - m_state[2])>0.5)
               {
                   m_move = TURN;
                   printf("Turn Around!\n");
               }
               else if (temp_pose.pose.position.z!=0)
               {
                   m_move = LIFTFORK;
                   fork_count++;
                   lift_param = temp_pose.pose.position.z;
               }
               else
                    m_move = IDLE;
               extracted_path.pop_back();
           }
           else
           {
                if (p_count == 2)
                {
                    lift_motor->power_on();
                    lift_motor->set_pos(0);
                    sleep(12);
                    lift_motor->shut_down();
                    sleep(0.5);
                }

               block_path_receiver = false;
               geometry_msgs::PoseStamped pose_msg;
         
               pose_msg.header.frame_id = "world";
               pose_msg.header.stamp = ros::Time::now();
               pose_msg.pose.position.x = m_state[0];
               pose_msg.pose.position.y = m_state[1];
               if (block_path_receiver)
                  pose_msg.pose.position.z = 1;
               else
                  pose_msg.pose.position.z = 0;

               pose_msg.pose.orientation.w = cos(m_state[2]/2);
               pose_msg.pose.orientation.x = 0;
               pose_msg.pose.orientation.y = 0;
               pose_msg.pose.orientation.z = sin(m_state[2]/2);
               m_posePub.publish(pose_msg);
               printf("IDLE!\n");
               sleep(1);
               m_move = IDLE;
           }

           break;
        }
    }
        
    vel.first = (vel.first + locomotor->get_vel().first)/2;
    vel.second = (vel.second + locomotor->get_vel().second)/2;

    cycle_period = (ros::Time::now() - cycle_start).toSec();

    m_state[0] = m_state[0] + (-vel.second+vel.first)/2 * cos(m_state[2]) * 0.018849555 * cycle_period/60;
    m_state[1] = m_state[1] + (-vel.second+vel.first)/2 * sin(m_state[2]) * 0.018849555 * cycle_period/60;        
    m_state[2] = m_state[2] + (vel.first + vel.second)/0.653 * 0.018849555 * cycle_period/60;


    if (abs_pose)
    {
        int id;
        id = abs_pose->id - 3;

        Eigen::Quaternion<float> quat;
        quat.w() = abs_pose->pose.pose.orientation.w;
        quat.x() = abs_pose->pose.pose.orientation.x;
        quat.y() = abs_pose->pose.pose.orientation.y;
        quat.z() = abs_pose->pose.pose.orientation.z;

        Eigen::Matrix3f Rotation;
        Eigen::Vector3f translation;
        translation << abs_pose->pose.pose.position.x,
                       abs_pose->pose.pose.position.y,
                       abs_pose->pose.pose.position.z;

        Rotation = qToRotation(quat);

        translation = -Rotation.inverse()*translation;

        Eigen::Matrix3f fixTF;
        fixTF << 1, 0,  0,
                 0, -1, 0,
                 0, 0, -1;

        Rotation = Rotation.inverse()*fixTF;

        m_state[0] = translation[0] + m_resolution * (id%10);
        m_state[1] = translation[1] + m_resolution * floor(id/10.0);
        m_state[2] = getYawFromMat(Rotation) + 0.04;
    }

    geometry_msgs::PoseStamped pose_msg;
         
    pose_msg.header.frame_id = "world";
    pose_msg.header.stamp = ros::Time::now();
    pose_msg.pose.position.x = m_state[0];
    pose_msg.pose.position.y = m_state[1];
    if (block_path_receiver)
        pose_msg.pose.position.z = 1;
    else
        pose_msg.pose.position.z = 0;

    pose_msg.pose.orientation.w = cos(m_state[2]/2);
    pose_msg.pose.orientation.x = 0;
    pose_msg.pose.orientation.y = 0;
    pose_msg.pose.orientation.z = sin(m_state[2]/2);

    m_posePub.publish(pose_msg);

    delta_y = m_cmd[1]-m_state[1];
    if(fabs(m_cmd[1]-m_state[1]) > 1.6)
    {
        delta_y = boost::math::sign(m_cmd[1]-m_state[1]) * 1.6;
    }

    delta_x = m_cmd[0]-m_state[0];
    if(fabs(m_cmd[0]-m_state[0]) > 1.6)
    {
        delta_x = boost::math::sign(m_cmd[0]-m_state[0]) * 1.6;
    }

    beta = angle(m_cmd[2], atan2(delta_y, delta_x));

    alpha = angle(m_state[2], atan2(delta_y, delta_x));

    beta1 = angle(m_cmd[2]+PI, atan2(delta_y, delta_x));

    alpha1 = angle(m_state[2]+3.1415926, atan2(delta_y, delta_x));

    dist = sqrt(delta_x*delta_x + delta_y* delta_y);

    k = -1/dist * (k2*(alpha-atan(-k1*beta)) + sin(alpha)*(1 + k1/(1+(k1*beta) * (k1*beta))));
    k_back = -1/dist * (k2*(alpha1-atan2(-k1*beta1,1)) + sin(alpha1)*(1 + k1/(1+(k1*beta1) * (k1*beta1))));
}
Пример #20
0
void SurfelMapPublisher::publishSurfelMarkers(const boost::shared_ptr<MapType>& map)
{
  if (m_markerPublisher.getNumSubscribers() == 0)
    return;

  unsigned int markerId = 0;
  std::vector<float> cellSizes;
  typename MapType::AlignedCellVectorVector occupiedCells;
  std::vector<std::vector<pcl::PointXYZ>> occupiedCellsCenters;

  int levels = map->getLevels();
  for (unsigned int i = 0; i < m_lastSurfelMarkerCount; ++i)
  {
    for (unsigned int l = 0; l < levels; l++)
    {
      visualization_msgs::Marker marker;
      marker.header.frame_id = map->getFrameId();
      marker.header.stamp = map->getLastUpdateTimestamp();
      marker.ns = boost::lexical_cast<std::string>(l);
      marker.id = i;
      marker.type = marker.SPHERE;
      marker.action = marker.DELETE;
      m_markerPublisher.publish(marker);
    }
  }

  for (unsigned int l = 0; l < levels; l++)
  {
    cellSizes.push_back(map->getCellSize(l));

    typename MapType::AlignedCellVector occupiedCellsTemp;
    std::vector<pcl::PointXYZ> occupiedCellsCentersTemp;
    map->getOccupiedCells(occupiedCellsTemp, l);
    map->getOccupiedCells(occupiedCellsCentersTemp, l);

    occupiedCells.push_back(occupiedCellsTemp);
    occupiedCellsCenters.push_back(occupiedCellsCentersTemp);
  }

  for (unsigned int l = 0; l < cellSizes.size(); l++)
  {
    float cellSize = cellSizes[l];

    for (size_t i = 0; i < occupiedCells[l].size(); i++)
    {
      const mrs_laser_maps::Surfel& surfel = occupiedCells[l][i].surfel_;

      //				if (surfel.num_points_ < 15 )
      //					continue;
      //
      //				if ( surfel.unevaluated_ ) {
      //					ROS_ERROR("not unevaluated surfel");
      //					continue;
      //				}

      Eigen::Matrix<double, 3, 3> cov = surfel.cov_.block(0, 0, 3, 3);
      Eigen::EigenSolver<Eigen::Matrix3d> solver(cov);
      Eigen::Matrix<double, 3, 3> eigenvectors = solver.eigenvectors().real();
      //				double eigenvalues[3];
      Eigen::Matrix<double, 3, 1> eigenvalues = solver.eigenvalues().real();
      //				for(int j = 0; j < 3; ++j) {
      //					Eigen::Matrix<double, 3, 1> mult = cov * eigenvectors.col(j);
      //					eigenvalues[j] = mult(0,0) / eigenvectors.col(j)(0);
      //				}
      if (eigenvectors.determinant() < 0)
      {
        eigenvectors.col(0)(0) = -eigenvectors.col(0)(0);
        eigenvectors.col(0)(1) = -eigenvectors.col(0)(1);
        eigenvectors.col(0)(2) = -eigenvectors.col(0)(2);
      }
      Eigen::Quaternion<double> q = Eigen::Quaternion<double>(eigenvectors);
      visualization_msgs::Marker marker;
      marker.header.frame_id = map->getFrameId();
      marker.header.stamp = map->getLastUpdateTimestamp();
      marker.ns = boost::lexical_cast<std::string>(l);
      marker.id = markerId++;
      marker.type = marker.SPHERE;
      marker.action = marker.ADD;
      marker.pose.position.x = occupiedCellsCenters[l][i].x - cellSize / 2 + surfel.mean_(0);
      marker.pose.position.y = occupiedCellsCenters[l][i].y - cellSize / 2 + surfel.mean_(1);
      marker.pose.position.z = occupiedCellsCenters[l][i].z - cellSize / 2 + surfel.mean_(2);
      marker.pose.orientation.w = q.w();
      marker.pose.orientation.x = q.x();
      marker.pose.orientation.y = q.y();
      marker.pose.orientation.z = q.z();
      marker.scale.x = std::max(sqrt(eigenvalues[0]) * 3, 0.01);
      marker.scale.y = std::max(sqrt(eigenvalues[1]) * 3, 0.01);
      marker.scale.z = std::max(sqrt(eigenvalues[2]) * 3, 0.01);
      marker.color.a = 1.0;

      double dot = surfel.normal_.dot(Eigen::Vector3d(0., 0., 1.));
      if (surfel.normal_.norm() > 1e-10)
        dot /= surfel.normal_.norm();
      double angle = acos(fabs(dot));
      double angle_normalized = 2. * angle / M_PI;
      marker.color.r = ColorMapJet::red(angle_normalized);  // fabs(surfel.normal_(0));
      marker.color.g = ColorMapJet::green(angle_normalized);
      marker.color.b = ColorMapJet::blue(angle_normalized);

      m_markerPublisher.publish(marker);
    }
  }
  m_lastSurfelMarkerCount = markerId;
}
Пример #21
0
int main(int argc, char **argv)
{
    // initialize the node and various publishers
    ros::init(argc, argv, "fovis_odom");
    ros::NodeHandle n;
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
    tf::TransformBroadcaster odom_broadcaster;

    // initialize the device
    fovis_example::DataCapture* cap = new fovis_example::DataCapture();
    if(!cap->initialize()) {
        fprintf(stderr, "Unable to initialize OpenNI sensor\n");
        return 1;
    }
    if(!cap->startDataCapture()) {
        fprintf(stderr, "Unable to start data capture\n");
        return 1;
    }

    // get the RGB camera parameters of our device
    fovis::Rectification rect(cap->getRgbParameters());

    fovis::VisualOdometryOptions options =
        fovis::VisualOdometry::getDefaultOptions();
    // If we wanted to play around with the different VO parameters, we could set
    // them here in the "options" variable.

    // setup the visual odometry
    fovis::VisualOdometry* odom = new fovis::VisualOdometry(&rect, options);

    // exit cleanly on CTL-C
    struct sigaction new_action;
    new_action.sa_sigaction = sig_action;
    sigemptyset(&new_action.sa_mask);
    new_action.sa_flags = 0;
    sigaction(SIGINT, &new_action, NULL);
    sigaction(SIGTERM, &new_action, NULL);
    sigaction(SIGHUP, &new_action, NULL);

    while(!shutdown_flag) {
        if(!cap->captureOne()) {
            fprintf(stderr, "Capture failed\n");
            break;
        }

        odom->processFrame(cap->getGrayImage(), cap->getDepthImage());

        // get the integrated pose estimate.
        Eigen::Isometry3d cam_to_local = odom->getPose();
        Eigen::Quaternion<double> quat = Eigen::Quaternion<double>(cam_to_local.rotation());

        double w = quat.w();
        double qx = quat.z();
        double qy = -quat.x();
        double qz = -quat.y();

        Eigen::Vector3d xyz = cam_to_local.translation();
        double x = xyz(2);
        double y = -xyz(0);
        double z = -xyz(1);

        tf::Quaternion tf_odom_quat = tf::Quaternion(qx,qy,qz,w);
        geometry_msgs::Quaternion gm_odom_quat;
        tf::quaternionTFToMsg(tf_odom_quat, gm_odom_quat);

        //first, we'll publish the transform over tf
        std::string odomName = "odom";
        std::string baseName = "base_link";
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = ros::Time::now();;
        odom_trans.header.frame_id = odomName;
        odom_trans.child_frame_id = baseName;

        odom_trans.transform.translation.x = x;
        odom_trans.transform.translation.y = y;
        odom_trans.transform.translation.z = z;
        odom_trans.transform.rotation = gm_odom_quat;

        //send the transform
        odom_broadcaster.sendTransform(odom_trans);

        //next, we'll publish the odometry message over ROS
        nav_msgs::Odometry odom_msg;
        odom_msg.header.stamp = ros::Time::now();
        odom_msg.header.frame_id = odomName;

        //set the position
        odom_msg.pose.pose.position.x = x;
        odom_msg.pose.pose.position.y = y;
        odom_msg.pose.pose.position.z = z;
        odom_msg.pose.pose.orientation = gm_odom_quat;

        //set the velocity
        odom_msg.child_frame_id = baseName;
        //odom_msg.twist.twist.linear.x = vx;
        //odom_msg.twist.twist.linear.y = vy;
        //odom_msg.twist.twist.angular.z = vth;

        //publish the message
        odom_pub.publish(odom_msg);



        // get the motion estimate for this frame to the previous frame.
        Eigen::Isometry3d motion_estimate = odom->getMotionEstimate();

        // display the motion estimate.  These values are all given in the RGB
        // camera frame, where +Z is forward, +X points right, +Y points down, and
        // the origin is located at the focal point of the RGB camera.
        std::cout << isometryToString(cam_to_local) << " " <<
                  isometryToString(motion_estimate) << "\n";
    }

    printf("Shutting down\n");
    cap->stopDataCapture();
    delete odom;
    delete cap;
    return 0;
}
Пример #22
0
void quaternion2eigen(const Eigen::Quaternion<T_qt> &quat_In, Eigen::Matrix<T_evec,4,1> &quat_Vec )
{
    quat_Vec = Eigen::Matrix<T_evec,4,1>( T_evec(quat_In.w()), T_evec(quat_In.x()), T_evec(quat_In.y()), T_evec(quat_In.z()) );
};
Пример #23
0
btQuaternion ToBullet(const Eigen::Quaternion<float>& q) { return btQuaternion((btScalar)q.x(), (btScalar)q.y(), (btScalar)q.z(), (btScalar)q.w()); }
Пример #24
0
  void ConP2::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes)
  {
    // node references
    Node &nr = nodes[ndr];
    Matrix<double,4,1> &tr = nr.trans;
    Quaternion<double> &qr = nr.qrot;
    Node &n1 = nodes[nd1];
    Matrix<double,4,1> &t1 = n1.trans;
    Quaternion<double> &q1 = n1.qrot;

    // first get the second frame in first frame coords
    Eigen::Matrix<double,3,1> pc = nr.w2n * t1;

    // Jacobians wrt first frame parameters

    // translational part of 0p1 wrt translational vars of p0
    // this is just -R0'  [from 0t1 = R0'(t1 - t0)]
    J0.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);


    // translational part of 0p1 wrt rotational vars of p0
    // dR'/dq * [pw - t]
    Eigen::Matrix<double,3,1> pwt;
    pwt = (t1-tr).head(3);   // transform translations

    // dx
    Eigen::Matrix<double,3,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
    J0.block<3,1>(0,3) = dp;
    // dy
    dp = nr.dRdy * pwt; // dR'/dq * [pw - t]
    J0.block<3,1>(0,4) = dp;
    // dz
    dp = nr.dRdz * pwt; // dR'/dq * [pw - t]
    J0.block<3,1>(0,5) = dp;

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J0.block<3,3>(3,0).setZero();

    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = qpmean * s0' * q0' * q1

    // dqdx
    Eigen::Quaternion<double> qr0, qr1, qrn, qrd;
    qr1.coeffs() = q1.coeffs();
    qrn.coeffs() = Vector4d(-qpmean.w(),-qpmean.z(),qpmean.y(),qpmean.x());  // qpmean * ds0'/dx
    qr0.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
    qr0 = qr0*qr1;              // rotate to zero mean
    qrd = qpmean*qr0;           // for normalization check
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,3) = qrn.vec();

    // dqdy
    qrn.coeffs() = Vector4d(qpmean.z(),-qpmean.w(),-qpmean.x(),qpmean.y());  // qpmean * ds0'/dy
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,4) = qrn.vec();

    // dqdz
    qrn.coeffs() = Vector4d(-qpmean.y(),qpmean.x(),-qpmean.w(),qpmean.z());  // qpmean * ds0'/dz
    qrn = qrn*qr0;

#ifdef NORMALIZE_Q
    if (qrd.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J0.block<3,1>(3,5) = qrn.vec();

    // transpose
    J0t = J0.transpose();

    //  cout << endl << "J0 " << ndr << endl << J0 << endl;

    // Jacobians wrt second frame parameters
    // translational part of 0p1 wrt translational vars of p1
    // this is just R0'  [from 0t1 = R0'(t1 - t0)]
    J1.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);

    // translational part of 0p1 wrt rotational vars of p1: zero
    J1.block<3,3>(0,3).setZero();

    // rotational part of 0p1 wrt translation vars of p0 => zero
    J1.block<3,3>(3,0).setZero();


    // rotational part of 0p1 wrt rotational vars of p0
    // from 0q1 = q0'*s1*q1

    Eigen::Quaternion<double> qrc;
    qrc.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
    qrc = qpmean*qrc*qr1;       // mean' * qr0' * qr1
    qrc.normalize();

    //    cout << endl << "QRC  : " << qrc.coeffs().transpose() << endl;

    double dq = 1.0e-8;
    double wdq = 1.0 - dq*dq;
    qr1.coeffs() = Vector4d(dq,0,0,wdq);
    //    cout << "QRC+x: " << (qrc*qr1).coeffs().transpose() << endl;    
    //    cout << "QRdx:  " << ((qrc*qr1).coeffs().transpose() - qrc.coeffs().transpose())/dq << endl;

    // dqdx
    qrn.coeffs() = Vector4d(1,0,0,0);
    qrn = qrc*qrn;

    //    cout << "J1dx:  " << qrn.coeffs().transpose() << endl;

#ifdef NORMALIZE_Q
    if (qrc.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J1.block<3,1>(3,3) = qrn.vec();

    // dqdy
    qrn.coeffs() = Vector4d(0,1,0,0);
    qrn = qrc*qrn;

#ifdef NORMALIZE_Q
    if (qrc.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J1.block<3,1>(3,4) = qrn.vec();

    // dqdz
    qrn.coeffs() = Vector4d(0,0,1,0);
    qrn = qrc*qrn;

#ifdef NORMALIZE_Q
    if (qrc.w() < 0.0)
      qrn.vec() = -qrn.vec();
#endif

    J1.block<3,1>(3,5) = qrn.vec();

    // transpose
    J1t = J1.transpose();

    //  cout << endl << "J1 " << nd1 << endl << J1 << endl;

  };
Пример #25
0
    visualization_msgs::Marker rviz_arrow(const Eigen::Vector3d & arrow, const Eigen::Vector3d & arrow_origin, int id, std::string name_space )
    {
        Eigen::Quaternion<double> rotation;
        if(arrow.norm()<0.0001)
        {
            rotation=Eigen::Quaternion<double>(1,0,0,0);
        }
        else
        {
            double rotation_angle=acos(arrow.normalized().dot(Eigen::Vector3d::UnitX()));
            Eigen::Vector3d rotation_axis=arrow.normalized().cross(Eigen::Vector3d::UnitX()).normalized();
            rotation=Eigen::AngleAxisd(-rotation_angle+PI,rotation_axis);
        }

        visualization_msgs::Marker marker;
        marker.header.frame_id = "/base_link";
        marker.header.stamp = ros::Time();
        marker.id = id;
        if(id==0)
        {
            marker.color.r = 0.0;
            marker.color.g = 0.0;
            marker.color.b = 1.0;
            marker.ns = name_space;
        }
        else
        {
            marker.color.r = 1.0;
            marker.color.g = 0.0;
            marker.color.b = 0.0;
            marker.ns = name_space;
        }
        marker.type = visualization_msgs::Marker::ARROW;
        marker.action = visualization_msgs::Marker::ADD;
        marker.pose.position.x = arrow_origin.x();
        marker.pose.position.y = arrow_origin.y();
        marker.pose.position.z = arrow_origin.z();
        marker.pose.orientation.x = rotation.x();
        marker.pose.orientation.y = rotation.y();
        marker.pose.orientation.z = rotation.z();
        marker.pose.orientation.w = rotation.w();
        //std::cout <<"position:" <<marker.pose.position << std::endl;
        //std::cout <<"orientation:" <<marker.pose.orientation << std::endl;
        //marker.pose.orientation.x = 0;
        //marker.pose.orientation.y = 0;
        //marker.pose.orientation.z = 0;
        //marker.pose.orientation.w = 1;
        if(arrow.norm()<0.0001)
        {
            marker.scale.x = 0.001;
            marker.scale.y = 0.001;
            marker.scale.z = 0.001;
        }
        else
        {
            marker.scale.x = arrow.norm();
            marker.scale.y = 0.1;
            marker.scale.z = 0.1;
        }
        marker.color.a = 1.0;


        return marker;
    }
Пример #26
0
void NavQuestSocketNode::publishDvlData(const NQRes& data)
{
	geometry_msgs::TwistStamped::Ptr twist(new geometry_msgs::TwistStamped());

	bool testDVL = data.velo_instrument[0] == data.velo_instrument[1];
	testDVL = testDVL && (data.velo_instrument[1] == data.velo_instrument[2]);
	testDVL = testDVL && (data.velo_instrument[2] == 0);

	//Data validity
	bool beamValidity = true;
	for (int i=0; i<4; ++i)
	{
		beamValidity = beamValidity && (data.beam_status[i] == 1);
		//ROS_INFO("Beam validity %d: %d", i, data.beam_status[i]);
		//ROS_INFO("Water vel credit %d: %f", i, data.wvelo_credit[i]);
	}

	if (testDVL)
	{
		ROS_INFO("All zero dvl. Ignore measurement.");
		return;
	}

	if (!beamValidity)
	{
		//ROS_INFO("One or more beams are invalid. Ignore measurement: %f %f", data.velo_instrument[0]/1000, data.velo_instrument[1]/1000);
		return;
	}
	else
	{
		ROS_INFO("Beams are valid. Accept measurement: %f %f", data.velo_instrument[0]/1000, data.velo_instrument[1]/1000);
	}

	(*beam_pub["velo_rad"])(data.velo_rad);
	(*beam_pub["wvelo_rad"])(data.wvelo_rad);
	(*speed_pub["velo_instrument"])(data.velo_instrument);
	(*speed_pub["velo_earth"])(data.velo_earth);
	(*speed_pub["water_velo_instrument"])(data.water_velo_instrument);
	(*speed_pub["water_velo_earth"])(data.water_velo_earth);

	enum{valid_flag=3};
	bool water_lock= (data.velo_instrument[valid_flag]==2) || (data.velo_earth[valid_flag]==2);
	bool valid=data.velo_instrument[valid_flag] && data.velo_earth[valid_flag];
	std_msgs::Bool bottom_lock;
	bottom_lock.data = !water_lock && valid;
	lock.publish(bottom_lock);

	//Altitude
	if (data.altitude_estimate > 0)
	{
		std_msgs::Float32Ptr alt(new std_msgs::Float32());
		alt->data = data.altitude_estimate;
		altitude.publish(alt);
	}

	//TF frame
	//Either use a fixed rotation here or the DVL measurements
	enum {roll=0, pitch, yaw};
	tf::Transform transform;
	//Moved 1.4m from the rotation origin.
	transform.setOrigin(tf::Vector3(1.4,0,0));
	//transform.setOrigin(tf::Vector3(0,0,0));
	if (useFixed)
	{
		transform.setRotation(tf::createQuaternionFromRPY(0,0, base_orientation));
		broadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "dvl_frame"));
	}
	else
	{
		Eigen::Quaternion<float> quat;
		labust::tools::quaternionFromEulerZYX(labust::math::wrapRad(data.rph[roll]/180*M_PI),
				labust::math::wrapRad(data.rph[pitch]/180*M_PI),
				labust::math::wrapRad(data.rph[yaw]/180*M_PI + magnetic_declination), quat);
		transform.setRotation(tf::Quaternion(quat.x(), quat.y(), quat.z(), quat.w()));
		broadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local", "dvl_frame"));
	}

	//RPY
	auv_msgs::RPY::Ptr rpy(new auv_msgs::RPY());
	rpy->roll = labust::math::wrapRad(data.rph[roll]/180*M_PI);
	rpy->pitch = labust::math::wrapRad(data.rph[pitch]/180*M_PI);
	rpy->yaw = labust::math::wrapRad(data.rph[yaw]/180*M_PI);
	imuPub.publish(rpy);
}