void LiveSLAMWrapper::BALoop()
{
ros::Rate BARate(2000) ;
list<ImageMeasurement>::iterator iterImage ;
std::list<visensor_node::visensor_imu>::iterator iterIMU ;
cv::Mat image0 ;
cv::Mat image1 ;
cv::Mat gradientMapForDebug(height, width, CV_8UC3) ;
sensor_msgs::Image msg;
double t ;
while ( nh.ok() )
{
monoOdometry->frameInfoList_mtx.lock();
int ttt = (monoOdometry->frameInfoListTail - monoOdometry->frameInfoListHead);
if ( ttt < 0 ){
ttt += frameInfoListSize ;
}
//printf("[BA thread] sz=%d\n", ttt ) ;
if ( ttt < 1 ){
monoOdometry->frameInfoList_mtx.unlock();
BARate.sleep() ;
continue ;
}
for ( int sz ; ; )
{
monoOdometry->frameInfoListHead++ ;
if ( monoOdometry->frameInfoListHead >= frameInfoListSize ){
monoOdometry->frameInfoListHead -= frameInfoListSize ;
}
sz = monoOdometry->frameInfoListTail - monoOdometry->frameInfoListHead ;
if ( sz == 0 ){
break ;
}
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag ){
break ;
}
}
ros::Time imageTimeStamp = monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].t ;
monoOdometry->frameInfoList_mtx.unlock();
//Pop out the image list
image1_queue_mtx.lock();
iterImage = image1Buf.begin() ;
while ( iterImage->t < imageTimeStamp ){
iterImage = image1Buf.erase( iterImage ) ;
}
image1 = iterImage->image.clone();
image1_queue_mtx.unlock();
image0_queue_mtx.lock();
iterImage = image0Buf.begin() ;
while ( iterImage->t < imageTimeStamp ){
iterImage = image0Buf.erase( iterImage ) ;
}
image0 = iterImage->image.clone();
image0_queue_mtx.unlock();
imu_queue_mtx.lock();
iterIMU = imuQueue.begin() ;
Vector3d linear_acceleration;
Vector3d angular_velocity;
//std::cout << "imageTime=" << imageTimeStamp << std::endl;
while ( iterIMU->header.stamp < imageTimeStamp )
{
linear_acceleration(0) = iterIMU->linear_acceleration.x;
linear_acceleration(1) = iterIMU->linear_acceleration.y;
linear_acceleration(2) = iterIMU->linear_acceleration.z;
angular_velocity(0) = iterIMU->angular_velocity.x;
angular_velocity(1) = iterIMU->angular_velocity.y;
angular_velocity(2) = iterIMU->angular_velocity.z;
// to_pub_info.x = angular_velocity(0)*180/PI ;
// to_pub_info.y = angular_velocity(1)*180/PI ;
// to_pub_info.z = angular_velocity(2)*180/PI ;
// monoOdometry->pub_angular_velocity.publish( to_pub_info ) ;
// outFile << to_pub_info.x << " "
// << to_pub_info.y << " "
// << to_pub_info.z << "\n";
double pre_t = iterIMU->header.stamp.toSec();
iterIMU = imuQueue.erase(iterIMU);
double next_t = iterIMU->header.stamp.toSec();
double dt = next_t - pre_t ;
// std::cout << linear_acceleration.transpose() << std::endl ;
// std::cout << angular_velocity.transpose() << std::endl ;
monoOdometry->processIMU( dt, linear_acceleration, angular_velocity );
}
imu_queue_mtx.unlock();
//propagate the last frame info to the current frame
Frame* lastFrame = monoOdometry->slidingWindow[monoOdometry->tail].get();
float dt = lastFrame->timeIntegral;
Vector3d T_bk1_2_b0 = lastFrame->T_bk_2_b0 - 0.5 * gravity_b0 * dt *dt
+ lastFrame->R_bk_2_b0*(lastFrame->v_bk * dt + lastFrame->alpha_c_k);
Vector3d v_bk1 = lastFrame->R_k1_k.transpose() *
(lastFrame->v_bk - lastFrame->R_bk_2_b0.transpose() * gravity_b0 * dt
+ lastFrame->beta_c_k);
Matrix3d R_bk1_2_b0 = lastFrame->R_bk_2_b0 * lastFrame->R_k1_k;
monoOdometry->insertFrame(imageSeqNumber, image0, imageTimeStamp, R_bk1_2_b0, T_bk1_2_b0, v_bk1);
Frame* currentFrame = monoOdometry->slidingWindow[monoOdometry->tail].get();
Frame* keyFrame = monoOdometry->currentKeyFrame.get();
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag )
{
//prepare key frame
cv::Mat disparity, depth ;
monoOdometry->bm_(image0, image1, disparity, CV_32F);
calculateDepthImage(disparity, depth, 0.11, fx );
currentFrame->setDepthFromGroundTruth( (float*)depth.data ) ;
#ifdef PRINT_DEBUG_INFO
//pub debugMap
cv::cvtColor(image0, gradientMapForDebug, CV_GRAY2BGR);
monoOdometry->generateDubugMap(currentFrame, gradientMapForDebug);
msg.header.stamp = imageTimeStamp;
sensor_msgs::fillImage(msg, sensor_msgs::image_encodings::BGR8, height,
width, width*3, gradientMapForDebug.data );
monoOdometry->pub_gradientMapForDebug.publish(msg) ;
#endif
int preKeyFrameID = monoOdometry->currentKeyFrame->id() ;
//set key frame
monoOdometry->currentKeyFrame = monoOdometry->slidingWindow[monoOdometry->tail] ;
monoOdometry->currentKeyFrame->keyFrameFlag = true ;
monoOdometry->currentKeyFrame->cameraLinkList.clear() ;
//reset the initial guess
monoOdometry->RefToFrame = Sophus::SE3() ;
//update tracking reference
monoOdometry->updateTrackingReference();
#ifdef PUB_POINT_CLOUD
pubPointCloud(valid_num, imageTimeStamp, R_vi_2_odometry);
#endif
//unlock dense tracking
monoOdometry->tracking_mtx.lock();
monoOdometry->lock_densetracking = false;
monoOdometry->tracking_mtx.unlock();
if ( (imageTimeStamp - lastLoopClorsureTime).toSec() > 0.18 )
{
//add possible loop closure link
t = (double)cvGetTickCount() ;
monoOdometry->setReprojectionListRelateToLastestKeyFrame( monoOdometry->head, preKeyFrameID,
monoOdometry->slidingWindow[monoOdometry->tail].get(),
R_i_2_c, T_i_2_c ) ;
ROS_WARN("loop closure link cost time: %f", ((double)cvGetTickCount() - t) / (cvGetTickFrequency() * 1000) );
t = (double)cvGetTickCount() ;
lastLoopClorsureTime = imageTimeStamp ;
}
}
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].trust )
{
// cout << "insert camera link" << endl ;
// cout << monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].T_k_2_c << endl ;
monoOdometry->insertCameraLink(keyFrame, currentFrame,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].R_k_2_c,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].T_k_2_c,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].lastestATA );
}
int control_flag = 0 ;
Vector3d preBAt = currentFrame->T_bk_2_b0 ;
// cout << "[-BA]current Position: " << currentFrame->T_bk_2_b0.transpose() << endl;
// cout << "[-BA]current Velocity: " << currentFrame->v_bk.transpose() << endl;
//BA
t = (double)cvGetTickCount() ;
monoOdometry->BA();
printf("BA cost time: %f\n", ((double)cvGetTickCount() - t) / (cvGetTickFrequency() * 1000) );
t = (double)cvGetTickCount() ;
//cout << "[BA-]current Position: " << currentFrame->T_bk_2_b0.transpose() << endl;
//cout << "[BA-]current Velocity: " << currentFrame->v_bk.transpose() << endl;
if ( (currentFrame->T_bk_2_b0 - preBAt ).norm() > 0.1 ){
control_flag = 1 ; //loop_closure or other sudden position change case
}
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].trust == false ){
control_flag = 2 ; //only IMU link, dense tracking fails
}
#ifdef PUB_GRAPH
pubCameraLink();
#endif
//marginalziation
monoOdometry->twoWayMarginalize();
monoOdometry->setNewMarginalzationFlag();
// R_vi_2_odometry.setIdentity() ;
// if ( onUAV )
// {
// pubOdometry(monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
// monoOdometry->slidingWindow[monoOdometry->tail]->v_bk,
// monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0,
// monoOdometry->pub_odometry, monoOdometry->pub_pose,
// control_flag, R_vi_2_odometry,
// monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag, imageTimeStamp );
// }
// else
// {
// pubOdometry(monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
// monoOdometry->slidingWindow[monoOdometry->tail]->v_bk,
// monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0,
// monoOdometry->pub_odometry, monoOdometry->pub_pose,
// control_flag, Eigen::Matrix3d::Identity(),
// monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag, imageTimeStamp );
// }
#ifdef PRINT_DEBUG_INFO
int colorFlag = 0 ;
colorFlag = monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag ;
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].trust == false ){
colorFlag = 2 ;
}
if ( onUAV ){
pubPath(monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
colorFlag,
monoOdometry->path_line, monoOdometry->pub_path, R_vi_2_odometry);
}
else{
pubPath(monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
colorFlag,
monoOdometry->path_line, monoOdometry->pub_path, Eigen::Matrix3d::Identity() );
}
#endif
#ifdef PUB_TF
static tf::TransformBroadcaster br;
tf::Transform transform;
Vector3d t_translation = R_vi_2_odometry * monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0 ;
Quaterniond t_q(monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0) ;
transform.setOrigin(tf::Vector3(t_translation(0),
t_translation(1),
t_translation(2)) );
tf::Quaternion q;
Quaterniond tt_q(R_vi_2_odometry * monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0 * R_vi_2_odometry.transpose());
q.setW(tt_q.w());
q.setX(tt_q.x());
q.setY(tt_q.y());
q.setZ(tt_q.z());
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "densebody"));
#endif
logCameraPose() ;
// int preIndex = monoOdometry->tail - 1 ;
// if ( preIndex < 0 ){
// preIndex += slidingWindowSize ;
// }
// Vector3d tt_dist = (monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0 -
// monoOdometry->slidingWindow[preIndex]->T_bk_2_b0) ;
// Matrix3d tt_rotate = monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0.transpose() *
// monoOdometry->slidingWindow[preIndex]->R_bk_2_b0 ;
// Quaterniond tt_q(tt_rotate) ;
// to_pub_info.x = monoOdometry->slidingWindow[monoOdometry->tail]->v_bk(0) ;
// to_pub_info.y = monoOdometry->slidingWindow[monoOdometry->tail]->v_bk(1) ;
// to_pub_info.z = monoOdometry->slidingWindow[monoOdometry->tail]->v_bk(2) ;
// monoOdometry->pub_linear_velocity.publish(to_pub_info) ;
}
}
void LiveSLAMWrapper::BALoop()
{
ros::Rate BARate(2000) ;
list<ImageMeasurement>::iterator iterImage ;
std::list<visensor_node::visensor_imu>::iterator iterIMU ;
cv::Mat image0 ;
cv::Mat image1 ;
cv::Mat gradientMapForDebug(height, width, CV_8UC3) ;
sensor_msgs::Image msg;
double t ;
while ( nh.ok() )
{
monoOdometry->frameInfoList_mtx.lock();
int ttt = (monoOdometry->frameInfoListTail - monoOdometry->frameInfoListHead);
if ( ttt < 0 ){
ttt += frameInfoListSize ;
}
//printf("[BA thread] sz=%d\n", ttt ) ;
if ( ttt < 1 ){
monoOdometry->frameInfoList_mtx.unlock();
BARate.sleep() ;
continue ;
}
for ( int sz ; ; )
{
monoOdometry->frameInfoListHead++ ;
if ( monoOdometry->frameInfoListHead >= frameInfoListSize ){
monoOdometry->frameInfoListHead -= frameInfoListSize ;
}
sz = monoOdometry->frameInfoListTail - monoOdometry->frameInfoListHead ;
if ( sz == 0 ){
break ;
}
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag ){
break ;
}
}
ros::Time imageTimeStamp = monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].t ;
monoOdometry->frameInfoList_mtx.unlock();
//Pop out the image list
image1_queue_mtx.lock();
iterImage = image1Buf.begin() ;
while ( iterImage->t < imageTimeStamp ){
iterImage = image1Buf.erase( iterImage ) ;
}
image1 = iterImage->image.clone();
image1_queue_mtx.unlock();
image0_queue_mtx.lock();
iterImage = image0Buf.begin() ;
while ( iterImage->t < imageTimeStamp ){
iterImage = image0Buf.erase( iterImage ) ;
}
image0 = iterImage->image.clone();
image0_queue_mtx.unlock();
imu_queue_mtx.lock();
iterIMU = imuQueue.begin() ;
Vector3d linear_acceleration;
Vector3d angular_velocity;
//std::cout << "imageTime=" << imageTimeStamp << std::endl;
while ( iterIMU->header.stamp < imageTimeStamp )
{
linear_acceleration(0) = iterIMU->linear_acceleration.x;
linear_acceleration(1) = iterIMU->linear_acceleration.y;
linear_acceleration(2) = iterIMU->linear_acceleration.z;
angular_velocity(0) = iterIMU->angular_velocity.x;
angular_velocity(1) = iterIMU->angular_velocity.y;
angular_velocity(2) = iterIMU->angular_velocity.z;
//linear_acceleration = -linear_acceleration;
//angular_velocity = -angular_velocity ;
// double pre_t = iterIMU->header.stamp.toSec();
// iterIMU = imuQueue.erase(iterIMU);
// //std::cout << imuQueue.size() <<" "<< iterIMU->header.stamp << std::endl;
// double next_t = iterIMU->header.stamp.toSec();
// double dt = next_t - pre_t ;
double pre_t = iterIMU->header.stamp.toSec();
iterIMU = imuQueue.erase(iterIMU);
double next_t = iterIMU->header.stamp.toSec();
double dt = next_t - pre_t ;
// std::cout << linear_acceleration.transpose() << std::endl ;
// std::cout << angular_velocity.transpose() << std::endl ;
monoOdometry->processIMU( dt, linear_acceleration, angular_velocity );
}
imu_queue_mtx.unlock();
//propagate the last frame info to the current frame
Frame* lastFrame = monoOdometry->slidingWindow[monoOdometry->tail].get();
float dt = lastFrame->timeIntegral;
Vector3d T_bk1_2_b0 = lastFrame->T_bk_2_b0 - 0.5 * gravity_b0 * dt *dt
+ lastFrame->R_bk_2_b0*(lastFrame->v_bk * dt + lastFrame->alpha_c_k);
Vector3d v_bk1 = lastFrame->R_k1_k.transpose() *
(lastFrame->v_bk - lastFrame->R_bk_2_b0.transpose() * gravity_b0 * dt
+ lastFrame->beta_c_k);
Matrix3d R_bk1_2_b0 = lastFrame->R_bk_2_b0 * lastFrame->R_k1_k;
monoOdometry->insertFrame(imageSeqNumber, image1, imageTimeStamp, R_bk1_2_b0, T_bk1_2_b0, v_bk1);
Frame* currentFrame = monoOdometry->slidingWindow[monoOdometry->tail].get();
Frame* keyFrame = monoOdometry->currentKeyFrame.get();
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag )
{
//prepare key frame
cv::Mat disparity, depth ;
monoOdometry->bm_(image1, image0, disparity, CV_32F);
calculateDepthImage(disparity, depth, 0.11, fx );
currentFrame->setDepthFromGroundTruth( (float*)depth.data ) ;
//pub debugMap
cv::cvtColor(image1, gradientMapForDebug, CV_GRAY2BGR);
monoOdometry->generateDubugMap(currentFrame, gradientMapForDebug);
msg.header.stamp = imageTimeStamp;
sensor_msgs::fillImage(msg, sensor_msgs::image_encodings::BGR8, height,
width, width*3, gradientMapForDebug.data );
monoOdometry->pub_gradientMapForDebug.publish(msg) ;
//set key frame
monoOdometry->currentKeyFrame = monoOdometry->slidingWindow[monoOdometry->tail] ;
monoOdometry->currentKeyFrame->keyFrameFlag = true ;
monoOdometry->currentKeyFrame->cameraLinkList.clear() ;
//reset the initial guess
monoOdometry->RefToFrame = Sophus::SE3() ;
//unlock dense tracking
monoOdometry->tracking_mtx.lock();
monoOdometry->lock_densetracking = false;
monoOdometry->tracking_mtx.unlock();
}
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].trust )
{
// cout << "insert camera link" << endl ;
// cout << monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].T_k_2_c << endl ;
monoOdometry->insertCameraLink(keyFrame, currentFrame,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].R_k_2_c,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].T_k_2_c,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].lastestATA );
}
cout << "[-BA]current Position: " << currentFrame->T_bk_2_b0.transpose() << endl;
cout << "[-BA]current Velocity: " << currentFrame->v_bk.transpose() << endl;
//BA
t = (double)cvGetTickCount() ;
monoOdometry->BA();
printf("BA cost time: %f\n", ((double)cvGetTickCount() - t) / (cvGetTickFrequency() * 1000) );
t = (double)cvGetTickCount() ;
cout << "[BA-]current Position: " << currentFrame->T_bk_2_b0.transpose() << endl;
cout << "[BA-]current Velocity: " << currentFrame->v_bk.transpose() << endl;
//marginalziation
monoOdometry->twoWayMarginalize();
monoOdometry->setNewMarginalzationFlag();
// pubOdometry(-T_bk1_2_b0, R_bk1_2_b0, pub_odometry, pub_pose );
// pubPath(-T_bk1_2_b0, path_line, pub_path );
pubOdometry(-monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0,
monoOdometry->pub_odometry, monoOdometry->pub_pose );
pubPath(-monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag,
monoOdometry->path_line, monoOdometry->pub_path);
}
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosIMU::UpdateChild()
{
common::Time cur_time = this->world_->GetSimTime();
// rate control
if (this->update_rate_ > 0 &&
(cur_time - this->last_time_).Double() < (1.0 / this->update_rate_))
return;
if ((this->pub_.getNumSubscribers() > 0 && this->topic_name_ != ""))
{
ignition::math::Pose3d pose;
ignition::math::Quaterniond rot;
ignition::math::Vector3d pos;
// Get Pose/Orientation ///@todo: verify correctness
#if GAZEBO_MAJOR_VERSION >= 8
pose = this->link->WorldPose();
#else
pose = this->link->GetWorldPose().Ign();
#endif
// apply xyz offsets and get position and rotation components
pos = pose.Pos() + this->offset_.Pos();
rot = pose.Rot();
// apply rpy offsets
rot = this->offset_.Rot()*rot;
rot.Normalize();
// get Rates
ignition::math::Vector3d vpos = this->link->GetWorldLinearVel().Ign();
ignition::math::Vector3d veul = this->link->GetWorldAngularVel().Ign();
// differentiate to get accelerations
double tmp_dt = this->last_time_.Double() - cur_time.Double();
if (tmp_dt != 0)
{
this->apos_ = (this->last_vpos_ - vpos) / tmp_dt;
this->aeul_ = (this->last_veul_ - veul) / tmp_dt;
this->last_vpos_ = vpos;
this->last_veul_ = veul;
}
// copy data into pose message
this->imu_msg_.header.frame_id = this->frame_name_;
this->imu_msg_.header.stamp.sec = cur_time.sec;
this->imu_msg_.header.stamp.nsec = cur_time.nsec;
// orientation quaternion
// uncomment this if we are reporting orientation in the local frame
// not the case for our imu definition
// // apply fixed orientation offsets of initial pose
// rot = this->initial_pose_.Rot()*rot;
// rot.Normalize();
this->imu_msg_.orientation.x = rot.X();
this->imu_msg_.orientation.y = rot.Y();
this->imu_msg_.orientation.z = rot.Z();
this->imu_msg_.orientation.w = rot.W();
// pass euler angular rates
ignition::math::Vector3d linear_velocity(
veul.X() + this->GaussianKernel(0, this->gaussian_noise_),
veul.Y() + this->GaussianKernel(0, this->gaussian_noise_),
veul.Z() + this->GaussianKernel(0, this->gaussian_noise_));
// rotate into local frame
// @todo: deal with offsets!
linear_velocity = rot.RotateVector(linear_velocity);
this->imu_msg_.angular_velocity.x = linear_velocity.X();
this->imu_msg_.angular_velocity.y = linear_velocity.Y();
this->imu_msg_.angular_velocity.z = linear_velocity.Z();
// pass accelerations
ignition::math::Vector3d linear_acceleration(
apos_.X() + this->GaussianKernel(0, this->gaussian_noise_),
apos_.Y() + this->GaussianKernel(0, this->gaussian_noise_),
apos_.Z() + this->GaussianKernel(0, this->gaussian_noise_));
// rotate into local frame
// @todo: deal with offsets!
linear_acceleration = rot.RotateVector(linear_acceleration);
this->imu_msg_.linear_acceleration.x = linear_acceleration.X();
this->imu_msg_.linear_acceleration.y = linear_acceleration.Y();
this->imu_msg_.linear_acceleration.z = linear_acceleration.Z();
// fill in covariance matrix
/// @todo: let user set separate linear and angular covariance values.
/// @todo: apply appropriate rotations from frame_pose
double gn2 = this->gaussian_noise_*this->gaussian_noise_;
this->imu_msg_.orientation_covariance[0] = gn2;
this->imu_msg_.orientation_covariance[4] = gn2;
this->imu_msg_.orientation_covariance[8] = gn2;
this->imu_msg_.angular_velocity_covariance[0] = gn2;
this->imu_msg_.angular_velocity_covariance[4] = gn2;
this->imu_msg_.angular_velocity_covariance[8] = gn2;
this->imu_msg_.linear_acceleration_covariance[0] = gn2;
this->imu_msg_.linear_acceleration_covariance[4] = gn2;
this->imu_msg_.linear_acceleration_covariance[8] = gn2;
{
boost::mutex::scoped_lock lock(this->lock_);
// publish to ros
if (this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "")
this->pub_Queue->push(this->imu_msg_, this->pub_);
}
// save last time stamp
this->last_time_ = cur_time;
}
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosIMU::UpdateChild()
{
if ((this->pub_.getNumSubscribers() > 0 && this->topic_name_ != ""))
{
math::Pose pose;
math::Quaternion rot;
math::Vector3 pos;
// Get Pose/Orientation ///@todo: verify correctness
pose = this->link->GetWorldPose();
// apply xyz offsets and get position and rotation components
pos = pose.pos + this->offset_.pos;
rot = pose.rot;
// apply rpy offsets
rot = this->offset_.rot*rot;
rot.Normalize();
common::Time cur_time = this->world_->GetSimTime();
// get Rates
math::Vector3 vpos = this->link->GetWorldLinearVel();
math::Vector3 veul = this->link->GetWorldAngularVel();
// differentiate to get accelerations
double tmp_dt = this->last_time_.Double() - cur_time.Double();
if (tmp_dt != 0)
{
this->apos_ = (this->last_vpos_ - vpos) / tmp_dt;
this->aeul_ = (this->last_veul_ - veul) / tmp_dt;
this->last_vpos_ = vpos;
this->last_veul_ = veul;
}
// copy data into pose message
this->imu_msg_.header.frame_id = this->frame_name_;
this->imu_msg_.header.stamp.sec = cur_time.sec;
this->imu_msg_.header.stamp.nsec = cur_time.nsec;
// orientation quaternion
// uncomment this if we are reporting orientation in the local frame
// not the case for our imu definition
// // apply fixed orientation offsets of initial pose
// rot = this->initial_pose_.rot*rot;
// rot.Normalize();
this->imu_msg_.orientation.x = rot.x;
this->imu_msg_.orientation.y = rot.y;
this->imu_msg_.orientation.z = rot.z;
this->imu_msg_.orientation.w = rot.w;
// pass euler angular rates
math::Vector3 linear_velocity(
veul.x + this->GaussianKernel(0, this->gaussian_noise_),
veul.y + this->GaussianKernel(0, this->gaussian_noise_),
veul.z + this->GaussianKernel(0, this->gaussian_noise_));
// rotate into local frame
// @todo: deal with offsets!
linear_velocity = rot.RotateVector(linear_velocity);
this->imu_msg_.angular_velocity.x = linear_velocity.x;
this->imu_msg_.angular_velocity.y = linear_velocity.y;
this->imu_msg_.angular_velocity.z = linear_velocity.z;
// pass accelerations
math::Vector3 linear_acceleration(
apos_.x + this->GaussianKernel(0, this->gaussian_noise_),
apos_.y + this->GaussianKernel(0, this->gaussian_noise_),
apos_.z + this->GaussianKernel(0, this->gaussian_noise_));
// rotate into local frame
// @todo: deal with offsets!
linear_acceleration = rot.RotateVector(linear_acceleration);
this->imu_msg_.linear_acceleration.x = linear_acceleration.x;
this->imu_msg_.linear_acceleration.y = linear_acceleration.y;
this->imu_msg_.linear_acceleration.z = linear_acceleration.z;
// fill in covariance matrix
/// @todo: let user set separate linear and angular covariance values.
/// @todo: apply appropriate rotations from frame_pose
double gn2 = this->gaussian_noise_*this->gaussian_noise_;
this->imu_msg_.orientation_covariance[0] = gn2;
this->imu_msg_.orientation_covariance[4] = gn2;
this->imu_msg_.orientation_covariance[8] = gn2;
this->imu_msg_.angular_velocity_covariance[0] = gn2;
this->imu_msg_.angular_velocity_covariance[4] = gn2;
this->imu_msg_.angular_velocity_covariance[8] = gn2;
this->imu_msg_.linear_acceleration_covariance[0] = gn2;
this->imu_msg_.linear_acceleration_covariance[4] = gn2;
this->imu_msg_.linear_acceleration_covariance[8] = gn2;
{
boost::mutex::scoped_lock lock(this->lock_);
// publish to ros
if (this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "")
this->pub_.publish(this->imu_msg_);
}
// save last time stamp
this->last_time_ = cur_time;
}
}
