GazeboTaskboard.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2013 TopCoder Inc., All Rights Reserved.
00003  */
00004 
00012 #ifndef GAZEBOTASKBOARD_H
00013 #define GAZEBOTASKBOARD_H
00014 
00015 #include <boost/scoped_ptr.hpp>
00016 
00017 #include <common/Plugin.hh>
00018 #include <physics/physics.hh>
00019 #include <ros/ros.h>
00020 
00021 #include <gazebo_taskboard/ManipulatePowerCover.h>
00022 #include <gazebo_taskboard/ManipulatePowerSwitch.h>
00023 #include <gazebo_taskboard/ManipulateRockerSwitch.h>
00024 #include <gazebo_taskboard/ManipulateNumPad.h>
00025 #include <gazebo_taskboard/ManipulateSafeToggle.h>
00026 
00027 #include <gazebo_taskboard/TaskboardPanelA.h>
00028 
00029 namespace gazebo
00030 {
00041     class GazeboTaskboardSlot1 : public ModelPlugin
00042     {
00043     public:
00045         GazeboTaskboardSlot1();
00047         ~GazeboTaskboardSlot1();
00048 
00050         virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00051         
00052     private:
00054         enum TwoWayToggleSwitchState
00055         {
00056             eTwoWayState_Down,
00057             eTwoWayState_Up
00058         };
00060         enum ThreeWayToggleSwitchState
00061         {
00062             eThreeWayState_Center,
00063             eThreeWayState_Down,
00064             eThreeWayState_Up
00065         };
00066 
00067     private:
00069         bool InitLinks();
00071         bool InitJoints();
00073         void DeriveStateFromModel();
00074 
00076         void OnUpdate();
00077 
00079         void MonitorPowerCoverStateChanges();
00081         void MonitorPowerSwitchStateChanges();
00083         void MonitorRockerSwitchA01StateChanges();
00085         void MonitorNumpadStateChanges();
00087         void MonitorSafeTogglesStateChanges();
00088 
00090         void PublishState();
00091 
00092         struct Led;
00094         void SetLedState(Led& led, bool on);
00096         void TurnOnLeds();
00098         void TurnOffAllLeds();
00099         
00101         bool ManipulatePowerCover(gazebo_taskboard::ManipulatePowerCover::Request& request, 
00102                                   gazebo_taskboard::ManipulatePowerCover::Response& response);
00104         bool ManipulatePowerSwitch(gazebo_taskboard::ManipulatePowerSwitch::Request& request,
00105                                    gazebo_taskboard::ManipulatePowerSwitch::Response& response);
00107         bool ManipulateRockerSwitch(gazebo_taskboard::ManipulateRockerSwitch::Request& request,
00108                                     gazebo_taskboard::ManipulateRockerSwitch::Response& response);
00110         bool ManipulateNumPad(gazebo_taskboard::ManipulateNumPad::Request& request,
00111                               gazebo_taskboard::ManipulateNumPad::Response& response);
00113         bool ManipulateSafeToggle(gazebo_taskboard::ManipulateSafeToggle::Request& request,
00114                                   gazebo_taskboard::ManipulateSafeToggle::Response& response);
00115 
00117         void HandleManipulation();
00118 
00120         math::Vector3 computeEmpiricalTorque(double deviationAngle, double initialValue, 
00121                                              double snapCoeff, double snapExp, double torqueCoeff) const;
00122                                              
00124         bool UpdateTransitionFromOutState2Way(physics::LinkPtr link, physics::JointPtr joint,
00125                                               TwoWayToggleSwitchState& state) const;
00127         bool UpdateTransitionFromOutState3Way(physics::LinkPtr link, physics::JointPtr joint, 
00128                                               ThreeWayToggleSwitchState& state) const;
00129           
00131         double GetTime() const;
00132 
00133     private:
00141         bool firstFrameInitializationDone;
00142         
00144         bool ledsReady;
00145         
00147         ros::NodeHandle node;
00148 
00150         physics::ModelPtr model;
00151 
00153         event::ConnectionPtr updateConnection;
00154 
00155         struct TaskboardSlot1State;
00157         boost::scoped_ptr<TaskboardSlot1State> state;
00158 
00159         struct TaskboardLeds;
00161         boost::scoped_ptr<TaskboardLeds> leds;
00162 
00163         // Links
00165         physics::LinkPtr linkPowerCover;
00167         physics::LinkPtr linkPowerSwitch;
00169         physics::LinkPtr linkA01Switch;
00170 
00172         static const int SAFE_TOGGLES_COUNT = 3;
00174         physics::LinkPtr linksBaseSafeToggle[SAFE_TOGGLES_COUNT];
00176         physics::LinkPtr linksSafeToggle[SAFE_TOGGLES_COUNT];
00178         physics::JointPtr safeTogglesRevoluteJoints[SAFE_TOGGLES_COUNT];
00179         
00181         static const int NUM_PAD_BUTTONS_COUNT = 9;
00183         physics::LinkPtr linksNumPad[NUM_PAD_BUTTONS_COUNT];
00184 
00186         ros::Publisher publisher;
00187 
00188         // Services
00190         ros::ServiceServer srv_manipulatePowerCover;
00192         ros::ServiceServer srv_manipulatePowerSwitch;
00194         ros::ServiceServer srv_manipulateRockerSwitch;
00196         ros::ServiceServer srv_manipulateNumPad;
00198         ros::ServiceServer srv_manipulateSafeToggle;
00199         
00200         struct ManipulationState;
00202         boost::scoped_ptr<ManipulationState> manipulationState;
00203     };
00204 
00205     // register this plugin with the simulator
00206     GZ_REGISTER_MODEL_PLUGIN(GazeboTaskboardSlot1)
00207 }
00208 
00209 #endif // GAZEBOTASKBOARD_H


gazebo_taskboard
Author(s): TCSASSEMBLER
autogenerated on Mon Oct 6 2014 02:45:46