00001
00002
00003
00004
00012 #include "../include/ISSTaskboardPanelA.h"
00013 #include <boost/bind.hpp>
00014 #include <boost/algorithm/string.hpp>
00015 #include <stdio.h>
00016 #include <math.h>
00017 #include <gazebo/common/Events.hh>
00018 #include <std_msgs/String.h>
00019 #include <gazebo/math/Angle.hh>
00020 #include <gazebo/math/Pose.hh>
00021 #include <gazebo/msgs/MessageTypes.hh>
00022
00023 using namespace gazebo;
00024 using namespace iss_taskboard_gazebo;
00025
00027 const double POWER_COVER_JOINT_THRESHOLD = M_PI/4;
00028
00030 const std::string LED_MATERIAL_OFF = "Gazebo/Grey";
00032 const std::string LED_MATERIAL_GREEN = "Gazebo/GreenGlow";
00034 const std::string LED_MATERIAL_BLUE = "Gazebo/BlueGlow";
00035
00037 const double SIMPLE_LED_LENGTH = 0.0004;
00039 const double SIMPLE_LED_RADIUS = 0.003;
00041 const double NUMPAD_LED_LENGTH = 0.0004;
00043 const double NUMPAD_LED_RADIUS = 0.0095;
00044
00046 const double LED_HIDE_OFFSET = 100.0;
00048 const double LED_HIDE_OFFSET2 = 105.0;
00049
00051 const double SAFE_TOGGLE_UPPER_LIMIT_ANGLE = 0.0;
00053 const double SAFE_TOGGLE_LOWER_LIMIT_ANGLE = -0.94;
00055 const double SAFE_TOGGLE_MIDDLE_LIMIT_ANGLE = -0.47;
00057 const double SAFE_TOGGLE_HALF_ANGLE = 0.47;
00058
00060 const math::Vector3 ZERO_VECTOR = math::Vector3(0, 0, 0);
00061
00063 enum LedColor
00064 {
00065 eLedColor_Green,
00066 eLedColor_Blue
00067 };
00068
00078 struct GazeboTaskboardSlot1::Led
00079 {
00081 bool isOn;
00082
00083
00084
00086 physics::ModelPtr onModel;
00088 physics::ModelPtr offModel;
00089
00090
00092 std::string onModelName;
00094 std::string offModelName;
00096 std::string onLinkName;
00098 std::string offLinkName;
00099
00100
00101
00103 int index;
00105 bool numPadLed;
00107 LedColor color;
00109 math::Pose pose;
00110
00112 Led();
00114 Led(int index_, bool numPadLed_, LedColor color_, math::Pose pose_);
00116 void CreateModel(physics::WorldPtr world);
00118 void SetupModel(physics::WorldPtr world);
00119 };
00120
00129 struct GazeboTaskboardSlot1::TaskboardLeds
00130 {
00132 Led powerSwitchLed;
00134 Led rockerSwitchUpLed;
00136 Led rockerSwitchDownLed;
00138 Led numPadLeds[NUM_PAD_BUTTONS_COUNT];
00140 Led toggleA03Led;
00142 Led toggleA04TopLed;
00144 Led toggleA04BottomLed;
00146 Led toggleA05Led;
00147
00149 TaskboardLeds(const math::Pose& pose);
00150
00152 void CreateModels(physics::WorldPtr world);
00153
00155 void SetupModels(physics::WorldPtr world);
00156 };
00157
00164 struct GazeboTaskboardSlot1::TaskboardSlot1State
00165 {
00167 bool isCoverOpen;
00168
00170 TwoWayToggleSwitchState powerSwitchState;
00171
00173 ThreeWayToggleSwitchState rockerSwitchA01State;
00174
00176 bool numPadButtonsSelectedState[NUM_PAD_BUTTONS_COUNT];
00177
00179 bool isSafeToggleOut[SAFE_TOGGLES_COUNT];
00180
00183 TwoWayToggleSwitchState toggleA03State;
00184
00187 ThreeWayToggleSwitchState toggleA04State;
00188
00191 TwoWayToggleSwitchState toggleA05State;
00192
00196 TaskboardSlot1State()
00197 {
00198 for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
00199 {
00200 numPadButtonsSelectedState[i] = false;
00201 }
00202 for (int i = 0; i < SAFE_TOGGLES_COUNT; i++)
00203 {
00204 isSafeToggleOut[i] = false;
00205 }
00206 isCoverOpen = false;
00207 powerSwitchState = eTwoWayState_Down;
00208 rockerSwitchA01State = eThreeWayState_Center;
00209 toggleA03State = eTwoWayState_Down;
00210 toggleA04State = eThreeWayState_Center;
00211 toggleA05State = eTwoWayState_Down;
00212 }
00213 };
00214
00221 struct BaseManipulationState
00222 {
00224 double startTime;
00226 double duration;
00227
00229 BaseManipulationState()
00230 : startTime(0.0)
00231 , duration(0.0)
00232 {}
00233 };
00235 struct PowerCoverManipulationState : BaseManipulationState
00236 {
00238 math::Vector3 angularVelocity;
00239 };
00241 struct PowerSwitchManipulationState : BaseManipulationState
00242 {
00244 math::Vector3 angularVelocity;
00245 };
00247 struct RockerSwitchManipulationState : BaseManipulationState
00248 {
00250 math::Vector3 torque;
00251 };
00253 struct NumPadButtonManipulationState : BaseManipulationState
00254 {
00256 math::Vector3 force;
00257 };
00259 struct SafeToggleManipulationState : BaseManipulationState
00260 {
00262 double value;
00263
00265 SafeToggleManipulationState()
00266 : value(0.0)
00267 {}
00268 };
00269
00281 struct GazeboTaskboardSlot1::ManipulationState
00282 {
00284 PowerCoverManipulationState powerCover;
00286 PowerSwitchManipulationState powerSwitch;
00288 RockerSwitchManipulationState rockerSwitch;
00290 NumPadButtonManipulationState numPadButtons[NUM_PAD_BUTTONS_COUNT];
00292 SafeToggleManipulationState safeToggles[SAFE_TOGGLES_COUNT][2];
00293 };
00294
00301 inline double deg2rad(double degrees)
00302 {
00303 return M_PI / 180.0 * degrees;
00304 }
00305
00306
00310 GazeboTaskboardSlot1::GazeboTaskboardSlot1()
00311 : firstFrameInitializationDone(false)
00312 , ledsReady(false)
00313 , node("taskboard")
00314 {}
00315
00321 GazeboTaskboardSlot1::~GazeboTaskboardSlot1()
00322 {
00323 if (updateConnection)
00324 {
00325 event::Events::DisconnectWorldUpdateBegin(updateConnection);
00326 updateConnection.reset();
00327 }
00328 }
00329
00336 void GazeboTaskboardSlot1::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00337 {
00338 model = _parent;
00339 if (!model)
00340 {
00341 ROS_FATAL("No model is provided for GazeboTaskboard model plugin");
00342 return;
00343 }
00344
00345 if (!InitLinks() || !InitJoints())
00346 {
00347 return;
00348 }
00349
00350
00351 updateConnection = event::Events::ConnectWorldUpdateBegin(
00352 boost::bind(&GazeboTaskboardSlot1::OnUpdate, this));
00353
00354
00355 srv_manipulatePowerCover = node.advertiseService("manipulate_power_cover",
00356 &GazeboTaskboardSlot1::ManipulatePowerCover, this);
00357 srv_manipulatePowerSwitch = node.advertiseService("manipulate_power_switch",
00358 &GazeboTaskboardSlot1::ManipulatePowerSwitch, this);
00359 srv_manipulateRockerSwitch = node.advertiseService("manipulate_rocker_switch_a01",
00360 &GazeboTaskboardSlot1::ManipulateRockerSwitch, this);
00361 srv_manipulateNumPad = node.advertiseService("manipulate_numpad",
00362 &GazeboTaskboardSlot1::ManipulateNumPad, this);
00363 srv_manipulateSafeToggle = node.advertiseService("manipulate_safe_toggle",
00364 &GazeboTaskboardSlot1::ManipulateSafeToggle, this);
00365
00366
00367 const int queueSize = 128;
00368 publisher = node.advertise<TaskboardPanelA>("TaskboardPanelA", queueSize);
00369
00370
00371 manipulationState.reset(new ManipulationState);
00372
00373
00374 leds.reset(new TaskboardLeds(model->GetWorldPose()));
00375
00376
00377 state.reset(new TaskboardSlot1State);
00378 DeriveStateFromModel();
00379 }
00380
00391 static physics::LinkPtr getLink(physics::ModelPtr model, const std::string& linkName)
00392 {
00393 physics::LinkPtr link = model->GetLink(linkName);
00394 if (!link)
00395 {
00396 ROS_FATAL("Failed to get %s link from the model", linkName.c_str());
00397 }
00398 return link;
00399 }
00400
00406 bool GazeboTaskboardSlot1::InitLinks()
00407 {
00408 struct
00409 {
00410 physics::LinkPtr& linkRef;
00411 std::string linkName;
00412 } linksInfo[] =
00413 {
00414 { linkPowerCover , "taskboard_slot1_switch1_cover" },
00415 { linkPowerSwitch , "taskboard_slot1_switch1" },
00416 { linkA01Switch , "taskboard_slot1_switch0" },
00417 { linksNumPad[0] , "taskboard_slot1_A1" },
00418 { linksNumPad[1] , "taskboard_slot1_A2" },
00419 { linksNumPad[2] , "taskboard_slot1_A3" },
00420 { linksNumPad[3] , "taskboard_slot1_B1" },
00421 { linksNumPad[4] , "taskboard_slot1_B2" },
00422 { linksNumPad[5] , "taskboard_slot1_B3" },
00423 { linksNumPad[6] , "taskboard_slot1_C1" },
00424 { linksNumPad[7] , "taskboard_slot1_C2" },
00425 { linksNumPad[8] , "taskboard_slot1_C3" },
00426 { linksBaseSafeToggle[0], "taskboard_slot1_switch2_pullout" },
00427 { linksBaseSafeToggle[1], "taskboard_slot1_switch3_pullout" },
00428 { linksBaseSafeToggle[2], "taskboard_slot1_switch4_pullout" },
00429 { linksSafeToggle[0] , "taskboard_slot1_switch2" },
00430 { linksSafeToggle[1] , "taskboard_slot1_switch3" },
00431 { linksSafeToggle[2] , "taskboard_slot1_switch4" }
00432 };
00433 const int numLinks = sizeof(linksInfo) / sizeof(linksInfo[0]);
00434 for (int i = 0; i < numLinks; i++)
00435 {
00436 linksInfo[i].linkRef = getLink(model, linksInfo[i].linkName);
00437 if (!linksInfo[i].linkRef)
00438 {
00439 return false;
00440 }
00441 }
00442 return true;
00443 }
00444
00455 static physics::JointPtr getJoint(physics::ModelPtr model, const std::string& jointName)
00456 {
00457 physics::JointPtr joint = model->GetJoint(jointName);
00458 if (!joint)
00459 {
00460 ROS_FATAL("Failed to get %s joint from the model", jointName.c_str());
00461 }
00462 return joint;
00463 }
00464
00470 bool GazeboTaskboardSlot1::InitJoints()
00471 {
00472 struct
00473 {
00474 physics::JointPtr& jointRef;
00475 std::string jointName;
00476 } jointsInfo[] =
00477 {
00478 { safeTogglesRevoluteJoints[0] , "taskboard_lower/taskboard_slot1_switch2" },
00479 { safeTogglesRevoluteJoints[1] , "taskboard_lower/taskboard_slot1_switch3" },
00480 { safeTogglesRevoluteJoints[2] , "taskboard_lower/taskboard_slot1_switch4" }
00481 };
00482 const int numJoints = sizeof(jointsInfo) / sizeof(jointsInfo[0]);
00483 for (int i = 0; i < numJoints; i++)
00484 {
00485 jointsInfo[i].jointRef = getJoint(model, jointsInfo[i].jointName);
00486 if (!jointsInfo[i].jointRef)
00487 {
00488 return false;
00489 }
00490 }
00491 return true;
00492 }
00493
00501 void GazeboTaskboardSlot1::DeriveStateFromModel()
00502 {
00503
00504 math::Pose pose = linkPowerCover->GetRelativePose();
00505 double angle = pose.rot.GetRoll();
00506 state->isCoverOpen = (angle < POWER_COVER_JOINT_THRESHOLD);
00507
00508
00509
00510 pose = linkPowerSwitch->GetRelativePose();
00511 angle = pose.rot.GetRoll();
00512 state->powerSwitchState = (angle < M_PI_2) ? eTwoWayState_Up : eTwoWayState_Down;
00513
00514
00515 pose = linksSafeToggle[0]->GetRelativePose();
00516 angle = pose.rot.GetRoll();
00517 state->toggleA03State = (angle < M_PI_2) ? eTwoWayState_Up : eTwoWayState_Down;
00518
00519
00520 pose = linksSafeToggle[1]->GetRelativePose();
00521 angle = pose.rot.GetRoll();
00522 if (angle < M_PI_2 - SAFE_TOGGLE_HALF_ANGLE/2)
00523 {
00524 state->toggleA04State = eThreeWayState_Up;
00525 }
00526 else if (angle < M_PI_2 + SAFE_TOGGLE_HALF_ANGLE/2)
00527 {
00528 state->toggleA04State = eThreeWayState_Center;
00529 }
00530 else
00531 {
00532 state->toggleA04State = eThreeWayState_Down;
00533 }
00534
00535
00536 pose = linksSafeToggle[2]->GetRelativePose();
00537 angle = pose.rot.GetRoll();
00538 state->toggleA05State = (angle < M_PI_2) ? eTwoWayState_Up : eTwoWayState_Down;
00539 }
00540
00547 void GazeboTaskboardSlot1::OnUpdate()
00548 {
00549
00550
00551 if (firstFrameInitializationDone && !ledsReady)
00552 {
00553 leds->SetupModels(model->GetWorld());
00554 TurnOnLeds();
00555 ledsReady = true;
00556 return;
00557 }
00558
00559
00560 if (!firstFrameInitializationDone)
00561 {
00562 leds->CreateModels(model->GetWorld());
00563 firstFrameInitializationDone = true;
00564 return;
00565 }
00566
00567
00568 MonitorPowerCoverStateChanges();
00569 MonitorPowerSwitchStateChanges();
00570 MonitorRockerSwitchA01StateChanges();
00571 MonitorNumpadStateChanges();
00572 MonitorSafeTogglesStateChanges();
00573
00574
00575 HandleManipulation();
00576
00577
00578 ros::spinOnce();
00579 }
00580
00584 void GazeboTaskboardSlot1::MonitorPowerCoverStateChanges()
00585 {
00586 math::Pose pose = linkPowerCover->GetRelativePose();
00587 const double angle = pose.rot.GetRoll();
00588 const double threshold = deg2rad(1.0);
00589
00590 if (angle > threshold && angle < M_PI_2 - threshold)
00591 {
00592 math::Vector3 torque = computeEmpiricalTorque(angle - POWER_COVER_JOINT_THRESHOLD, 1.0, 1.0, 1.5, 15);
00593 linkPowerCover->SetTorque(torque);
00594 }
00595 else if (angle <= threshold)
00596 {
00597 if (!state->isCoverOpen)
00598 {
00599 state->isCoverOpen = true;
00600 PublishState();
00601
00602 linkPowerCover->SetAngularVel(ZERO_VECTOR);
00603 linkPowerCover->SetTorque(ZERO_VECTOR);
00604 }
00605 }
00606 else if (angle >= M_PI_2 - threshold)
00607 {
00608 if (state->isCoverOpen)
00609 {
00610 state->isCoverOpen = false;
00611 PublishState();
00612
00613 linkPowerCover->SetAngularVel(ZERO_VECTOR);
00614 linkPowerCover->SetTorque(ZERO_VECTOR);
00615 }
00616 }
00617 }
00618
00622 void GazeboTaskboardSlot1::MonitorPowerSwitchStateChanges()
00623 {
00624 const double halfAngle = 0.419;
00625 const double upDirLimit = M_PI/2 - halfAngle;
00626 const double downDirLimit = M_PI/2 + halfAngle;
00627 const double threshold = deg2rad(1.0);
00628
00629 math::Pose pose = linkPowerSwitch->GetRelativePose();
00630 const double angle = pose.rot.GetRoll();
00631
00632 if (angle > upDirLimit + threshold && angle < downDirLimit - threshold)
00633 {
00634 math::Vector3 torque = computeEmpiricalTorque(angle - M_PI_2, 1.0, 1.0, 1.3, 20);
00635 linkPowerSwitch->SetTorque(torque);
00636 }
00637 else if (angle <= upDirLimit + threshold)
00638 {
00639 if (state->powerSwitchState == eTwoWayState_Down)
00640 {
00641 state->powerSwitchState = eTwoWayState_Up;
00642 TurnOnLeds();
00643 PublishState();
00644
00645 linkPowerSwitch->SetAngularVel(ZERO_VECTOR);
00646 linkPowerSwitch->SetTorque(ZERO_VECTOR);
00647 }
00648 }
00649 else if (angle >= downDirLimit - threshold)
00650 {
00651 if (state->powerSwitchState == eTwoWayState_Up)
00652 {
00653 state->powerSwitchState = eTwoWayState_Down;
00654 TurnOffAllLeds();
00655 PublishState();
00656
00657 linkPowerSwitch->SetAngularVel(ZERO_VECTOR);
00658 linkPowerSwitch->SetTorque(ZERO_VECTOR);
00659 }
00660 }
00661 }
00662
00666 void GazeboTaskboardSlot1::MonitorRockerSwitchA01StateChanges()
00667 {
00668 const double halfAngle = 0.5;
00669 const double upStateLimit = M_PI_2 - halfAngle;
00670 const double downStateLimit = M_PI_2 + halfAngle;
00671 const double threshold = deg2rad(2.0);
00672
00673 math::Pose pose = linkA01Switch->GetRelativePose();
00674 double angle = pose.rot.GetRoll();
00675
00676 if (angle < M_PI_2 - threshold || angle > M_PI_2 + threshold)
00677 {
00678
00679 const double sign = (angle < M_PI_2) ? 1.0 : -1.0;
00680 const double deviationAngle = sign * std::max(halfAngle - fabs(angle - M_PI_2), 0.0);
00681 math::Vector3 torque = computeEmpiricalTorque(deviationAngle, 8.5, 4.0, 2.5, 7);
00682 linkA01Switch->SetTorque(torque);
00683
00684
00685 if (state->rockerSwitchA01State == eThreeWayState_Center)
00686 {
00687 if (angle < upStateLimit + threshold)
00688 {
00689 state->rockerSwitchA01State = eThreeWayState_Up;
00690 SetLedState(leds->rockerSwitchUpLed, true);
00691 PublishState();
00692 }
00693 else if (angle > downStateLimit - threshold)
00694 {
00695 state->rockerSwitchA01State = eThreeWayState_Down;
00696 SetLedState(leds->rockerSwitchDownLed, true);
00697 PublishState();
00698 }
00699 }
00700 }
00701 else
00702 {
00703
00704 if (state->rockerSwitchA01State != eThreeWayState_Center)
00705 {
00706 if (leds->rockerSwitchUpLed.isOn)
00707 {
00708 SetLedState(leds->rockerSwitchUpLed, false);
00709 }
00710 else if (leds->rockerSwitchDownLed.isOn)
00711 {
00712 SetLedState(leds->rockerSwitchDownLed, false);
00713 }
00714 linkA01Switch->SetAngularVel(ZERO_VECTOR);
00715 state->rockerSwitchA01State = eThreeWayState_Center;
00716 PublishState();
00717 }
00718 }
00719 }
00720
00724 void GazeboTaskboardSlot1::MonitorNumpadStateChanges()
00725 {
00726 const double selRelOffset = -0.4811;
00727 const double epsilon = 0.0003;
00728
00729
00730
00731 for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
00732 {
00733 linksNumPad[i]->SetForce(ZERO_VECTOR);
00734 linksNumPad[i]->AddRelativeForce(math::Vector3(0, 0, 5));
00735 }
00736
00737 for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
00738 {
00739 const double offset = linksNumPad[i]->GetRelativePose().pos.y;
00740
00741 bool selected = (offset < selRelOffset + epsilon);
00742 if (selected ^ state->numPadButtonsSelectedState[i])
00743 {
00744 state->numPadButtonsSelectedState[i] = selected;
00745 if (selected)
00746 {
00747 SetLedState(leds->numPadLeds[i], !leds->numPadLeds[i].isOn);
00748 }
00749 PublishState();
00750 }
00751 }
00752 }
00753
00764 bool GazeboTaskboardSlot1::UpdateTransitionFromOutState2Way(
00765 physics::LinkPtr link, physics::JointPtr joint, TwoWayToggleSwitchState& state) const
00766 {
00767 const double halfAngle = 0.47;
00768 const double threshold = deg2rad(1.0);
00769 const double upDirLimit = M_PI_2 - halfAngle;
00770 const double downDirLimit = M_PI_2 + halfAngle;
00771
00772 math::Pose pose = link->GetRelativePose();
00773 const double angle = pose.rot.GetRoll();
00774
00775 if (angle > upDirLimit + threshold && angle < downDirLimit - threshold)
00776 {
00777 math::Vector3 torque = computeEmpiricalTorque(angle - M_PI_2, 0.8, 1.0, 1.5, 15);
00778 link->SetTorque(torque);
00779 return false;
00780 }
00781 if (angle <= upDirLimit + threshold)
00782 {
00783 state = eTwoWayState_Up;
00784
00785 joint->SetHighStop(0, math::Angle(SAFE_TOGGLE_LOWER_LIMIT_ANGLE + threshold));
00786 joint->SetLowStop(0, math::Angle(SAFE_TOGGLE_LOWER_LIMIT_ANGLE - threshold));
00787 }
00788 else
00789 {
00790 state = eTwoWayState_Down;
00791
00792 joint->SetHighStop(0, math::Angle(SAFE_TOGGLE_UPPER_LIMIT_ANGLE + threshold));
00793 joint->SetLowStop(0, math::Angle(SAFE_TOGGLE_UPPER_LIMIT_ANGLE - threshold));
00794 }
00795 return true;
00796 }
00797
00808 bool GazeboTaskboardSlot1::UpdateTransitionFromOutState3Way(
00809 physics::LinkPtr link, physics::JointPtr joint, ThreeWayToggleSwitchState& state) const
00810 {
00811 const double halfAngle = 0.47;
00812 const double upDirLimit = M_PI/2 - halfAngle;
00813 const double downDirLimit = M_PI/2 + halfAngle;
00814 const double threshold = deg2rad(1.0);
00815
00816 math::Pose pose = link->GetRelativePose();
00817 double angle = pose.rot.GetRoll();
00818
00819 if (angle > upDirLimit + threshold && angle < M_PI_2 - threshold)
00820 {
00821 math::Vector3 torque = computeEmpiricalTorque(angle - (M_PI_2 - halfAngle/2), 0.8, 1.0, 1.5, 15);
00822 link->SetTorque(torque);
00823 return false;
00824 }
00825 else if (angle > M_PI_2 + threshold && angle < downDirLimit - threshold)
00826 {
00827 math::Vector3 torque = computeEmpiricalTorque(angle - (M_PI_2 + halfAngle/2), 0.8, 1.0, 1.5, 15);
00828 link->SetTorque(torque);
00829 return false;
00830 }
00831 if (angle <= upDirLimit + threshold)
00832 {
00833 state = eThreeWayState_Up;
00834
00835 joint->SetHighStop(0, math::Angle(SAFE_TOGGLE_LOWER_LIMIT_ANGLE + threshold));
00836 joint->SetLowStop(0, math::Angle(SAFE_TOGGLE_LOWER_LIMIT_ANGLE - threshold));
00837 link->SetTorque(ZERO_VECTOR);
00838 }
00839 else if (angle >= downDirLimit - threshold)
00840 {
00841 state = eThreeWayState_Down;
00842
00843 joint->SetHighStop(0, math::Angle(SAFE_TOGGLE_UPPER_LIMIT_ANGLE + threshold));
00844 joint->SetLowStop(0, math::Angle(SAFE_TOGGLE_UPPER_LIMIT_ANGLE - threshold));
00845 link->SetTorque(ZERO_VECTOR);
00846 }
00847 else
00848 {
00849 state = eThreeWayState_Center;
00850
00851 joint->SetHighStop(0, math::Angle(SAFE_TOGGLE_MIDDLE_LIMIT_ANGLE + threshold));
00852 joint->SetLowStop(0, math::Angle(SAFE_TOGGLE_MIDDLE_LIMIT_ANGLE - threshold));
00853 link->SetTorque(ZERO_VECTOR);
00854 }
00855 return true;
00856 }
00857
00861 void GazeboTaskboardSlot1::MonitorSafeTogglesStateChanges()
00862 {
00863
00864 const double pullOutDistance = 0.002;
00865
00866 const double delta = 0.0004;
00867
00868
00869 bool isOut[SAFE_TOGGLES_COUNT];
00870 float offsets[SAFE_TOGGLES_COUNT];
00871 for (int i = 0; i < SAFE_TOGGLES_COUNT; i++)
00872 {
00873 const math::Pose outPose = linksBaseSafeToggle[i]->GetRelativePose();
00874 const math::Pose pose = linksSafeToggle[i]->GetRelativePose();
00875 offsets[i] = std::max(pullOutDistance - outPose.CoordPositionSub(pose).z, 0.0);
00876 isOut[i] = (offsets[i] > pullOutDistance - delta);
00877 }
00878
00879 for (int i = 0; i < SAFE_TOGGLES_COUNT; i++)
00880 {
00881 if (!state->isSafeToggleOut[i] && isOut[i])
00882 {
00883 state->isSafeToggleOut[i] = true;
00884
00885
00886 safeTogglesRevoluteJoints[i]->SetHighStop(0, math::Angle(SAFE_TOGGLE_UPPER_LIMIT_ANGLE));
00887 safeTogglesRevoluteJoints[i]->SetLowStop(0, math::Angle(SAFE_TOGGLE_LOWER_LIMIT_ANGLE));
00888
00889
00890 switch (i)
00891 {
00892 case 0:
00893 SetLedState(leds->toggleA03Led, false);
00894 break;
00895 case 1:
00896 SetLedState(leds->toggleA04TopLed, false);
00897 SetLedState(leds->toggleA04BottomLed, false);
00898 break;
00899 case 2:
00900 SetLedState(leds->toggleA05Led, false);
00901 break;
00902 }
00903 PublishState();
00904 }
00905 }
00906
00907 if (state->isSafeToggleOut[0] && offsets[0] < pullOutDistance - 2*delta)
00908 {
00909 bool transitionDone = UpdateTransitionFromOutState2Way(
00910 linksSafeToggle[0], safeTogglesRevoluteJoints[0], state->toggleA03State);
00911 if (transitionDone)
00912 {
00913 state->isSafeToggleOut[0] = false;
00914 SetLedState(leds->toggleA03Led, state->toggleA03State == eTwoWayState_Up);
00915 PublishState();
00916 }
00917 }
00918 if (state->isSafeToggleOut[2] && offsets[2] < pullOutDistance - 2*delta)
00919 {
00920 bool transitionDone = UpdateTransitionFromOutState2Way(
00921 linksSafeToggle[2], safeTogglesRevoluteJoints[2], state->toggleA05State);
00922 if (transitionDone)
00923 {
00924 state->isSafeToggleOut[2] = false;
00925 SetLedState(leds->toggleA05Led, state->toggleA05State == eTwoWayState_Up);
00926 PublishState();
00927 }
00928 }
00929
00930 if (state->isSafeToggleOut[1] && offsets[1] < pullOutDistance - 2*delta)
00931 {
00932 bool transitionDone = UpdateTransitionFromOutState3Way(
00933 linksSafeToggle[1], safeTogglesRevoluteJoints[1], state->toggleA04State);
00934 if (transitionDone)
00935 {
00936 state->isSafeToggleOut[1] = false;
00937 SetLedState(leds->toggleA04TopLed, state->toggleA04State == eThreeWayState_Up);
00938 SetLedState(leds->toggleA04BottomLed, state->toggleA04State == eThreeWayState_Down);
00939 PublishState();
00940 }
00941 }
00942
00943 for (int i = 0; i < SAFE_TOGGLES_COUNT; i++)
00944 {
00945 if (state->isSafeToggleOut[i] || offsets[i] > delta)
00946 {
00947 linksSafeToggle[i]->SetForce(ZERO_VECTOR);
00948 linksSafeToggle[i]->AddRelativeForce(math::Vector3(0, 0, -2));
00949 }
00950 else if (offsets[i] <= delta/2)
00951 {
00952 linksSafeToggle[i]->SetForce(ZERO_VECTOR);
00953 linksSafeToggle[i]->SetLinearVel(ZERO_VECTOR);
00954 linksSafeToggle[i]->SetAngularVel(ZERO_VECTOR);
00955 }
00956 }
00957 }
00958
00964 void GazeboTaskboardSlot1::PublishState()
00965 {
00966 const std::string UP = "UP";
00967 const std::string DOWN = "DOWN";
00968 const std::string CENTER = "CENTER";
00969 const std::string ON = "ON";
00970 const std::string OFF = "OFF";
00971 const std::string SEL = "SEL";
00972 const std::string UNSEL = "UNSEL";
00973 const std::string OUT = "OUT";
00974
00975
00976 TaskboardPanelA msg;
00977
00978 msg.PANEL_POWER_COVER = state->isCoverOpen ? UP : DOWN;
00979 msg.PANEL_POWER_SWITCH = state->powerSwitchState == eTwoWayState_Up ? UP : DOWN;
00980 msg.PANEL_POWER_LED = leds->powerSwitchLed.isOn ? ON : OFF;
00981
00982 msg.A01_ROCKER_SWITCH = state->rockerSwitchA01State == eThreeWayState_Center
00983 ? CENTER
00984 : (state->rockerSwitchA01State == eThreeWayState_Up ? UP : DOWN);
00985
00986 msg.A01_ROCKER_LED_TOP = leds->rockerSwitchUpLed.isOn ? ON : OFF;
00987 msg.A01_ROCKER_LED_BOTTOM = leds->rockerSwitchDownLed.isOn ? ON : OFF;
00988
00989 msg.A02_LED_NUM_PAD_A1 = leds->numPadLeds[0].isOn ? ON : OFF;
00990 msg.A02_LED_NUM_PAD_A2 = leds->numPadLeds[1].isOn ? ON : OFF;
00991 msg.A02_LED_NUM_PAD_A3 = leds->numPadLeds[2].isOn ? ON : OFF;
00992 msg.A02_LED_NUM_PAD_B1 = leds->numPadLeds[3].isOn ? ON : OFF;
00993 msg.A02_LED_NUM_PAD_B2 = leds->numPadLeds[4].isOn ? ON : OFF;
00994 msg.A02_LED_NUM_PAD_B3 = leds->numPadLeds[5].isOn ? ON : OFF;
00995 msg.A02_LED_NUM_PAD_C1 = leds->numPadLeds[6].isOn ? ON : OFF;
00996 msg.A02_LED_NUM_PAD_C2 = leds->numPadLeds[7].isOn ? ON : OFF;
00997 msg.A02_LED_NUM_PAD_C3 = leds->numPadLeds[8].isOn ? ON : OFF;
00998
00999 msg.A02_NUM_PAD_A1 = state->numPadButtonsSelectedState[0] ? SEL : UNSEL;
01000 msg.A02_NUM_PAD_A2 = state->numPadButtonsSelectedState[1] ? SEL : UNSEL;
01001 msg.A02_NUM_PAD_A3 = state->numPadButtonsSelectedState[2] ? SEL : UNSEL;
01002 msg.A02_NUM_PAD_B1 = state->numPadButtonsSelectedState[3] ? SEL : UNSEL;
01003 msg.A02_NUM_PAD_B2 = state->numPadButtonsSelectedState[4] ? SEL : UNSEL;
01004 msg.A02_NUM_PAD_B3 = state->numPadButtonsSelectedState[5] ? SEL : UNSEL;
01005 msg.A02_NUM_PAD_C1 = state->numPadButtonsSelectedState[6] ? SEL : UNSEL;
01006 msg.A02_NUM_PAD_C2 = state->numPadButtonsSelectedState[7] ? SEL : UNSEL;
01007 msg.A02_NUM_PAD_C3 = state->numPadButtonsSelectedState[8] ? SEL : UNSEL;
01008
01009 msg.A03_LED = leds->toggleA03Led.isOn ? ON : OFF;
01010 msg.A04_LED_TOP = leds->toggleA04TopLed.isOn ? ON : OFF;
01011 msg.A04_LED_BOTTOM = leds->toggleA04BottomLed.isOn ? ON : OFF;
01012 msg.A05_LED = leds->toggleA05Led.isOn ? ON : OFF;
01013
01014 msg.A03_TOGGLE = state->isSafeToggleOut[0]
01015 ? OUT
01016 : (state->toggleA03State == eTwoWayState_Up ? UP : DOWN);
01017
01018 msg.A04_TOGGLE = state->isSafeToggleOut[1]
01019 ? OUT
01020 : (state->toggleA04State == eThreeWayState_Center
01021 ? CENTER
01022 : (state->toggleA04State == eThreeWayState_Up ? UP : DOWN));
01023
01024 msg.A05_TOGGLE = state->isSafeToggleOut[2]
01025 ? OUT
01026 : (state->toggleA05State == eTwoWayState_Up ? UP : DOWN);
01027
01028
01029 publisher.publish(msg);
01030 }
01031
01038 void GazeboTaskboardSlot1::SetLedState(Led& led, bool on)
01039 {
01040
01041 if (led.isOn == on)
01042 {
01043 return;
01044 }
01045
01046 if (state->powerSwitchState != eTwoWayState_Up && on)
01047 {
01048 return;
01049 }
01050
01051
01052 math::Pose pose = led.onModel->GetRelativePose();
01053 math::Pose pose2 = led.offModel->GetRelativePose();
01054
01055
01056 if (on)
01057 {
01058 pose.pos.z -= LED_HIDE_OFFSET;
01059 pose2.pos.z += LED_HIDE_OFFSET2;
01060 }
01061 else
01062 {
01063 pose.pos.z += LED_HIDE_OFFSET;
01064 pose2.pos.z -= LED_HIDE_OFFSET2;
01065 }
01066 led.onModel->SetLinkWorldPose(pose, led.onLinkName);
01067 led.offModel->SetLinkWorldPose(pose2, led.offLinkName);
01068 led.isOn = on;
01069 }
01070
01074 void GazeboTaskboardSlot1::TurnOnLeds()
01075 {
01076 if (state->powerSwitchState == eTwoWayState_Up)
01077 {
01078
01079 SetLedState(leds->powerSwitchLed, true);
01080
01081
01082 if (state->rockerSwitchA01State == eThreeWayState_Up)
01083 {
01084 SetLedState(leds->rockerSwitchUpLed, true);
01085 }
01086 else if (state->rockerSwitchA01State == eThreeWayState_Down)
01087 {
01088 SetLedState(leds->rockerSwitchDownLed, true);
01089 }
01090
01091 if (state->toggleA03State == eTwoWayState_Up)
01092 {
01093 SetLedState(leds->toggleA03Led, true);
01094 }
01095
01096 if (state->toggleA05State == eTwoWayState_Up)
01097 {
01098 SetLedState(leds->toggleA05Led, true);
01099 }
01100
01101 if (state->toggleA04State == eThreeWayState_Up)
01102 {
01103 SetLedState(leds->toggleA04TopLed, true);
01104 }
01105 else if (state->toggleA04State == eThreeWayState_Down)
01106 {
01107 SetLedState(leds->toggleA04BottomLed, true);
01108 }
01109 }
01110 }
01111
01115 void GazeboTaskboardSlot1::TurnOffAllLeds()
01116 {
01117 SetLedState(leds->powerSwitchLed, false);
01118 SetLedState(leds->rockerSwitchUpLed, false);
01119 SetLedState(leds->rockerSwitchDownLed, false);
01120 SetLedState(leds->toggleA03Led, false);
01121 SetLedState(leds->toggleA05Led, false);
01122 SetLedState(leds->toggleA04TopLed, false);
01123 SetLedState(leds->toggleA04BottomLed, false);
01124
01125 for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
01126 {
01127 SetLedState(leds->numPadLeds[i], false);
01128 }
01129 }
01130
01131
01132
01133
01144 bool GazeboTaskboardSlot1::ManipulatePowerCover(
01145 ManipulatePowerCover::Request& request, ManipulatePowerCover::Response& response)
01146 {
01147 math::Pose globalPose = model->GetWorldPose();
01148 const double velocity = request.angle / request.duration;
01149
01150 PowerCoverManipulationState &manipState = manipulationState->powerCover;
01151
01152 manipState.duration = request.duration;
01153 manipState.startTime = GetTime();
01154 manipState.angularVelocity = globalPose.rot * math::Vector3(velocity, 0, 0);
01155
01156 linkPowerCover->SetAngularVel(manipState.angularVelocity);
01157 return true;
01158 }
01159
01170 bool GazeboTaskboardSlot1::ManipulatePowerSwitch(
01171 ManipulatePowerSwitch::Request& request, ManipulatePowerSwitch::Response& response)
01172 {
01173 math::Pose globalPose = model->GetWorldPose();
01174 const double velocity = request.angle / request.duration;
01175
01176 PowerSwitchManipulationState &manipState = manipulationState->powerSwitch;
01177
01178 manipState.duration = request.duration;
01179 manipState.startTime = GetTime();
01180 manipState.angularVelocity = globalPose.rot * math::Vector3(velocity, 0, 0);
01181
01182 linkPowerSwitch->SetAngularVel(manipState.angularVelocity);
01183 return true;
01184 }
01185
01196 bool GazeboTaskboardSlot1::ManipulateRockerSwitch(
01197 ManipulateRockerSwitch::Request& request, ManipulateRockerSwitch::Response& response)
01198 {
01199 math::Pose globalPose = model->GetWorldPose();
01200
01201 RockerSwitchManipulationState &manipState = manipulationState->rockerSwitch;
01202
01203 manipState.startTime = GetTime();
01204 manipState.duration = request.duration;
01205 manipState.torque = globalPose.rot * math::Vector3(request.torque, 0, 0);
01206
01207 linkA01Switch->SetTorque(manipState.torque);
01208 return true;
01209 }
01210
01221 bool GazeboTaskboardSlot1::ManipulateNumPad(
01222 ManipulateNumPad::Request& request, ManipulateNumPad::Response& response)
01223 {
01224 if (request.index < 0 || request.index >= NUM_PAD_BUTTONS_COUNT)
01225 {
01226 return false;
01227 }
01228 NumPadButtonManipulationState &manipState = manipulationState->numPadButtons[request.index];
01229
01230 manipState.startTime = GetTime();
01231 manipState.duration = request.duration;
01232 manipState.force = math::Vector3(0, 0, -request.force);
01233
01234 linksNumPad[request.index]->AddRelativeForce(manipState.force);
01235 return true;
01236 }
01237
01249 bool GazeboTaskboardSlot1::ManipulateSafeToggle(
01250 ManipulateSafeToggle::Request& request, ManipulateSafeToggle::Response& response)
01251 {
01252 if (request.index < 0 || request.index >= SAFE_TOGGLES_COUNT)
01253 {
01254 return false;
01255 }
01256 if (request.mode < 0 || request.mode >= 2)
01257 {
01258 return false;
01259 }
01260 SafeToggleManipulationState &manipState = manipulationState->safeToggles[request.index][request.mode];
01261
01262 manipState.startTime = GetTime();
01263 manipState.duration = request.duration;
01264 manipState.value = request.value;
01265
01266 physics::LinkPtr link = linksSafeToggle[request.index];
01267 if (request.mode == 0)
01268 {
01269 link->SetForce(ZERO_VECTOR);
01270 link->AddRelativeForce(math::Vector3(0, 0, manipState.value));
01271 }
01272 else
01273 {
01274 const double velocity = manipState.value / manipState.duration;
01275 math::Pose basePose = model->GetWorldPose();
01276 math::Vector3 angularVelocity = basePose.rot * math::Vector3(velocity, 0, 0);
01277 link->SetAngularVel(angularVelocity);
01278 }
01279 return true;
01280 }
01281
01285 void GazeboTaskboardSlot1::HandleManipulation()
01286 {
01287 const double time = GetTime();
01288
01289 if (manipulationState->powerCover.startTime)
01290 {
01291 const double elapsedTime = time - manipulationState->powerCover.startTime;
01292 if (elapsedTime >= manipulationState->powerCover.duration)
01293 {
01294 linkPowerCover->SetAngularVel(ZERO_VECTOR);
01295 manipulationState->powerCover.startTime = 0.0;
01296 }
01297 else
01298 {
01299 linkPowerCover->SetAngularVel(manipulationState->powerCover.angularVelocity);
01300 }
01301 }
01302
01303 if (manipulationState->powerSwitch.startTime)
01304 {
01305 const double elapsedTime = time - manipulationState->powerSwitch.startTime;
01306 if (elapsedTime >= manipulationState->powerSwitch.duration)
01307 {
01308 linkPowerSwitch->SetAngularVel(ZERO_VECTOR);
01309 manipulationState->powerSwitch.startTime = 0.0;
01310 }
01311 else
01312 {
01313 linkPowerSwitch->SetAngularVel(manipulationState->powerSwitch.angularVelocity);
01314 }
01315 }
01316
01317 if (manipulationState->rockerSwitch.startTime)
01318 {
01319 const double elapsedTime = time - manipulationState->rockerSwitch.startTime;
01320 if (elapsedTime >= manipulationState->rockerSwitch.duration)
01321 {
01322 linkA01Switch->SetTorque(ZERO_VECTOR);
01323 manipulationState->rockerSwitch.startTime = 0.0;
01324 }
01325 else
01326 {
01327 linkA01Switch->SetTorque(manipulationState->rockerSwitch.torque);
01328 }
01329 }
01330
01331 for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
01332 {
01333 NumPadButtonManipulationState &manipState = manipulationState->numPadButtons[i];
01334 if (manipState.startTime)
01335 {
01336 const double elapsedTime = time - manipState.startTime;
01337 if (elapsedTime >= manipState.duration)
01338 {
01339 manipState.startTime = 0.0;
01340 }
01341 else
01342 {
01343 linksNumPad[i]->AddRelativeForce(manipState.force);
01344 }
01345 }
01346 }
01347
01348 for (int i = 0; i < SAFE_TOGGLES_COUNT; i++)
01349 {
01350 SafeToggleManipulationState &manipState = manipulationState->safeToggles[i][0];
01351 if (manipState.startTime)
01352 {
01353 physics::LinkPtr link = linksSafeToggle[i];
01354 const double elapsedTime = time - manipState.startTime;
01355 if (elapsedTime >= manipState.duration)
01356 {
01357 link->SetForce(ZERO_VECTOR);
01358 manipState.startTime = 0.0;
01359 }
01360 else
01361 {
01362 link->SetForce(ZERO_VECTOR);
01363 link->AddRelativeForce(math::Vector3(0, 0, manipState.value));
01364 }
01365 }
01366 SafeToggleManipulationState &manipState2 = manipulationState->safeToggles[i][1];
01367 if (manipState2.startTime)
01368 {
01369 physics::LinkPtr link = linksSafeToggle[i];
01370 const double elapsedTime = time - manipState2.startTime;
01371 if (elapsedTime >= manipState2.duration)
01372 {
01373 link->SetAngularVel(ZERO_VECTOR);
01374 link->SetLinearVel(ZERO_VECTOR);
01375 manipState2.startTime = 0.0;
01376 }
01377 else
01378 {
01379 const double velocity = manipState2.value / manipState2.duration;
01380 math::Pose basePose = model->GetWorldPose();
01381 math::Vector3 angularVelocity = basePose.rot * math::Vector3(velocity, 0, 0);
01382 link->SetAngularVel(angularVelocity);
01383 }
01384 }
01385 }
01386 }
01387
01399 math::Vector3 GazeboTaskboardSlot1::computeEmpiricalTorque(
01400 double deviationAngle, double initialValue,
01401 double snapCoeff, double snapExp, double torqueCoeff) const
01402 {
01403
01404 const math::Vector3 axis = model->GetWorldPose().rot * math::Vector3(1, 0, 0);
01405
01406 const double sign = (deviationAngle < 0) ? 1.0 : -1.0;
01407
01408 const double torqueValue = (initialValue + pow(snapCoeff * fabs(deviationAngle), snapExp)) * sign * torqueCoeff;
01409 return axis * torqueValue;
01410 }
01411
01417 double GazeboTaskboardSlot1::GetTime() const
01418 {
01419 return model->GetWorld()->GetSimTime().Double();
01420 }
01421
01422
01426 GazeboTaskboardSlot1::Led::Led()
01427 : isOn(false)
01428 , index(-1)
01429 , color(eLedColor_Green)
01430 {}
01431
01440 GazeboTaskboardSlot1::Led::Led(int index_, bool numPadLed_, LedColor color_, math::Pose pose_)
01441 : isOn(false)
01442 , index(index_)
01443 , numPadLed(numPadLed_)
01444 , color(color_)
01445 , pose(pose_)
01446 {}
01447
01453 void GazeboTaskboardSlot1::Led::CreateModel(physics::WorldPtr world)
01454 {
01455 const std::string sdfTemplate =
01456 "<sdf version ='1.4'>\
01457 <model name ='MODELNAME'>\
01458 <static>true</static>\
01459 <pose>%1% %2% ZPOS %3% %4% %5%</pose>\
01460 <link name ='LINKNAME'>\
01461 <kinematic>1</kinematic>\
01462 <collision name='COLLISIONNAME'>\
01463 <geometry>GEOMETRY</geometry>\
01464 </collision>\
01465 <visual name='VISUALNAME'>\
01466 <geometry>GEOMETRY</geometry>\
01467 <material>\
01468 <script>\
01469 <name>MATERIAL</name>\
01470 <uri>__default__</uri>\
01471 </script>\
01472 </material>\
01473 </visual>\
01474 <inertial>\
01475 <mass>0.5</mass>\
01476 <inertia>\
01477 <ixx>1</ixx>\
01478 <ixy>0.0</ixy>\
01479 <ixz>0.0</ixz>\
01480 <iyy>1</iyy>\
01481 <iyz>0.0</iyz>\
01482 <izz>1</izz>\
01483 </inertial>\
01484 </link>\
01485 </model>\
01486 </sdf>";
01487
01488
01489 const std::string material = (color == eLedColor_Green) ? LED_MATERIAL_GREEN
01490 : LED_MATERIAL_BLUE;
01491
01492 const std::string ledNameBase = boost::str(boost::format("taskboardLed%1%") % index);
01493 const std::string ledLinkNameBase = boost::str(boost::format("taskboardLedLink%1%") % index);
01494 const std::string ledCollisionNameBase = boost::str(boost::format("taskboardLedCollision%1%") % index);
01495 const std::string ledVisualNameBase = boost::str(boost::format("taskboardLedVisual%1%") % index);
01496
01497 const std::string ledName = ledNameBase + "_ON";
01498 const std::string ledName2 = ledNameBase + "_OFF";
01499 const std::string ledLinkName = ledLinkNameBase + "_ON";
01500 const std::string ledLinkName2 = ledLinkNameBase + "_OFF";
01501 const std::string ledCollisionName = ledCollisionNameBase + "_ON";
01502 const std::string ledCollisionName2 = ledCollisionNameBase + "_OFF";
01503 const std::string ledVisualName = ledVisualNameBase + "_ON";
01504 const std::string ledVisualName2 = ledVisualNameBase + "_OFF";
01505
01506
01507 std::string ledModel;
01508 ledModel = boost::str(boost::format("<cylinder> <length>%1%</length> <radius>%2%</radius> </cylinder>")
01509 % (numPadLed ? NUMPAD_LED_LENGTH : SIMPLE_LED_LENGTH)
01510 % (numPadLed ? NUMPAD_LED_RADIUS : SIMPLE_LED_RADIUS));
01511
01512
01513 double roll = pose.rot.GetRoll();
01514 double pitch = pose.rot.GetPitch();
01515 double yaw = pose.rot.GetYaw();
01516 std::string xml = boost::str(boost::format(sdfTemplate)
01517 % pose.pos.x % pose.pos.y
01518 % roll % pitch % yaw);
01519 std::string xml2 = xml;
01520
01521
01522 boost::replace_all(xml, "MODELNAME", ledName);
01523 boost::replace_all(xml, "ZPOS", boost::str(boost::format("%1%") % (pose.pos.z + LED_HIDE_OFFSET)));
01524 boost::replace_all(xml, "LINKNAME", ledLinkName);
01525 boost::replace_all(xml, "COLLISIONNAME", ledCollisionName);
01526 boost::replace_all(xml, "VISUALNAME", ledVisualName);
01527 boost::replace_all(xml, "GEOMETRY", ledModel);
01528 boost::replace_all(xml, "MATERIAL", material);
01529
01530
01531 boost::replace_all(xml2, "MODELNAME", ledName2);
01532 boost::replace_all(xml2, "ZPOS", boost::str(boost::format("%1%") % (pose.pos.z + LED_HIDE_OFFSET2)));
01533 boost::replace_all(xml2, "LINKNAME", ledLinkName2);
01534 boost::replace_all(xml2, "COLLISIONNAME", ledCollisionName2);
01535 boost::replace_all(xml2, "VISUALNAME", ledVisualName2);
01536 boost::replace_all(xml2, "GEOMETRY", ledModel);
01537 boost::replace_all(xml2, "MATERIAL", LED_MATERIAL_OFF);
01538
01539
01540 sdf::SDF ledSDF;
01541 ledSDF.SetFromString(xml);
01542
01543 sdf::SDF ledSDF2;
01544 ledSDF2.SetFromString(xml2);
01545
01546 #if ROS_VERSION_MINIMUM(1, 9, 0)
01547
01548 world->InsertModelSDF(ledSDF);
01549 world->InsertModelSDF(ledSDF2);
01550 #else
01551
01552 world->InsertModel(ledSDF);
01553 world->InsertModel(ledSDF2);
01554 #endif
01555
01556
01557
01558
01559 onModelName = ledName;
01560 onLinkName = ledLinkName;
01561 offModelName = ledName2;
01562 offLinkName = ledLinkName2;
01563 }
01564
01570 void GazeboTaskboardSlot1::Led::SetupModel(physics::WorldPtr world)
01571 {
01572 if (!onModel)
01573 {
01574 onModel = world->GetModel(onModelName);
01575 if (!onModel)
01576 {
01577 ROS_FATAL("SetupModel: failed to get model %s", onModelName.c_str());
01578 return;
01579 }
01580 onModel->SetCollideMode("none");
01581
01582 offModel = world->GetModel(offModelName);
01583 if (!offModel)
01584 {
01585 ROS_FATAL("SetupModel: failed to get model %s", offModelName.c_str());
01586 return;
01587 }
01588 offModel->SetCollideMode("none");
01589
01590 math::Pose pose2 = offModel->GetRelativePose();
01591 pose2.pos.z -= LED_HIDE_OFFSET2;
01592 offModel->SetWorldPose(pose2);
01593 }
01594 }
01595
01596
01604 GazeboTaskboardSlot1::TaskboardLeds::TaskboardLeds(const math::Pose& modelPose)
01605 {
01606 struct LedsInfo {
01607 Led& led;
01608 bool numPadLed;
01609 LedColor color;
01610 math::Vector3 offset;
01611 } ledsInfo[] = {
01612 { powerSwitchLed, false, eLedColor_Green, math::Vector3(0.3250, -0.4720, 1.4110) },
01613 { rockerSwitchUpLed, false, eLedColor_Green, math::Vector3(0.3245, -0.4720, 1.3340) },
01614 { rockerSwitchDownLed, false, eLedColor_Blue, math::Vector3(0.3245, -0.4720, 1.2834) },
01615 { numPadLeds[0], true, eLedColor_Blue, math::Vector3(0.4496, -0.4755, 1.3358) },
01616 { numPadLeds[1], true, eLedColor_Green, math::Vector3(0.4230, -0.4755, 1.3358) },
01617 { numPadLeds[2], true, eLedColor_Blue, math::Vector3(0.3964, -0.4755, 1.3358) },
01618 { numPadLeds[3], true, eLedColor_Green, math::Vector3(0.4496, -0.4755, 1.3095) },
01619 { numPadLeds[4], true, eLedColor_Blue, math::Vector3(0.4230, -0.4755, 1.3095) },
01620 { numPadLeds[5], true, eLedColor_Green, math::Vector3(0.3964, -0.4755, 1.3095) },
01621 { numPadLeds[6], true, eLedColor_Blue, math::Vector3(0.4496, -0.4755, 1.2833) },
01622 { numPadLeds[7], true, eLedColor_Green, math::Vector3(0.4230, -0.4755, 1.2833) },
01623 { numPadLeds[8], true, eLedColor_Blue, math::Vector3(0.3964, -0.4755, 1.2833) },
01624 { toggleA03Led, false, eLedColor_Green, math::Vector3(0.4449, -0.4720, 1.2365) },
01625 { toggleA04TopLed, false, eLedColor_Green, math::Vector3(0.3812, -0.4720, 1.2365) },
01626 { toggleA04BottomLed, false, eLedColor_Blue, math::Vector3(0.3812, -0.4720, 1.1855) },
01627 { toggleA05Led, false, eLedColor_Green, math::Vector3(0.3185, -0.4720, 1.2365) }
01628 };
01629 const int ledsCount = sizeof(ledsInfo) / sizeof(ledsInfo[0]);
01630 const math::Quaternion rot = math::Quaternion(0.707, 0.707, 0, 0);
01631
01632 for (int i = 0; i < ledsCount; i++)
01633 {
01634 const LedsInfo &info = ledsInfo[i];
01635 const math::Pose pose(modelPose.pos + modelPose.rot * info.offset, modelPose.rot * rot);
01636 info.led = Led(i, info.numPadLed, info.color, pose);
01637 }
01638 }
01639
01648 void GazeboTaskboardSlot1::TaskboardLeds::CreateModels(physics::WorldPtr world)
01649 {
01650 for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
01651 {
01652 numPadLeds[i].CreateModel(world);
01653 }
01654 powerSwitchLed.CreateModel(world);
01655 rockerSwitchUpLed.CreateModel(world);
01656 rockerSwitchDownLed.CreateModel(world);
01657 toggleA03Led.CreateModel(world);
01658 toggleA04TopLed.CreateModel(world);
01659 toggleA04BottomLed.CreateModel(world);
01660 toggleA05Led.CreateModel(world);
01661 }
01662
01668 void GazeboTaskboardSlot1::TaskboardLeds::SetupModels(physics::WorldPtr world)
01669 {
01670 for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
01671 {
01672 numPadLeds[i].SetupModel(world);
01673 }
01674 powerSwitchLed.SetupModel(world);
01675 rockerSwitchUpLed.SetupModel(world);
01676 rockerSwitchDownLed.SetupModel(world);
01677 toggleA03Led.SetupModel(world);
01678 toggleA04TopLed.SetupModel(world);
01679 toggleA04BottomLed.SetupModel(world);
01680 toggleA05Led.SetupModel(world);
01681 }