Go to the documentation of this file.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 #ifndef SIMCORE_HPP_
00038 #define SIMCORE_HPP_
00039 #include <labust/simulation/RBModel.hpp>
00040
00041 #include <labust/tools/conversions.hpp>
00042
00043 #include <auv_msgs/BodyForceReq.h>
00044 #include <geometry_msgs/WrenchStamped.h>
00045 #include <geometry_msgs/TwistStamped.h>
00046 #include <geometry_msgs/Vector3.h>
00047 #include <auv_msgs/NavSts.h>
00048 #include <nav_msgs/Odometry.h>
00049 #include <ros/ros.h>
00050 #include <tf/transform_broadcaster.h>
00051 #include <tf/transform_listener.h>
00052
00053 #include <boost/thread.hpp>
00054
00055 namespace labust
00056 {
00057 namespace simulation
00058 {
00065 void configureModel(const ros::NodeHandle& nh, RBModel& model);
00066
00075 class SimCore
00076 {
00077 public:
00081 SimCore();
00082
00086 void onInit();
00087
00091 void start();
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102 private:
00106 void modelReport();
00110 void step();
00115 template <class ROSMsg>
00116 void onTau(const typename ROSMsg::ConstPtr& msg)
00117 {
00118 boost::mutex::scoped_lock l(tau_mux);
00119 labust::tools::pointToVector(msg->wrench.force, tau);
00120 labust::tools::pointToVector(msg->wrench.torque, tau, 3);
00121
00122
00123 vector tauAch(tau);
00124 model.allocator.allocate(tau,tauAch);
00125 tau = tauAch;
00126
00127
00128 typename ROSMsg::Ptr ach(new ROSMsg());
00129 labust::tools::vectorToPoint(tau,ach->wrench.force);
00130 labust::tools::vectorToPoint(tau,ach->wrench.torque,3);
00131 publish_windup(ach, model.allocator.getCoercion());
00132 publish_dispatch(ach);
00133 }
00134
00138 template <class WindupType>
00139 inline void publish_windup(auv_msgs::BodyForceReq::Ptr& tau, const WindupType& windup)
00140 {
00141 labust::tools::vectorToPoint(windup,tau->disable_axis);
00142
00143 tau->disable_axis.roll = windup(3);
00144 tau->disable_axis.pitch = windup(4);
00145 tau->disable_axis.yaw = windup(5);
00146 }
00150 template <class WindupType>
00151 inline void publish_windup(geometry_msgs::WrenchStamped::Ptr& tau, const WindupType& windup){};
00152
00156 inline void publish_dispatch(auv_msgs::BodyForceReq::Ptr& tau)
00157 {
00158 this->tauAch.publish(tau);
00159 }
00163 inline void publish_dispatch(geometry_msgs::WrenchStamped::Ptr& tau)
00164 {
00165 this->tauAchWrench.publish(tau);
00166 }
00167
00173 void allocate();
00174
00178 void onCurrents(const geometry_msgs::TwistStamped::ConstPtr& currents);
00179
00183 void publishNavSts();
00187 void etaNuToNavSts(const vector& eta, const vector& nu, auv_msgs::NavSts& state);
00188
00192 void publishOdom();
00196 void etaNuToOdom(const vector& eta, const vector& nu, nav_msgs::Odometry& state);
00197
00201 void publishWorld();
00205 void publishSimBaseLink();
00206
00210 RBModel model;
00214 tf::TransformBroadcaster broadcast;
00218 ros::Subscriber tauIn, tauInWrench, currentsSub;
00222 ros::Publisher tauAch, tauAchWrench, meas, measn, odom, odomn, acc;
00226 vector tau;
00230 boost::mutex tau_mux, model_mux, sensor_mux;
00234 boost::thread runner;
00238
00242 ros::Rate rate;
00246 int wrap;
00250 bool enablePublishWorld;
00254 bool enablePublishSimBaseLink;
00258 double originLat, originLon;
00259 };
00260 }
00261 }
00262
00263
00264 #endif