C++ Vector3::getY方法代码示例

tf::Matrix3x3 GetTfRotationMatrix(tf::Vector3 xaxis, tf::Vector3 zaxis) {
    tf::Matrix3x3 m;
    tf::Vector3 yaxis = zaxis.cross(xaxis);
    m.setValue(xaxis.getX(), yaxis.getX(), zaxis.getX(),
            xaxis.getY(), yaxis.getY(), zaxis.getY(),
            xaxis.getZ(), yaxis.getZ(), zaxis.getZ());
    return m;
}

tf::Vector3 JPCT_Math::rotate(tf::Vector3 vec, tf::Matrix3x3& mat) {
    float xr = vec.getX() * mat[0][0] + vec.getY() * mat[1][0] + vec.getZ() * mat[2][0];
    float yr = vec.getX() * mat[0][1] + vec.getY() * mat[1][1] + vec.getZ() * mat[2][1];
    float zr = vec.getX() * mat[0][2] + vec.getY() * mat[1][2] + vec.getZ() * mat[2][2];
    vec.setX(xr);
    vec.setY(yr);
    vec.setZ(zr);

    return vec;
}              

    float getVel(tf::Vector3 & pose1, tf::Vector3 & pose2, const ros::Duration & dur)
    {
      float vel;
      float delta_x = fabs(pose1.getX() - pose2.getX());
      float delta_y = fabs(pose1.getY() - pose2.getY());
      float dist = sqrt(delta_x*delta_x + delta_y*delta_y);

      vel = dist / dur.toSec();

      return vel;
    }

const float Utility::findAngleFromAToB(const tf::Vector3 a, const tf::Vector3 b) const {
  std::vector<float> a_vec;
  a_vec.push_back(a.getX());
  a_vec.push_back(a.getY());

  std::vector<float> b_vec;
  b_vec.push_back(b.getX());
  b_vec.push_back(b.getY());

  return findAngleFromAToB(a_vec, b_vec);
}

static tf::Transform twoPointTransform(const tf::Vector3 &a1, const tf::Vector3 &a2, const tf::Vector3 &b1, const tf::Vector3 &b2)
{
  double tha, thb, theta;

  // tha is the angle of the line segment from a1 to a2, in frame {A}
  tha = atan2(a2.getY() - a1.getY(), a2.getX() - a1.getX());
  // thb is the angle of the line segment from b1 to b2, in frame {B}
  thb = atan2(b2.getY() - b1.getY(), b2.getX() - b1.getX());
  // their difference is the rotation from {A} to {B}
  theta = thb - tha;
  // normalize it, [-pi .. pi]
  while (theta > M_PI) theta -= (M_PI + M_PI);
  while (theta < -M_PI) theta += (M_PI + M_PI);
  // make it a rotation matrix, where theta is rotation about Z
  tf::Matrix3x3 B_R_A;
  B_R_A.setRPY(0, 0, theta);

  // ac is the vector to the centroid of a1 and a2, in {A}
  tf::Vector3 ac((a1.getX() + a2.getX()) * 0.5, (a1.getY() + a2.getY()) * 0.5, 0);
  // bc is the vector to the centroid of b1 and b2, in {B}
  tf::Vector3 bc((b1.getX() + b2.getX()) * 0.5, (b1.getY() + b2.getY()) * 0.5, 0);
  // get the vector from {A} to {B} in frame {B} by rotating ac into {B}
  // and subtracting from bc
  tf::Vector3 B_P_A(bc - B_R_A * ac);

  // convert it to a complete transform
  return tf::Transform(B_R_A, B_P_A);
}

geometry_msgs::Point tfVector3ToPoint(tf::Vector3 vector) {
	geometry_msgs::Point position;
	position.x = vector.getX();
	position.y = vector.getY();
	position.z = vector.getZ();
	return position;
}

math::Vector3 RayTracePluginUtils::vectorTFToGazebo(const tf::Vector3 t)
{
  math::Vector3 v;
  v.x = t.getX();
  v.y = t.getY();
  v.z = t.getZ();
  return v;
}

