C++ Pose::getRotation方法代码示例

  tf::Pose MotionModel::updateAction(tf::Pose& p)
    {
      double delta_rot1;
      if (dxy < 0.01)
        delta_rot1 = 0.0;
      else
        delta_rot1 = angle_diff(atan2(delta_y, delta_x), delta_yaw);
      double delta_trans = dxy;
      double delta_rot2 = angle_diff(delta_yaw, delta_rot1);

      double delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1, M_PI)));
      double delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2, M_PI)));

      double delta_rot1_hat = angle_diff(delta_rot1, sampleGaussian(alpha1 * delta_rot1_noise*delta_rot1_noise + 
                                                                    alpha2 * delta_trans*delta_trans));
      double delta_trans_hat = delta_trans - sampleGaussian(alpha3 * delta_trans*delta_trans +
                                                            alpha4 * delta_rot1_noise*delta_rot1_noise +
                                                            alpha4 * delta_rot2_noise*delta_rot2_noise);
      double delta_rot2_hat = angle_diff(delta_rot2, sampleGaussian(alpha1 * delta_rot2_noise*delta_rot2_noise +
                                                                    alpha2 * delta_trans*delta_trans));

      delta_x = delta_trans_hat * cos(delta_rot1_hat);
      delta_y = delta_trans_hat * sin(delta_rot1_hat);
      delta_yaw = delta_rot1_hat + delta_rot2_hat;

   		tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
      tf::Transform base_to_global_ = tf::Transform(p.getRotation());
      noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
      p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
      p.setRotation(p.getRotation() * noisy_pose.getRotation());
    }

int CRvizMarker::Send(tf::Pose p, std::string frame) {
    visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = frame;
    marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    marker.ns = "nist_fanuc";
    marker.id = _id++;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = shape;

    // Set the marker action.  Options are ADD and DELETE
    marker.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
#if 0
    marker.pose.position.x = 0;
    marker.pose.position.y = 0;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
#endif
    marker.pose.position.x = p.getOrigin().x();
    marker.pose.position.y = p.getOrigin().y();
    marker.pose.position.z = p.getOrigin().z();
    marker.pose.orientation.x = p.getRotation().x();
    marker.pose.orientation.y = p.getRotation().y();
    marker.pose.orientation.z = p.getRotation().z();
    marker.pose.orientation.w = p.getRotation().w();

    // Set the scale of the marker -- 1x1x1 here means 1m on a side!!!
    marker.scale.x = scalex;
    marker.scale.y = scaley;
    marker.scale.z = scalez;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = r;
    marker.color.g = g;
    marker.color.b = b;
    marker.color.a = a;

    marker.lifetime = ros::Duration(0.0);

    // Publish the marker
    marker_pub.publish(marker);
    ros::spinOnce();
    ros::spinOnce();

    return 0;
}

	// Go down until hit the table. For safety min_height is specified. If no table found until this height, returns false.
	// vertical_speed with which to move downwards
	// thr_force - normal force threshold at which table is assumed to be detected
	bool find_table_for_rolling(double min_height, double vertical_speed, double thr_force) {
		double rate = 200;
		thr_force = fabs(thr_force);
		ros::Rate thread_rate(rate);

		double startz = ee_pose.getOrigin().z();

		msg_pose.pose.position.x = ee_pose.getOrigin().x();
		msg_pose.pose.position.y = ee_pose.getOrigin().y();
		msg_pose.pose.position.z = startz;
		msg_pose.pose.orientation.x = ee_pose.getRotation().x();
		msg_pose.pose.orientation.y = ee_pose.getRotation().y();
		msg_pose.pose.orientation.z = ee_pose.getRotation().z();
		msg_pose.pose.orientation.w = ee_pose.getRotation().w();

		// Publish attractors if running in simulation or with fixed values
		if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
		    static tf::TransformBroadcaster br;
			tf::Transform  table;
		    table.setOrigin(tf::Vector3 (ee_pose.getOrigin().x(),ee_pose.getOrigin().y(),ee_pose.getOrigin().z() - min_height));
		    table.setRotation(tf::Quaternion (ee_pose.getRotation().x(),ee_pose.getRotation().y(),ee_pose.getRotation().z(),ee_pose.getRotation().w()));
		    br.sendTransform(tf::StampedTransform(table, ros::Time::now(), world_frame, "/attractor"));
		}

		ROS_INFO_STREAM("Finding table up to max dist. "<<min_height<<" with vertical speed "<<vertical_speed<<" and threshold force "<<thr_force<<"N.");
		while(ros::ok()) {
			msg_pose.pose.position.z = msg_pose.pose.position.z - vertical_speed/rate;
			pub_.publish(msg_pose);

			// Go down until force reaches the threshold
			if(fabs(ee_ft[2]) > thr_force) {
				break;
			}
			if(fabs(ee_pose.getOrigin().z()-startz) > min_height) {
				ROS_INFO("Max distance reached");
				return false;
			}
			thread_rate.sleep();
			feedback_.progress = ee_ft[2];
			as_.publishFeedback(feedback_);
		}
		if(!ros::ok()) {
			return false;
		}
		tf::Vector3 table(ee_pose.getOrigin());
		ROS_INFO_STREAM("Table found at height "<<table[2]);
		msg_pose.pose.position.z = table[2];

		pub_.publish(msg_pose);
		sendAndWaitForNormalForce(0);


		return true;
	}

