Go to the documentation of this file.00001
00002
00003
00004
00012 #ifndef GAZEBOTASKBOARD_H
00013 #define GAZEBOTASKBOARD_H
00014
00015 #include <boost/scoped_ptr.hpp>
00016 #include <gazebo/common/Plugin.hh>
00017 #include <gazebo/physics/physics.hh>
00018 #include <ros/ros.h>
00019
00020 #include <iss_taskboard_gazebo/ManipulatePowerCover.h>
00021 #include <iss_taskboard_gazebo/ManipulatePowerSwitch.h>
00022 #include <iss_taskboard_gazebo/ManipulateRockerSwitch.h>
00023 #include <iss_taskboard_gazebo/ManipulateNumPad.h>
00024 #include <iss_taskboard_gazebo/ManipulateSafeToggle.h>
00025
00026 #include <iss_taskboard_gazebo/TaskboardPanelA.h>
00027
00028 namespace gazebo
00029 {
00040 class GazeboTaskboardSlot1 : public ModelPlugin
00041 {
00042 public:
00044 GazeboTaskboardSlot1();
00046 ~GazeboTaskboardSlot1();
00047
00049 virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00050
00051 private:
00053 enum TwoWayToggleSwitchState
00054 {
00055 eTwoWayState_Down,
00056 eTwoWayState_Up
00057 };
00059 enum ThreeWayToggleSwitchState
00060 {
00061 eThreeWayState_Center,
00062 eThreeWayState_Down,
00063 eThreeWayState_Up
00064 };
00065
00066 private:
00068 bool InitLinks();
00070 bool InitJoints();
00072 void DeriveStateFromModel();
00073
00075 void OnUpdate();
00076
00078 void MonitorPowerCoverStateChanges();
00080 void MonitorPowerSwitchStateChanges();
00082 void MonitorRockerSwitchA01StateChanges();
00084 void MonitorNumpadStateChanges();
00086 void MonitorSafeTogglesStateChanges();
00087
00089 void PublishState();
00090
00091 struct Led;
00093 void SetLedState(Led& led, bool on);
00095 void TurnOnLeds();
00097 void TurnOffAllLeds();
00098
00100 bool ManipulatePowerCover(iss_taskboard_gazebo::ManipulatePowerCover::Request& request,
00101 iss_taskboard_gazebo::ManipulatePowerCover::Response& response);
00103 bool ManipulatePowerSwitch(iss_taskboard_gazebo::ManipulatePowerSwitch::Request& request,
00104 iss_taskboard_gazebo::ManipulatePowerSwitch::Response& response);
00106 bool ManipulateRockerSwitch(iss_taskboard_gazebo::ManipulateRockerSwitch::Request& request,
00107 iss_taskboard_gazebo::ManipulateRockerSwitch::Response& response);
00109 bool ManipulateNumPad(iss_taskboard_gazebo::ManipulateNumPad::Request& request,
00110 iss_taskboard_gazebo::ManipulateNumPad::Response& response);
00112 bool ManipulateSafeToggle(iss_taskboard_gazebo::ManipulateSafeToggle::Request& request,
00113 iss_taskboard_gazebo::ManipulateSafeToggle::Response& response);
00114
00116 void HandleManipulation();
00117
00119 math::Vector3 computeEmpiricalTorque(double deviationAngle, double initialValue,
00120 double snapCoeff, double snapExp, double torqueCoeff) const;
00121
00123 bool UpdateTransitionFromOutState2Way(physics::LinkPtr link, physics::JointPtr joint,
00124 TwoWayToggleSwitchState& state) const;
00126 bool UpdateTransitionFromOutState3Way(physics::LinkPtr link, physics::JointPtr joint,
00127 ThreeWayToggleSwitchState& state) const;
00128
00130 double GetTime() const;
00131
00132 private:
00140 bool firstFrameInitializationDone;
00141
00143 bool ledsReady;
00144
00146 ros::NodeHandle node;
00147
00149 physics::ModelPtr model;
00150
00152 event::ConnectionPtr updateConnection;
00153
00154 struct TaskboardSlot1State;
00156 boost::scoped_ptr<TaskboardSlot1State> state;
00157
00158 struct TaskboardLeds;
00160 boost::scoped_ptr<TaskboardLeds> leds;
00161
00162
00164 physics::LinkPtr linkPowerCover;
00166 physics::LinkPtr linkPowerSwitch;
00168 physics::LinkPtr linkA01Switch;
00169
00171 static const int SAFE_TOGGLES_COUNT = 3;
00173 physics::LinkPtr linksBaseSafeToggle[SAFE_TOGGLES_COUNT];
00175 physics::LinkPtr linksSafeToggle[SAFE_TOGGLES_COUNT];
00177 physics::JointPtr safeTogglesRevoluteJoints[SAFE_TOGGLES_COUNT];
00178
00180 static const int NUM_PAD_BUTTONS_COUNT = 9;
00182 physics::LinkPtr linksNumPad[NUM_PAD_BUTTONS_COUNT];
00183
00185 ros::Publisher publisher;
00186
00187
00189 ros::ServiceServer srv_manipulatePowerCover;
00191 ros::ServiceServer srv_manipulatePowerSwitch;
00193 ros::ServiceServer srv_manipulateRockerSwitch;
00195 ros::ServiceServer srv_manipulateNumPad;
00197 ros::ServiceServer srv_manipulateSafeToggle;
00198
00199 struct ManipulationState;
00201 boost::scoped_ptr<ManipulationState> manipulationState;
00202 };
00203
00204
00205 GZ_REGISTER_MODEL_PLUGIN(GazeboTaskboardSlot1)
00206 }
00207
00208 #endif // GAZEBOTASKBOARD_H