00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <labust/ros/SimCore.hpp>
00038 #include <labust/tools/DynamicsLoader.hpp>
00039 #include <labust/tools/conversions.hpp>
00040
00041 #include <sstream>
00042
00043 using namespace labust::simulation;
00044
00045 void labust::simulation::configureModel(const ros::NodeHandle& nh, RBModel& model)
00046 {
00047 labust::tools::loadDynamicsParams(nh, model);
00048
00049 nh.param("sampling_time",model.dT, model.dT);
00050 nh.param("coupled",model.isCoupled,model.isCoupled);
00051 Eigen::Vector3d bdg;
00052 labust::tools::getMatrixParam(nh,"bounding_ellipsoid",bdg);
00053 model.ae = bdg(0);
00054 model.be = bdg(1);
00055 model.ce = bdg(2);
00056 labust::tools::getMatrixParam(nh,"eta0",model.eta0);
00057 labust::tools::getMatrixParam(nh,"nu0",model.nu0);
00058 labust::tools::getMatrixParam(nh,"current",model.current);
00059
00060 typedef Eigen::Matrix<double,6,1> NoiseVec;
00061 NoiseVec pn(NoiseVec::Zero()),mn(NoiseVec::Zero());
00062 labust::tools::getMatrixParam(nh,"process_noise",pn);
00063 labust::tools::getMatrixParam(nh,"measurement_noise",mn);
00064 model.noise.setNoiseParams(pn,mn);
00065
00066
00067 int alloc_type(-1);
00068 Eigen::MatrixXd alloc;
00069 Eigen::MatrixXi dofs, groups;
00070 nh.param("allocation_type",alloc_type,-1);
00071 labust::tools::getMatrixParam(nh,"allocation_matrix",alloc);
00072 labust::tools::getMatrixParam(nh,"allocation_dofs",dofs);
00073 labust::tools::getMatrixParam(nh,"allocation_thruster_groups",groups);
00074
00075 model.allocator.init(alloc_type, alloc, dofs, groups);
00076 model.init();
00077 }
00078
00079 SimCore::SimCore():
00080 tau(vector::Zero()),
00081 rate(10),
00082 wrap(1),
00083 enablePublishWorld(true),
00084 enablePublishSimBaseLink(true),
00085 originLat(0),
00086 originLon(0)
00087 {
00088 this->onInit();
00089 }
00090
00091 void SimCore::onInit()
00092 {
00093 ros::NodeHandle nh,ph("~");
00094 configureModel(nh, model);
00095 modelReport();
00096
00097
00098
00099 tauIn = nh.subscribe<auv_msgs::BodyForceReq>("tauIn", 1, &SimCore::onTau<auv_msgs::BodyForceReq>, this);
00100
00101 tauInWrench = nh.subscribe<geometry_msgs::WrenchStamped>("tauInWrench", 1, &SimCore::onTau<geometry_msgs::WrenchStamped>, this);
00102
00103 currentsSub = nh.subscribe<geometry_msgs::TwistStamped>("currents", 1, &SimCore::onCurrents, this);
00104
00105
00106
00107 meas = nh.advertise<auv_msgs::NavSts>("meas_ideal",1);
00108 measn = nh.advertise<auv_msgs::NavSts>("meas_noisy",1);
00109 tauAch = nh.advertise<auv_msgs::BodyForceReq>("tauAch",1);
00110
00111 odom = nh.advertise<nav_msgs::Odometry>("meas_odom",1);
00112 odomn = nh.advertise<nav_msgs::Odometry>("meas_odom_noisy",1);
00113 tauAchWrench = nh.advertise<geometry_msgs::WrenchStamped>("tauAchWrench",1);
00114
00115 acc = nh.advertise<geometry_msgs::Vector3>("nuacc_ideal",1);
00116
00117 double fs(10);
00118 double maxThrust(1), minThrust(-1);
00119 ph.param("maxThrust",maxThrust,maxThrust);
00120 ph.param("minThrust",minThrust,minThrust);
00121 model.allocator.setThrusterMinMax(minThrust, maxThrust);
00122 ph.param("Rate",fs,fs);
00123 ph.param("ModelWrap", wrap,wrap);
00124 model.dT = 1/(fs*wrap);
00125 rate = ros::Rate(fs);
00126 ph.param("publish_world", enablePublishWorld, enablePublishWorld);
00127 ph.param("publish_sim_base", enablePublishSimBaseLink, enablePublishSimBaseLink);
00128 ph.param("originLat", originLat, originLat);
00129 ph.param("originLon", originLon, originLon);
00130 runner = boost::thread(boost::bind(&SimCore::start, this));
00131 }
00132
00133 void SimCore::onCurrents(const geometry_msgs::TwistStamped::ConstPtr& currents)
00134 {
00135 boost::mutex::scoped_lock l(model_mux);
00136 labust::tools::pointToVector(currents->twist.linear, model.current);
00137 }
00138
00139 void SimCore::etaNuToNavSts(const vector& eta, const vector& nu, auv_msgs::NavSts& state)
00140 {
00141 state.position.north = eta(RBModel::x);
00142 state.position.east = eta(RBModel::y);
00143 state.position.depth = eta(RBModel::z);
00144 state.orientation.roll = eta(RBModel::phi);
00145 state.orientation.pitch = eta(RBModel::theta);
00146 state.orientation.yaw = eta(RBModel::psi);
00147
00148 state.body_velocity.x = nu(RBModel::u);
00149 state.body_velocity.y = nu(RBModel::v);
00150 state.body_velocity.z = nu(RBModel::w);
00151 state.orientation_rate.roll = nu(RBModel::p);
00152 state.orientation_rate.pitch = nu(RBModel::q);
00153 state.orientation_rate.yaw = nu(RBModel::r);
00154 }
00155
00156 void SimCore::publishNavSts()
00157 {
00158 auv_msgs::NavStsPtr state(new auv_msgs::NavSts()),
00159 nstate(new auv_msgs::NavSts());
00160
00161 etaNuToNavSts(model.Eta(),model.Nu(),*state);
00162 etaNuToNavSts(model.EtaNoisy(),model.NuNoisy(),*nstate);
00163
00164
00165
00166
00167
00168 state->header.stamp = nstate->header.stamp = ros::Time::now();
00169 state->header.frame_id = nstate->header.frame_id = "local";
00170 meas.publish(state);
00171 measn.publish(nstate);
00172 }
00173
00174 void SimCore::publishOdom()
00175 {
00176 nav_msgs::OdometryPtr state(new nav_msgs::Odometry()),
00177 nstate(new nav_msgs::Odometry());
00178
00179 etaNuToOdom(model.Eta(),model.Nu(),*state);
00180 etaNuToOdom(model.EtaNoisy(),model.NuNoisy(),*nstate);
00181
00182
00183
00184
00185
00186 state->header.stamp = nstate->header.stamp = ros::Time::now();
00187 state->header.frame_id = nstate->header.frame_id = "local";
00188 state->child_frame_id = nstate->child_frame_id = "base_link";
00189 odom.publish(state);
00190 odomn.publish(nstate);
00191 }
00192
00193 void SimCore::etaNuToOdom(const vector& eta, const vector& nu, nav_msgs::Odometry& state)
00194 {
00195 labust::tools::vectorToPoint(eta,state.pose.pose.position);
00196
00197 Eigen::Quaternion<double> quat;
00198 labust::tools::quaternionFromEulerZYX(eta(RBModel::phi),
00199 eta(RBModel::theta),
00200 eta(RBModel::psi), quat);
00201 state.pose.pose.orientation.x = quat.x();
00202 state.pose.pose.orientation.y = quat.y();
00203 state.pose.pose.orientation.z = quat.z();
00204 state.pose.pose.orientation.w = quat.w();
00205
00206 labust::tools::vectorToPoint(nu,state.twist.twist.linear);
00207 labust::tools::vectorToPoint(nu,state.twist.twist.angular,3);
00208 }
00209
00210 void SimCore::publishWorld()
00211 {
00212 tf::Transform transform;
00213 transform.setOrigin(tf::Vector3(originLon, originLat, 0));
00214 transform.setRotation(tf::createQuaternionFromRPY(0,0,0));
00215 broadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/worldLatLon", "/world"));
00216 transform.setOrigin(tf::Vector3(0, 0, 0));
00217 Eigen::Quaternion<float> q;
00218 labust::tools::quaternionFromEulerZYX(M_PI,0,M_PI/2,q);
00219 transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
00220 broadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "local"));
00221 }
00222
00223 void SimCore::publishSimBaseLink()
00224 {
00225 const vector& eta = model.Eta();
00226 tf::Transform transform;
00227 transform.setOrigin(tf::Vector3(eta(RBModel::x),
00228 eta(RBModel::y),
00229 eta(RBModel::z)));
00230 Eigen::Quaternion<double> q;
00231 labust::tools::quaternionFromEulerZYX(eta(RBModel::phi),
00232 eta(RBModel::theta),
00233 eta(RBModel::psi), q);
00234 transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
00235 broadcast.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local", "base_link_sim"));
00236 }
00237
00238 void SimCore::start()
00239 {
00240
00241
00242 while (ros::ok())
00243 {
00244 boost::mutex::scoped_lock model_lock(model_mux);
00245 {
00246 boost::mutex::scoped_lock l(tau_mux);
00247 for (size_t i=0; i<wrap;++i) model.step(tau);
00248 }
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260 publishNavSts();
00261 publishOdom();
00262
00263 if (enablePublishWorld) publishWorld();
00264 if (enablePublishSimBaseLink) publishSimBaseLink();
00265
00266 geometry_msgs::Vector3::Ptr accout(new geometry_msgs::Vector3());
00267 const vector& vacc=model.NuAcc();
00268 labust::tools::vectorToPoint(vacc, *accout);
00269 acc.publish(accout);
00270
00271 model_lock.unlock();
00272 rate.sleep();
00273 }
00274 }
00275
00276 void SimCore::modelReport()
00277 {
00278 ROS_INFO("Loaded the model:");
00279 ROS_INFO(" sampling-time: %f, mass: %f, gravity: %f, density: %f",
00280 model.dT, model.m, model.g_acc, model.rho);
00281 std::ostringstream str;
00282 str<<"["<<model.eta0.transpose()<<"], ["<<model.nu0.transpose()<<"]"<<"\n";
00283 ROS_INFO(" (Eta0,Nu0): %s",str.str().c_str());
00284 str.str("");
00285 str<<"["<<model.current.transpose()<<"]";
00286 ROS_INFO(" Current: %s",str.str().c_str());
00287 ROS_INFO(" Bounding ellipsoid: (%f,%f,%f)",model.ae, model.be, model.ce);
00288 ROS_INFO(" Is coupled model: %d",model.isCoupled);
00289 str.str("");
00290 str<<model.Io<<"\n"<<model.Ma<<"\n"<<model.Dlin<<"\n"<<model.Dquad;
00291 ROS_INFO("(Io,Ma,Dlin,Dquad):\n%s",str.str().c_str());
00292 }