/*
* Odometry (encoders & IMU)
*/
void GazeboRosKobuki::updateOdometry(common::Time& step_time)
{
std::string odom_frame = gazebo_ros_->resolveTF("odom");
std::string base_frame = gazebo_ros_->resolveTF("base_footprint");
odom_.header.stamp = joint_state_.header.stamp;
odom_.header.frame_id = odom_frame;
odom_.child_frame_id = base_frame;
// Distance travelled by main wheels
double d1, d2;
double dr, da;
d1 = d2 = 0;
dr = da = 0;
// 多分ここがencoder 直接速度を取得してる
// (wheel_diam_ / 2)= 直径/2= 半径
// 弧度法なら (theta/2*pi)*radius
d1 = step_time.Double() * (wheel_diam_ / 2) * joints_[LEFT]->GetVelocity(0);
d2 = step_time.Double() * (wheel_diam_ / 2) * joints_[RIGHT]->GetVelocity(0);
// Can see NaN values here, just zero them out if needed
if (isnan(d1))
{
ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d1. Step time: " << step_time.Double()
<< ", WD: " << wheel_diam_ << ", velocity: " << joints_[LEFT]->GetVelocity(0));
d1 = 0;
}
if (isnan(d2))
{
ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d2. Step time: " << step_time.Double()
<< ", WD: " << wheel_diam_ << ", velocity: " << joints_[RIGHT]->GetVelocity(0));
d2 = 0;
}
// 参照
// http://www.mech.tohoku-gakuin.ac.jp/rde/contents/course/robotics/wheelrobot.html
// もし旋回ならd1+d2が0になって、並進距離は0になる？
dr = (d1 + d2) / 2; // 台車中心の速度
// wheel_sep_は2車輪の距離
da = (d2 - d1) / wheel_sep_; // 台車の旋回速度
// std::cout << "omega[rad/s]: " << da << std::endl;
// imuから直接旋回速度を取得してる
// Just as in the Kobuki driver, the angular velocity is taken directly from the IMU
// vel_angular_ = imu_->GetAngularVelocity();
// Compute odometric pose
odom_pose_[0] += dr * cos( odom_pose_[2] );
odom_pose_[1] += dr * sin( odom_pose_[2] );
// original
// odom_pose_[2] += vel_angular_.z * step_time.Double();
// 車輪速度から算出した、旋回速度。の積分
odom_pose_[2] += da;
// Compute odometric instantaneous velocity
odom_vel_[0] = dr / step_time.Double();
odom_vel_[1] = 0.0;
// odom_vel_[2] = vel_angular_.z;
odom_.pose.pose.position.x = odom_pose_[0];
odom_.pose.pose.position.y = odom_pose_[1];
odom_.pose.pose.position.z = 0;
tf::Quaternion qt;
qt.setEuler(0,0,odom_pose_[2]);
odom_.pose.pose.orientation.x = qt.getX();
odom_.pose.pose.orientation.y = qt.getY();
odom_.pose.pose.orientation.z = qt.getZ();
odom_.pose.pose.orientation.w = qt.getW();
// odom_.pose.covariance[0] = 0; // x
// odom_.pose.covariance[7] = 0; // y
// odom_.pose.covariance[35] = 0 ; // yaw
odom_.pose.covariance[0] = 0.1; // x
odom_.pose.covariance[7] = 0.1; // y
odom_.pose.covariance[35] = 0.05 ; // yaw
// odom_.pose.covariance[35] = 0.05; // yaw
odom_.pose.covariance[14] = 1e6;
odom_.pose.covariance[21] = 1e6;
odom_.pose.covariance[28] = 1e6;
odom_.twist.twist.linear.x = odom_vel_[0];
odom_.twist.twist.linear.y = 0;
odom_.twist.twist.linear.z = 0;
odom_.twist.twist.angular.x = 0;
odom_.twist.twist.angular.y = 0;
odom_.twist.twist.angular.z = odom_vel_[2];
odom_pub_.publish(odom_); // publish odom message
if (publish_tf_)
{
odom_tf_.header = odom_.header;
odom_tf_.child_frame_id = odom_.child_frame_id;
odom_tf_.transform.translation.x = odom_.pose.pose.position.x;
odom_tf_.transform.translation.y = odom_.pose.pose.position.y;
odom_tf_.transform.translation.z = odom_.pose.pose.position.z;
odom_tf_.transform.rotation = odom_.pose.pose.orientation;
tf_broadcaster_.sendTransform(odom_tf_);
}
}
/*
* Odometry (encoders & IMU)
*/
void GazeboRosKobuki::updateOdometry(common::Time& step_time)
{
std::string odom_frame = gazebo_ros_->resolveTF("odom");
std::string base_frame = gazebo_ros_->resolveTF("base_footprint");
odom_.header.stamp = joint_state_.header.stamp;
odom_.header.frame_id = odom_frame;
odom_.child_frame_id = base_frame;
// Distance travelled by main wheels
double d1, d2;
double dr, da;
d1 = d2 = 0;
dr = da = 0;
d1 = step_time.Double() * (wheel_diam_ / 2) * joints_[LEFT]->GetVelocity(0);
d2 = step_time.Double() * (wheel_diam_ / 2) * joints_[RIGHT]->GetVelocity(0);
// Can see NaN values here, just zero them out if needed
if (isnan(d1))
{
ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d1. Step time: " << step_time.Double()
<< ", WD: " << wheel_diam_ << ", velocity: " << joints_[LEFT]->GetVelocity(0));
d1 = 0;
}
if (isnan(d2))
{
ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d2. Step time: " << step_time.Double()
<< ", WD: " << wheel_diam_ << ", velocity: " << joints_[RIGHT]->GetVelocity(0));
d2 = 0;
}
dr = (d1 + d2) / 2;
da = (d2 - d1) / wheel_sep_; // ignored
// Just as in the Kobuki driver, the angular velocity is taken directly from the IMU
vel_angular_ = imu_->GetAngularVelocity();
// Compute odometric pose
odom_pose_[0] += dr * cos( odom_pose_[2] );
odom_pose_[1] += dr * sin( odom_pose_[2] );
odom_pose_[2] += vel_angular_.z * step_time.Double();
// Compute odometric instantaneous velocity
odom_vel_[0] = dr / step_time.Double();
odom_vel_[1] = 0.0;
odom_vel_[2] = vel_angular_.z;
odom_.pose.pose.position.x = odom_pose_[0];
odom_.pose.pose.position.y = odom_pose_[1];
odom_.pose.pose.position.z = 0;
tf::Quaternion qt;
qt.setEuler(0,0,odom_pose_[2]);
odom_.pose.pose.orientation.x = qt.getX();
odom_.pose.pose.orientation.y = qt.getY();
odom_.pose.pose.orientation.z = qt.getZ();
odom_.pose.pose.orientation.w = qt.getW();
odom_.pose.covariance[0] = 0.1;
odom_.pose.covariance[7] = 0.1;
odom_.pose.covariance[35] = 0.05;
odom_.pose.covariance[14] = 1e6;
odom_.pose.covariance[21] = 1e6;
odom_.pose.covariance[28] = 1e6;
odom_.twist.twist.linear.x = odom_vel_[0];
odom_.twist.twist.linear.y = 0;
odom_.twist.twist.linear.z = 0;
odom_.twist.twist.angular.x = 0;
odom_.twist.twist.angular.y = 0;
odom_.twist.twist.angular.z = odom_vel_[2];
odom_pub_.publish(odom_); // publish odom message
if (publish_tf_)
{
odom_tf_.header = odom_.header;
odom_tf_.child_frame_id = odom_.child_frame_id;
odom_tf_.transform.translation.x = odom_.pose.pose.position.x;
odom_tf_.transform.translation.y = odom_.pose.pose.position.y;
odom_tf_.transform.translation.z = odom_.pose.pose.position.z;
odom_tf_.transform.rotation = odom_.pose.pose.orientation;
tf_broadcaster_.sendTransform(odom_tf_);
}
}