00001 #ifndef GRIPPERPOSITIONSTATEMACHINE_H 00002 #define GRIPPERPOSITIONSTATEMACHINE_H 00003 00004 #include <boost/statechart/event.hpp> 00005 #include <boost/statechart/state_machine.hpp> 00006 #include <boost/statechart/simple_state.hpp> 00007 #include <boost/statechart/state.hpp> 00008 #include <boost/statechart/transition.hpp> 00009 #include <boost/statechart/in_state_reaction.hpp> 00010 #include <boost/statechart/custom_reaction.hpp> 00011 #include <boost/mpl/list.hpp> 00012 #include <boost/shared_ptr.hpp> 00013 #include <boost/function.hpp> 00014 #include <iostream> 00015 #include <iomanip> 00016 #include <algorithm> 00017 #include "nasa_common_logging/Logger.h" 00018 #include <r2_msgs/JointControlData.h> 00019 #include <sensor_msgs/JointState.h> 00020 #include <actionlib_msgs/GoalStatusArray.h> 00021 #include <r2_msgs/JointCommand.h> 00022 #include <boost/regex.hpp> 00023 #include "robodyn_mechanisms/GripperKinematics.h" 00024 #include "robodyn_utilities/AbsSlewFilter.h" 00025 #include "robot_instance/GripperSetpointFactory.h" 00026 #include "fsm_utils/StatechartUtilities.h" 00027 00029 00035 struct e_do : boost::statechart::event<e_do> {}; 00036 struct i_do_movement : boost::statechart::event<i_do_movement> {}; 00037 struct i_do_active : boost::statechart::event<i_do_active> {}; 00038 struct i_do_load : boost::statechart::event<i_do_load> {}; 00039 struct i_do_lock : boost::statechart::event<i_do_lock> {}; 00040 struct i_do_lockStatus : boost::statechart::event<i_do_lockStatus> {}; 00041 struct i_do_status : boost::statechart::event<i_do_status> {}; 00042 struct i_do_position : boost::statechart::event<i_do_position> {}; 00043 struct i_do_positionOrLess : boost::statechart::event<i_do_positionOrLess> {}; 00044 struct i_do_stall : boost::statechart::event<i_do_stall> {}; 00045 struct i_do_fault : boost::statechart::event<i_do_fault> {}; 00046 struct i_do_moving : boost::statechart::event<i_do_moving> {}; 00047 struct i_do_open : boost::statechart::event<i_do_open> {}; 00048 struct i_do_force : boost::statechart::event<i_do_force> {}; 00049 struct e_loaded : boost::statechart::event<e_loaded> {}; 00050 struct e_unloaded : boost::statechart::event<e_unloaded> {}; 00051 struct e_locked : boost::statechart::event<e_locked> {}; 00052 struct i_locked_action : boost::statechart::event<i_locked_action> {}; 00053 struct i_locked_lock : boost::statechart::event<i_locked_lock> {}; 00054 struct e_unlocked : boost::statechart::event<e_unlocked> {}; 00055 struct e_lockedStatus : boost::statechart::event<e_lockedStatus> {}; 00056 struct e_unlockedStatus : boost::statechart::event<e_unlockedStatus> {}; 00057 struct e_ready : boost::statechart::event<e_ready> {}; 00058 struct i_ready_command : boost::statechart::event<i_ready_command> {}; 00059 struct i_ready_status : boost::statechart::event<i_ready_status> {}; 00060 struct e_unready : boost::statechart::event<e_unready> {}; 00061 struct i_unready_command : boost::statechart::event<i_unready_command> {}; 00062 struct i_unready_status : boost::statechart::event<i_unready_status> {}; 00063 struct e_lessThanDesiredJawPosition : boost::statechart::event<e_lessThanDesiredJawPosition> {}; 00064 struct e_atDesiredJawPosition : boost::statechart::event<e_atDesiredJawPosition> {}; 00065 struct i_atDesiredJawPosition_action : boost::statechart::event<i_atDesiredJawPosition_action> {}; 00066 struct i_atDesiredJawPosition_position : boost::statechart::event<i_atDesiredJawPosition_position> {}; 00067 struct e_moreThanDesiredJawPosition : boost::statechart::event<e_moreThanDesiredJawPosition> {}; 00068 struct e_faulted : boost::statechart::event<e_faulted> {}; 00069 struct i_faulted_command : boost::statechart::event<i_faulted_command> {}; 00070 struct i_faulted_fault : boost::statechart::event<i_faulted_fault> {}; 00071 struct e_notFaulted : boost::statechart::event<e_notFaulted> {}; 00072 struct e_stalled : boost::statechart::event<e_stalled> {}; 00073 struct i_stalled_action : boost::statechart::event<i_stalled_action> {}; 00074 struct i_stalled_stall : boost::statechart::event<i_stalled_stall> {}; 00075 struct e_notStalled : boost::statechart::event<e_notStalled> {}; 00076 struct e_stop : boost::statechart::event<e_stop> {}; 00077 struct e_open : boost::statechart::event<e_open> {}; 00078 struct e_close : boost::statechart::event<e_close> {}; 00079 struct e_isOpen : boost::statechart::event<e_isOpen> {}; 00080 struct i_isOpen_movement : boost::statechart::event<i_isOpen_movement> {}; 00081 struct i_isOpen_action : boost::statechart::event<i_isOpen_action> {}; 00082 struct i_isOpen_open : boost::statechart::event<i_isOpen_open> {}; 00083 struct e_isNotOpen : boost::statechart::event<e_isNotOpen> {}; 00084 struct e_isClosed : boost::statechart::event<e_isClosed> {}; 00085 struct e_still : boost::statechart::event<e_still> {}; 00086 struct i_still_action : boost::statechart::event<i_still_action> {}; 00087 struct i_still_moving : boost::statechart::event<i_still_moving> {}; 00088 struct e_moving : boost::statechart::event<e_moving> {}; 00089 struct e_lessThanForceThreshold : boost::statechart::event<e_lessThanForceThreshold> {}; 00090 struct e_moreThanForceThreshold : boost::statechart::event<e_moreThanForceThreshold> {}; 00091 struct e_done : boost::statechart::event<e_done> {}; 00092 struct c_cancel : boost::statechart::event<c_cancel> {}; 00093 struct c_set : boost::statechart::event<c_set> {}; 00094 struct e_set : boost::statechart::event<e_set> {}; 00095 struct c_lock : boost::statechart::event<c_lock> {}; 00096 struct e_lock : boost::statechart::event<e_lock> {}; 00097 struct c_release : boost::statechart::event<c_release> {}; 00098 struct e_release : boost::statechart::event<e_release> {}; 00099 struct e_listen : boost::statechart::event<e_listen> {}; 00100 struct e_next : boost::statechart::event<e_next> {}; 00101 struct c_setParameters : boost::statechart::event<c_setParameters> 00102 { 00103 GripperSetpoint sp; 00104 c_setParameters(const GripperSetpoint& setPoint) : sp(setPoint) {} 00105 }; 00106 00108 struct Inactive; 00109 struct Active; 00110 struct Stopped; 00111 struct Opening; 00112 struct Closing; 00113 struct Listening; 00114 struct Acting; 00115 struct OpenWide; 00116 struct JawPositionCheck; 00117 struct Close; 00118 struct LockCheck; 00119 struct OpenUntilStall; 00120 struct Done; 00121 struct Unloaded; 00122 struct Loaded; 00123 struct Unlocked; 00124 struct Locked; 00125 struct UnlockedStatus; 00126 struct LockedStatus; 00127 struct Unready; 00128 struct Ready; 00129 struct LessThanDesiredJawPosition; 00130 struct AtDesiredJawPosition; 00131 struct MoreThanDesiredJawPosition; 00132 struct NotStalled; 00133 struct Stalled; 00134 struct NotFaulted; 00135 struct Faulted; 00136 struct Still; 00137 struct Moving; 00138 struct NotOpen; 00139 struct Open; 00140 struct LessThanForceThreshold; 00141 struct MoreThanForceThreshold; 00142 struct Container; 00143 00145 struct GripperPosition : boost::statechart::state_machine<GripperPosition, Container> 00146 { 00147 GripperPosition(); 00148 00149 std::string mechanism; 00150 00151 r2_msgs::JointControlData jointControlData; 00152 sensor_msgs::JointState encoderState, jawLeftState, jawRightState; 00153 r2_msgs::JointCommand jointCommand; 00154 double currentMax; 00155 uint16_t dutyCycleMax; 00156 00157 boost::function<float(const std::string&)> getFloat; 00158 boost::function<void(const std::string&, float)> setFloat; 00159 boost::function<uint16_t(const std::string&)> getUInt16; 00160 boost::function<void(const std::string&, uint16_t)> setUInt16; 00161 boost::function<void(const r2_msgs::JointCommand& msg)> sendCommand; 00162 boost::function<void(const actionlib_msgs::GoalStatus& status, bool clear)> sendGoalStatus; 00163 actionlib_msgs::GoalStatus goalStatus; 00164 00165 std::string dutyCycleLimitName; 00166 uint16_t dutyCycleLimit; 00167 std::string currentLimitName; 00168 double currentLimit; 00169 std::string measuredCurrentName; 00170 double measuredCurrent; 00171 00172 double jawOpenPosition; 00173 double jawPositionNoise; 00174 double encoderOpenVelocity; 00175 double encoderCloseVelocity; 00176 double encoderClosedPosition; 00177 double encoderPositionNoise; 00178 double encoderVelocityNoise; 00179 double currentNoise; 00180 double weakForce; 00181 double strongForce; 00182 double desiredForce; 00183 double jawPositionForceThreshold; 00184 AbsSlewFilter outputCurrentFilter; 00185 AbsSlewFilter dutyCycleLimitFilter; 00186 00187 std::string initializationStateName; 00188 00189 GripperSetpoint setPoint; 00190 GripperKinematicsPtr gripperKinematics; 00191 }; 00192 00193 struct Container : boost::statechart::simple_state < Container, GripperPosition, 00194 boost::mpl::list<Inactive, Locked, LockedStatus, Unloaded, LessThanDesiredJawPosition, NotStalled, Unready, NotFaulted, Still, NotOpen, LessThanForceThreshold> > 00195 { 00196 void doo(const e_do&); 00197 void ready(const e_ready&); 00198 void unready(const e_unready&); 00199 void locked(const e_locked&); 00200 void atDesiredJawPosition(const e_atDesiredJawPosition&); 00201 void stalled(const e_stalled&); 00202 void faulted(const e_faulted&); 00203 void still(const e_still&); 00204 void isOpen(const e_isOpen&); 00205 void setParameters(const c_setParameters& e); 00206 00207 typedef boost::mpl::list < boost::statechart::in_state_reaction< e_do, Container, &Container::doo >, 00208 boost::statechart::in_state_reaction< e_ready, Container, &Container::ready >, 00209 boost::statechart::in_state_reaction< e_unready, Container, &Container::unready >, 00210 boost::statechart::in_state_reaction< e_locked, Container, &Container::locked >, 00211 boost::statechart::in_state_reaction< e_atDesiredJawPosition, Container, &Container::atDesiredJawPosition >, 00212 boost::statechart::in_state_reaction< e_stalled, Container, &Container::stalled >, 00213 boost::statechart::in_state_reaction< e_faulted, Container, &Container::faulted >, 00214 boost::statechart::in_state_reaction< e_still, Container, &Container::still >, 00215 boost::statechart::in_state_reaction< e_isOpen, Container, &Container::isOpen > > reactions; 00216 }; 00217 00219 struct Inactive : boost::statechart::state<Inactive, Container::orthogonal<0> > 00220 { 00221 Inactive(my_context ctx); 00222 boost::statechart::result react(const i_ready_command&); 00223 00224 typedef boost::mpl::list < boost::statechart::custom_reaction< i_ready_command>, 00225 boost::statechart::transition< c_setParameters, Inactive, Container, &Container::setParameters> > reactions; 00226 00227 }; 00228 00230 struct Active : boost::statechart::state<Active, Container::orthogonal<0>, boost::mpl::list<Stopped, Listening> > 00231 { 00232 Active(my_context ctx); 00233 00234 void doo(const i_do_active&); 00235 void set(const c_set&); 00236 void lock(const c_lock&); 00237 void release(const c_release&); 00238 void cancel(const c_cancel&); 00239 00240 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_active, Active, &Active::doo >, 00241 boost::statechart::in_state_reaction< c_set, Active, &Active::set >, 00242 boost::statechart::in_state_reaction< c_lock, Active, &Active::lock >, 00243 boost::statechart::in_state_reaction< c_release, Active, &Active::release >, 00244 boost::statechart::in_state_reaction< c_cancel, Active, &Active::cancel >, 00245 boost::statechart::transition< i_unready_command, Inactive >, 00246 boost::statechart::transition< i_faulted_command, Inactive > > reactions; 00247 00248 double desiredCurrent; 00249 double outputCurrent; 00250 }; 00251 00253 struct Stopped : boost::statechart::state<Stopped, Active::orthogonal<0> > 00254 { 00255 Stopped(my_context ctx); 00256 00257 typedef boost::mpl::list < boost::statechart::transition< e_open, Opening >, 00258 boost::statechart::transition< e_close, Closing > > reactions; 00259 00260 }; 00261 00262 struct Opening : boost::statechart::simple_state<Opening, Active::orthogonal<0> > 00263 { 00264 void doo(const i_do_movement&); 00265 00266 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_movement, Opening, &Opening::doo >, 00267 boost::statechart::transition< e_stop, Stopped >, 00268 boost::statechart::transition< i_isOpen_movement, Stopped >, 00269 boost::statechart::transition< e_close, Closing > > reactions; 00270 }; 00271 00272 struct Closing : boost::statechart::simple_state<Closing, Active::orthogonal<0> > 00273 { 00274 void doo(const i_do_movement&); 00275 00276 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_movement, Closing, &Closing::doo >, 00277 boost::statechart::transition< e_stop, Stopped >, 00278 boost::statechart::transition< e_isClosed, Stopped >, 00279 boost::statechart::transition< e_open, Opening > > reactions; 00280 }; 00281 00283 struct Listening : boost::statechart::state<Listening, Active::orthogonal<1> > 00284 { 00285 Listening(my_context ctx); 00286 00287 typedef boost::mpl::list < boost::statechart::transition< e_set, OpenWide >, 00288 boost::statechart::transition< e_lock, Close >, 00289 boost::statechart::transition< e_release, OpenUntilStall >, 00290 boost::statechart::transition< c_setParameters, Listening, Container, &Container::setParameters> > reactions; 00291 }; 00292 00294 struct Acting : boost::statechart::simple_state<Acting, Active::orthogonal<1>, Done > 00295 { 00296 typedef boost::mpl::list< boost::statechart::transition< e_listen, Listening > > reactions; 00297 }; 00298 00300 struct OpenWide : boost::statechart::state<OpenWide, Acting > 00301 { 00302 OpenWide(my_context ctx); 00303 boost::statechart::result react(const i_stalled_action&); 00304 00305 typedef boost::mpl::list < boost::statechart::custom_reaction< i_stalled_action >, 00306 boost::statechart::transition< i_isOpen_action, JawPositionCheck>, 00307 boost::statechart::transition< e_next, JawPositionCheck > > reactions; 00308 }; 00309 00311 struct JawPositionCheck : boost::statechart::state<JawPositionCheck, Acting > 00312 { 00313 JawPositionCheck(my_context ctx); 00314 boost::statechart::result react(const i_stalled_action&); 00315 00316 typedef boost::mpl::list < boost::statechart::transition< i_atDesiredJawPosition_action, Done >, 00317 boost::statechart::transition< e_next, Done >, 00318 boost::statechart::custom_reaction< i_stalled_action > > reactions; 00319 }; 00320 00322 struct Close : boost::statechart::state<Close, Acting > 00323 { 00324 Close(my_context ctx); 00325 boost::statechart::result react(const i_stalled_action&); 00326 00327 typedef boost::mpl::list < boost::statechart::transition< i_atDesiredJawPosition_action, LockCheck >, 00328 boost::statechart::transition< e_next, LockCheck >, 00329 boost::statechart::custom_reaction< i_stalled_action > > reactions; 00330 }; 00331 00333 struct LockCheck : boost::statechart::state<LockCheck, Acting > 00334 { 00335 LockCheck(my_context ctx); 00336 boost::statechart::result react(const i_stalled_action&); 00337 00338 typedef boost::mpl::list < boost::statechart::transition< e_next, Done >, 00339 boost::statechart::custom_reaction< i_stalled_action > > reactions; 00340 }; 00341 00343 struct OpenUntilStall : boost::statechart::state<OpenUntilStall, Acting > 00344 { 00345 OpenUntilStall(my_context ctx); 00346 00347 typedef boost::mpl::list < boost::statechart::transition< i_stalled_action, Done >, 00348 boost::statechart::transition< e_next, Done >, 00349 boost::statechart::transition< i_isOpen_action, Done > > reactions; 00350 }; 00351 00353 struct Done : boost::statechart::state<Done, Acting > 00354 { 00355 Done(my_context ctx); 00356 00357 typedef boost::statechart::transition< e_done, Listening > reactions; 00358 }; 00359 00360 struct Unlocked : boost::statechart::simple_state<Unlocked, Container::orthogonal<1> > 00361 { 00362 void doo(const i_do_lock&); 00363 00364 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_lock, Unlocked, &Unlocked::doo >, 00365 boost::statechart::transition< i_locked_lock, Locked > > reactions; 00366 }; 00367 00368 struct Locked : boost::statechart::simple_state<Locked, Container::orthogonal<1> > 00369 { 00370 void doo(const i_do_lock&); 00371 00372 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_lock, Locked, &Locked::doo >, 00373 boost::statechart::transition< e_unlocked, Unlocked > > reactions; 00374 }; 00375 00376 struct UnlockedStatus : boost::statechart::simple_state<UnlockedStatus, Container::orthogonal<2> > 00377 { 00378 void doo(const i_do_lockStatus&); 00379 00380 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_lockStatus, UnlockedStatus, &UnlockedStatus::doo >, 00381 boost::statechart::transition< e_lockedStatus, LockedStatus > > reactions; 00382 }; 00383 00384 struct LockedStatus : boost::statechart::simple_state<LockedStatus, Container::orthogonal<2> > 00385 { 00386 void doo(const i_do_lockStatus&); 00387 00388 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_lockStatus, LockedStatus, &LockedStatus::doo >, 00389 boost::statechart::transition< e_unlockedStatus, UnlockedStatus > > reactions; 00390 }; 00391 00392 struct Unloaded : boost::statechart::simple_state<Unloaded, Container::orthogonal<3> > 00393 { 00394 void doo(const i_do_load&); 00395 00396 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_load, Unloaded, &Unloaded::doo >, 00397 boost::statechart::transition< e_loaded, Loaded > > reactions; 00398 }; 00399 00400 struct Loaded : boost::statechart::simple_state<Loaded, Container::orthogonal<3> > 00401 { 00402 void doo(const i_do_load&); 00403 00404 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_load, Loaded, &Loaded::doo >, 00405 boost::statechart::transition< e_unloaded, Unloaded > > reactions; 00406 }; 00407 00408 struct LessThanDesiredJawPosition : boost::statechart::simple_state<LessThanDesiredJawPosition, Container::orthogonal<4> > 00409 { 00410 void doo(const i_do_position&); 00411 00412 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_position, LessThanDesiredJawPosition, &LessThanDesiredJawPosition::doo >, 00413 boost::statechart::transition< i_atDesiredJawPosition_position, AtDesiredJawPosition >, 00414 boost::statechart::transition< e_moreThanDesiredJawPosition, MoreThanDesiredJawPosition > > reactions; 00415 }; 00416 00417 struct AtDesiredJawPosition : boost::statechart::simple_state<AtDesiredJawPosition, Container::orthogonal<4> > 00418 { 00419 void doo(const i_do_position&); 00420 00421 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_position, AtDesiredJawPosition, &AtDesiredJawPosition::doo >, 00422 boost::statechart::transition< e_lessThanDesiredJawPosition, LessThanDesiredJawPosition >, 00423 boost::statechart::transition< e_moreThanDesiredJawPosition, MoreThanDesiredJawPosition > > reactions; 00424 }; 00425 00426 struct MoreThanDesiredJawPosition : boost::statechart::simple_state<MoreThanDesiredJawPosition, Container::orthogonal<4> > 00427 { 00428 void doo(const i_do_position&); 00429 00430 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_position, MoreThanDesiredJawPosition, &MoreThanDesiredJawPosition::doo >, 00431 boost::statechart::transition< e_lessThanDesiredJawPosition, LessThanDesiredJawPosition >, 00432 boost::statechart::transition< i_atDesiredJawPosition_position, AtDesiredJawPosition > > reactions; 00433 }; 00434 00435 struct NotStalled : boost::statechart::simple_state<NotStalled, Container::orthogonal<5> > 00436 { 00437 void doo(const i_do_stall&); 00438 00439 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_stall, NotStalled, &NotStalled::doo >, 00440 boost::statechart::transition< i_stalled_stall, Stalled > > reactions; 00441 }; 00442 00443 struct Stalled : boost::statechart::simple_state<Stalled, Container::orthogonal<5> > 00444 { 00445 void doo(const i_do_stall&); 00446 00447 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_stall, Stalled, &Stalled::doo >, 00448 boost::statechart::transition< e_notStalled, NotStalled > > reactions; 00449 }; 00450 00451 struct Unready : boost::statechart::simple_state<Unready, Container::orthogonal<6> > 00452 { 00453 void doo(const i_do_status&); 00454 00455 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_status, Unready, &Unready::doo >, 00456 boost::statechart::transition< i_ready_status, Ready > > reactions; 00457 }; 00458 00459 struct Ready : boost::statechart::simple_state<Ready, Container::orthogonal<6> > 00460 { 00461 void doo(const i_do_status&); 00462 00463 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_status, Ready, &Ready::doo >, 00464 boost::statechart::transition< i_unready_status, Unready > > reactions; 00465 }; 00466 00467 struct NotFaulted : boost::statechart::simple_state<NotFaulted, Container::orthogonal<7> > 00468 { 00469 void doo(const i_do_fault&); 00470 00471 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_fault, NotFaulted, &NotFaulted::doo >, 00472 boost::statechart::transition< i_faulted_fault, Faulted > > reactions; 00473 }; 00474 00475 struct Faulted : boost::statechart::simple_state<Faulted, Container::orthogonal<7> > 00476 { 00477 void doo(const i_do_fault&); 00478 00479 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_fault, Faulted, &Faulted::doo >, 00480 boost::statechart::transition< e_notFaulted, NotFaulted > > reactions; 00481 }; 00482 00483 struct Still : boost::statechart::simple_state<Still, Container::orthogonal<8> > 00484 { 00485 void doo(const i_do_moving&); 00486 00487 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_moving, Still, &Still::doo >, 00488 boost::statechart::transition< e_moving, Moving > > reactions; 00489 }; 00490 00491 struct Moving : boost::statechart::simple_state<Moving, Container::orthogonal<8> > 00492 { 00493 void doo(const i_do_moving&); 00494 00495 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_moving, Moving, &Moving::doo >, 00496 boost::statechart::transition< i_still_moving, Still > > reactions; 00497 }; 00498 00499 struct NotOpen : boost::statechart::simple_state<NotOpen, Container::orthogonal<9> > 00500 { 00501 void doo(const i_do_open&); 00502 00503 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_open, NotOpen, &NotOpen::doo >, 00504 boost::statechart::transition< i_isOpen_open, Open > > reactions; 00505 }; 00506 00507 struct Open : boost::statechart::simple_state<Open, Container::orthogonal<9> > 00508 { 00509 void doo(const i_do_open&); 00510 00511 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_open, Open, &Open::doo >, 00512 boost::statechart::transition< e_isNotOpen, NotOpen > > reactions; 00513 }; 00514 00515 struct LessThanForceThreshold : boost::statechart::state<LessThanForceThreshold, Container::orthogonal<10> > 00516 { 00517 LessThanForceThreshold(my_context ctx); 00518 void doo(const i_do_force&); 00519 00520 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_force, LessThanForceThreshold, &LessThanForceThreshold::doo >, 00521 boost::statechart::transition< e_moreThanForceThreshold, MoreThanForceThreshold > > reactions; 00522 }; 00523 00524 struct MoreThanForceThreshold : boost::statechart::state<MoreThanForceThreshold, Container::orthogonal<10> > 00525 { 00526 MoreThanForceThreshold(my_context ctx); 00527 void doo(const i_do_force&); 00528 00529 typedef boost::mpl::list < boost::statechart::in_state_reaction< i_do_force, MoreThanForceThreshold, &MoreThanForceThreshold::doo >, 00530 boost::statechart::transition< e_lessThanForceThreshold, LessThanForceThreshold > > reactions; 00531 }; 00532 00533 #endif