geometry_msgs::Pose tfTransformToGeometryPose(const tf::Pose& goal_pose)
{
    geometry_msgs::Pose target_pose1;
    target_pose1.orientation.x = goal_pose.getRotation().getX();
    target_pose1.orientation.y = goal_pose.getRotation().getY();
    target_pose1.orientation.z = goal_pose.getRotation().getZ();
    target_pose1.orientation.w = goal_pose.getRotation().getW();
    target_pose1.position.x = goal_pose.getOrigin().getX(); // + std::sin(angle)*radius;
    target_pose1.position.y = goal_pose.getOrigin().getY(); // + std::cos(angle)*radius;
    target_pose1.position.z = goal_pose.getOrigin().getZ();
    return target_pose1;
}

  void MotionModel::setMotion(const tf::Pose& pnew, const tf::Pose& pold)
    {
      tf::Pose delta_pose;
      tf::Transform odom_to_base(pold.inverse().getRotation());
      delta_pose.setOrigin(odom_to_base * (pnew.getOrigin() - pold.getOrigin()));
      delta_pose.setRotation(pnew.getRotation() * pold.getRotation().inverse());

      delta_x = delta_pose.getOrigin().x();
      delta_y = delta_pose.getOrigin().y();
      delta_yaw = atan2(sin(tf::getYaw(delta_pose.getRotation())), cos(tf::getYaw(delta_pose.getRotation())));

			dxy=sqrt(delta_x*delta_x+delta_y*delta_y);
    }

	// The famous sendPose!
	void sendPose(const tf::Pose& pose_) {
		geometry_msgs::PoseStamped msg;
		msg.pose.position.x = pose_.getOrigin().x();
		msg.pose.position.y = pose_.getOrigin().y();
		msg.pose.position.z = pose_.getOrigin().z();

		msg.pose.orientation.x = pose_.getRotation().x();
		msg.pose.orientation.y = pose_.getRotation().y();
		msg.pose.orientation.z = pose_.getRotation().z();
		msg.pose.orientation.w = pose_.getRotation().w();

		pub_.publish(msg);
	}

geometry_msgs::Pose tfPoseToGeometryPose(const tf::Pose tPose)
{
    geometry_msgs::Pose gPose;
    gPose.position.x = tPose.getOrigin().x();
    gPose.position.y = tPose.getOrigin().y();
    gPose.position.z = tPose.getOrigin().z();
    gPose.orientation.x = tPose.getRotation().x();
    gPose.orientation.y = tPose.getRotation().y();
    gPose.orientation.z = tPose.getRotation().z();
    gPose.orientation.w = tPose.getRotation().w();

    return gPose;
}

/**
 * Convert tf::Pose to string
 */
