SimCore.hpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, LABUST, UNIZG-FER
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the LABUST nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Author: Dula Nad
 *  Created: 01.02.2013.
 *********************************************************************/
#ifndef SIMCORE_HPP_
#define SIMCORE_HPP_
#include <labust/simulation/RBModel.hpp>
//#include <labust/ros/SimSensors.hpp>
#include <labust/tools/conversions.hpp>

#include <auv_msgs/BodyForceReq.h>
#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3.h>
#include <auv_msgs/NavSts.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include <boost/thread.hpp>

namespace labust
{
        namespace simulation
        {
                void configureModel(const ros::NodeHandle& nh, RBModel& model);

                class SimCore
                {
                public:
                        SimCore();

                        void onInit();

                        void start();

//                      /**
//                       * Add a new sensor to the simulation.
//                       */
//                      inline void addSensor(SimSensorInterface::Ptr sensor)
//                      {
//                                      boost::mutex::scoped_lock l(sensor_mux);
//                                      sensors.push_back(sensor);
//                      }

                private:
                        void modelReport();
                        void step();
                        template <class ROSMsg>
                        void onTau(const typename ROSMsg::ConstPtr& msg)
                        {
                                boost::mutex::scoped_lock l(tau_mux);
                                labust::tools::pointToVector(msg->wrench.force, tau);
                                labust::tools::pointToVector(msg->wrench.torque, tau, 3);

                                //Allocate
                                vector tauAch(tau);
                                model.allocator.allocate(tau,tauAch);
                                tau = tauAch;

                                //Publish allocated.
                                typename ROSMsg::Ptr ach(new ROSMsg());
                                labust::tools::vectorToPoint(tau,ach->wrench.force);
                                labust::tools::vectorToPoint(tau,ach->wrench.torque,3);
                                publish_windup(ach, model.allocator.getCoercion());
                                publish_dispatch(ach);
                        }

                        template <class WindupType>
                        inline void publish_windup(auv_msgs::BodyForceReq::Ptr& tau, const WindupType& windup)
                        {
                                labust::tools::vectorToPoint(windup,tau->disable_axis);

                                tau->disable_axis.roll = windup(3);
                                tau->disable_axis.pitch = windup(4);
                                tau->disable_axis.yaw = windup(5);
                        }
                        template <class WindupType>
                        inline void publish_windup(geometry_msgs::WrenchStamped::Ptr& tau, const WindupType& windup){};

                        inline void publish_dispatch(auv_msgs::BodyForceReq::Ptr& tau)
                        {
                                this->tauAch.publish(tau);
                        }
                        inline void publish_dispatch(geometry_msgs::WrenchStamped::Ptr& tau)
                        {
                                this->tauAchWrench.publish(tau);
                        }

                        void allocate();

                        void onCurrents(const geometry_msgs::TwistStamped::ConstPtr& currents);

                        void publishNavSts();
                        void etaNuToNavSts(const vector& eta, const vector& nu, auv_msgs::NavSts& state);

                        void publishOdom();
                        void etaNuToOdom(const vector& eta, const vector& nu, nav_msgs::Odometry& state);

                        void publishWorld();
                        void publishSimBaseLink();

                        RBModel model;
                        tf::TransformBroadcaster broadcast;
                        ros::Subscriber tauIn, tauInWrench, currentsSub;
                        ros::Publisher tauAch, tauAchWrench, meas, measn, odom, odomn, acc;
                        vector tau;
                        boost::mutex tau_mux, model_mux, sensor_mux;
                        boost::thread runner;
//                      std::vector<SimSensorInterface::Ptr> sensors;
                        ros::Rate rate;
                        int wrap;
                        bool enablePublishWorld;
                        bool enablePublishSimBaseLink;
                        double originLat, originLon;
                };
        }
}

/* SIMCORE_HPP_ */
#endif