void HapticsPSM::compute_force_in_tip_frame(geometry_msgs::Wrench &wrench){
    rot_mat6wrt0.setRPY(group->getCurrentRPY().at(0),
                        group->getCurrentRPY().at(1),
                        group->getCurrentRPY().at(2));
    tf_vec3.setValue(wrench.force.x,wrench.force.y,wrench.force.z);
    tf_vec3 = rot_mat6wrt0.transpose() * tf_vec3;
    wrench.force.x = tf_vec3.getX();
    wrench.force.y = tf_vec3.getY();
    wrench.force.z = tf_vec3.getZ();
}

		/********** callback for the cmd velocity from the autonomy **********/
		void cmd_vel_callback(const geometry_msgs::Twist& msg)
		{
			watchdogTimer.stop();
			
			error.setValue(msg.linear.x - body_vel.linear.x, msg.linear.y - body_vel.linear.y, msg.linear.z - body_vel.linear.z);
			//std::cout << "error x: " << error.getX() << " y: " << error.getY() << " z: " << error.getZ() << std::endl;
			//std::cout << std::abs(curr_body_vel_time.toSec() - last_body_vel_time.toSec()) << std::endl;
			error_yaw = msg.angular.z - body_vel.angular.z;
			//std::cout << "error yaw: " << error_yaw << std::endl;
			
			// if some time has passed between the last body velocity time and the current body velocity time then will calculate the (feed forward PD)
			if (std::abs(curr_body_vel_time.toSec() - last_body_vel_time.toSec()) > 0.00001)
			{	
				errorDot = (1/(curr_body_vel_time - last_body_vel_time).toSec()) * (error - last_error);
				//std::cout << "errordot x: " << errorDot.getX() << " y: " << errorDot.getY() << " z: " << errorDot.getZ() << std::endl;
				
				errorDot_yaw = (1/(curr_body_vel_time - last_body_vel_time).toSec()) * (error_yaw - last_error_yaw);
				//std::cout << "error dot yaw " << errorDot_yaw << std::endl;
				velocity_command.linear.x = cap_vel_auton(kx*msg.linear.x + (kp*error).getX() + (kd*errorDot).getX());
				velocity_command.linear.y = cap_vel_auton(ky*msg.linear.y + (kp*error).getY() + (kd*errorDot).getY());
				velocity_command.linear.z = cap_vel_auton(kz*msg.linear.z + (kp*error).getZ() + (kd*errorDot).getZ());
				velocity_command.angular.z = -1*cap_vel_auton(kyaw*msg.angular.z + kp_yaw*error_yaw + kd_yaw*errorDot_yaw); // z must be switched because bebop driver http://bebop-autonomy.readthedocs.org/en/latest/piloting.html
			}
			
			last_body_vel_time = curr_body_vel_time;// update last time body velocity was recieved
			last_error = error;
			last_error_yaw = error_yaw;
			
			error_gm.linear.x = error.getX(); error_gm.linear.y = error.getY(); error_gm.linear.z = error.getZ(); error_gm.angular.z = error_yaw;
			errorDot_gm.linear.x = errorDot.getX(); errorDot_gm.linear.y = errorDot.getY(); errorDot_gm.linear.z = errorDot.getZ(); errorDot_gm.angular.z = kyaw*msg.angular.z + kp_yaw*error_yaw + kd_yaw*errorDot_yaw;
			error_pub.publish(error_gm);
			errorDot_pub.publish(errorDot_gm);
			
			if (start_autonomous)
			{
				recieved_command_from_tracking = true;
			}
			
			watchdogTimer.start();
		}

GridRayTrace::GridRayTrace(tf::Vector3 start, tf::Vector3 end, const OccupancyGrid& grid_ref)
	: _x_store(), _y_store()
{
	//Transform (x,y) into map frame
	start = grid_ref.toGridFrame(start);
	end = grid_ref.toGridFrame(end);
	
	double x0 = start.getX(), y0 = start.getY();
	double x1 = end.getX(), y1 = end.getY();
	
	//ROS_WARN("start: (%f, %f) -> end: (%f, %f)", x0, y0, x1, y1);
	
	bresenham
		(
			floor(x0), floor(y0), 
			floor(x1), floor(y1), 
			_x_store, _y_store
		);
	
	_cur_idx = 0;
	_max_idx = std::min(_x_store.size(), _y_store.size());
}