template<> std::string toString<tf::Pose>(const tf::Pose& p_pose)
{
  std::stringstream ss;

  ss << "Pose(Quaternion=" << toString(p_pose.getRotation()) << "; Vector3=" << toString(p_pose.getOrigin()) << ")";

  return ss.str();
}

	tf::Pose MotionModel::drawFromMotion(tf::Pose& p) 
		{
			double sxy=0.3*srr;
			delta_x+=sampleGaussian(srr*fabs(delta_x)+str*fabs(delta_yaw)+sxy*fabs(delta_y));
			delta_y+=sampleGaussian(srr*fabs(delta_y)+str*fabs(delta_yaw)+sxy*fabs(delta_x));
			delta_yaw+=sampleGaussian(stt*fabs(delta_yaw)+srt*dxy);
			delta_yaw=fmod(delta_yaw, 2*M_PI);
			if (delta_yaw>M_PI)
				delta_yaw-=2*M_PI;

   		tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
      tf::Transform base_to_global_ = tf::Transform(p.getRotation());
      noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
      p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
      p.setRotation(p.getRotation() * noisy_pose.getRotation());

			return p;
		}

  geometry_msgs::Twist PoseFollower::diff2D(const tf::Pose& pose1, const tf::Pose& pose2)
  {
    geometry_msgs::Twist res;
    tf::Pose diff = pose2.inverse() * pose1;
    res.linear.x = diff.getOrigin().x();
    res.linear.y = diff.getOrigin().y();
    res.angular.z = tf::getYaw(diff.getRotation());

    if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))
      return res;

    //in the case that we're not rotating to our goal position and we have a non-holonomic robot
    //we'll need to command a rotational velocity that will help us reach our desired heading
    
    //we want to compute a goal based on the heading difference between our pose and the target
    double yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(), 
        pose2.getOrigin().x(), pose2.getOrigin().y(), tf::getYaw(pose2.getRotation()));

    //we'll also check if we can move more effectively backwards
    if (allow_backwards_) 
    {
      double neg_yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(), 
					pose2.getOrigin().x(), pose2.getOrigin().y(), M_PI + tf::getYaw(pose2.getRotation()));

      //check if its faster to just back up
      if(fabs(neg_yaw_diff) < fabs(yaw_diff)){
        ROS_DEBUG("Negative is better: %.2f", neg_yaw_diff);
        yaw_diff = neg_yaw_diff;
      }
    }

    //compute the desired quaterion
    tf::Quaternion rot_diff = tf::createQuaternionFromYaw(yaw_diff);
    tf::Quaternion rot = pose2.getRotation() * rot_diff;
    tf::Pose new_pose = pose1;
    new_pose.setRotation(rot);

    diff = pose2.inverse() * new_pose;
    res.linear.x = diff.getOrigin().x();
    res.linear.y = diff.getOrigin().y();
    res.angular.z = tf::getYaw(diff.getRotation());
    return res;
  }

	MathLib::Matrix4 toMatrix4(const tf::Pose& pose) {
		MathLib::Matrix4 mat;
		mat.Identity();
		tf::Matrix3x3 mat33(pose.getRotation());

		mat.SetTranslation(MathLib::Vector3(pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()));
		mat.SetOrientation(MathLib::Matrix3(mat33[0][0], mat33[0][1], mat33[0][2],
				mat33[1][0], mat33[1][1], mat33[1][2],
				mat33[2][0], mat33[2][1], mat33[2][2]));
		return mat;
	}

	// Go to this pose
	bool go_home(tf::Pose& pose_) {


		tf::Vector3 start_p(pose_.getOrigin());
		tf::Quaternion start_o(pose_.getRotation());

		msg_pose.pose.position.x = start_p.x();
		msg_pose.pose.position.y = start_p.y();
		msg_pose.pose.position.z = start_p.z();
		msg_pose.pose.orientation.w = start_o.w();
		msg_pose.pose.orientation.x = start_o.x();
		msg_pose.pose.orientation.y = start_o.y();
		msg_pose.pose.orientation.z = start_o.z();
		pub_.publish(msg_pose);
		sendNormalForce(0);

		ros::Rate thread_rate(2);
		ROS_INFO("Homing...");
		while(ros::ok()) {
			double oerr =(ee_pose.getRotation() - start_o).length() ;
			double perr = (ee_pose.getOrigin() - start_p).length();
			feedback_.progress = 0.5*(perr+oerr);
			as_.publishFeedback(feedback_);
			ROS_INFO_STREAM("Error: "<<perr<<", "<<oerr);
			if(perr< 0.02 && oerr < 0.02) {
				break;
			}
			if (as_.isPreemptRequested() || !ros::ok() )
			{
				sendNormalForce(0);
				sendPose(ee_pose);
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
				return false;
			}
			thread_rate.sleep();
		}
		return ros::ok();

	}

// Convert from tf::Pose to CameraPose (position and orientation)
inline void TFPose2CameraPose(const tf::Pose& pose, CameraPose& cameraPose)
{
  // convert to position opencv vector
  tf::Vector3 position_tf = pose.getOrigin();
  cv::Point3d position = cv::Point3d(position_tf.getX(), position_tf.getY(), position_tf.getZ());

  // Convert to orientation opencv quaternion
  tf::Quaternion orientation_tf = pose.getRotation();
  cv::Vec4d orientation(orientation_tf.getW(), orientation_tf.getX(), orientation_tf.getY(), orientation_tf.getZ());

  cameraPose = CameraPose(position, orientation);
}

    void toMsgPose(const tf::Pose& tf_pose, geometry_msgs::Pose& msg_pose)
    {
        msg_pose.position.x = tf_pose.getOrigin().getX();
        msg_pose.position.y = tf_pose.getOrigin().getY();
        msg_pose.position.z = tf_pose.getOrigin().getZ();

        tf::Quaternion orientation = tf_pose.getRotation();
        msg_pose.orientation.w = orientation.getW();
        msg_pose.orientation.x = orientation.getX();
        msg_pose.orientation.y = orientation.getY();
        msg_pose.orientation.z = orientation.getZ();
    }

// Callback for the desired cartesian pose
void cartCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
	ROS_INFO_STREAM("Received Position Command");
	const geometry_msgs::PoseStamped* data = msg.get();
	des_ee_pose.setOrigin(tf::Vector3(data->pose.position.x,data->pose.position.y,data->pose.position.z));
	des_ee_pose.setRotation(tf::Quaternion(data->pose.orientation.x,data->pose.orientation.y,data->pose.orientation.z,data->pose.orientation.w));

	ROS_INFO_STREAM_THROTTLE(1, "Received Position: "<<des_ee_pose.getOrigin().x()<<","<<des_ee_pose.getOrigin().y()<<","<<des_ee_pose.getOrigin().z());

	if(bOrientCtrl) {
		tf::Quaternion q = des_ee_pose.getRotation();
		ROS_INFO_STREAM_THROTTLE(1, "Received Orientation: "<<q.x()<<","<<q.y()<<","<<q.z()<<","<<q.w());
	}
}