int birdTracker::init()
{
if (wakeUp() < 0)
{
fprintf(stderr, "birdTracker: ERROR - wakeUp failed\a\n");
return (-1);
}
if (getSystemStatus() < 0)
{
fprintf(stderr, "birdTracker: ERROR - getSystemStatus failed\a\n");
return (-1);
}
return (0);
}
void PoseEstimationTaskContext::updateOutputs()
{
getState(state_);
state_output_.write(state_);
if (imu_output_.connected()) {
imu_out_.header = state_.header;
imu_out_.orientation = state_.pose.pose.orientation;
getImuWithBiases(imu_out_.linear_acceleration, imu_out_.angular_velocity);
imu_output_.write(imu_out_);
}
if (pose_output_.connected()) {
pose_.header = state_.header;
pose_.pose = state_.pose.pose;
pose_output_.write(pose_);
}
if (velocity_output_.connected()) {
velocity_.header = state_.header;
velocity_.vector = state_.twist.twist.linear;
velocity_output_.write(velocity_);
}
if (global_position_output_.connected()) {
global_position_.header = state_.header;
getGlobalPosition(global_position_.latitude, global_position_.longitude, global_position_.altitude);
global_position_.latitude *= 180.0/M_PI;
global_position_.longitude *= 180.0/M_PI;
if (getSystemStatus() & STATE_XY_POSITION) {
global_position_.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
} else {
global_position_.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
}
global_position_output_.write(global_position_);
}
if (angular_velocity_bias_output_.connected() || linear_acceleration_bias_output_.connected()) {
getHeader(angular_velocity_bias_.header);
getHeader(linear_acceleration_bias_.header);
getBias(angular_velocity_bias_, linear_acceleration_bias_);
angular_velocity_bias_output_.write(angular_velocity_bias_);
linear_acceleration_bias_output_.write(linear_acceleration_bias_);
}
}
bool State::inSystemStatus(SystemStatus test_status) const {
return (getSystemStatus() & test_status) == test_status;
}
