GazeboTaskboard.h

Go to the documentation of this file.

/*
 * Copyright (C) 2013 TopCoder Inc., All Rights Reserved.
 */

#ifndef GAZEBOTASKBOARD_H
#define GAZEBOTASKBOARD_H

#include <boost/scoped_ptr.hpp>

#include <common/Plugin.hh>
#include <physics/physics.hh>
#include <ros/ros.h>

#include <gazebo_taskboard/ManipulatePowerCover.h>
#include <gazebo_taskboard/ManipulatePowerSwitch.h>
#include <gazebo_taskboard/ManipulateRockerSwitch.h>
#include <gazebo_taskboard/ManipulateNumPad.h>
#include <gazebo_taskboard/ManipulateSafeToggle.h>

#include <gazebo_taskboard/TaskboardPanelA.h>

namespace gazebo
{
    class GazeboTaskboardSlot1 : public ModelPlugin
    {
    public:
        GazeboTaskboardSlot1();
        ~GazeboTaskboardSlot1();

        virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
        
    private:
        enum TwoWayToggleSwitchState
        {
            eTwoWayState_Down,
            eTwoWayState_Up
        };
        enum ThreeWayToggleSwitchState
        {
            eThreeWayState_Center,
            eThreeWayState_Down,
            eThreeWayState_Up
        };

    private:
        bool InitLinks();
        bool InitJoints();
        void DeriveStateFromModel();

        void OnUpdate();

        void MonitorPowerCoverStateChanges();
        void MonitorPowerSwitchStateChanges();
        void MonitorRockerSwitchA01StateChanges();
        void MonitorNumpadStateChanges();
        void MonitorSafeTogglesStateChanges();

        void PublishState();

        struct Led;
        void SetLedState(Led& led, bool on);
        void TurnOnLeds();
        void TurnOffAllLeds();
        
        bool ManipulatePowerCover(gazebo_taskboard::ManipulatePowerCover::Request& request, 
                                  gazebo_taskboard::ManipulatePowerCover::Response& response);
        bool ManipulatePowerSwitch(gazebo_taskboard::ManipulatePowerSwitch::Request& request,
                                   gazebo_taskboard::ManipulatePowerSwitch::Response& response);
        bool ManipulateRockerSwitch(gazebo_taskboard::ManipulateRockerSwitch::Request& request,
                                    gazebo_taskboard::ManipulateRockerSwitch::Response& response);
        bool ManipulateNumPad(gazebo_taskboard::ManipulateNumPad::Request& request,
                              gazebo_taskboard::ManipulateNumPad::Response& response);
        bool ManipulateSafeToggle(gazebo_taskboard::ManipulateSafeToggle::Request& request,
                                  gazebo_taskboard::ManipulateSafeToggle::Response& response);

        void HandleManipulation();

        math::Vector3 computeEmpiricalTorque(double deviationAngle, double initialValue, 
                                             double snapCoeff, double snapExp, double torqueCoeff) const;
                                             
        bool UpdateTransitionFromOutState2Way(physics::LinkPtr link, physics::JointPtr joint,
                                              TwoWayToggleSwitchState& state) const;
        bool UpdateTransitionFromOutState3Way(physics::LinkPtr link, physics::JointPtr joint, 
                                              ThreeWayToggleSwitchState& state) const;
          
        double GetTime() const;

    private:
        bool firstFrameInitializationDone;
        
        bool ledsReady;
        
        ros::NodeHandle node;

        physics::ModelPtr model;

        event::ConnectionPtr updateConnection;

        struct TaskboardSlot1State;
        boost::scoped_ptr<TaskboardSlot1State> state;

        struct TaskboardLeds;
        boost::scoped_ptr<TaskboardLeds> leds;

        // Links
        physics::LinkPtr linkPowerCover;
        physics::LinkPtr linkPowerSwitch;
        physics::LinkPtr linkA01Switch;

        static const int SAFE_TOGGLES_COUNT = 3;
        physics::LinkPtr linksBaseSafeToggle[SAFE_TOGGLES_COUNT];
        physics::LinkPtr linksSafeToggle[SAFE_TOGGLES_COUNT];
        physics::JointPtr safeTogglesRevoluteJoints[SAFE_TOGGLES_COUNT];
        
        static const int NUM_PAD_BUTTONS_COUNT = 9;
        physics::LinkPtr linksNumPad[NUM_PAD_BUTTONS_COUNT];

        ros::Publisher publisher;

        // Services
        ros::ServiceServer srv_manipulatePowerCover;
        ros::ServiceServer srv_manipulatePowerSwitch;
        ros::ServiceServer srv_manipulateRockerSwitch;
        ros::ServiceServer srv_manipulateNumPad;
        ros::ServiceServer srv_manipulateSafeToggle;
        
        struct ManipulationState;
        boost::scoped_ptr<ManipulationState> manipulationState;
    };

    // register this plugin with the simulator
    GZ_REGISTER_MODEL_PLUGIN(GazeboTaskboardSlot1)
}

#endif // GAZEBOTASKBOARD_H