00001 #include "segway_rmp400_odom_nokalman_alg_node.h"
00002
00003 SegwayRmp400OdomAlgNode::SegwayRmp400OdomAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<SegwayRmp400OdomAlgorithm>(),
00005 r_(0),
00006 p_(0),
00007 y_(0)
00008 {
00009
00010 loop_rate_ = 100;
00011
00012 public_node_handle_.param<std::string>("tf_prefix", tf_prefix_, "");
00013 public_node_handle_.param<std::string>("odom_id", odom_id_, "odom" );
00014 public_node_handle_.param<std::string>("base_link_id", base_link_id_, "base_footprint" );
00015 public_node_handle_.param<bool> ("six_d", six_d_, false );
00016 public_node_handle_.param<bool> ("publish_tf", publish_tf_, true );
00017
00018
00019 if( tf_prefix_.size()!=0 )
00020 {
00021 odom_id_ = tf_prefix_ + "/" + odom_id_;
00022 base_link_id_ = tf_prefix_ + "/" + base_link_id_;
00023 }
00024
00025 last_time_ = ros::Time::now();
00026 usleep(10);
00027 current_time_ = ros::Time::now();
00028
00029 rk_.resize(3,0);
00030 pk_.resize(3,0);
00031 vyk_.resize(3,0);
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049 double tmp[] ={100,100,0.00000676};
00050 double tmp2[]={100,100,0.00002375};
00051 double tmp3[]={100,100,0.00003790};
00052 rkc_.assign(tmp, tmp+3);
00053 pkc_.assign(tmp2, tmp2+3);
00054 vykc_.assign(tmp3, tmp3+3);
00055
00056
00057 odom_publisher_ = public_node_handle_.advertise<nav_msgs::Odometry>("odom", 100);
00058
00059
00060 segway_status_subscriber_ = public_node_handle_.subscribe("segway_status", 100, &SegwayRmp400OdomAlgNode::segway_status_callback, this);
00061 imu_subscriber_ = public_node_handle_.subscribe("imu", 100, &SegwayRmp400OdomAlgNode::imu_callback, this);
00062
00063
00064
00065
00066
00067
00068
00069
00070 }
00071
00072 SegwayRmp400OdomAlgNode::~SegwayRmp400OdomAlgNode(void)
00073 {
00074
00075 }
00076
00077 double SegwayRmp400OdomAlgNode::pseudokalman3(std::vector<double> v, std::vector<double> c)
00078 {
00079 return (v[0]*c[1]*c[2] + v[1]*c[0]*c[2] + v[2]*c[0]*c[1] ) / ( c[1]*c[2] + c[0]*c[2] + c[0]*c[1] );
00080 }
00081
00082 void SegwayRmp400OdomAlgNode::mainNodeThread(void)
00083 {
00084
00085 geometry_msgs::TransformStamped odom_trans_msg;
00086
00087
00088
00089 double dt = (current_time_ - last_time_).toSec();
00090 if (dt > 10) dt = 0.01;
00091
00092
00093 last_time_ = current_time_;
00094
00095
00096
00097 segway_status_mutex_.enter();
00098 double vT = (left_wheels_velocity_ + right_wheels_velocity_) / 2;
00099 segway_status_mutex_.exit();
00100
00101
00102
00103 imu_mutex_.enter();
00104 float vrimu = vrimu_;
00105 float vpimu = vpimu_;
00106 float vyimu = vyimu_;
00107
00108
00109
00110 imu_mutex_.exit();
00111
00112 r_ = pseudokalman3(rk_,rkc_);
00113 p_ = pseudokalman3(pk_,pkc_);
00114 y_ += pseudokalman3(vyk_,vykc_) * dt/2;
00115
00116
00117 double vTx = six_d_ ? vT*cos(y_)*cos(p_) : vT*cos(y_);
00118 double vTy = six_d_ ? vT*sin(y_)*cos(r_) : vT*sin(y_);
00119 double vTz = six_d_ ? vT*cos(r_)*sin(p_) : 0.0;
00120
00121
00122 double dx = vTx * dt;
00123 double dy = vTy * dt;
00124 double dz = vTz * dt;
00125
00126
00127 y_ += pseudokalman3(vyk_,vykc_) * dt/2;
00128
00129
00130 geometry_msgs::Quaternion rot;
00131 if(six_d_) rot = tf::createQuaternionMsgFromRollPitchYaw(r_,-p_,y_);
00132 else rot = tf::createQuaternionMsgFromYaw (y_);
00133
00134
00135 transform_.translation.x += dx;
00136 transform_.translation.y += dy;
00137 transform_.translation.z += dz;
00138 transform_.rotation = rot;
00139
00140 ROS_INFO("r %f p %f y %f | vTx %f vTy %f vTz %f | x %f y %f z %f",r_, p_, y_, vTx, vTy, vTz, transform_.translation.x, transform_.translation.y, transform_.translation.z);
00141
00142
00143 pose_.pose.position.x += dx;
00144 pose_.pose.position.y += dy;
00145 pose_.pose.position.z += dz;
00146 pose_.pose.orientation = rot;
00147
00148 pose_.covariance[0] =
00149 pose_.covariance[7] =
00150 pose_.covariance[14] =
00151 pose_.covariance[21] =
00152 pose_.covariance[28] =
00153 pose_.covariance[35] = 0.5;
00154
00155
00156 twist_.twist.linear.x = vT;
00157 twist_.twist.linear.y = 0.0;
00158 twist_.twist.linear.z = 0.0;
00159 twist_.twist.angular.x = vrimu;
00160 twist_.twist.angular.y = vpimu;
00161 twist_.twist.angular.z = vyimu;
00162
00163
00164
00165 twist_.covariance[0] =
00166 twist_.covariance[7] =
00167 twist_.covariance[14] =
00168 twist_.covariance[21] =
00169 twist_.covariance[28] =
00170 twist_.covariance[35] = 0.0001;
00171
00172
00173 if(publish_tf_)
00174 {
00175
00176 odom_trans_msg.header.stamp = current_time_;
00177 odom_trans_msg.header.frame_id = odom_id_;
00178 odom_trans_msg.child_frame_id = base_link_id_;
00179 odom_trans_msg.transform = transform_;
00180
00181 odom_broadcaster_.sendTransform(odom_trans_msg);
00182 }
00183
00184
00185 Odometry_msg_.header.stamp = current_time_;
00186 Odometry_msg_.header.frame_id = odom_id_;
00187 Odometry_msg_.child_frame_id = base_link_id_;
00188 Odometry_msg_.pose = pose_;
00189 Odometry_msg_.twist = twist_;
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199 odom_publisher_.publish(Odometry_msg_);
00200
00201 }
00202
00203
00204 void SegwayRmp400OdomAlgNode::segway_status_callback(const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr& msg)
00205 {
00206 ROS_DEBUG("SegwayRmp400OdomAlgNode::segway_status_callback: New Message Received");
00207
00208
00209
00210 segway_status_mutex_.enter();
00211
00212
00213 left_wheels_velocity_ = (msg->rmp200[0].left_wheel_velocity + msg->rmp200[1].left_wheel_velocity)/2;
00214 right_wheels_velocity_ = (msg->rmp200[0].right_wheel_velocity + msg->rmp200[1].right_wheel_velocity)/2;
00215
00216 rk_[0] = msg->rmp200[0].roll_angle;
00217 rk_[1] = msg->rmp200[1].roll_angle;
00218 pk_[0] = msg->rmp200[0].pitch_angle;
00219 pk_[1] = msg->rmp200[1].pitch_angle;
00220 vyk_[0] = msg->rmp200[0].yaw_rate;
00221 vyk_[1] = msg->rmp200[1].yaw_rate;
00222
00223 segway_status_mutex_.exit();
00224 }
00225
00226 void SegwayRmp400OdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
00227 {
00228 ROS_DEBUG("SegwayRmp400OdomAlgNode::imu_callback: New Message Received");
00229
00230
00231
00232 imu_mutex_.enter();
00233 vrimu_ = msg->angular_velocity.x;
00234 vpimu_ = msg->angular_velocity.y;
00235 vyimu_ = msg->angular_velocity.z;
00236
00237 current_time_ = msg->header.stamp;
00238
00239 tf::Quaternion q;
00240 tf::quaternionMsgToTF(msg->orientation,q);
00241 tf::Matrix3x3(q).getRPY(rimu_,pimu_,yimu_);
00242 rk_[2] = rimu_;
00243 pk_[2] = pimu_;
00244 vyk_[2] = -vyimu_;
00245
00246
00247 rk_[2] = rk_[2]<0 ? rk_[2]+M_PI : rk_[2]-M_PI;
00248
00249
00250
00251 imu_mutex_.exit();
00252 }
00253
00254
00255
00256
00257
00258
00259
00260 void SegwayRmp400OdomAlgNode::node_config_update(Config &config, uint32_t level)
00261 {
00262 alg_.lock();
00263
00264 alg_.unlock();
00265 }
00266
00267 void SegwayRmp400OdomAlgNode::addNodeDiagnostics(void)
00268 {
00269 }
00270
00271
00272 int main(int argc,char *argv[])
00273 {
00274 return algorithm_base::main<SegwayRmp400OdomAlgNode>(argc, argv, "segway_rmp400_odom_alg_node");
00275 }