dead_reckoning.cpp
Go to the documentation of this file.
00001 
00026 #include "ros/ros.h"
00027 #include "tf/transform_datatypes.h"
00028 #include "grizzly_motion/dead_reckoning.h"
00029 #include <angles/angles.h>
00030 
00031 using Eigen::Vector2f;
00032 
00039 bool DeadReckoning::next(const grizzly_msgs::DriveConstPtr& encoders, nav_msgs::Odometry* odom, sensor_msgs::JointState* joints)
00040 {
00041   bool success = false;
00042   VectorDrive wheels_speed_ = grizzly_msgs::vectorFromDriveMsg(*encoders);
00043   uint8_t wheels_num_ = wheels_speed_.size();
00044   uint8_t joints_num_ = wheels_num_ + 1; // add front_axle_joint
00045 
00046   if(!initialize) { 
00047     last_joint_pos_.resize(joints_num_, 0.0); 
00048     last_time_ = encoders->header.stamp; 
00049     yaw_ = 0.0;  
00050     initialize = true;
00051     ROS_INFO("initialization complete.");
00052   }
00053 
00054   // Angular velocity per-side in rad/s, velocity per-size in m/s
00055   Vector2f avg_rotations((encoders->front_left + encoders->rear_left) / 2,
00056                          (encoders->front_right + encoders->rear_right) / 2);
00057   Vector2f vels = avg_rotations * radius_;
00058 
00059   joints->position.resize(joints_num_, 0.0);
00060   joints->effort.resize(joints_num_,0);
00061 
00062   for(uint8_t i = 0; i < wheels_num_; i++){
00063     joints->velocity.push_back(wheels_speed_[i]);
00064     joints->name.push_back("joint_"+grizzly_msgs::nameFromDriveIndex(i)+"_wheel");
00065   }
00066   joints->velocity.push_back(0);
00067   joints->name.push_back("front_axle_joint");
00068 
00069 
00070   ros::Duration dt = encoders->header.stamp - last_time_;
00071 
00072   if (dt <= max_dt_) {
00073     // Integrate position based on previous yaw and speed
00074      position_.x += cos(yaw_) * twist_.linear.x * dt.toSec();
00075      position_.y += sin(yaw_) * twist_.linear.x * dt.toSec();
00076 
00077     // Update heading by integrating previous angular velocity.
00078      yaw_ += twist_.angular.z * dt.toSec();
00079 
00080     // Update linear and angular velocity
00081     twist_.linear.x = vels.mean();
00082     twist_.angular.z = (vels[1] - vels[0]) / width_;
00083 
00084     // Timestamp from encoder message, set frames correctly.
00085     odom->header = encoders->header;
00086     odom->header.frame_id = "odom";
00087     odom->child_frame_id = "base_link"; 
00088     odom->pose.pose.position = position_;
00089     odom->pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_);
00090     odom->twist.twist = twist_;
00091 
00092     Eigen::Map<Eigen::MatrixXd> poseCov(odom->pose.covariance.data(), 6, 6);
00093     Eigen::Map<Eigen::MatrixXd> twistCov(odom->twist.covariance.data(), 6, 6);
00094 
00095     if(fabs(twist_.linear.x) <= 1e-3 && fabs(twist_.angular.z) <= 1e-3) {
00096       poseCov = ODOM_POSE_COVAR_NOMOVE;
00097       twistCov = ODOM_TWIST_COVAR_NOMOVE;
00098       joints->position = last_joint_pos_;
00099     } 
00100     else {
00101       poseCov = ODOM_POSE_COVAR_MOTION;
00102       twistCov = ODOM_TWIST_COVAR_MOTION;
00103 
00104       // update joint's position
00105       for(uint8_t i = 0; i < wheels_num_; i++) {
00106         joints->position[i] = last_joint_pos_[i] + dt.toSec() * joints->velocity[i]; 
00107         joints->position[i] = angles::normalize_angle(joints->position[i]);
00108       }
00109     }
00110 
00111     joints->header = encoders->header;
00112     success = true;
00113   } else {
00114     static bool first = true;
00115     ROS_WARN_COND(first, "Gap between encoders messages is too large, no odom generated.");
00116     first = false;
00117   }
00118 
00119   last_time_ = encoders->header.stamp;
00120   last_joint_pos_ = joints->position;
00121   last_vels_ = vels;
00122   return success;
00123 }


grizzly_motion
Author(s):
autogenerated on Thu Jun 6 2019 21:44:02