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