00001 #include "robodyn_mechanisms/GripperPositionStateMachine.h"
00002
00003 using namespace log4cpp;
00004
00005 GripperPosition::GripperPosition()
00006 : mechanism(""),
00007 currentMax(0.0),
00008 currentLimitName(""),
00009 currentLimit(0.0),
00010 measuredCurrentName(""),
00011 measuredCurrent(0.0),
00012 jawOpenPosition(0.0),
00013 jawPositionNoise(0.0),
00014 encoderOpenVelocity(0.0),
00015 encoderCloseVelocity(0.0),
00016 encoderClosedPosition(0.0),
00017 encoderPositionNoise(0.0),
00018 encoderVelocityNoise(0.0),
00019 currentNoise(0.0),
00020 weakForce(0.0),
00021 strongForce(0.0),
00022 desiredForce(0.0),
00023 jawPositionForceThreshold(0.0),
00024 initializationStateName("")
00025 {
00026 gripperKinematics.reset(new GripperKinematics());
00027 }
00028
00029 void Container::doo(const e_do&)
00030 {
00031 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Container::do() - post(i_do_*)";
00032 post_event(i_do_fault());
00033 post_event(i_do_status());
00034 post_event(i_do_load());
00035 post_event(i_do_lock());
00036 post_event(i_do_lockStatus());
00037 post_event(i_do_position());
00038 post_event(i_do_open());
00039 post_event(i_do_force());
00040 post_event(i_do_stall());
00041 post_event(i_do_moving());
00042 post_event(i_do_active());
00043 post_event(i_do_movement());
00044 }
00045
00046 void Container::ready(const e_ready&)
00047 {
00048 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Container::ready() - post(i_ready_*)";
00049 post_event(i_ready_command());
00050 post_event(i_ready_status());
00051 }
00052
00053 void Container::unready(const e_unready&)
00054 {
00055 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Container::unready() - post(i_unready_*)";
00056 post_event(i_unready_command());
00057 post_event(i_unready_status());
00058 }
00059
00060 void Container::locked(const e_locked&)
00061 {
00062 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Container::locked() - post(i_locked_*)";
00063 post_event(i_locked_action());
00064 post_event(i_locked_lock());
00065 }
00066
00067 void Container::atDesiredJawPosition(const e_atDesiredJawPosition&)
00068 {
00069 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Container::atDesiredJawPosition() - post(i_atDesiredJawPosition_*)";
00070 post_event(i_atDesiredJawPosition_action());
00071 post_event(i_atDesiredJawPosition_position());
00072 }
00073
00074 void Container::stalled(const e_stalled&)
00075 {
00076 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Container::stalled() - post(i_stalled_*)";
00077 post_event(i_stalled_action());
00078 post_event(i_stalled_stall());
00079 }
00080
00081 void Container::faulted(const e_faulted&)
00082 {
00083 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Container::faulted() - post(i_faulted_*)";
00084 post_event(i_faulted_command());
00085 post_event(i_faulted_fault());
00086 }
00087
00088 void Container::still(const e_still&)
00089 {
00090 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Container::still() - post(i_still_*)";
00091 post_event(i_still_action());
00092 post_event(i_still_moving());
00093 }
00094
00095 void Container::isOpen(const e_isOpen&)
00096 {
00097 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Container::isOpen() - post(i_isOpen_*)";
00098 post_event(i_isOpen_movement());
00099 post_event(i_isOpen_action());
00100 post_event(i_isOpen_open());
00101 }
00102
00103 void Container::setParameters(const c_setParameters& e)
00104 {
00105 context<GripperPosition>().setPoint = e.sp;
00106 }
00107
00108 Inactive::Inactive(my_context ctx)
00109 : my_base(ctx)
00110 {
00112 if (not context<GripperPosition>().encoderState.position.empty())
00113 {
00114 context<GripperPosition>().jointCommand.desiredPosition.assign(1, context<GripperPosition>().encoderState.position[0]);
00115 }
00116
00117 if (not context<GripperPosition>().sendCommand.empty())
00118 {
00119 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Inactive::entry() - command(stop)";
00120 context<GripperPosition>().sendCommand(context<GripperPosition>().jointCommand);
00121 }
00122 }
00123
00124 boost::statechart::result Inactive::react(const i_ready_command&)
00125 {
00127 if (state_downcast< const NotFaulted* >() != 0)
00128 {
00129 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Inactive::react() - transit(Active)";
00130 return transit<Active>();
00131 }
00132
00133 return discard_event();
00134 }
00135
00136 Active::Active(my_context ctx)
00137 : my_base(ctx)
00138 {
00139 desiredCurrent = 0;
00140 outputCurrent = context<GripperPosition>().getFloat(context<GripperPosition>().currentLimitName);
00141 }
00142
00143 void Active::doo(const i_do_active&)
00144 {
00145 if ((not context<GripperPosition>().encoderState.position.empty()) && (not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00146 {
00148 double averageJawPosition = (context<GripperPosition>().jawLeftState.position[0] + context<GripperPosition>().jawRightState.position[0]) / 2.0;
00149
00150 if (context<GripperPosition>().getUInt16(context<GripperPosition>().initializationStateName) > 0)
00151 {
00153 if (averageJawPosition <= (context<GripperPosition>().jawPositionForceThreshold + context<GripperPosition>().jawPositionNoise))
00154 {
00155 desiredCurrent = context<GripperPosition>().currentMax;
00156 }
00157 else
00158 {
00159 desiredCurrent = context<GripperPosition>().gripperKinematics->getMotorCurrentLimit(context<GripperPosition>().encoderState.position[0], averageJawPosition, context<GripperPosition>().desiredForce);
00160 }
00161 }
00162 else
00163 {
00164 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Active::doo() - Gripper not initialized yet. desiredCurrent = currentMax";
00165 desiredCurrent = context<GripperPosition>().currentMax;
00166 }
00167
00169 outputCurrent = context<GripperPosition>().outputCurrentFilter.filter(outputCurrent, desiredCurrent);
00170
00172 if (outputCurrent > context<GripperPosition>().currentMax)
00173 {
00174 outputCurrent = context<GripperPosition>().currentMax;
00175 }
00176
00177 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachineDebug") << Priority::DEBUG << "encPos: " << context<GripperPosition>().encoderState.position[0] << " jawPos: " << averageJawPosition << " desiredCurrent: " << desiredCurrent << " outputCurrent: " << outputCurrent << " currentMax: " << context<GripperPosition>().currentMax;
00178
00179 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Active::do() - setCurrentLimit()";
00180 context<GripperPosition>().setFloat(context<GripperPosition>().currentLimitName, outputCurrent);
00181
00183 uint16_t previousDutyCycleLimit = context<GripperPosition>().getUInt16(context<GripperPosition>().dutyCycleLimitName);
00184 uint16_t newDutyCycleLimit = context<GripperPosition>().dutyCycleLimitFilter.filter(previousDutyCycleLimit, context<GripperPosition>().dutyCycleLimit);
00185
00186 if (newDutyCycleLimit > context<GripperPosition>().dutyCycleMax)
00187 {
00188 newDutyCycleLimit = context<GripperPosition>().dutyCycleMax;
00189 }
00190
00191 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Active::do() - setDutyCycleLimit()";
00192 context<GripperPosition>().setUInt16(context<GripperPosition>().dutyCycleLimitName, newDutyCycleLimit);
00193 }
00194 else
00195 {
00196 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] Active::do() - Missing position(s).";
00197 }
00198 }
00199
00200 void Active::set(const c_set&)
00201 {
00202 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::PREEMPTING;
00203 context<GripperPosition>().goalStatus.text = "received new command, set";
00204
00205 if (not context<GripperPosition>().sendGoalStatus.empty())
00206 {
00207 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00208 }
00209
00210 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Active::set() - post(e_stop, e_listen, e_set)";
00211 post_event(e_stop());
00212 post_event(e_listen());
00213 post_event(e_set());
00214 }
00215
00216 void Active::lock(const c_lock&)
00217 {
00218 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::PREEMPTING;
00219 context<GripperPosition>().goalStatus.text = "received new command, lock";
00220
00221 if (not context<GripperPosition>().sendGoalStatus.empty())
00222 {
00223 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00224 }
00225
00226 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Active::lock() - post(e_stop, e_listen, e_lock)";
00227 post_event(e_stop());
00228 post_event(e_listen());
00229 post_event(e_lock());
00230 }
00231
00232 void Active::release(const c_release&)
00233 {
00234 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::PREEMPTING;
00235 context<GripperPosition>().goalStatus.text = "received new command, release";
00236
00237 if (not context<GripperPosition>().sendGoalStatus.empty())
00238 {
00239 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00240 }
00241
00242 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Active::release() - post(e_stop, e_listen, e_release)";
00243 post_event(e_stop());
00244 post_event(e_listen());
00245 post_event(e_release());
00246 }
00247
00248 void Active::cancel(const c_cancel&)
00249 {
00250 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::PREEMPTED;
00251 context<GripperPosition>().goalStatus.text = "cancelled";
00252
00253 if (not context<GripperPosition>().sendGoalStatus.empty())
00254 {
00255 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00256 }
00257
00258 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Active::cancel() - post(e_stop, e_listen)";
00259 post_event(e_stop());
00260 post_event(e_listen());
00261 }
00262
00263 Stopped::Stopped(my_context ctx)
00264 : my_base(ctx)
00265 {
00267 if (not context<GripperPosition>().encoderState.position.empty())
00268 {
00269 context<GripperPosition>().jointCommand.desiredPositionVelocityLimit.assign(1, 0);
00270 }
00271
00272 if (not context<GripperPosition>().sendCommand.empty())
00273 {
00274 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Stopped::entry() - command(stop)";
00275 context<GripperPosition>().sendCommand(context<GripperPosition>().jointCommand);
00276 }
00277 }
00278
00279 void Opening::doo(const i_do_movement&)
00280 {
00281 if ((not context<GripperPosition>().encoderState.position.empty()) && (not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00282 {
00283 context<GripperPosition>().jointCommand.desiredPositionVelocityLimit.assign(1, context<GripperPosition>().encoderOpenVelocity);
00284
00285 if (not context<GripperPosition>().sendCommand.empty())
00286 {
00287 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Opening::do() - command(open)";
00288 context<GripperPosition>().sendCommand(context<GripperPosition>().jointCommand);
00289 }
00290 }
00291 else
00292 {
00293 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] Opening::do() - Missing position(s).";
00294 }
00295 }
00296
00297 void Closing::doo(const i_do_movement&)
00298 {
00300 if ((not context<GripperPosition>().encoderState.position.empty()) && (not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00301 {
00302 context<GripperPosition>().jointCommand.desiredPositionVelocityLimit.assign(1, context<GripperPosition>().encoderCloseVelocity);
00303
00304 if (not context<GripperPosition>().sendCommand.empty())
00305 {
00306 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Closing::do() - command(close)";
00307 context<GripperPosition>().sendCommand(context<GripperPosition>().jointCommand);
00308 }
00309 }
00310 else
00311 {
00312 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] Closing::do() - Missing position(s).";
00313 }
00314
00317
00318
00319
00320
00321
00322
00323
00324
00325 }
00326
00327 Listening::Listening(my_context ctx)
00328 : my_base(ctx)
00329 {
00330 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Listening::entry() - post(e_stop)";
00331 post_event(e_stop());
00332 }
00333
00334 OpenWide::OpenWide(my_context ctx)
00335 : my_base(ctx)
00336 {
00337 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::ACTIVE;
00338 context<GripperPosition>().goalStatus.text = "opening wide";
00339
00340 if (not context<GripperPosition>().sendGoalStatus.empty())
00341 {
00342 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00343 }
00344
00346 if (state_downcast< const NotOpen* >() != 0)
00347 {
00348 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] OpenWide::entry() - post(e_open)";
00349 post_event(e_open());
00350 }
00351 else
00352 {
00353 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] OpenWide::entry() - post(e_next)";
00354 post_event(e_next());
00355 }
00356 }
00357
00358 boost::statechart::result OpenWide::react(const i_stalled_action&)
00359 {
00361 if (state_downcast< const Open* >() != 0)
00362 {
00363 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] OpenWide - transit(JawPositionCheck)";
00364 return transit<JawPositionCheck>();
00365 }
00366 else
00367 {
00368 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] OpenWide - Failed to complete.";
00369
00370 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::ABORTED;
00371 context<GripperPosition>().goalStatus.text = "encoder still and jaws not open";
00372
00373 if (not context<GripperPosition>().sendGoalStatus.empty())
00374 {
00375 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00376 }
00377
00378 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] OpenWide::react() - transit(Listening)";
00379 return transit<Listening>();
00380 }
00381
00382 return discard_event();
00383 }
00384
00385 JawPositionCheck::JawPositionCheck(my_context ctx)
00386 : my_base(ctx)
00387 {
00388 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::ACTIVE;
00389 context<GripperPosition>().goalStatus.text = "checking jaw position";
00390
00391 if (not context<GripperPosition>().sendGoalStatus.empty())
00392 {
00393 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00394 }
00395
00397 if (state_downcast< const MoreThanDesiredJawPosition* >() != 0)
00398 {
00399 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] JawPositionCheck::entry() - post(e_close)";
00400 post_event(e_close());
00401 }
00402 else
00403 {
00404 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] JawPositionCheck::entry() - post(e_next)";
00405 post_event(e_next());
00406 }
00407 }
00408
00409 boost::statechart::result JawPositionCheck::react(const i_stalled_action&)
00410 {
00412 if (state_downcast< const AtDesiredJawPosition* >() == 0)
00413 {
00414 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] JawPositionCheck - Failed to complete.";
00415
00416 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::ABORTED;
00417 context<GripperPosition>().goalStatus.text = "encoder still and not at desired jaw position";
00418
00419 if (not context<GripperPosition>().sendGoalStatus.empty())
00420 {
00421 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00422 }
00423
00424 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] JawPositionCheck::react() - transit(Listening)";
00425 return transit<Listening>();
00426 }
00427
00428 return discard_event();
00429 }
00430
00431 Close::Close(my_context ctx)
00432 : my_base(ctx)
00433 {
00434 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::ACTIVE;
00435 context<GripperPosition>().goalStatus.text = "closing";
00436
00437 if (not context<GripperPosition>().sendGoalStatus.empty())
00438 {
00439 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00440 }
00441
00443 if (state_downcast< const MoreThanDesiredJawPosition* >() != 0)
00444 {
00445 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Close::entry() - post(e_close)";
00446 post_event(e_close());
00447 }
00448 else
00449 {
00450 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Close::entry() - post(e_next)";
00451 post_event(e_next());
00452 }
00453 }
00454
00455 boost::statechart::result Close::react(const i_stalled_action&)
00456 {
00458 if (state_downcast< const MoreThanDesiredJawPosition* >() != 0)
00459 {
00460 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] Close - Failed to complete.";
00461
00462 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::ABORTED;
00463 context<GripperPosition>().goalStatus.text = "encoder still and not at desired jaw position or less";
00464
00465 if (not context<GripperPosition>().sendGoalStatus.empty())
00466 {
00467 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00468 }
00469
00470 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Close::react() - transit(Listening)";
00471 return transit<Listening>();
00472 }
00473
00474 return discard_event();
00475 }
00476
00477 LockCheck::LockCheck(my_context ctx)
00478 : my_base(ctx)
00479 {
00480 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::ACTIVE;
00481 context<GripperPosition>().goalStatus.text = "checking for over center";
00482
00483 if (not context<GripperPosition>().sendGoalStatus.empty())
00484 {
00485 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00486 }
00487
00488 post_event(e_close());
00489 }
00490
00491 boost::statechart::result LockCheck::react(const i_stalled_action&)
00492 {
00494 if (state_downcast< const Unlocked* >() != 0)
00495 {
00496 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] LockCheck - Failed to complete.";
00497
00498 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::ABORTED;
00499 context<GripperPosition>().goalStatus.text = "encoder still and not over center";
00500
00501 if (not context<GripperPosition>().sendGoalStatus.empty())
00502 {
00503 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00504 }
00505
00506 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] LockCheck::react() - transit(Listening)";
00507 return transit<Listening>();
00508 }
00509 else
00510 {
00511 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] LockCheck::react() - post(e_next)";
00512 post_event(e_next());
00513 }
00514
00515 return discard_event();
00516 }
00517
00518 OpenUntilStall::OpenUntilStall(my_context ctx)
00519 : my_base(ctx)
00520 {
00521 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::ACTIVE;
00522 context<GripperPosition>().goalStatus.text = "opening until stall detected";
00523
00524 if (not context<GripperPosition>().sendGoalStatus.empty())
00525 {
00526 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00527 }
00528
00530 if (state_downcast< const NotOpen* >() != 0)
00531 {
00532 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] OpenUntilStall::entry() - post(e_open)";
00533 post_event(e_open());
00534 }
00535 else
00536 {
00537 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] OpenUntilStall::entry() - post(e_next)";
00538 post_event(e_next());
00539 }
00540 }
00541
00542 Done::Done(my_context ctx)
00543 : my_base (ctx)
00544 {
00545 context<GripperPosition>().goalStatus.status = actionlib_msgs::GoalStatus::SUCCEEDED;
00546 context<GripperPosition>().goalStatus.text = "complete";
00547
00548 if (not context<GripperPosition>().sendGoalStatus.empty())
00549 {
00550 context<GripperPosition>().sendGoalStatus(context<GripperPosition>().goalStatus, true);
00551 }
00552
00553 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Done::entry() - post(e_stop, e_done)";
00554 post_event(e_stop());
00555 post_event(e_done());
00556 }
00557
00558 void Unlocked::doo(const i_do_lock&)
00559 {
00560 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Unlocked::do()";
00561
00562 if ((not context<GripperPosition>().encoderState.position.empty()) && (not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00563 {
00564 float averageJawPosition = (context<GripperPosition>().jawLeftState.position[0] + context<GripperPosition>().jawRightState.position[0]) / 2.0;
00565
00566 if (context<GripperPosition>().getUInt16(context<GripperPosition>().initializationStateName) > 0)
00567 {
00568 if (context<GripperPosition>().gripperKinematics->isOverCenter(context<GripperPosition>().encoderState.position[0], averageJawPosition))
00569 {
00570 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Unlocked::doo() - post(e_locked)";
00571 post_event(e_locked());
00572 }
00573 }
00574 else
00575 {
00576 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Unlocked::doo() - Gripper not initialized yet.";
00577 }
00578 }
00579 else
00580 {
00581 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] Unlocked::do() - Missing position(s).";
00582 }
00583 }
00584
00585 void Locked::doo(const i_do_lock&)
00586 {
00587 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Locked::do()";
00588
00589 if ((not context<GripperPosition>().encoderState.position.empty()) && (not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00590 {
00591 float averageJawPosition = (context<GripperPosition>().jawLeftState.position[0] + context<GripperPosition>().jawRightState.position[0]) / 2.0;
00592
00593 if (context<GripperPosition>().getUInt16(context<GripperPosition>().initializationStateName) > 0)
00594 {
00595 if (not context<GripperPosition>().gripperKinematics->isOverCenter(context<GripperPosition>().encoderState.position[0], averageJawPosition))
00596 {
00597 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Locked::doo() - post(e_unlocked)";
00598 post_event(e_unlocked());
00599 }
00600 }
00601 else
00602 {
00603 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Locked::doo() - Gripper not initialized yet.";
00604 }
00605 }
00606 else
00607 {
00608 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] Locked::do() - Missing position(s).";
00609 }
00610 }
00611
00612 void UnlockedStatus::doo(const i_do_lockStatus&)
00613 {
00614 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] UnlockedStatus::do()";
00615
00616 if ((not context<GripperPosition>().encoderState.position.empty()) && (not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00617 {
00618 float averageJawPosition = (context<GripperPosition>().jawLeftState.position[0] + context<GripperPosition>().jawRightState.position[0]) / 2.0;
00619
00620 if (context<GripperPosition>().getUInt16(context<GripperPosition>().initializationStateName) > 0)
00621 {
00622 if (context<GripperPosition>().gripperKinematics->isOverCenterStatus(context<GripperPosition>().encoderState.position[0], averageJawPosition))
00623 {
00624 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] UnlockedStatus::doo() - post(e_lockedStatus)";
00625 post_event(e_lockedStatus());
00626 }
00627 }
00628 else
00629 {
00630 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] UnlockedStatus::doo() - Gripper not initialized yet.";
00631 }
00632 }
00633 else
00634 {
00635 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] UnlockedStatus::do() - Missing position(s).";
00636 }
00637 }
00638
00639 void LockedStatus::doo(const i_do_lockStatus&)
00640 {
00641 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] LockedStatus::do()";
00642
00643 if ((not context<GripperPosition>().encoderState.position.empty()) && (not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00644 {
00645 float averageJawPosition = (context<GripperPosition>().jawLeftState.position[0] + context<GripperPosition>().jawRightState.position[0]) / 2.0;
00646
00647 if (context<GripperPosition>().getUInt16(context<GripperPosition>().initializationStateName) > 0)
00648 {
00649 if (not context<GripperPosition>().gripperKinematics->isOverCenterStatus(context<GripperPosition>().encoderState.position[0], averageJawPosition))
00650 {
00651 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] LockedStatus::doo() - post(e_unlockedStatus)";
00652 post_event(e_unlockedStatus());
00653 }
00654 }
00655 else
00656 {
00657 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] LockedStatus::doo() - Gripper not initialized yet.";
00658 }
00659 }
00660 else
00661 {
00662 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] LockedStatus::do() - Missing position(s).";
00663 }
00664 }
00665
00666 void Unloaded::doo(const i_do_load&)
00667 {
00668 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Unloaded::do()";
00669
00670 if ((not context<GripperPosition>().jawLeftState.effort.empty()) && (not context<GripperPosition>().jawRightState.effort.empty()))
00671 {
00672 float averageJawLoad = (context<GripperPosition>().jawLeftState.effort[0] + context<GripperPosition>().jawRightState.effort[0]) / 2.0;
00673
00674 if (averageJawLoad >= (context<GripperPosition>().setPoint.expectedLoad - context<GripperPosition>().setPoint.expectedLoadDelta))
00675 {
00676 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Unloaded::do() - post(e_loaded)";
00677 post_event(e_loaded());
00678 }
00679 }
00680 else
00681 {
00682 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] Unloaded::do() - Missing effort(s).";
00683 }
00684 }
00685
00686 void Loaded::doo(const i_do_load&)
00687 {
00688 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Loaded::do()";
00689
00690 if ((not context<GripperPosition>().jawLeftState.effort.empty()) && (not context<GripperPosition>().jawRightState.effort.empty()))
00691 {
00692 float averageJawLoad = (context<GripperPosition>().jawLeftState.effort[0] + context<GripperPosition>().jawRightState.effort[0]) / 2.0;
00693
00694 if (averageJawLoad < (context<GripperPosition>().setPoint.expectedLoad - context<GripperPosition>().setPoint.expectedLoadDelta))
00695 {
00696 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Loaded::do() - post(e_unloaded)";
00697 post_event(e_unloaded());
00698 }
00699 }
00700 else
00701 {
00702 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] Loaded::do() - Missing effort(s).";
00703 }
00704 }
00705
00706 void LessThanDesiredJawPosition::doo(const i_do_position&)
00707 {
00708 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] LessThanDesiredJawPosition::do()";
00709
00711 if ((not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00712 {
00714 float averageJawPosition = (context<GripperPosition>().jawLeftState.position[0] + context<GripperPosition>().jawRightState.position[0]) / 2.0;
00715
00716 if (std::abs(averageJawPosition - context<GripperPosition>().setPoint.jawPosition) <= context<GripperPosition>().setPoint.jawPositionDelta)
00717 {
00718 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] LessThanDesiredJawPosition::do() - post(e_atDesiredJawPosition)";
00719 post_event(e_atDesiredJawPosition());
00720 }
00721
00722 if (averageJawPosition > (context<GripperPosition>().setPoint.jawPosition + context<GripperPosition>().setPoint.jawPositionDelta))
00723 {
00724 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] LessThanDesiredJawPosition::do() - post(e_moreThanDesiredJawPosition)";
00725 post_event(e_moreThanDesiredJawPosition());
00726 }
00727 }
00728 else
00729 {
00730 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] LessThanDesiredJawPosition::do() - Missing jaw position(s).";
00731 }
00732 }
00733
00734 void AtDesiredJawPosition::doo(const i_do_position&)
00735 {
00736 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] AtDesiredJawPosition::do()";
00737
00739 if ((not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00740 {
00742 float averageJawPosition = (context<GripperPosition>().jawLeftState.position[0] + context<GripperPosition>().jawRightState.position[0]) / 2.0;
00743
00744 if (averageJawPosition < (context<GripperPosition>().setPoint.jawPosition - context<GripperPosition>().setPoint.jawPositionDelta))
00745 {
00746 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] AtDesiredJawPosition::do() - post(e_lessThanDesiredJawPosition)";
00747 post_event(e_lessThanDesiredJawPosition());
00748 }
00749
00750 if (averageJawPosition > (context<GripperPosition>().setPoint.jawPosition + context<GripperPosition>().setPoint.jawPositionDelta))
00751 {
00752 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] AtDesiredJawPosition::do() - post(e_moreThanDesiredJawPosition)";
00753 post_event(e_moreThanDesiredJawPosition());
00754 }
00755 }
00756 else
00757 {
00758 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] AtDesiredJawPosition::do() - Missing jaw position(s).";
00759 }
00760 }
00761
00762 void MoreThanDesiredJawPosition::doo(const i_do_position&)
00763 {
00764 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] MoreThanDesiredJawPosition::do()";
00765
00767 if ((not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00768 {
00770 float averageJawPosition = (context<GripperPosition>().jawLeftState.position[0] + context<GripperPosition>().jawRightState.position[0]) / 2.0;
00771
00772 if (std::abs(averageJawPosition - context<GripperPosition>().setPoint.jawPosition) <= context<GripperPosition>().setPoint.jawPositionDelta)
00773 {
00774 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] MoreThanDesiredJawPosition::do() - post(e_atDesiredJawPosition)";
00775 post_event(e_atDesiredJawPosition());
00776 }
00777
00778 if (averageJawPosition < (context<GripperPosition>().setPoint.jawPosition - context<GripperPosition>().setPoint.jawPositionDelta))
00779 {
00780 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] MoreThanDesiredJawPosition::do() - post(e_lessThanDesiredJawPosition)";
00781 post_event(e_lessThanDesiredJawPosition());
00782 }
00783 }
00784 else
00785 {
00786 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] MoreThanDesiredJawPosition::do() - Missing jaw position(s).";
00787 }
00788 }
00789
00790 void NotStalled::doo(const i_do_stall&)
00791 {
00792 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] NotStalled::do()";
00793
00795 if (std::abs(context<GripperPosition>().measuredCurrent) >= (context<GripperPosition>().currentLimit - context<GripperPosition>().currentNoise))
00796 {
00798 if (state_downcast< const Still* >() != 0)
00799 {
00800 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] NotStalled::do() - post(e_stalled)";
00801 post_event(e_stalled());
00802 }
00803 }
00804 }
00805
00806 void Stalled::doo(const i_do_stall&)
00807 {
00808 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Stalled::do()";
00809
00810 if (std::abs(context<GripperPosition>().measuredCurrent) < (context<GripperPosition>().currentLimit + context<GripperPosition>().currentNoise))
00811 {
00812 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Stalled::do() - post(e_notStalled)";
00813 post_event(e_notStalled());
00814 }
00815 }
00816
00817 void Unready::doo(const i_do_status&)
00818 {
00819 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Unready::do()";
00820
00821 if (context<GripperPosition>().jointControlData.controlMode.state == r2_msgs::JointControlMode::DRIVE)
00822 {
00823 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Unready::do() - post(e_ready)";
00824 post_event(e_ready());
00825 }
00826 }
00827
00828 void Ready::doo(const i_do_status&)
00829 {
00830 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Ready::do()";
00831
00832 if (context<GripperPosition>().jointControlData.controlMode.state != r2_msgs::JointControlMode::DRIVE)
00833 {
00834 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Ready::do() - post(e_unready)";
00835 post_event(e_unready());
00836 }
00837 }
00838
00839 void NotFaulted::doo(const i_do_fault&)
00840 {
00841 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] NotFaulted::do()";
00843
00844
00845
00846
00847 }
00848
00849 void Faulted::doo(const i_do_fault&)
00850 {
00851 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Faulted::do()";
00853
00854
00855
00856
00857 }
00858
00859 void Still::doo(const i_do_moving&)
00860 {
00861 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Still::do()";
00862
00863 if (not context<GripperPosition>().encoderState.velocity.empty())
00864 {
00865 if (std::abs(context<GripperPosition>().encoderState.velocity[0]) >= context<GripperPosition>().encoderVelocityNoise)
00866 {
00867 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Still::do() - post(e_moving)";
00868 post_event(e_moving());
00869 }
00870 }
00871 else
00872 {
00873 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] Still::do() - Missing encoder velocity.";
00874 }
00875 }
00876
00877 void Moving::doo(const i_do_moving&)
00878 {
00879 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Moving::do()";
00880
00881 if (not context<GripperPosition>().encoderState.velocity.empty())
00882 {
00883 if (std::abs(context<GripperPosition>().encoderState.velocity[0]) < context<GripperPosition>().encoderVelocityNoise)
00884 {
00885 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Moving::do() - post(e_still)";
00886 post_event(e_still());
00887 }
00888 }
00889 else
00890 {
00891 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::WARN << "[" << context<GripperPosition>().mechanism << "] Moving::do() - Missing encoder velocity.";
00892 }
00893 }
00894
00895 void NotOpen::doo(const i_do_open&)
00896 {
00897 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] NotOpen::do()";
00898
00899 if ((not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00900 {
00901 double averageJawPosition = (context<GripperPosition>().jawLeftState.position[0] + context<GripperPosition>().jawRightState.position[0]) / 2.0;
00902
00903 if (averageJawPosition >= (context<GripperPosition>().jawOpenPosition - context<GripperPosition>().jawPositionNoise))
00904 {
00905 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] NotOpen::do() - post(e_isOpen)";
00906 post_event(e_isOpen());
00907 }
00908 }
00909 }
00910
00911 void Open::doo(const i_do_open&)
00912 {
00913 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] Open::do()";
00914
00915 if ((not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00916 {
00917 double averageJawPosition = (context<GripperPosition>().jawLeftState.position[0] + context<GripperPosition>().jawRightState.position[0]) / 2.0;
00918
00919 if (averageJawPosition < (context<GripperPosition>().jawOpenPosition - context<GripperPosition>().jawPositionNoise))
00920 {
00921 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] Open::do() - post(e_isNotOpen)";
00922 post_event(e_isNotOpen());
00923 }
00924 }
00925 }
00926
00927 LessThanForceThreshold::LessThanForceThreshold(my_context ctx)
00928 : my_base(ctx)
00929 {
00930 context<GripperPosition>().desiredForce = context<GripperPosition>().strongForce;
00931 }
00932
00933 void LessThanForceThreshold::doo(const i_do_force&)
00934 {
00935 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] LessThanForceThreshold::do()";
00936
00937 if ((not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00938 {
00939 double averageJawPosition = (context<GripperPosition>().jawLeftState.position[0] + context<GripperPosition>().jawRightState.position[0]) / 2.0;
00940
00941 if (averageJawPosition >= (context<GripperPosition>().jawPositionForceThreshold + context<GripperPosition>().jawPositionNoise))
00942 {
00943 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] LessThanForceThreshold::do() - post(e_moreThanForceThreshold)";
00944 post_event(e_moreThanForceThreshold());
00945 }
00946 }
00947 }
00948
00949 MoreThanForceThreshold::MoreThanForceThreshold(my_context ctx)
00950 : my_base(ctx)
00951 {
00952 context<GripperPosition>().desiredForce = context<GripperPosition>().weakForce;
00953 }
00954
00955 void MoreThanForceThreshold::doo(const i_do_force&)
00956 {
00957 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::DEBUG << "[" << context<GripperPosition>().mechanism << "] MoreThanForceThreshold::do()";
00958
00959 if ((not context<GripperPosition>().jawLeftState.position.empty()) && (not context<GripperPosition>().jawRightState.position.empty()))
00960 {
00961 double averageJawPosition = (context<GripperPosition>().jawLeftState.position[0] + context<GripperPosition>().jawRightState.position[0]) / 2.0;
00962
00963 if (averageJawPosition < (context<GripperPosition>().jawPositionForceThreshold + context<GripperPosition>().jawPositionNoise))
00964 {
00965 NasaCommonLogging::Logger::getCategory("gov.nasa.robodyn.mechanisms.GripperPositionStateMachine") << Priority::INFO << "[" << context<GripperPosition>().mechanism << "] MoreThanForceThreshold::do() - post(e_lessThanForceThreshold)";
00966 post_event(e_lessThanForceThreshold());
00967 }
00968 }
00969 }