コード例 #1
0
ファイル: helioToLsr.c プロジェクト: nrao/gbt-pipeline
char * helioToLsr( double raJ2000, double decJ2000, double vHelio,
		   double * vLsrOut)
/* helioToLsr() takes input angle coordinates and barycentric velocity */
/* and calculates the projected velocity in the LSR frame */
{ char * errMsg = NULL;
  double vLsr = VLSR, vRa = 0, vDec = 0, dV = 0, 
    vectorBary[3] = { 0, 0, 0}, sumV[3] = {0, 0, 0};
  static double vectorLsr[3] = { 0, 0, 0};

  if (vectorLsr[0] == 0) {
    vectorLsr[0] = VLSR * cos( RALSR) * cos( DECLSR);
    vectorLsr[1] = VLSR * sin( RALSR) * cos( DECLSR);
    vectorLsr[2] = VLSR * sin( DECLSR);
  }

  vectorBary[0] = vHelio * cos( raJ2000) * cos( decJ2000);
  vectorBary[1] = vHelio * sin( raJ2000) * cos( decJ2000);
  vectorBary[2] = vHelio * sin( decJ2000);

  errMsg = relativisticVectorSum( vectorBary, vectorLsr, sumV);

  magnitude ( sumV, &dV);

  if (dV > 0) {
    sumV[0] /= dV;
    sumV[1] /= dV;
    sumV[2] /= dV;
  }

  /* now convert to J2000 angles and velocity magnitude */
  rectpol( sumV, &vRa, &vDec);
 
  /* compute projected velocity in direction of the source */
  vLsr = dV * cos( angularDistance( vRa, vDec, raJ2000, decJ2000));

  *vLsrOut = vLsr;

  return(errMsg);
} /* helioToLsr() */
bool BaseMotionController::rotateRelative(double rotation)
{
  geometry_msgs::Twist zero_velocity;
  while (!odom_received_)
  {
    ros::spinOnce();
    if (!start_relative_movement_)
    {      
      base_velocities_pub_.publish(zero_velocity);
      return false;
    }
  }
  odom_received_ = false;

  double theta_goal = (theta_current_ + rotation);
  // if the goal exceeds M_PI, move it to the range -M_PI to 0
  if (theta_goal > M_PI)
  {
    theta_goal = -(2.0 * M_PI - theta_goal);
  }
  // if the goal is less than -M_PI, move it to the range 0 to M_PI
  else if (theta_goal < -M_PI)
  {
    theta_goal = (2.0 * M_PI + theta_goal);
  }

  ROS_DEBUG("Current theta: %f", theta_current_);
  ROS_DEBUG("Goal theta: %f", theta_goal);

  if (isMovementDone(theta_goal, theta_current_))
  {
    base_velocities_pub_.publish(zero_velocity);
    return true;
  }

  while (!isMovementDone(theta_goal, theta_current_))
  {
    geometry_msgs::Twist base_velocity;
    // calculate difference in current and goal position in radians
    double theta_diff = angularDistance(theta_goal, theta_current_);

    if (theta_diff < 0.0)
    {
      base_velocity.angular.z = -theta_vel_;
    }
    else
    {
      base_velocity.angular.z = theta_vel_;
    }

    base_velocities_pub_.publish(base_velocity);

    // wait for another odom reading before checking again
    while (!odom_received_)
    {
      ros::spinOnce();
      if (!start_relative_movement_)
      {
        base_velocities_pub_.publish(zero_velocity);
        return false;
      }
    }
    odom_received_ = false;
  }
  base_velocities_pub_.publish(zero_velocity);
  return true;
}