/**
* Find the current positonal error of the robot
* @param curPose The current position of the robot
* @returns the error of the position, in the robot's current frame of reference
*/
Twist Tracking::getError(Pose& curPose) {
float xError = mExpectedPose.position.x - curPose.position.x;
float yError = mExpectedPose.position.y - curPose.position.y;
tf::Quaternion q;
double unusedRoll, unusedPitch;
double curYaw, expectedYaw;
quaternionMsgToTF(curPose.orientation, q);
tf::Matrix3x3(q).getRPY(unusedRoll, unusedPitch, curYaw);
quaternionMsgToTF(mExpectedPose.orientation, q);
tf::Matrix3x3(q).getRPY(unusedRoll, unusedPitch, expectedYaw);
Twist error;
error.angular.z = fmod(expectedYaw - curYaw, 2*PI);
if (error.angular.z > PI) {
error.angular.z -= PI;
}
// put x/y error in terms of the robot's orientation
error.linear.x = xError * cos(curYaw) + yError * sin(curYaw);
error.linear.y = xError * (-sin(curYaw)) + yError * cos(curYaw);
return error;
}
/**
* Propogates the dynamics for 1 step
*/
Pose propogateDynamics(Pose start, float speed, float turnRate) {
Pose result = start;
double roll, pitch, yaw;
tf::Quaternion bt_q;
quaternionMsgToTF(start.orientation, bt_q);
tf::Matrix3x3(bt_q).getRPY(roll, pitch, yaw);
result.position.x += CYCLE_TIME * speed * cos(yaw);
result.position.y += CYCLE_TIME * speed * sin(yaw);
yaw += CYCLE_TIME * turnRate;
quaternionTFToMsg(
tf::createQuaternionFromRPY(roll, pitch, yaw),
result.orientation);
return result;
}
void PointCloudLinker::link(DispPointCloudIMUPtr& msg)
{
if(first)
{
relative.x = msg->pose.eulerPose.pose.position.x;
relative.y = msg->pose.eulerPose.pose.position.y;
relative.z = msg->pose.eulerPose.pose.position.z;
first = false;
}
PointCloudXYZRGB pc;
fromROSMsg(msg->pointCloud.pointCloud, pc);
PointXYZRGB sensorOrigin;
sensorOrigin.x = sensorOrigin.y = sensorOrigin.z = 0;
PointXYZRGB frameOriginTrans;
frameOriginTrans.x = msg->pose.eulerPose.pose.position.x - relative.x;
frameOriginTrans.y = msg->pose.eulerPose.pose.position.y - relative.y;
frameOriginTrans.z = msg->pose.eulerPose.pose.position.z - relative.z;
printf("\n\nbefore\n");
printf("x: %.2f\n", msg->pose.quartenionPose.pose.orientation.x);
printf("y: %.2f\n", msg->pose.quartenionPose.pose.orientation.y);
printf("z: %.2f\n", msg->pose.quartenionPose.pose.orientation.z);
printf("w: %.2f\n", msg->pose.quartenionPose.pose.orientation.w);
msg->pose.eulerPose.pose.orientation.x += inclination;
IMU::convertEulerToQuartenion(msg->pose.eulerPose.pose, msg->pose.quartenionPose.pose);
printf("\nafter\n");
printf("x: %.2f\n", msg->pose.quartenionPose.pose.orientation.x);
printf("y: %.2f\n", msg->pose.quartenionPose.pose.orientation.y);
printf("z: %.2f\n", msg->pose.quartenionPose.pose.orientation.z);
printf("w: %.2f\n\n", msg->pose.quartenionPose.pose.orientation.w);
tf::Quaternion frameOriginRot;
quaternionMsgToTF(msg->pose.quartenionPose.pose.orientation, frameOriginRot);
map.insertScan<PointXYZRGB, tf::Quaternion>(pc, sensorOrigin, frameOriginTrans, frameOriginRot);
}
void MatFromIMU::imuCallback(const ImuConstPtr& imu)
{
imu_stamp_ = imu->header.stamp;
tf::Quaternion orientation;
quaternionMsgToTF(imu->orientation, orientation);
imu_meas_ = tf::Transform(orientation, tf::Vector3(0,0,0));
//orientation_covariance = imu->orientation_covariance;
for (unsigned int i=0; i<3; i++)
for (unsigned int j=0; j<3; j++)
imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j];
/*/ Transforms imu data to base_footprint frame
if (!robot_state_.waitForTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){
ROS_ERROR("Could not transform imu message from %s to %s", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
ROS_DEBUG("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
return;
}
tf::StampedTransform base_imu_offset;
robot_state_.lookupTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, base_imu_offset);
imu_meas_ = imu_meas_ * base_imu_offset;
*/
// imu_time_ = ros::Time::now();
// manually set covariance untile imu sends covariance
if (imu_covariance_(1,1) == 0.0){
MatrixWrapper::SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
measNoiseImu_Cov(1,1) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(2,2) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(3,3) = pow(0.00017,2); // = 0.01 degrees / sec
imu_covariance_ = measNoiseImu_Cov;
}
robot_broadcaster_.sendTransform(tf::StampedTransform(imu_meas_.inverse(), imu_stamp_, base_footprint_frame_, "imuTF"));
};
// callback function for imu data
void OdomEstimationNode::imuCallback(const ImuConstPtr& imu)
{
imu_callback_counter_++;
assert(imu_used_);
// receive data
imu_stamp_ = imu->header.stamp;
tf::Quaternion orientation;
quaternionMsgToTF(imu->orientation, orientation);
imu_meas_ = tf::Transform(orientation, tf::Vector3(0,0,0));
for (unsigned int i=0; i<3; i++)
for (unsigned int j=0; j<3; j++)
imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j];
// Transforms imu data to base_footprint frame
if (!robot_state_.waitForTransform("base_footprint", imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){
// warn when imu was already activated, not when imu is not active yet
if (imu_active_)
ROS_ERROR("Could not transform imu message from %s to base_footprint", imu->header.frame_id.c_str());
else if (my_filter_.isInitialized())
ROS_WARN("Could not transform imu message from %s to base_footprint. Imu will not be activated yet.", imu->header.frame_id.c_str());
else
ROS_DEBUG("Could not transform imu message from %s to base_footprint. Imu will not be activated yet.", imu->header.frame_id.c_str());
return;
}
StampedTransform base_imu_offset;
robot_state_.lookupTransform("base_footprint", imu->header.frame_id, imu_stamp_, base_imu_offset);
imu_meas_ = imu_meas_ * base_imu_offset;
imu_time_ = Time::now();
// manually set covariance untile imu sends covariance
if (imu_covariance_(1,1) == 0.0){
SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
measNoiseImu_Cov(1,1) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(2,2) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(3,3) = pow(0.00017,2); // = 0.01 degrees / sec
imu_covariance_ = measNoiseImu_Cov;
}
my_filter_.addMeasurement(StampedTransform(imu_meas_.inverse(), imu_stamp_, "base_footprint", "imu"), imu_covariance_);
// activate imu
if (!imu_active_) {
if (!imu_initializing_){
imu_initializing_ = true;
imu_init_stamp_ = imu_stamp_;
ROS_INFO("Initializing Imu sensor");
}
if ( filter_stamp_ >= imu_init_stamp_){
imu_active_ = true;
imu_initializing_ = false;
ROS_INFO("Imu sensor activated");
}
else ROS_DEBUG("Waiting to activate IMU, because IMU measurements are still %f sec in the future.",
(imu_init_stamp_ - filter_stamp_).toSec());
}
if (debug_){
// write to file
double tmp, yaw;
imu_meas_.getBasis().getEulerYPR(yaw, tmp, tmp);
imu_file_ << yaw << endl;
}
};