double getMinimumDistance(tf::Vector3 position, nav_msgs::GridCells grid) {

  double distance_sq=-1;
  int size = grid.cells.size();
  double my_x=position.getX(), my_y=position.getY();
  double stop_radius_sq = stop_radius*stop_radius;
  for (int i=0; i<size; i++) {
    double d_sq = pow(grid.cells[i].x-my_x,2)+pow(grid.cells[i].y-my_y,2);
    if (distance_sq==-1 || d_sq<distance_sq) distance_sq=d_sq;
    if (d_sq<=stop_radius_sq) return 0;
  }

  return sqrt(distance_sq);
}

//.........这里部分代码省略.........
      }
        temp_counter++;
    }

    //New marker ?
    if(existing == false)
    {
      index = marker_counter_;
      markers_[index].marker_id = current_marker_id;
      existing = true;
      ROS_DEBUG_STREAM("New marker with ID: " << current_marker_id << " found");
    }

    // Change visibility flag of new marker
    for(size_t j = 0;j < marker_counter_; j++)
    {
      for(size_t k = 0;k < temp_markers.size(); k++)
      {
        if(markers_[j].marker_id == temp_markers[k].id)
          markers_[j].visible = true;
      }
    }

    //------------------------------------------------------
    // For existing marker do
    //------------------------------------------------------
    if((index < marker_counter_) && (first_marker_detected_ == true))
    {
      markers_[index].current_camera_tf = arucoMarker2Tf(temp_markers[i]);
      markers_[index].current_camera_tf = markers_[index].current_camera_tf.inverse();

      const tf::Vector3 marker_origin = markers_[index].current_camera_tf.getOrigin();
      markers_[index].current_camera_pose.position.x = marker_origin.getX();
      markers_[index].current_camera_pose.position.y = marker_origin.getY();
      markers_[index].current_camera_pose.position.z = marker_origin.getZ();

      const tf::Quaternion marker_quaternion = markers_[index].current_camera_tf.getRotation();
      markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX();
      markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY();
      markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ();
      markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW();
    }

    //------------------------------------------------------
    // For new marker do
    //------------------------------------------------------
    if((index == marker_counter_) && (first_marker_detected_ == true))
    {
      markers_[index].current_camera_tf=arucoMarker2Tf(temp_markers[i]);

      tf::Vector3 marker_origin = markers_[index].current_camera_tf.getOrigin();
      markers_[index].current_camera_pose.position.x = marker_origin.getX();
      markers_[index].current_camera_pose.position.y = marker_origin.getY();
      markers_[index].current_camera_pose.position.z = marker_origin.getZ();

      tf::Quaternion marker_quaternion = markers_[index].current_camera_tf.getRotation();
      markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX();
      markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY();
      markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ();
      markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW();

      // Naming - TFs
      std::stringstream camera_tf_id;
      std::stringstream camera_tf_id_old;
      std::stringstream marker_tf_id_old;

void get_veclocity(arma::colvec3& u,const tf::Vector3& origin, const tf::Vector3& origin_tmp)
{
    u(0) = origin.getX() - origin_tmp.getX();
    u(1) = origin.getY() - origin_tmp.getY();
    u(2) = origin.getZ() - origin_tmp.getZ();
}

void HapticsPSM::compute_deflection_force(geometry_msgs::Wrench &wrench, tf::Vector3 &d_along_n){

    wrench.force.x = d_along_n.getX();
    wrench.force.y = d_along_n.getY();
    wrench.force.z = d_along_n.getZ();
}

 double calcEuklideanDistance(tf::Vector3 origin, tf::Vector3 oldOrigin) {
 	double distance = 0.0;
 	distance = sqrt(((origin.getX()-oldOrigin.getX())*(origin.getX()-oldOrigin.getX())) + ((origin.getY()-oldOrigin.getY())*(origin.getY()-oldOrigin.getY())) + ((origin.getZ()-oldOrigin.getZ())*(origin.getZ()-oldOrigin.getZ())));
 	return distance;
 }