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

// copied from channelcontroller
double CalibrateAction::getDistanceAtPose(const tf::Pose & pose, bool* in_bounds) const
{
    // determine current dist
    int pose_x, pose_y;
    costmap_.worldToMapNoBounds(pose.getOrigin().x(), pose.getOrigin().y(),
            pose_x, pose_y);
    //ROS_INFO("pose_x: %i, pose_y: %i", pose_x, pose_y);
    if(pose_x < 0 || pose_y < 0 ||
            pose_x >= (int)voronoi_.getSizeX() || pose_y >= (int)voronoi_.getSizeY()) {
        if(in_bounds == NULL) {
            // only warn if in_bounds isn't checked externally
            ROS_WARN_THROTTLE(1.0, "%s: Pose out of voronoi bounds (%.2f, %.2f) = (%d, %d)", __func__,
                    pose.getOrigin().x(), pose.getOrigin().y(), pose_x, pose_y);
        } else {
            *in_bounds = false;
        }
        return 0.0;
    }
    if(in_bounds)  {
        *in_bounds = true;
    }
    float dist = voronoi_.getDistance(pose_x, pose_y);
    dist *= costmap_.getResolution();
    return dist;
}

void LoopClosure::visualizeEdge(const tf::Pose& pose1, const tf::Pose& pose2, visualization_msgs::MarkerArray& markers)
{
  visualization_msgs::Marker marker;
  marker.header.frame_id = "map";
  marker.header.stamp = ros::Time::now();
  marker.action = visualization_msgs::Marker::ADD;
  marker.ns = "loop_closure";
  marker.id = marker_id_++;
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  geometry_msgs::Point p;
  p.x = pose1.getOrigin().x();
  p.y = pose1.getOrigin().y();
  marker.points.push_back(p);
  p.x = pose2.getOrigin().x();
  p.y = pose2.getOrigin().y();
  marker.points.push_back(p);
  marker.scale.x = 0.25;
  marker.scale.y = 0.25;
  marker.scale.z = 0.25;
  marker.color.a = 1.0;
  marker.color.r = 1.0;
  marker.color.g = 0.0;
  marker.color.b = 0.0;
  //marker.lifetime = ros::Duration(5);
  markers.markers.push_back(marker);
  //marker_publisher_.publish(marker);
}

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;
}

	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;
	}

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 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();
    }

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;
}

  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);
	}

static RVector<double> PoseValues(tf::Pose & pose) {
   RVector<double> values;
    values.push_back( pose.getOrigin().x());
    values.push_back( pose.getOrigin().y());
    values.push_back( pose.getOrigin().z());
    double roll, pitch, yaw;
    tf::Quaternion q = pose.getRotation();
    tf::Matrix3x3(q).getRPY(roll, pitch, yaw );
    values.push_back( roll );
    values.push_back( pitch );
    values.push_back( yaw );
    return values;
}

 void poseTFToKDL(const tf::Pose& t, KDL::Frame& k)
 {
   for (unsigned int i = 0; i < 3; ++i)
     k.p[i] = t.getOrigin()[i];
   for (unsigned int i = 0; i < 9; ++i)
     k.M.data[i] = t.getBasis()[i/3][i%3];
 }

  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());
    }

void StepCostKey::mirrorPoseOnXPlane(tf::Pose &mirror, const tf::Pose &orig) const
{
  mirror.setBasis(orig.getBasis());
  tf::Matrix3x3& basis = mirror.getBasis();
  basis[0][1] = -basis[0][1];
  basis[1][0] = -basis[1][0];
  basis[2][1] = -basis[2][1];

  mirror.setOrigin(orig.getOrigin());
  tf::Vector3& origin = mirror.getOrigin();
  origin[1] = -origin[1];


//  double r, p, y;
//  tf::Matrix3x3(orig.getRotation()).getRPY(r, p, y);
//  mirror.setRotation(tf::createQuaternionFromRPY(-r, p, -y));

//  tf::Vector3 v = orig.getOrigin();
//  v.setY(-v.getY());
//  mirror.setOrigin(v);


//  tfScalar m[16];
//  ROS_INFO("------------------");
//  mirror.getOpenGLMatrix(m);
//  ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]);
//  ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]);
//  ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]);
//  ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);

//  ROS_INFO("-");
//  orig.getOpenGLMatrix(m);

//  ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]);
//  ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]);
//  ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]);
//  ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);

//  ROS_INFO("-");

//  ROS_INFO("%f %f %f %f", m[0], -m[4], m[8], m[12]);
//  ROS_INFO("%f %f %f %f", -m[1], m[5], m[9], -m[13]);
//  ROS_INFO("%f %f %f %f", m[2], -m[6], m[10], m[14]);
//  ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
}

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()));


    //---------------Modified----------------------
    //we'll also check if we can move more effectively 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;
}

	// 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();

	}