00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #include <YouBotOODLWrapper.h>
00041 #include <joint_state_observer.h>
00042
00043 #include <youbot_trajectory_action_server/joint_trajectory_action.h>
00044
00045 #include <sstream>
00046
00047 namespace youBot
00048 {
00049
00050 YouBotOODLWrapper::YouBotOODLWrapper()
00051 {
00052 }
00053
00054 YouBotOODLWrapper::YouBotOODLWrapper(ros::NodeHandle n) :
00055 node(n)
00056 {
00057
00058 youBotConfiguration.hasBase = false;
00059 youBotConfiguration.hasArms = false;
00060 areBaseMotorsSwitchedOn = false;
00061 areArmMotorsSwitchedOn = false;
00062
00063 youBotChildFrameID = "base_link";
00064 armJointStateMessages.clear();
00065
00066 n.param("youBotDriverCycleFrequencyInHz", youBotDriverCycleFrequencyInHz, 50.0);
00067
00068
00069
00070 gripperCycleCounter = 0;
00071 diagnosticNameArms = "platform_Arms";
00072 diagnosticNameBase = "platform_Base";
00073 dashboardMessagePublisher = n.advertise < youbot_common::PowerBoardState > ("/dashboard/platform_state", 1);
00074 diagnosticArrayPublisher = n.advertise < diagnostic_msgs::DiagnosticArray > ("/diagnostics", 1);
00075 }
00076
00077 YouBotOODLWrapper::~YouBotOODLWrapper()
00078 {
00079
00080 this->stop();
00081 dashboardMessagePublisher.shutdown();
00082 diagnosticArrayPublisher.shutdown();
00083
00084 }
00085
00086 void YouBotOODLWrapper::initializeBase(std::string baseName)
00087 {
00088
00089
00090 try
00091 {
00092 ROS_INFO("Configuration file path: %s", youBotConfiguration.configurationFilePath.c_str());
00093 youBotConfiguration.baseConfiguration.youBotBase = new youbot::YouBotBase(
00094 baseName, youBotConfiguration.configurationFilePath);
00095 youBotConfiguration.baseConfiguration.youBotBase->doJointCommutation();
00096 }
00097 catch (std::exception& e)
00098 {
00099 std::string errorMessage = e.what();
00100 ROS_FATAL("%s", errorMessage.c_str());
00101 ROS_ERROR("Base \"%s\" could not be initialized.", baseName.c_str());
00102 youBotConfiguration.hasBase = false;
00103 return;
00104 }
00105
00106
00107 youBotConfiguration.baseConfiguration.baseCommandSubscriber = node.subscribe("cmd_vel", 1000,
00108 &YouBotOODLWrapper::baseCommandCallback,
00109 this);
00110 youBotConfiguration.baseConfiguration.baseOdometryPublisher = node.advertise < nav_msgs::Odometry > ("odom", 1);
00111 youBotConfiguration.baseConfiguration.baseJointStatePublisher = node.advertise < sensor_msgs::JointState
00112 > ("base/joint_states", 1);
00113
00114
00115 youBotConfiguration.baseConfiguration.switchOffMotorsService = node.advertiseService(
00116 "base/switchOffMotors", &YouBotOODLWrapper::switchOffBaseMotorsCallback, this);
00117 youBotConfiguration.baseConfiguration.switchONMotorsService = node.advertiseService(
00118 "base/switchOnMotors", &YouBotOODLWrapper::switchOnBaseMotorsCallback, this);
00119
00120
00121 youBotOdometryFrameID = "odom";
00122 youBotOdometryChildFrameID = "base_footprint";
00123
00124 ROS_INFO("Base is initialized.");
00125 youBotConfiguration.hasBase = true;
00126 areBaseMotorsSwitchedOn = true;
00127
00128 }
00129
00130 void YouBotOODLWrapper::initializeArm(std::string armName, bool enableStandardGripper)
00131 {
00132 int armIndex;
00133 youbot::JointName jointNameParameter;
00134 std::string jointName;
00135 stringstream topicName;
00136 stringstream serviceName;
00137
00138 try
00139 {
00140 ROS_INFO("Configuration file path: %s", youBotConfiguration.configurationFilePath.c_str());
00141 YouBotArmConfiguration tmpArmConfig;
00142 youBotConfiguration.youBotArmConfigurations.push_back(tmpArmConfig);
00143 armIndex = static_cast<int>(youBotConfiguration.youBotArmConfigurations.size()) - 1;
00144 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm = new youbot::YouBotManipulator(
00145 armName, youBotConfiguration.configurationFilePath);
00146 youBotConfiguration.youBotArmConfigurations[armIndex].armID = armName;
00147 topicName.str("");
00148 topicName << "arm_" << (armIndex + 1) << "/";
00149 youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName = topicName.str();
00150 youBotConfiguration.youBotArmConfigurations[armIndex].parentFrameIDName = "base_link";
00151 youBotConfiguration.armNameToArmIndexMapping.insert(
00152 make_pair(armName, static_cast<int>(youBotConfiguration.youBotArmConfigurations.size())));
00153
00154
00155 youBotConfiguration.youBotArmConfigurations[armIndex].jointNames.clear();
00156 for (int i = 0; i < youBotArmDoF; ++i)
00157 {
00158 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).getConfigurationParameter(
00159 jointNameParameter);
00160 jointNameParameter.getParameter(jointName);
00161 youBotConfiguration.youBotArmConfigurations[armIndex].jointNames.push_back(jointName);
00162 ROS_INFO("Joint %i for arm %s has name: %s", i + 1,
00163 youBotConfiguration.youBotArmConfigurations[armIndex].armID.c_str(),
00164 youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i].c_str());
00165
00166 }
00167
00168 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->doJointCommutation();
00169 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->calibrateManipulator();
00170 if (enableStandardGripper)
00171 {
00172 youbot::GripperBarName barName;
00173 std::string gripperBarName;
00174
00175 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar1().getConfigurationParameter(
00176 barName);
00177 barName.getParameter(gripperBarName);
00178 youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::LEFT_FINGER_INDEX] =
00179 gripperBarName;
00180 ROS_INFO("Joint %i for gripper of arm %s has name: %s", 1,
00181 youBotConfiguration.youBotArmConfigurations[armIndex].armID.c_str(), gripperBarName.c_str());
00182
00183 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar2().getConfigurationParameter(
00184 barName);
00185 barName.getParameter(gripperBarName);
00186 youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::RIGHT_FINGER_INDEX] =
00187 gripperBarName;
00188 ROS_INFO("Joint %i for gripper of arm %s has name: %s", 2,
00189 youBotConfiguration.youBotArmConfigurations[armIndex].armID.c_str(), gripperBarName.c_str());
00190
00191 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->calibrateGripper();
00192 }
00193 }
00194 catch (std::exception& e)
00195 {
00196 youBotConfiguration.youBotArmConfigurations.pop_back();
00197 std::string errorMessage = e.what();
00198 ROS_FATAL("%s", errorMessage.c_str());
00199 ROS_ERROR("Arm \"%s\" could not be initialized.", armName.c_str());
00200 ROS_INFO("System has %i initialized arm(s).", static_cast<int>(youBotConfiguration.youBotArmConfigurations.size()));
00201 return;
00202 }
00203
00204
00205 topicName.str("");
00206 topicName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName
00207 << "arm_controller/position_command";
00208 youBotConfiguration.youBotArmConfigurations[armIndex].armPositionCommandSubscriber = node.subscribe
00209 < brics_actuator::JointPositions
00210 > (topicName.str(), 1000, boost::bind(&YouBotOODLWrapper::armPositionsCommandCallback, this, _1, armIndex));
00211
00212 topicName.str("");
00213 topicName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName
00214 << "arm_controller/velocity_command";
00215 youBotConfiguration.youBotArmConfigurations[armIndex].armVelocityCommandSubscriber = node.subscribe
00216 < brics_actuator::JointVelocities
00217 > (topicName.str(), 1000, boost::bind(&YouBotOODLWrapper::armVelocitiesCommandCallback, this, _1, armIndex));
00218
00219 topicName.str("");
00220 topicName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName
00221 << "arm_controller/follow_joint_trajectory";
00222
00223 youBotConfiguration.youBotArmConfigurations[armIndex].armJointTrajectoryAction = new actionlib::ActionServer<
00224 control_msgs::FollowJointTrajectoryAction>(
00225 node, topicName.str(), boost::bind(&YouBotOODLWrapper::armJointTrajectoryGoalCallback, this, _1, armIndex),
00226 boost::bind(&YouBotOODLWrapper::armJointTrajectoryCancelCallback, this, _1, armIndex), false);
00227
00228 topicName.str("");
00229 topicName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName << "joint_states";
00230 youBotConfiguration.youBotArmConfigurations[armIndex].armJointStatePublisher = node.advertise
00231 < sensor_msgs::JointState > (topicName.str(), 1);
00232
00233 if (enableStandardGripper)
00234 {
00235 topicName.str("");
00236 topicName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName
00237 << "gripper_controller/position_command";
00238 youBotConfiguration.youBotArmConfigurations[armIndex].gripperPositionCommandSubscriber = node.subscribe
00239 < brics_actuator::JointPositions
00240 > (topicName.str(), 1000, boost::bind(&YouBotOODLWrapper::gripperPositionsCommandCallback, this, _1, armIndex));
00241 youBotConfiguration.youBotArmConfigurations[armIndex].lastGripperCommand = 0.0;
00242 }
00243
00244
00245 serviceName.str("");
00246 serviceName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName << "switchOffMotors";
00247 youBotConfiguration.youBotArmConfigurations[armIndex].switchOffMotorsService = node.advertiseService<
00248 std_srvs::Empty::Request, std_srvs::Empty::Response>(
00249 serviceName.str(), boost::bind(&YouBotOODLWrapper::switchOffArmMotorsCallback, this, _1, _2, armIndex));
00250
00251 serviceName.str("");
00252 serviceName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName << "switchOnMotors";
00253 youBotConfiguration.youBotArmConfigurations[armIndex].switchONMotorsService = node.advertiseService<
00254 std_srvs::Empty::Request, std_srvs::Empty::Response>(
00255 serviceName.str(), boost::bind(&YouBotOODLWrapper::switchOnArmMotorsCallback, this, _1, _2, armIndex));
00256
00257 serviceName.str("");
00258 serviceName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName << "calibrate";
00259 youBotConfiguration.youBotArmConfigurations[armIndex].calibrateService = node.advertiseService<
00260 std_srvs::Empty::Request, std_srvs::Empty::Response>(
00261 serviceName.str(), boost::bind(&YouBotOODLWrapper::calibrateArmCallback, this, _1, _2, armIndex));
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283 sensor_msgs::JointState dummyMessage;
00284 armJointStateMessages.push_back(dummyMessage);
00285
00286
00287 youBotArmFrameID = "arm";
00288 ROS_INFO("Arm \"%s\" is initialized.", armName.c_str());
00289 ROS_INFO("System has %i initialized arm(s).", static_cast<int>(youBotConfiguration.youBotArmConfigurations.size()));
00290 youBotConfiguration.hasArms = true;
00291 areArmMotorsSwitchedOn = true;
00292
00293
00294 armHasActiveJointTrajectoryGoal = false;
00295
00296
00297
00298
00299
00300 youBotConfiguration.youBotArmConfigurations[armIndex].armJointTrajectoryAction->start();
00301 }
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315 void YouBotOODLWrapper::stop()
00316 {
00317
00318 if (youBotConfiguration.baseConfiguration.youBotBase)
00319 {
00320 delete youBotConfiguration.baseConfiguration.youBotBase;
00321 youBotConfiguration.baseConfiguration.youBotBase = 0;
00322 }
00323
00324 youBotConfiguration.baseConfiguration.baseCommandSubscriber.shutdown();
00325 youBotConfiguration.baseConfiguration.baseJointStatePublisher.shutdown();
00326 youBotConfiguration.baseConfiguration.baseOdometryPublisher.shutdown();
00327 youBotConfiguration.baseConfiguration.switchONMotorsService.shutdown();
00328 youBotConfiguration.baseConfiguration.switchOffMotorsService.shutdown();
00329
00330 youBotConfiguration.hasBase = false;
00331 areBaseMotorsSwitchedOn = false;
00332
00333 for (int armIndex = 0; armIndex < static_cast<int>(youBotConfiguration.youBotArmConfigurations.size()); armIndex++)
00334 {
00335 if (youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm)
00336 {
00337 delete youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm;
00338 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm = 0;
00339 }
00340
00341 youBotConfiguration.youBotArmConfigurations[armIndex].armJointStatePublisher.shutdown();
00342 youBotConfiguration.youBotArmConfigurations[armIndex].armPositionCommandSubscriber.shutdown();
00343 youBotConfiguration.youBotArmConfigurations[armIndex].armVelocityCommandSubscriber.shutdown();
00344 youBotConfiguration.youBotArmConfigurations[armIndex].calibrateService.shutdown();
00345 youBotConfiguration.youBotArmConfigurations[armIndex].gripperPositionCommandSubscriber.shutdown();
00346 youBotConfiguration.youBotArmConfigurations[armIndex].switchONMotorsService.shutdown();
00347 youBotConfiguration.youBotArmConfigurations[armIndex].switchOffMotorsService.shutdown();
00348 }
00349
00350 youBotConfiguration.hasArms = false;
00351 areArmMotorsSwitchedOn = false;
00352 youBotConfiguration.youBotArmConfigurations.clear();
00353 armJointStateMessages.clear();
00354
00355 youbot::EthercatMaster::destroy();
00356 }
00357
00358 void YouBotOODLWrapper::baseCommandCallback(const geometry_msgs::Twist& youbotBaseCommand)
00359 {
00360
00361 if (youBotConfiguration.hasBase)
00362 {
00363 quantity < si::velocity > longitudinalVelocity;
00364 quantity < si::velocity > transversalVelocity;
00365 quantity < si::angular_velocity > angularVelocity;
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385 longitudinalVelocity = youbotBaseCommand.linear.x * meter_per_second;
00386 transversalVelocity = youbotBaseCommand.linear.y * meter_per_second;
00387 angularVelocity = youbotBaseCommand.angular.z * radian_per_second;
00388
00389 try
00390 {
00391 youBotConfiguration.baseConfiguration.youBotBase->setBaseVelocity(longitudinalVelocity, transversalVelocity,
00392 angularVelocity);
00393 }
00394 catch (std::exception& e)
00395 {
00396 std::string errorMessage = e.what();
00397 ROS_WARN("Cannot set base velocities: %s", errorMessage.c_str());
00398 }
00399
00400 }
00401 else
00402 {
00403 ROS_ERROR("No base initialized!");
00404 }
00405 }
00406
00407 void YouBotOODLWrapper::armPositionsCommandCallback(const brics_actuator::JointPositionsConstPtr& youbotArmCommand,
00408 int armIndex)
00409 {
00410 ROS_DEBUG("Command for arm%i received", armIndex + 1);
00411 ROS_ASSERT(0 <= armIndex && armIndex < static_cast<int>(youBotConfiguration.youBotArmConfigurations.size()));
00412
00413 if (youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm != 0)
00414 {
00415
00416 ROS_DEBUG("Arm ID is: %s", youBotConfiguration.youBotArmConfigurations[armIndex].armID.c_str());
00417 if (youbotArmCommand->positions.size() < 1)
00418 {
00419 ROS_WARN("youBot driver received an invalid joint positions command.");
00420 return;
00421 }
00422
00423 youbot::JointAngleSetpoint desiredAngle;
00424 string unit = boost::units::to_string(boost::units::si::radian);
00425
00426
00427 std::map<string, double> jointNameToValueMapping;
00428 for (int i = 0; i < static_cast<int>(youbotArmCommand->positions.size()); ++i)
00429 {
00430 if (unit == youbotArmCommand->positions[i].unit)
00431 {
00432 jointNameToValueMapping.insert(
00433 make_pair(youbotArmCommand->positions[i].joint_uri, youbotArmCommand->positions[i].value));
00434 }
00435 else
00436 {
00437 ROS_WARN("Unit incompatibility. Are you sure you want to command %s instead of %s ?",
00438 youbotArmCommand->positions[i].unit.c_str(), unit.c_str());
00439 }
00440 }
00441
00442
00443 ROS_ASSERT(
00444 youBotConfiguration.youBotArmConfigurations[armIndex].jointNames.size()
00445 == static_cast<unsigned int>(youBotArmDoF));
00446 youbot::EthercatMaster::getInstance().AutomaticSendOn(false);
00447 for (int i = 0; i < youBotArmDoF; ++i)
00448 {
00449
00450
00451 map<string, double>::const_iterator jointIterator = jointNameToValueMapping.find(
00452 youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i]);
00453 if (jointIterator != jointNameToValueMapping.end())
00454 {
00455
00456
00457 ROS_DEBUG("Trying to set joint %s to new position value %f",
00458 (youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i]).c_str(), jointIterator->second);
00459 desiredAngle.angle = jointIterator->second * radian;
00460 try
00461 {
00462 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).setData(desiredAngle);
00463 }
00464 catch (std::exception& e)
00465 {
00466 std::string errorMessage = e.what();
00467 ROS_WARN("Cannot set arm joint %i: %s", i + 1, errorMessage.c_str());
00468 }
00469 }
00470 }
00471 youbot::EthercatMaster::getInstance().AutomaticSendOn(true);
00472 }
00473 else
00474 {
00475 ROS_ERROR("Arm%i is not correctly initialized!", armIndex + 1);
00476 }
00477
00478 }
00479
00480 void YouBotOODLWrapper::armVelocitiesCommandCallback(const brics_actuator::JointVelocitiesConstPtr& youbotArmCommand,
00481 int armIndex)
00482 {
00483 ROS_DEBUG("Command for arm%i received", armIndex + 1);
00484 ROS_ASSERT(0 <= armIndex && armIndex < static_cast<int>(youBotConfiguration.youBotArmConfigurations.size()));
00485
00486 if (youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm != 0)
00487 {
00488
00489 if (youbotArmCommand->velocities.size() < 1)
00490 {
00491 ROS_WARN("youBot driver received an invalid joint velocities command.");
00492 return;
00493 }
00494
00495 youbot::JointVelocitySetpoint desiredAngularVelocity;
00496 string unit = boost::units::to_string(boost::units::si::radian_per_second);
00497
00498
00499 std::map<string, double> jointNameToValueMapping;
00500 for (int i = 0; i < static_cast<int>(youbotArmCommand->velocities.size()); ++i)
00501 {
00502 if (unit == youbotArmCommand->velocities[i].unit)
00503 {
00504 jointNameToValueMapping.insert(
00505 make_pair(youbotArmCommand->velocities[i].joint_uri, youbotArmCommand->velocities[i].value));
00506 }
00507 else
00508 {
00509 ROS_WARN("Unit incompatibility. Are you sure you want to command %s instead of %s ?",
00510 youbotArmCommand->velocities[i].unit.c_str(), unit.c_str());
00511 }
00512
00513 }
00514
00515
00516 ROS_ASSERT(
00517 youBotConfiguration.youBotArmConfigurations[armIndex].jointNames.size()
00518 == static_cast<unsigned int>(youBotArmDoF));
00519 youbot::EthercatMaster::getInstance().AutomaticSendOn(false);
00520 for (int i = 0; i < youBotArmDoF; ++i)
00521 {
00522
00523
00524 map<string, double>::const_iterator jointIterator = jointNameToValueMapping.find(
00525 youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i]);
00526 if (jointIterator != jointNameToValueMapping.end())
00527 {
00528
00529
00530 ROS_DEBUG("Trying to set joint %s to new velocity value %f",
00531 (youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i]).c_str(), jointIterator->second);
00532 desiredAngularVelocity.angularVelocity = jointIterator->second * radian_per_second;
00533 try
00534 {
00535 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).setData(
00536 desiredAngularVelocity);
00537
00538 }
00539 catch (std::exception& e)
00540 {
00541 std::string errorMessage = e.what();
00542 ROS_WARN("Cannot set arm joint %i: %s", i + 1, errorMessage.c_str());
00543 }
00544 }
00545 }
00546 youbot::EthercatMaster::getInstance().AutomaticSendOn(true);
00547 }
00548 else
00549 {
00550 ROS_ERROR("Arm%i is not correctly initialized!", armIndex + 1);
00551 }
00552 }
00553
00554 void YouBotOODLWrapper::armJointTrajectoryGoalCallback(
00555 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::GoalHandle youbotArmGoal, unsigned int armIndex)
00556 {
00557 ROS_INFO("Goal for arm%i received", armIndex + 1);
00558 ROS_ASSERT(armIndex < youBotConfiguration.youBotArmConfigurations.size());
00559
00560 if (youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm == 0)
00561 {
00562 ROS_ERROR("Arm%i is not correctly initialized!", armIndex + 1);
00563 youbotArmGoal.setRejected();
00564 return;
00565 }
00566
00567 trajectory_msgs::JointTrajectory trajectory = youbotArmGoal.getGoal()->trajectory;
00568
00569
00570 if (trajectory.joint_names.size() != static_cast<unsigned int>(youBotArmDoF))
00571 {
00572 ROS_ERROR("Trajectory is malformed! Goal has %i joint names, but only %i joints are supported",
00573 static_cast<int>(trajectory.joint_names.size()), youBotArmDoF);
00574 youbotArmGoal.setRejected();
00575 return;
00576 }
00577
00578
00579 for (unsigned int i = 0; i < youBotConfiguration.youBotArmConfigurations[armIndex].jointNames.size(); i++)
00580 {
00581 bool jointNameFound = false;
00582 for (unsigned int j = 0; j < trajectory.joint_names.size(); j++)
00583 {
00584 if (youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i] == trajectory.joint_names[j])
00585 {
00586 jointNameFound = true;
00587 break;
00588 }
00589 }
00590
00591 if (!jointNameFound)
00592 {
00593 ROS_ERROR("Trajectory is malformed! Joint %s is missing in the goal",
00594 youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i].c_str());
00595 youbotArmGoal.setRejected();
00596 return;
00597 }
00598 }
00599
00600 std::vector < youbot::JointTrajectory > jointTrajectories(youBotArmDoF);
00601
00602
00603 std::vector < std::vector<quantity<plane_angle> > > positions(youBotArmDoF);
00604 std::vector < std::vector<quantity<angular_velocity> > > velocities(youBotArmDoF);
00605 std::vector < std::vector<quantity<angular_acceleration> > > accelerations(youBotArmDoF);
00606 youbot::TrajectorySegment segment;
00607 for (unsigned int i = 0; i < trajectory.points.size(); i++)
00608 {
00609 trajectory_msgs::JointTrajectoryPoint point = trajectory.points[i];
00610
00611 if ((point.positions.size() != static_cast<unsigned int>(youBotArmDoF)
00612 || point.velocities.size() != static_cast<unsigned int>(youBotArmDoF)
00613 || point.accelerations.size() != static_cast<unsigned int>(youBotArmDoF)))
00614 {
00615 ROS_ERROR("A trajectory point is malformed! %i positions, velocities and accelerations must be provided",
00616 youBotArmDoF);
00617 youbotArmGoal.setRejected();
00618 return;
00619 }
00620
00621 for (int j = 0; j < youBotArmDoF; j++)
00622 {
00623 segment.positions = point.positions[j] * radian;
00624 segment.velocities = point.velocities[j] * radian_per_second;
00625 segment.accelerations = point.accelerations[j] * radian_per_second / second;
00626 segment.time_from_start = boost::posix_time::microsec(point.time_from_start.toNSec() / 1000);
00627 jointTrajectories[j].segments.push_back(segment);
00628 }
00629 }
00630 for (int j = 0; j < youBotArmDoF; j++)
00631 {
00632 jointTrajectories[j].start_time = boost::posix_time::microsec_clock::local_time();
00633 }
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647 youbotArmGoal.setAccepted();
00648 armActiveJointTrajectoryGoal = youbotArmGoal;
00649 armHasActiveJointTrajectoryGoal = true;
00650
00651
00652
00653
00654 for (int i = 0; i < youBotArmDoF; ++i)
00655 {
00656 try
00657 {
00658
00659 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).trajectoryController.setTrajectory(
00660 jointTrajectories[i]);
00661 ROS_INFO("set trajectories %d", i);
00662 }
00663 catch (std::exception& e)
00664 {
00665 std::string errorMessage = e.what();
00666 ROS_WARN("Cannot set trajectory for joint %i: %s", i + 1, errorMessage.c_str());
00667 }
00668 }
00669 ROS_INFO("set all trajectories");
00670 }
00671
00672 void YouBotOODLWrapper::armJointTrajectoryCancelCallback(
00673 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::GoalHandle youbotArmGoal, unsigned int armIndex)
00674 {
00675 ROS_DEBUG("Goal for arm%i received", armIndex + 1);
00676 ROS_ASSERT(armIndex < youBotConfiguration.youBotArmConfigurations.size());
00677
00678
00679 for (int i = 0; i < youBotArmDoF; ++i)
00680 {
00681 try
00682 {
00683
00684
00685 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).trajectoryController.cancelCurrentTrajectory();
00686 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).stopJoint();
00687 }
00688 catch (std::exception& e)
00689 {
00690 std::string errorMessage = e.what();
00691 ROS_WARN("Cannot stop joint %i: %s", i + 1, errorMessage.c_str());
00692 }
00693 }
00694
00695 if (armActiveJointTrajectoryGoal == youbotArmGoal)
00696 {
00697
00698 youbotArmGoal.setCanceled();
00699 armHasActiveJointTrajectoryGoal = false;
00700 }
00701 }
00702
00703 void YouBotOODLWrapper::gripperPositionsCommandCallback(
00704 const brics_actuator::JointPositionsConstPtr& youbotGripperCommand, int armIndex)
00705 {
00706 ROS_DEBUG("Command for gripper%i received", armIndex + 1);
00707 ROS_ASSERT(0 <= armIndex && armIndex < static_cast<int>(youBotConfiguration.youBotArmConfigurations.size()));
00708
00709 if (youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm != 0)
00710 {
00711
00712 if (youbotGripperCommand->positions.size() < 1)
00713 {
00714 ROS_WARN("youBot driver received an invalid gripper positions command.");
00715 return;
00716 }
00717
00718 map<string, double>::const_iterator gripperIterator;
00719 youbot::GripperBarPositionSetPoint leftGripperFingerPosition;
00720 youbot::GripperBarPositionSetPoint rightGripperFingerPosition;
00721 string unit = boost::units::to_string(boost::units::si::meter);
00722
00723
00724 std::map<string, double> jointNameToValueMapping;
00725 for (int i = 0; i < static_cast<int>(youbotGripperCommand->positions.size()); ++i)
00726 {
00727 if (unit == youbotGripperCommand->positions[i].unit)
00728 {
00729 jointNameToValueMapping.insert(
00730 make_pair(youbotGripperCommand->positions[i].joint_uri, youbotGripperCommand->positions[i].value));
00731 }
00732 else
00733 {
00734 ROS_WARN("Unit incompatibility. Are you sure you want to command %s instead of %s ?",
00735 youbotGripperCommand->positions[i].unit.c_str(), unit.c_str());
00736 }
00737 }
00738
00739 youbot::EthercatMaster::getInstance().AutomaticSendOn(false);
00740
00741
00742 gripperIterator =
00743 jointNameToValueMapping.find(
00744 youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::LEFT_FINGER_INDEX]);
00745 if (gripperIterator != jointNameToValueMapping.end())
00746 {
00747 ROS_DEBUG("Trying to set the left gripper finger to new value %f", gripperIterator->second);
00748
00749 leftGripperFingerPosition.barPosition = gripperIterator->second * meter;
00750 try
00751 {
00752 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar1().setData(
00753 leftGripperFingerPosition);
00754 }
00755 catch (std::exception& e)
00756 {
00757 std::string errorMessage = e.what();
00758 ROS_WARN("Cannot set the left gripper finger: %s", errorMessage.c_str());
00759 }
00760 }
00761
00762
00763 gripperIterator =
00764 jointNameToValueMapping.find(
00765 youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::RIGHT_FINGER_INDEX]);
00766 if (gripperIterator != jointNameToValueMapping.end())
00767 {
00768 ROS_DEBUG("Trying to set the right gripper to new value %f", gripperIterator->second);
00769
00770 rightGripperFingerPosition.barPosition = gripperIterator->second * meter;
00771 try
00772 {
00773 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar2().setData(
00774 rightGripperFingerPosition);
00775 }
00776 catch (std::exception& e)
00777 {
00778 std::string errorMessage = e.what();
00779 ROS_WARN("Cannot set the right gripper finger: %s", errorMessage.c_str());
00780 }
00781 }
00782
00783 youbot::EthercatMaster::getInstance().AutomaticSendOn(true);
00784 }
00785 else
00786 {
00787 ROS_ERROR("Arm%i is not correctly initialized!", armIndex + 1);
00788 }
00789 }
00790
00791 void YouBotOODLWrapper::computeOODLSensorReadings()
00792 {
00793
00794
00795 try
00796 {
00797 currentTime = ros::Time::now();
00798 youbot::JointSensedAngle currentAngle;
00799 youbot::JointSensedVelocity currentVelocity;
00800
00801 youbot::EthercatMaster::getInstance().AutomaticReceiveOn(false);
00802
00803 if (youBotConfiguration.hasBase == true)
00804 {
00805 double x = 0.0;
00806 double y = 0.0;
00807 double theta = 0.0;
00808
00809 double vx = 0.0;
00810 double vy = 0.0;
00811 double vtheta = 0.0;
00812
00813 quantity < si::length > longitudinalPosition;
00814 quantity < si::length > transversalPosition;
00815 quantity < plane_angle > orientation;
00816
00817 quantity < si::velocity > longitudinalVelocity;
00818 quantity < si::velocity > transversalVelocity;
00819 quantity < si::angular_velocity > angularVelocity;
00820
00821
00822 youBotConfiguration.baseConfiguration.youBotBase->getBasePosition(longitudinalPosition, transversalPosition, orientation);
00823
00824 tf::Transform currentOdom;
00825 tf::Vector3 rotatedOrigin;
00826 rotatedOrigin.setX(longitudinalPosition.value());
00827 rotatedOrigin.setY(transversalPosition.value());
00828 rotatedOrigin=rotatedOrigin.rotate(tf::Vector3(0,0,1),tf::getYaw(odometryOffset.getRotation()));
00829 currentOdom.setOrigin(rotatedOrigin+odometryOffset.getOrigin());
00830
00831 x = currentOdom.getOrigin().getX();
00832 y = currentOdom.getOrigin().getY();
00833 theta = orientation.value()+tf::getYaw(odometryOffset.getRotation());
00834
00835 last_x=x;
00836 last_y=y;
00837 last_theta=theta;
00838
00839
00840 youBotConfiguration.baseConfiguration.youBotBase->getBaseVelocity(longitudinalVelocity, transversalVelocity,
00841 angularVelocity);
00842 vx = longitudinalVelocity.value();
00843 vy = transversalVelocity.value();
00844 vtheta = angularVelocity.value();
00845
00846
00847
00848 odometryQuaternion = tf::createQuaternionMsgFromYaw(theta);
00849
00850 odometryTransform.header.stamp = currentTime;
00851 odometryTransform.header.frame_id = youBotOdometryFrameID;
00852 odometryTransform.child_frame_id = youBotOdometryChildFrameID;
00853
00854 odometryTransform.transform.translation.x = x;
00855 odometryTransform.transform.translation.y = y;
00856 odometryTransform.transform.translation.z = 0.0;
00857 odometryTransform.transform.rotation = odometryQuaternion;
00858
00859
00860 odometryMessage.header.stamp = currentTime;
00861 odometryMessage.header.frame_id = youBotOdometryFrameID;
00862
00863 odometryMessage.pose.pose.position.x = x;
00864 odometryMessage.pose.pose.position.y = y;
00865 odometryMessage.pose.pose.position.z = 0.0;
00866 odometryMessage.pose.pose.orientation = odometryQuaternion;
00867
00868 odometryMessage.child_frame_id = youBotOdometryChildFrameID;
00869
00870 odometryMessage.twist.twist.linear.x = vx;
00871 odometryMessage.twist.twist.linear.y = vy;
00872 odometryMessage.twist.twist.angular.z = vtheta;
00873
00874
00875 baseJointStateMessage.header.stamp = currentTime;
00876 baseJointStateMessage.name.resize(youBotNumberOfWheels * 2);
00877 baseJointStateMessage.position.resize(youBotNumberOfWheels * 2);
00878 baseJointStateMessage.velocity.resize(youBotNumberOfWheels * 2);
00879
00880 ROS_ASSERT(
00881 (youBotConfiguration.baseConfiguration.wheelNames.size() == static_cast<unsigned int>(youBotNumberOfWheels)));
00882 for (int i = 0; i < youBotNumberOfWheels; ++i)
00883 {
00884 youBotConfiguration.baseConfiguration.youBotBase->getBaseJoint(i + 1).getData(currentAngle);
00885 youBotConfiguration.baseConfiguration.youBotBase->getBaseJoint(i + 1).getData(currentVelocity);
00886
00887 baseJointStateMessage.name[i] = youBotConfiguration.baseConfiguration.wheelNames[i];
00888 baseJointStateMessage.position[i] = currentAngle.angle.value();
00889 baseJointStateMessage.velocity[i] = currentVelocity.angularVelocity.value();
00890 }
00891
00892
00893
00894
00895
00896 baseJointStateMessage.name[4] = "caster_joint_fl";
00897 baseJointStateMessage.position[4] = 0.0;
00898
00899 baseJointStateMessage.name[5] = "caster_joint_fr";
00900 baseJointStateMessage.position[5] = 0.0;
00901
00902 baseJointStateMessage.name[6] = "caster_joint_bl";
00903 baseJointStateMessage.position[6] = 0.0;
00904
00905 baseJointStateMessage.name[7] = "caster_joint_br";
00906 baseJointStateMessage.position[7] = 0.0;
00907
00908
00909
00910
00911
00912
00913 baseJointStateMessage.position[0] = -baseJointStateMessage.position[0];
00914 baseJointStateMessage.position[2] = -baseJointStateMessage.position[2];
00915
00916 }
00917
00918 if (youBotConfiguration.hasArms == true)
00919 {
00920
00921 for (int armIndex = 0; armIndex < static_cast<int>(youBotConfiguration.youBotArmConfigurations.size());
00922 armIndex++)
00923 {
00924 ROS_ASSERT(youBotConfiguration.youBotArmConfigurations.size() == armJointStateMessages.size());
00925
00926
00927 armJointStateMessages[armIndex].header.stamp = currentTime;
00928 armJointStateMessages[armIndex].name.resize(youBotArmDoF + youBotNumberOfFingers);
00929 armJointStateMessages[armIndex].position.resize(youBotArmDoF + youBotNumberOfFingers);
00930 armJointStateMessages[armIndex].velocity.resize(youBotArmDoF + youBotNumberOfFingers);
00931
00932 if (youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm == 0)
00933 {
00934 ROS_ERROR("Arm%i is not correctly initialized! Cannot publish data.", armIndex + 1);
00935 continue;
00936 }
00937
00938 ROS_ASSERT(
00939 youBotConfiguration.youBotArmConfigurations[armIndex].jointNames.size()
00940 == static_cast<unsigned int>(youBotArmDoF));
00941 for (int i = 0; i < youBotArmDoF; ++i)
00942 {
00943 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).getData(currentAngle);
00944 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).getData(currentVelocity);
00945
00946 armJointStateMessages[armIndex].name[i] = youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i];
00947 armJointStateMessages[armIndex].position[i] = currentAngle.angle.value();
00948 armJointStateMessages[armIndex].velocity[i] = currentVelocity.angularVelocity.value();
00949 }
00950
00951
00952 bool areTrajectoryControllersDone = true;
00953 for (int i = 0; i < youBotArmDoF; ++i)
00954 {
00955 if (youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).trajectoryController.isTrajectoryControllerActive())
00956 {
00957 areTrajectoryControllersDone = false;
00958 break;
00959 }
00960 }
00961 if (areTrajectoryControllersDone && armHasActiveJointTrajectoryGoal)
00962 {
00963 armHasActiveJointTrajectoryGoal = false;
00964 control_msgs::FollowJointTrajectoryResult trajectoryResult;
00965 trajectoryResult.error_code = trajectoryResult.SUCCESSFUL;
00966 armActiveJointTrajectoryGoal.setSucceeded(trajectoryResult, "trajectory successful");
00967
00968
00969
00970 }
00971
00972
00973
00974
00975
00976
00977
00978 try
00979 {
00980 youbot::YouBotGripperBar& gripperBar1 =
00981 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar1();
00982 youbot::YouBotGripperBar& gripperBar2 =
00983 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar2();
00984
00985 if (gripperCycleCounter == 0)
00986 {
00987 gripperCycleCounter = youBotDriverCycleFrequencyInHz / 5;
00988 gripperBar1.getData(gripperBar1Position);
00989 gripperBar2.getData(gripperBar2Position);
00990 }
00991 gripperCycleCounter--;
00992
00993 armJointStateMessages[armIndex].name[youBotArmDoF + 0] =
00994 youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::LEFT_FINGER_INDEX];
00995 double leftGipperFingerPosition = gripperBar1Position.barPosition.value();
00996 armJointStateMessages[armIndex].position[youBotArmDoF + 0] = leftGipperFingerPosition;
00997
00998 double rightGipperFingerPosition = gripperBar2Position.barPosition.value();
00999 armJointStateMessages[armIndex].name[youBotArmDoF + 1] =
01000 youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::RIGHT_FINGER_INDEX];
01001 armJointStateMessages[armIndex].position[youBotArmDoF + 1] = rightGipperFingerPosition;
01002 }
01003 catch (std::exception& e)
01004 {
01005 std::string errorMessage = e.what();
01006 ROS_WARN("Cannot read gripper values: %s", errorMessage.c_str());
01007 }
01008
01009
01010
01011
01012
01013
01014
01015 }
01016 }
01017
01018 youbot::EthercatMaster::getInstance().AutomaticReceiveOn(true);
01019 }
01020 catch (youbot::EtherCATConnectionException& e)
01021 {
01022 ROS_WARN("%s", e.what());
01023 youBotConfiguration.hasBase = false;
01024 youBotConfiguration.hasArms = false;
01025 }
01026 catch (std::exception& e)
01027 {
01028 ROS_WARN_ONCE("%s", e.what());
01029 }
01030 }
01031
01032 void YouBotOODLWrapper::publishOODLSensorReadings()
01033 {
01034
01035 if (youBotConfiguration.hasBase)
01036 {
01037 youBotConfiguration.baseConfiguration.odometryBroadcaster.sendTransform(odometryTransform);
01038 youBotConfiguration.baseConfiguration.baseOdometryPublisher.publish(odometryMessage);
01039 youBotConfiguration.baseConfiguration.baseJointStatePublisher.publish(baseJointStateMessage);
01040 }
01041
01042 if (youBotConfiguration.hasArms)
01043 {
01044 for (int armIndex = 0; armIndex < static_cast<int>(youBotConfiguration.youBotArmConfigurations.size()); armIndex++)
01045 {
01046 youBotConfiguration.youBotArmConfigurations[armIndex].armJointStatePublisher.publish(
01047 armJointStateMessages[armIndex]);
01048 }
01049 }
01050
01051 }
01052
01053 bool YouBotOODLWrapper::switchOffBaseMotorsCallback(std_srvs::Empty::Request& request,
01054 std_srvs::Empty::Response& response)
01055 {
01056 ROS_INFO("Switch off the base motors");
01057 if (youBotConfiguration.hasBase)
01058 {
01059
01060 youbot::JointCurrentSetpoint currentStopMovement;
01061 currentStopMovement.current = 0.0 * ampere;
01062 try
01063 {
01064 youbot::EthercatMaster::getInstance().AutomaticSendOn(false);
01065 youBotConfiguration.baseConfiguration.youBotBase->getBaseJoint(1).setData(currentStopMovement);
01066 youBotConfiguration.baseConfiguration.youBotBase->getBaseJoint(2).setData(currentStopMovement);
01067 youBotConfiguration.baseConfiguration.youBotBase->getBaseJoint(3).setData(currentStopMovement);
01068 youBotConfiguration.baseConfiguration.youBotBase->getBaseJoint(4).setData(currentStopMovement);
01069 youbot::EthercatMaster::getInstance().AutomaticSendOn(true);
01070 }
01071 catch (std::exception& e)
01072 {
01073 std::string errorMessage = e.what();
01074 ROS_WARN("Cannot switch off the base motors: %s", errorMessage.c_str());
01075 return false;
01076 }
01077 }
01078 else
01079 {
01080 ROS_ERROR("No base initialized!");
01081 return false;
01082 }
01083 areBaseMotorsSwitchedOn = false;
01084 return true;
01085 }
01086
01087 bool YouBotOODLWrapper::switchOnBaseMotorsCallback(std_srvs::Empty::Request& request,
01088 std_srvs::Empty::Response& response)
01089 {
01090 ROS_INFO("Switch on the base motors");
01091 if (youBotConfiguration.hasBase)
01092 {
01093 quantity < si::velocity > longitudinalVelocity;
01094 quantity < si::velocity > transversalVelocity;
01095 quantity < si::angular_velocity > angularVelocity;
01096
01097 longitudinalVelocity = 0.0 * meter_per_second;
01098 transversalVelocity = 0.0 * meter_per_second;
01099 angularVelocity = 0.0 * radian_per_second;
01100
01101 try
01102 {
01103 youBotConfiguration.baseConfiguration.youBotBase->setBaseVelocity(longitudinalVelocity, transversalVelocity,
01104 angularVelocity);
01105 }
01106 catch (std::exception& e)
01107 {
01108 std::string errorMessage = e.what();
01109 ROS_WARN("Cannot set base velocities: %s", errorMessage.c_str());
01110 return false;
01111 }
01112 }
01113 else
01114 {
01115 ROS_ERROR("No base initialized!");
01116 return false;
01117 }
01118 areBaseMotorsSwitchedOn = true;
01119 return true;
01120 }
01121
01122 bool YouBotOODLWrapper::switchOffArmMotorsCallback(std_srvs::Empty::Request& request,
01123 std_srvs::Empty::Response& response, int armIndex)
01124 {
01125 ROS_INFO("Switch off the arm%i motors", armIndex + 1);
01126 ROS_ASSERT(0 <= armIndex && armIndex < static_cast<int>(youBotConfiguration.youBotArmConfigurations.size()));
01127
01128 if (youBotConfiguration.hasArms && youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm != 0)
01129 {
01130
01131 youbot::JointCurrentSetpoint currentStopMovement;
01132 currentStopMovement.current = 0.0 * ampere;
01133 try
01134 {
01135 youbot::EthercatMaster::getInstance().AutomaticSendOn(false);
01136 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(1).setData(currentStopMovement);
01137 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(2).setData(currentStopMovement);
01138 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(3).setData(currentStopMovement);
01139 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(4).setData(currentStopMovement);
01140 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(5).setData(currentStopMovement);
01141 youbot::EthercatMaster::getInstance().AutomaticSendOn(true);
01142 }
01143 catch (std::exception& e)
01144 {
01145 std::string errorMessage = e.what();
01146 ROS_WARN("Cannot switch off the arm motors: %s", errorMessage.c_str());
01147 return false;
01148 }
01149 }
01150 else
01151 {
01152 ROS_ERROR("Arm%i not initialized!", armIndex + 1);
01153 return false;
01154 }
01155 areArmMotorsSwitchedOn = false;
01156 return true;
01157 }
01158
01159 bool YouBotOODLWrapper::switchOnArmMotorsCallback(std_srvs::Empty::Request& request,
01160 std_srvs::Empty::Response& response, int armIndex)
01161 {
01162 ROS_INFO("Switch on the arm%i motors", armIndex + 1);
01163 ROS_ASSERT(0 <= armIndex && armIndex < static_cast<int>(youBotConfiguration.youBotArmConfigurations.size()));
01164
01165 if (youBotConfiguration.hasArms && youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm != 0)
01166 {
01167 try
01168 {
01169 std::vector < youbot::JointSensedAngle > sensedJointAngleVector;
01170 std::vector < youbot::JointAngleSetpoint > desiredJointAngleVector;
01171 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getJointData(sensedJointAngleVector);
01172 youbot::JointAngleSetpoint desiredJointAngle;
01173 for (unsigned int i = 0; i < sensedJointAngleVector.size(); i++)
01174 {
01175 desiredJointAngle = sensedJointAngleVector[i].angle;
01176 desiredJointAngleVector.push_back(desiredJointAngle);
01177 }
01178 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->setJointData(desiredJointAngleVector);
01179 }
01180 catch (std::exception& e)
01181 {
01182 std::string errorMessage = e.what();
01183 ROS_WARN("Cannot switch on the arm motors: %s", errorMessage.c_str());
01184 return false;
01185 }
01186 }
01187 else
01188 {
01189 ROS_ERROR("Arm%i not initialized!", armIndex + 1);
01190 return false;
01191 }
01192 areArmMotorsSwitchedOn = true;
01193 return true;
01194 }
01195
01196 bool YouBotOODLWrapper::calibrateArmCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response,
01197 int armIndex)
01198 {
01199 ROS_INFO("Calibrate the arm%i", armIndex + 1);
01200 ROS_ASSERT(0 <= armIndex && armIndex < static_cast<int>(youBotConfiguration.youBotArmConfigurations.size()));
01201
01202 if (youBotConfiguration.hasArms && youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm != 0)
01203 {
01204
01205 try
01206 {
01207 youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->calibrateManipulator(true);
01208 }
01209 catch (std::exception& e)
01210 {
01211 std::string errorMessage = e.what();
01212 ROS_WARN("Cannot calibrate the arm: %s", errorMessage.c_str());
01213 return false;
01214 }
01215 }
01216 else
01217 {
01218 ROS_ERROR("Arm%i not initialized!", armIndex + 1);
01219 return false;
01220 }
01221 return true;
01222 }
01223
01224 bool YouBotOODLWrapper::reconnectCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
01225 {
01226
01227
01228 quantity < si::length > longitudinalPosition;
01229 quantity < si::length > transversalPosition;
01230 quantity < plane_angle > orientation;
01231
01232
01233
01234
01235 if (youBotConfiguration.hasBase) {
01236
01237 try {
01238 youBotConfiguration.baseConfiguration.youBotBase->getBasePosition(longitudinalPosition, transversalPosition,
01239 orientation);
01240
01241 tf::Transform currentOdom;
01242 tf::Vector3 rotatedOrigin;
01243 rotatedOrigin.setX(longitudinalPosition.value());
01244 rotatedOrigin.setY(transversalPosition.value());
01245 rotatedOrigin=rotatedOrigin.rotate(tf::Vector3(0,0,1),tf::getYaw(odometryOffset.getRotation()));
01246 currentOdom.setOrigin(rotatedOrigin+odometryOffset.getOrigin());
01247 tf::Quaternion q;
01248 q.setRPY(0,0,orientation.value()+tf::getYaw(odometryOffset.getRotation()));
01249 currentOdom.setRotation(q);
01250 odometryOffset.setOrigin(currentOdom.getOrigin());
01251 odometryOffset.setRotation(q);
01252 ROS_INFO_STREAM("Odom before reconnect: (x,y,theta)=(" << odometryOffset.getOrigin().getX() << ", " << odometryOffset.getOrigin().getY() << ", " << tf::getYaw(odometryOffset.getRotation()) << ")");
01253
01254
01255
01256
01257
01258
01259
01260
01261
01262
01263
01264 }
01265 catch (std::exception& e) {
01266 std::string errorMessage = e.what();
01267 ROS_WARN("Cannot get the base position for reconnect: %s", errorMessage.c_str());
01268
01269 try {
01270 ROS_INFO("Using the last-recorded position instead");
01271 longitudinalPosition=last_x*si::meter;
01272 transversalPosition=last_y*si::meter;
01273 orientation=last_theta*si::radian;
01274 }
01275 catch (std::exception& darn) {
01276 ROS_WARN("No last-recorded position, teleporting to zero :O error:%s",darn.what());
01277 longitudinalPosition=0*si::meter;
01278 transversalPosition=0*si::meter;
01279 orientation=0*si::radian;
01280 }
01281 odometryOffset.getOrigin().setX(longitudinalPosition.value());
01282 odometryOffset.getOrigin().setY(transversalPosition.value());
01283 tf::Quaternion q;
01284 q.setRPY(0,0,last_theta);
01285 odometryOffset.setRotation(q);
01286 ROS_INFO_STREAM("Odom before reconnect: (x,y,theta)=(" << odometryOffset.getOrigin().getX() << ", " << odometryOffset.getOrigin().getY() << ", " << tf::getYaw(odometryOffset.getRotation()) << ")");
01287 }
01288 }
01289
01290
01291 this->stop();
01292
01293
01294 bool youBotHasBase;
01295 bool youBotHasArms;
01296 node.param("youBotHasBase", youBotHasBase, false);
01297 node.param("youBotHasArms", youBotHasArms, false);
01298 std::vector<std::string> armNames;
01299
01300
01301 int i = 1;
01302 std::stringstream armNameParam;
01303 armNameParam << "youBotArmName" << i;
01304 while (node.hasParam(armNameParam.str()))
01305 {
01306 std::string armName;
01307 node.getParam(armNameParam.str(), armName);
01308 armNames.push_back(armName);
01309 armNameParam.str("");
01310 armNameParam << "youBotArmName" << (++i);
01311 }
01312
01313
01314 ROS_ASSERT((youBotHasBase == true) || (youBotHasArms == true));
01315 if (youBotHasBase == true)
01316 {
01317 this->initializeBase(this->youBotConfiguration.baseConfiguration.baseID);
01318
01319
01320
01321
01322
01323
01324
01325
01326
01327
01328 quantity < si::velocity > longitudinalVelocity;
01329 quantity < si::velocity > transversalVelocity;
01330 quantity < si::angular_velocity > angularVelocity;
01331
01332 longitudinalVelocity = 0.0 * meter_per_second;
01333 transversalVelocity = 0.0 * meter_per_second;
01334 angularVelocity = 0.0 * radian_per_second;
01335
01336 try
01337 {
01338 youBotConfiguration.baseConfiguration.youBotBase->setBaseVelocity(longitudinalVelocity, transversalVelocity,
01339 angularVelocity);
01340 }
01341 catch (std::exception& e)
01342 {
01343 std::string errorMessage = e.what();
01344 ROS_WARN("Cannot set base velocities: %s", errorMessage.c_str());
01345 return false;
01346 }
01347
01348
01349 try {
01350 youBotConfiguration.baseConfiguration.youBotBase->getBasePosition(longitudinalPosition, transversalPosition,
01351 orientation);
01352 tf::Transform currentOdom;
01353 tf::Vector3 rotatedOrigin;
01354 rotatedOrigin.setX(longitudinalPosition.value());
01355 rotatedOrigin.setY(transversalPosition.value());
01356 rotatedOrigin=rotatedOrigin.rotate(tf::Vector3(0,0,1),tf::getYaw(odometryOffset.getRotation()));
01357 currentOdom.setOrigin(rotatedOrigin+odometryOffset.getOrigin());
01358 currentOdom.getRotation().setRPY(0,0,orientation.value()+tf::getYaw(odometryOffset.getRotation()));
01359 ROS_INFO_STREAM("Odom after reconnect: (x,y,theta)=(" << currentOdom.getOrigin().getX() << ", " << currentOdom.getOrigin().getY() << ", " << tf::getYaw(currentOdom.getRotation()) << ")");
01360 }
01361 catch (std::exception& e) {
01362 std::string errorMessage = e.what();
01363 ROS_WARN("Cannot get the base position at the end of reconnect: %s", errorMessage.c_str());
01364 longitudinalPosition=0*si::meter;
01365 transversalPosition=0*si::meter;
01366 orientation=0*si::radian;
01367 }
01368
01369 }
01370
01371
01372 if (youBotHasArms == true)
01373 {
01374 std::vector<std::string>::iterator armNameIter;
01375 for (armNameIter = armNames.begin(); armNameIter != armNames.end(); ++armNameIter)
01376 {
01377 this->initializeArm(*armNameIter);
01378 }
01379 }
01380 return true;
01381 }
01382
01383 void YouBotOODLWrapper::publishArmAndBaseDiagnostics(double publish_rate_in_secs)
01384 {
01385
01386 if ((ros::Time::now() - lastDiagnosticPublishTime).toSec() < publish_rate_in_secs)
01387 return;
01388
01389 lastDiagnosticPublishTime = ros::Time::now();
01390
01391 diagnosticArrayMessage.header.stamp = ros::Time::now();
01392 diagnosticArrayMessage.status.clear();
01393
01394
01395 diagnosticStatusMessage.name = "platform_Base";
01396 if (youBotConfiguration.hasBase)
01397 {
01398 diagnosticStatusMessage.message = "base is present";
01399 diagnosticStatusMessage.level = diagnostic_msgs::DiagnosticStatus::OK;
01400 }
01401 else
01402 {
01403 diagnosticStatusMessage.message = "base is not connected or switched off";
01404 diagnosticStatusMessage.level = diagnostic_msgs::DiagnosticStatus::ERROR;
01405 }
01406
01407 diagnosticArrayMessage.status.push_back(diagnosticStatusMessage);
01408
01409 diagnosticStatusMessage.name = "platform_Arm";
01410 if (youBotConfiguration.hasArms)
01411 {
01412 diagnosticStatusMessage.message = "arm is present";
01413 diagnosticStatusMessage.level = diagnostic_msgs::DiagnosticStatus::OK;
01414 }
01415 else
01416 {
01417 diagnosticStatusMessage.message = "arm is not connected or switched off";
01418 diagnosticStatusMessage.level = diagnostic_msgs::DiagnosticStatus::ERROR;
01419 }
01420
01421 diagnosticArrayMessage.status.push_back(diagnosticStatusMessage);
01422
01423
01424 platformStateMessage.header.stamp = ros::Time::now();
01425
01426 if (youBotConfiguration.hasBase && areBaseMotorsSwitchedOn)
01427 platformStateMessage.circuit_state[0] = youbot_common::PowerBoardState::STATE_ENABLED;
01428 else if (youBotConfiguration.hasBase && !areBaseMotorsSwitchedOn)
01429 platformStateMessage.circuit_state[0] = youbot_common::PowerBoardState::STATE_STANDBY;
01430 else
01431 platformStateMessage.circuit_state[0] = youbot_common::PowerBoardState::STATE_DISABLED;
01432
01433 if (youBotConfiguration.hasArms && areArmMotorsSwitchedOn)
01434 platformStateMessage.circuit_state[1] = youbot_common::PowerBoardState::STATE_ENABLED;
01435 else if (youBotConfiguration.hasArms && !areArmMotorsSwitchedOn)
01436 platformStateMessage.circuit_state[1] = youbot_common::PowerBoardState::STATE_STANDBY;
01437 else
01438 platformStateMessage.circuit_state[1] = youbot_common::PowerBoardState::STATE_DISABLED;
01439
01440
01441 dashboardMessagePublisher.publish(platformStateMessage);
01442 diagnosticArrayPublisher.publish(diagnosticArrayMessage);
01443 }
01444
01445 }
01446
01447