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