00001 #include "segway_rmp400_odom_6dex_fused_alg_node.h"
00002
00003 SegwayRmp400OdomAlgNode::SegwayRmp400OdomAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<SegwayRmp400OdomAlgorithm>()
00005 {
00006
00007 loop_rate_ = 100;
00008
00009 public_node_handle_.param<std::string>("tf_prefix", tf_prefix_, "");
00010 public_node_handle_.param<std::string>("odom_id", odom_id_, "odom" );
00011 public_node_handle_.param<std::string>("base_link_id", base_link_id_, "base_footprint" );
00012 public_node_handle_.param<bool> ("publish_tf", publish_tf_, true );
00013
00014
00015 if( tf_prefix_.size()!=0 )
00016 {
00017 odom_id_ = tf_prefix_ + "/" + odom_id_;
00018 base_link_id_ = tf_prefix_ + "/" + base_link_id_;
00019 }
00020
00021 last_time_ = ros::Time::now();
00022 usleep(10);
00023 current_time_ = ros::Time::now();
00024
00025 accum_.translation.x=0.0; accum_.translation.y=0.0; accum_.translation.z=0.0;
00026 accum_.rotation.x=0.0; accum_.rotation.y=0.0; accum_.rotation.z=0.0; accum_.rotation.w=1.0;
00027
00028 H.setIdentity();
00029 ROS_DEBUG("odo3d Hinit: %f %f %f %f", H(0,0), H(0,1), H(0,2), H(0,3));
00030 ROS_DEBUG(" %f %f %f %f", H(1,0), H(1,1), H(1,2), H(1,3));
00031 ROS_DEBUG(" %f %f %f %f", H(2,0), H(2,1), H(2,2), H(2,3));
00032 ROS_DEBUG(" %f %f %f %f", H(3,0), H(3,1), H(3,2), H(3,3));
00033
00034
00035 odom_publisher_ = public_node_handle_.advertise<nav_msgs::Odometry>("odom", 100);
00036
00037
00038 segway_status_subscriber_ = public_node_handle_.subscribe("segway_status", 100, &SegwayRmp400OdomAlgNode::segway_status_callback, this);
00039 imu_subscriber_ = public_node_handle_.subscribe("imu", 100, &SegwayRmp400OdomAlgNode::imu_callback, this);
00040
00041
00042
00043
00044
00045
00046
00047
00048 }
00049
00050 SegwayRmp400OdomAlgNode::~SegwayRmp400OdomAlgNode(void)
00051 {
00052
00053 }
00054
00055 void SegwayRmp400OdomAlgNode::mainNodeThread(void)
00056 {
00057 geometry_msgs::TransformStamped odom_trans_msg;
00058
00059
00060 double dt = (current_time_ - last_time_).toSec();
00061
00062
00063 if(dt<1.0) {
00064
00065
00066 imu_mutex_.enter();
00067 w << vrimu_, vpimu_, vyimu_;
00068 imu_mutex_.exit();
00069 ROS_DEBUG("odo3D w: %f %f %f", w(0), w(1), w(2));
00070
00071
00072 wt = w*dt;
00073 ROS_DEBUG("odo3D wt: %f %f %f", wt(0), wt(1), wt(2));
00074
00075
00076 wt_norm = wt.norm();
00077 ROS_DEBUG("odo3D wtnorm: %f", wt_norm);
00078
00079
00080 ww = wt/wt_norm;
00081 ROS_DEBUG("odo3D ww: %f %f %f", ww(0), ww(1), ww(2));
00082
00083
00084 wt_hat << 0.0, -ww(2), ww(1), ww(2), 0.0, -ww(0), -ww(1), ww(0), 0.0;
00085
00086
00087 segway_status_mutex_.enter();
00088 v << ((left_wheels_velocity_ + right_wheels_velocity_)/2), 0.0, 0.0;
00089 segway_status_mutex_.exit();
00090 ROS_DEBUG("odo3D v: %f %f %f", v(0), v(1), v(2));
00091
00092
00093
00094 epsilon = 1e-9;
00095
00096 I.setIdentity();
00097
00098
00099 if (wt_norm < epsilon) {
00100 R.setIdentity();
00101 t =v*dt;
00102 ROS_DEBUG("odo3D: warning rotation < epsilon!");
00103 }
00104 else {
00105 R = I + wt_hat * sin(wt_norm) + wt_hat*wt_hat*(1-cos(wt_norm));
00106
00107
00108 ta=(I-R)*wt_hat*v*dt/wt_norm ;
00109 tb= (ww(0)*v(0)+ww(1)*v(1)+ww(2)*v(2))*ww*dt;
00110 t = ta+tb;
00111 ROS_DEBUG("odo3D: warning rotation > epsilon: t = %f %f %f + %f %f %f",ta(0),ta(1),ta(2),tb(0),tb(1),tb(2));
00112 }
00113
00114 ROS_DEBUG("odo3d R: %f %f %f", R(0,0), R(0,1), R(0,2));
00115 ROS_DEBUG(" %f %f %f", R(1,0), R(1,1), R(1,2));
00116 ROS_DEBUG(" %f %f %f", R(2,0), R(2,1), R(2,2));
00117 ROS_DEBUG("odo3D t: %f %f %f", t(0), t(1), t(2));
00118
00119 g.setIdentity();
00120 g.block<3,3>(0,0) = R;
00121 g.block<3,1>(0,3) = t;
00122
00123 H = H*g;
00124
00125 ROS_DEBUG("odo3d H: %f %f %f", H(0,0), H(0,1), H(0,2));
00126 ROS_DEBUG(" %f %f %f", H(1,0), H(1,1), H(1,2));
00127 ROS_DEBUG(" %f %f %f", H(2,0), H(2,1), H(2,2));
00128
00129 d(0) = 1 + H(0,0) - H(1,1) - H(2,2);
00130 d(1) = 1 - H(0,0) + H(1,1) - H(2,2);
00131 d(2) = 1 - H(0,0) - H(1,1) + H(2,2);
00132 d(3) = 1 + H(0,0) + H(1,1) + H(2,2);
00133
00134 int i;
00135 d.maxCoeff(&i);
00136
00137 switch (i) {
00138 case 0:
00139 q(0) = sqrt(d(0))/2;
00140 q(1) = (H(1,0) + H(0,1)) / (4 * q(0));
00141 q(2) = (H(2,0) + H(0,2)) / (4 * q(0));
00142 q(3) = (H(2,1) - H(1,2)) / (4 * q(0));
00143 break;
00144 case 1:
00145 q(1) = sqrt(d(1))/2;
00146 q(0) = (H(1,0) + H(0,1)) / (4 * q(1));
00147 q(2) = (H(2,1) + H(1,2)) / (4 * q(1));
00148 q(3) = (H(0,2) - H(2,0)) / (4 * q(1));
00149 break;
00150 case 2:
00151 q(2) = sqrt(d(2))/2;
00152 q(0) = (H(2,0) + H(0,2)) / (4 * q(2));
00153 q(1) = (H(2,1) + H(1,2)) / (4 * q(2));
00154 q(3) = (H(1,0) - H(0,1)) / (4 * q(2));
00155 break;
00156 case 3:
00157 q(3)= sqrt(d(3))/2;
00158 q(0) = (H(2,1) - H(1,2)) / (4 * q(3));
00159 q(1) = (H(0,2) - H(2,0)) / (4 * q(3));
00160 q(2) = (H(1,0) - H(0,1)) / (4 * q(3));
00161 break;
00162 default:
00163 ROS_ERROR("Error computing quaternion");
00164 }
00165
00166 ROS_DEBUG("odo3D q: %f %f %f %f", q(0), q(1), q(2), q(3));
00167
00168
00169
00170
00171 accum_.translation.x = H(0,3);
00172 accum_.translation.y = H(1,3);
00173 accum_.translation.z = H(2,3);
00174 accum_.rotation.x = q(0);
00175 accum_.rotation.y = q(1);
00176 accum_.rotation.z = q(2);
00177 accum_.rotation.w = q(3);
00178
00179 transform_.translation = accum_.translation;
00180 transform_.rotation = accum_.rotation;
00181
00182 ROS_INFO("x %f y %f z %f",transform_.translation.x, transform_.translation.y, transform_.translation.z);
00183
00184 pose_.pose.position.x = accum_.translation.x;
00185 pose_.pose.position.y = accum_.translation.y;
00186 pose_.pose.position.z = accum_.translation.z;
00187 pose_.pose.orientation = accum_.rotation;
00188 pose_.covariance[0] =
00189 pose_.covariance[7] = 0.01;
00190 pose_.covariance[14] =
00191 pose_.covariance[21] =
00192 pose_.covariance[28] =
00193 pose_.covariance[35] = 1e10;
00194
00195
00196
00197
00198 twist_.twist.linear.x = v(0);
00199 twist_.twist.linear.y = 0.0;
00200 twist_.twist.linear.z = 0.0;
00201 twist_.twist.angular.x = w(0);
00202 twist_.twist.angular.y = w(1);
00203 twist_.twist.angular.z = w(2);
00204
00205
00206
00207 twist_.covariance[0] = 0.0001;
00208 twist_.covariance[7] = 0.0001;
00209 twist_.covariance[14] =
00210 twist_.covariance[21] =
00211 twist_.covariance[28] =
00212
00213 twist_.covariance[35] = 999;
00214 }
00215
00216
00217 last_time_ = current_time_;
00218
00219
00220 if(publish_tf_)
00221 {
00222
00223 odom_trans_msg.header.stamp = current_time_;
00224 odom_trans_msg.header.frame_id = odom_id_;
00225 odom_trans_msg.child_frame_id = base_link_id_;
00226 odom_trans_msg.transform = transform_;
00227
00228 odom_broadcaster_.sendTransform(odom_trans_msg);
00229 }
00230
00231
00232 Odometry_msg_.header.stamp = current_time_;
00233 Odometry_msg_.header.frame_id = odom_id_;
00234 Odometry_msg_.child_frame_id = base_link_id_;
00235 Odometry_msg_.pose = pose_;
00236 Odometry_msg_.twist = twist_;
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246 odom_publisher_.publish(Odometry_msg_);
00247 }
00248
00249
00250 void SegwayRmp400OdomAlgNode::segway_status_callback(const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr& msg)
00251 {
00252 ROS_DEBUG("SegwayRmp400OdomAlgNode::segway_status_callback: New Message Received");
00253
00254
00255
00256 segway_status_mutex_.enter();
00257
00258
00259 left_wheels_velocity_ = (msg->rmp200[0].left_wheel_velocity + msg->rmp200[1].left_wheel_velocity)/2;
00260 right_wheels_velocity_ = (msg->rmp200[0].right_wheel_velocity + msg->rmp200[1].right_wheel_velocity)/2;
00261
00262
00263
00264 segway_status_mutex_.exit();
00265 }
00266
00267 void SegwayRmp400OdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
00268 {
00269 ROS_DEBUG("SegwayRmp400OdomAlgNode::imu_callback: New Message Received");
00270
00271
00272
00273 imu_mutex_.enter();
00274
00275
00276 current_time_ = msg->header.stamp;
00277
00278 vrimu_ = msg->angular_velocity.x;
00279 vpimu_ = -msg->angular_velocity.y;
00280 vyimu_ = -msg->angular_velocity.z;
00281
00282
00283
00284 imu_mutex_.exit();
00285 }
00286
00287
00288
00289
00290
00291
00292
00293 void SegwayRmp400OdomAlgNode::node_config_update(Config &config, uint32_t level)
00294 {
00295 alg_.lock();
00296
00297 alg_.unlock();
00298 }
00299
00300 void SegwayRmp400OdomAlgNode::addNodeDiagnostics(void)
00301 {
00302 }
00303
00304
00305 int main(int argc,char *argv[])
00306 {
00307 return algorithm_base::main<SegwayRmp400OdomAlgNode>(argc, argv, "segway_rmp400_odom_alg_node");
00308 }