00001 #include "robodyn_controllers/TrajectoryMonitor.h"
00002
00003 namespace SU = StringUtilities;
00004
00005 TrajectoryMonitor::TrajectoryMonitor(const std::string& name)
00006 : GoalStatusSender(name)
00007 , embeddedSuffix("embeddedCommand")
00008 {
00009 defaultFactors.torqueLimitFactor = 2.5;
00010 defaultFactors.distanceFactor = 1.5;
00011 replanTraj.points.resize(1);
00012 replanTraj.points[0].time_from_start = ros::Duration(-1.);
00013 }
00014
00015 TrajectoryMonitor::~TrajectoryMonitor()
00016 {
00017 }
00018
00019 void TrajectoryMonitor::setBases(const r2_msgs::StringArray& basesMsg)
00020 {
00021 if (bases != basesMsg.data)
00022 {
00023 bases = basesMsg.data;
00024 writeBases(basesMsg);
00025 }
00026 }
00027
00028 void TrajectoryMonitor::setFactors(const r2_msgs::TrajectoryMonitorFactors& factorsMsg)
00029 {
00030 if (factorsMsg.joint_names.size() != factorsMsg.torqueLimitFactors.size()
00031 || factorsMsg.joint_names.size() != factorsMsg.distanceFactors.size())
00032 {
00033 std::stringstream ss;
00034 ss << "invalid factorsMsg was received";
00035 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::ERROR, ss.str());
00036 return;
00037 }
00038
00039 for (unsigned int i = 0; i < factorsMsg.joint_names.size(); ++i)
00040 {
00041 TrajectoryMonitorFactors& tempFactors = factors[factorsMsg.joint_names[i]];
00042 tempFactors.torqueLimitFactor = factorsMsg.torqueLimitFactors[i];
00043 tempFactors.distanceFactor = factorsMsg.distanceFactors[i];
00044 }
00045 }
00046
00047 void TrajectoryMonitor::evaluateReplanTrigger(const sensor_msgs::JointState& jointStatesMsg, const r2_msgs::JointCommand& jointCommandMsg,
00048 const r2_msgs::JointControlDataArray& jointStatusMsg, const r2_msgs::JointCapability& jointCapMsg)
00049 {
00050 JointData newJointData;
00051 jointData.clear();
00052
00053 if (jointStatesMsg.name.size() != jointStatesMsg.position.size() || jointStatesMsg.name.size() != jointStatesMsg.velocity.size())
00054 {
00055 std::stringstream ss;
00056 ss << "invalid jointStatesMsg was received";
00057 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::ERROR, ss.str());
00058 return;
00059 }
00060
00061 if (jointCommandMsg.name.size() != jointCommandMsg.desiredPosition.size()
00062 || jointCommandMsg.name.size() != jointCommandMsg.desiredPositionVelocityLimit.size()
00063 || jointCommandMsg.name.size() != jointCommandMsg.proportionalGain.size()
00064 || jointCommandMsg.name.size() != jointCommandMsg.derivativeGain.size()
00065 || jointCommandMsg.name.size() != jointCommandMsg.positionLoopTorqueLimit.size())
00066 {
00067 std::stringstream ss;
00068 ss << "invalid jointCommandMsg was received";
00069 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::ERROR, ss.str());
00070 return;
00071 }
00072
00073
00074 for (unsigned int i = 0; i < jointStatusMsg.joint.size(); ++i)
00075 {
00076 if (jointStatusMsg.data.at(i).controlMode.state == r2_msgs::JointControlMode::DRIVE)
00077 {
00078 std::vector<std::string>::const_iterator capIt = std::find(jointCapMsg.name.begin(), jointCapMsg.name.end(), jointStatusMsg.joint[i]);
00079
00080 if (capIt != jointCapMsg.name.end() && jointCapMsg.torqueLimit.at(capIt - jointCapMsg.name.begin()) > 0.)
00081 {
00082 jointData[jointStatusMsg.joint[i]] = newJointData;
00083 }
00084 }
00085 }
00086
00087
00088 for (unsigned int i = 0; i < jointStatesMsg.name.size(); ++i)
00089 {
00090 std::string jnt = SU::getRoboDynJoint(jointStatesMsg.name[i]);
00091
00092 if (jnt.find(embeddedSuffix) != std::string::npos && SU::isItemWithNumber(jnt))
00093 {
00094
00095 jointDataIt = jointData.find(SU::getRoboDynEverythingButJoint(jointStatesMsg.name[i]) + SU::TOKEN_DELIMITER + "joint" + SU::getNumber(jnt));
00096
00097 if (jointDataIt != jointData.end())
00098 {
00099 jointDataIt->second.embeddedCommand = true;
00100 jointDataIt->second.commandPosition = jointStatesMsg.position[i];
00101 jointDataIt->second.commandVelocity = jointStatesMsg.velocity[i];
00102 }
00103
00104 continue;
00105 }
00106
00107
00108 jointDataIt = jointData.find(jointStatesMsg.name[i]);
00109
00110 if (jointDataIt != jointData.end())
00111 {
00112 jointDataIt->second.positionValid = true;
00113 jointDataIt->second.actualPosition = jointStatesMsg.position[i];
00114 jointDataIt->second.actualVelocity = jointStatesMsg.velocity[i];
00115 }
00116 }
00117
00118
00119 for (unsigned int i = 0; i < jointCommandMsg.name.size(); ++i)
00120 {
00121 jointDataIt = jointData.find(jointCommandMsg.name[i]);
00122
00123 if (jointDataIt != jointData.end() && jointDataIt->second.positionValid)
00124 {
00125
00126 double torqueLimitFactor = defaultFactors.torqueLimitFactor;
00127 double distanceFactor = defaultFactors.distanceFactor;
00128 std::map<std::string, TrajectoryMonitorFactors>::const_iterator factorIt = factors.find(jointCommandMsg.name[i]);
00129
00130 if (factorIt != factors.end())
00131 {
00132 torqueLimitFactor = factorIt->second.torqueLimitFactor;
00133 distanceFactor = factorIt->second.distanceFactor;
00134 }
00135
00136 jointDataIt->second.commandValid = true;
00137
00138
00139 if (!jointDataIt->second.embeddedCommand)
00140 {
00141 jointDataIt->second.commandPosition = jointCommandMsg.desiredPosition[i];
00142 jointDataIt->second.commandVelocity = jointCommandMsg.desiredPositionVelocityLimit[i];
00143 }
00144
00145 double posTorque = jointCommandMsg.proportionalGain[i] * (jointDataIt->second.commandPosition - jointDataIt->second.actualPosition);
00146
00147 if (posTorque > torqueLimitFactor * jointCommandMsg.positionLoopTorqueLimit[i])
00148 {
00149 jointDataIt->second.replan.type = r2_msgs::ReplanType::PAUSE;
00150 jointDataIt->second.torqueLimitCommand = jointDataIt->second.actualPosition +
00151 distanceFactor * (jointCommandMsg.positionLoopTorqueLimit[i] / jointCommandMsg.proportionalGain[i]);
00152 std::stringstream ss;
00153 ss << "replan for " << jointDataIt->first;
00154 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::NOTICE, ss.str());
00155 ss << ":";
00156 ss << "\n\tcommandPosition: " << jointDataIt->second.commandPosition;
00157 ss << "\n\tactualPosition: " << jointDataIt->second.actualPosition;
00158 ss << "\n\tactualVelocity: " << jointDataIt->second.actualVelocity;
00159 ss << "\n\tposTorque: " << posTorque;
00160 ss << "\n\ttorqueLimit: " << torqueLimitFactor* jointCommandMsg.positionLoopTorqueLimit[i];
00161 ss << "\n\tnewCommand: " << jointDataIt->second.torqueLimitCommand;
00162 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::DEBUG, ss.str());
00163 }
00164 else if (posTorque < -torqueLimitFactor * jointCommandMsg.positionLoopTorqueLimit[i])
00165 {
00166 jointDataIt->second.replan.type = r2_msgs::ReplanType::PAUSE;
00167 jointDataIt->second.torqueLimitCommand = jointDataIt->second.actualPosition -
00168 distanceFactor * (jointCommandMsg.positionLoopTorqueLimit[i] / jointCommandMsg.proportionalGain[i]);
00169 std::stringstream ss;
00170 ss << "replan for " << jointDataIt->first;
00171 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::NOTICE, ss.str());
00172 ss << ":";
00173 ss << "\n\tcommandPosition: " << jointDataIt->second.commandPosition;
00174 ss << "\n\tactualPosition: " << jointDataIt->second.actualPosition;
00175 ss << "\n\tactualVelocity: " << jointDataIt->second.actualVelocity;
00176 ss << "\n\tposTorque: " << posTorque;
00177 ss << "\n\ttorqueLimit: " << torqueLimitFactor* jointCommandMsg.positionLoopTorqueLimit[i];
00178 ss << "\n\tnewCommand: " << jointDataIt->second.torqueLimitCommand;
00179 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::DEBUG, ss.str());
00180 }
00181 }
00182 }
00183 }
00184
00185 void TrajectoryMonitor::replan()
00186 {
00187
00188 replanTraj.joint_names.clear();
00189 replanTraj.points[0].positions.clear();
00190 replanTraj.points[0].velocities.clear();
00191 replanVec.clear();
00192
00193 std::stringstream replanJoints;
00194
00195 for (jointDataIt = jointData.begin(); jointDataIt != jointData.end(); ++jointDataIt)
00196 {
00197 if (jointDataIt->second.replan.type == r2_msgs::ReplanType::PAUSE && jointDataIt->second.commandValid)
00198 {
00199 replanTraj.joint_names.push_back(jointDataIt->first);
00200 replanVec.push_back(jointDataIt->second.replan);
00201 replanTraj.points[0].positions.push_back(jointDataIt->second.torqueLimitCommand);
00202 replanTraj.points[0].velocities.push_back(jointDataIt->second.actualVelocity);
00203
00204 replanJoints << jointDataIt->first << ", ";
00205 }
00206 }
00207
00208 if (!replanTraj.joint_names.empty())
00209 {
00210 replanTraj.header.stamp = ros::Time::now();
00211 replanTraj.header.frame_id = std::string("joints ") + replanJoints.str() + " replan:hold";
00212
00213 addJointWaypoints(replanTraj, replanVec);
00214 }
00215
00216 }
00217
00218 void TrajectoryMonitor::updateMonitor(const r2_msgs::JointCommand& jointCommandMsg,
00219 const r2_msgs::PoseState& poseCommandsMsg)
00220 {
00221 if (jointCommandMsg.name.size() != jointCommandMsg.desiredPosition.size())
00222 {
00223 std::stringstream ss;
00224 ss << "invalid jointCommandMsg was received";
00225 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::ERROR, ss.str());
00226 return;
00227 }
00228
00229
00230 std::map<std::string, double> jointCommands;
00231
00232 for (unsigned int i = 0; i < jointCommandMsg.name.size(); ++i)
00233 {
00234 jointCommands[jointCommandMsg.name[i]] = jointCommandMsg.desiredPosition[i];
00235 }
00236
00237 std::map<std::string, double>::const_iterator jointIt;
00238
00239
00240 for (JointTrajectoriesMap_type::iterator trajIt = jointTrajectories.begin(); trajIt != jointTrajectories.end();)
00241 {
00242 bool achieved = true;
00243
00244 for (unsigned int i = 0; i < trajIt->second.joint_names.size(); ++i)
00245 {
00246 jointIt = jointCommands.find(trajIt->second.joint_names[i]);
00247
00248 if (jointIt == jointCommands.end())
00249 {
00250 std::stringstream ss;
00251 ss << "no position available for trajectory (" << trajIt->first.stamp.toSec() << ") " << trajIt->first.id << " joint "
00252 << trajIt->second.joint_names[i] << ". Removing trajectory from monitored list.";
00253 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::ERROR, ss.str());
00254 JointTrajectoriesMap_type::iterator tempIt = trajIt++;
00255 jointTrajectories.erase(tempIt);
00256 achieved = false;
00257 break;
00258 }
00259 else
00260 {
00261
00262 if (fabs(trajIt->second.points[0].positions[i] - jointIt->second) > 0.0001)
00263 {
00264 achieved = false;
00265 ++trajIt;
00266 break;
00267 }
00268 }
00269 }
00270
00271 if (achieved)
00272 {
00273 std::stringstream ss;
00274 ss << "waypoint reached";
00275 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::DEBUG, ss.str());
00276 sendStatusMessage(trajIt->first.stamp, trajIt->first.id, actionlib_msgs::GoalStatus::ACTIVE, ss.str(), true);
00277
00278 if (trajIt->second.points.size() == 1)
00279 {
00280 JointTrajectoriesMap_type::iterator tempIt = trajIt++;
00281 jointTrajectories.erase(tempIt);
00282 }
00283 else
00284 {
00285
00286 trajIt->second.points.erase(trajIt->second.points.begin());
00287
00288 }
00289 }
00290 }
00291
00292
00293 for (PoseTrajectoriesMap_type::iterator trajIt = poseTrajectories.begin(); trajIt != poseTrajectories.end();)
00294 {
00295 bool achieved = true;
00296
00297 for (unsigned int nodeIndex = 0; nodeIndex < trajIt->second.nodes.size(); ++nodeIndex)
00298 {
00299 try
00300 {
00301 KDL::Frame actualFrame, goalFrame;
00302 RosMsgConverter::PoseStateToFrame(poseCommandsMsg, trajIt->second.refFrames[nodeIndex], trajIt->second.nodes[nodeIndex], actualFrame);
00303 tf::poseMsgToKDL(trajIt->second.points[0].positions[nodeIndex], goalFrame);
00304 KDL::Twist twist = KDL::diff(goalFrame, actualFrame);
00305
00306 for (unsigned int axisIndex = 0; axisIndex < 3; ++axisIndex)
00307 {
00308 if ((fabs(twist.vel[axisIndex]) > 0.0001 && trajIt->second.node_priorities[nodeIndex].axis_priorities[axisIndex] == r2_msgs::PriorityArray::CRITICAL)
00309 || (fabs(twist.rot[axisIndex]) > 0.0001 && trajIt->second.node_priorities[nodeIndex].axis_priorities[3 + axisIndex] == r2_msgs::PriorityArray::CRITICAL))
00310 {
00311 achieved = false;
00312 break;
00313 }
00314 }
00315 }
00316 catch (std::exception& e)
00317 {
00318 std::stringstream ss;
00319 ss << "invalid jointCommandMsg was received -- " << e.what();
00320 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::ERROR, ss.str());
00321 return;
00322 }
00323 }
00324
00325 if (achieved)
00326 {
00327 std::stringstream ss;
00328 ss << "waypoint reached";
00329 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::DEBUG, ss.str());
00330 sendStatusMessage(trajIt->first.stamp, trajIt->first.id, actionlib_msgs::GoalStatus::ACTIVE, ss.str(), true);
00331
00332 if (trajIt->second.points.size() == 1)
00333 {
00334 PoseTrajectoriesMap_type::iterator tempIt = trajIt++;
00335 poseTrajectories.erase(tempIt);
00336 }
00337 else
00338 {
00339
00340 trajIt->second.points.erase(trajIt->second.points.begin());
00341
00342 }
00343 }
00344 else
00345 {
00346 ++trajIt;
00347 }
00348 }
00349 }
00350
00351 void TrajectoryMonitor::updateGoals(const actionlib_msgs::GoalStatusArray& goalStatuses)
00352 {
00353 writeStatus(goalStatuses);
00354
00355
00356 for (unsigned int i = 0; i < goalStatuses.status_list.size(); ++i)
00357 {
00358 if (goalStatuses.status_list[i].status == actionlib_msgs::GoalStatus::SUCCEEDED
00359 || goalStatuses.status_list[i].status == actionlib_msgs::GoalStatus::ABORTED
00360 || goalStatuses.status_list[i].status == actionlib_msgs::GoalStatus::PREEMPTED
00361 || goalStatuses.status_list[i].status == actionlib_msgs::GoalStatus::REJECTED)
00362 {
00363 jointTrajectories.erase(goalStatuses.status_list[i].goal_id);
00364 poseTrajectories.erase(goalStatuses.status_list[i].goal_id);
00365 }
00366 }
00367 }
00368
00369 void TrajectoryMonitor::addCartesianWaypoints(r2_msgs::PoseTrajectory trajectory, const r2_msgs::PoseState& poseStatesMsg)
00370 {
00371
00372 if (trajectory.nodes.size() == 0 || trajectory.points.size() == 0)
00373 {
00374 sendCartesianWaypoints(trajectory);
00375 return;
00376 }
00377
00378
00379 if (bases.size() == 0)
00380 {
00381 std::stringstream ss;
00382 ss << "no bases set";
00383 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::WARN, ss.str());
00384 sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
00385 return;
00386 }
00387
00388 if (trajectory.refFrames.size() == 1)
00389 {
00390 trajectory.refFrames.resize(trajectory.nodes.size(), trajectory.refFrames[0]);
00391 }
00392 else if (trajectory.refFrames.size() != trajectory.nodes.size())
00393 {
00394 std::stringstream ss;
00395 ss << "invalid refFrames";
00396 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::WARN, ss.str());
00397 sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
00398 return;
00399 }
00400
00401 if (trajectory.node_priorities.size() == 0)
00402 {
00403 trajectory.node_priorities.resize(trajectory.nodes.size());
00404 }
00405 else if (trajectory.node_priorities.size() != trajectory.nodes.size())
00406 {
00407 std::stringstream ss;
00408 ss << "invalid node_priorities";
00409 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::WARN, ss.str());
00410 sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
00411 return;
00412 }
00413
00414 for (unsigned int i = 0; i < trajectory.node_priorities.size(); ++i)
00415 {
00416 if (trajectory.node_priorities[i].axis_priorities.size() == 0)
00417 {
00418 trajectory.node_priorities[i].axis_priorities.resize(6, r2_msgs::PriorityArray::CRITICAL);
00419 }
00420 else if (trajectory.node_priorities[i].axis_priorities.size() == 1)
00421 {
00422 trajectory.node_priorities[i].axis_priorities.resize(6, trajectory.node_priorities[i].axis_priorities[0]);
00423 }
00424 else if (trajectory.node_priorities[i].axis_priorities.size() != 6)
00425 {
00426 std::stringstream ss;
00427 ss << "invalid node_priorities";
00428 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::WARN, ss.str());
00429 sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
00430 return;
00431 }
00432 }
00433
00434 bool valid = true;
00435
00436 for (unsigned int pointIndex = 0; pointIndex < trajectory.points.size(); ++pointIndex)
00437 {
00438 if (trajectory.points[pointIndex].positions.size() != trajectory.nodes.size())
00439 {
00440 valid = false;
00441 break;
00442 }
00443 else
00444 {
00445 for (unsigned int posIndex = 0; posIndex < trajectory.points[pointIndex].positions.size(); ++posIndex)
00446 {
00447 KDL::FrameAcc refFrame, goalFrame;
00448
00449 try
00450 {
00451 RosMsgConverter::PoseStateToFrameAcc(poseStatesMsg, bases[0], trajectory.refFrames[posIndex], refFrame);
00452 }
00453 catch (std::exception& e)
00454 {
00455 valid = false;
00456 break;
00457 }
00458
00459
00460 goalFrame.p.p.x(trajectory.points[pointIndex].positions[posIndex].position.x);
00461 goalFrame.p.p.y(trajectory.points[pointIndex].positions[posIndex].position.y);
00462 goalFrame.p.p.z(trajectory.points[pointIndex].positions[posIndex].position.z);
00463 goalFrame.M.R = KDL::Rotation::Quaternion(trajectory.points[pointIndex].positions[posIndex].orientation.x,
00464 trajectory.points[pointIndex].positions[posIndex].orientation.y,
00465 trajectory.points[pointIndex].positions[posIndex].orientation.z,
00466 trajectory.points[pointIndex].positions[posIndex].orientation.w);
00467
00468
00469 if (trajectory.points[pointIndex].velocities.size() == trajectory.nodes.size())
00470 {
00471 goalFrame.p.v.x(trajectory.points[pointIndex].velocities[posIndex].linear.x);
00472 goalFrame.p.v.y(trajectory.points[pointIndex].velocities[posIndex].linear.y);
00473 goalFrame.p.v.z(trajectory.points[pointIndex].velocities[posIndex].linear.z);
00474 goalFrame.M.w.x(trajectory.points[pointIndex].velocities[posIndex].angular.x);
00475 goalFrame.M.w.y(trajectory.points[pointIndex].velocities[posIndex].angular.y);
00476 goalFrame.M.w.z(trajectory.points[pointIndex].velocities[posIndex].angular.z);
00477 }
00478 else if (trajectory.points[pointIndex].velocities.size() != 0)
00479 {
00480 valid = false;
00481 break;
00482 }
00483
00484
00485 if (trajectory.points[pointIndex].accelerations.size() == trajectory.nodes.size())
00486 {
00487 goalFrame.p.dv.x(trajectory.points[pointIndex].accelerations[posIndex].linear.x);
00488 goalFrame.p.dv.y(trajectory.points[pointIndex].accelerations[posIndex].linear.y);
00489 goalFrame.p.dv.z(trajectory.points[pointIndex].accelerations[posIndex].linear.z);
00490 goalFrame.M.dw.x(trajectory.points[pointIndex].accelerations[posIndex].angular.x);
00491 goalFrame.M.dw.y(trajectory.points[pointIndex].accelerations[posIndex].angular.y);
00492 goalFrame.M.dw.z(trajectory.points[pointIndex].accelerations[posIndex].angular.z);
00493 }
00494 else if (trajectory.points[pointIndex].accelerations.size() != 0)
00495 {
00496 valid = false;
00497 break;
00498 }
00499
00500
00501 goalFrame = refFrame * goalFrame;
00502
00503
00504 trajectory.points[pointIndex].positions[posIndex].position.x = goalFrame.p.p.x();
00505 trajectory.points[pointIndex].positions[posIndex].position.y = goalFrame.p.p.y();
00506 trajectory.points[pointIndex].positions[posIndex].position.z = goalFrame.p.p.z();
00507 goalFrame.M.R.GetQuaternion(trajectory.points[pointIndex].positions[posIndex].orientation.x,
00508 trajectory.points[pointIndex].positions[posIndex].orientation.y,
00509 trajectory.points[pointIndex].positions[posIndex].orientation.z,
00510 trajectory.points[pointIndex].positions[posIndex].orientation.w);
00511
00512
00513 if (trajectory.points[pointIndex].velocities.size() == trajectory.nodes.size())
00514 {
00515 trajectory.points[pointIndex].velocities[posIndex].linear.x = goalFrame.p.v.x();
00516 trajectory.points[pointIndex].velocities[posIndex].linear.y = goalFrame.p.v.y();
00517 trajectory.points[pointIndex].velocities[posIndex].linear.z = goalFrame.p.v.z();
00518 trajectory.points[pointIndex].velocities[posIndex].angular.x = goalFrame.M.w.x();
00519 trajectory.points[pointIndex].velocities[posIndex].angular.y = goalFrame.M.w.y();
00520 trajectory.points[pointIndex].velocities[posIndex].angular.z = goalFrame.M.w.z();
00521 }
00522
00523
00524 if (trajectory.points[pointIndex].accelerations.size() == trajectory.nodes.size())
00525 {
00526 trajectory.points[pointIndex].accelerations[posIndex].linear.x = goalFrame.p.dv.x();
00527 trajectory.points[pointIndex].accelerations[posIndex].linear.y = goalFrame.p.dv.y();
00528 trajectory.points[pointIndex].accelerations[posIndex].linear.z = goalFrame.p.dv.z();
00529 trajectory.points[pointIndex].accelerations[posIndex].angular.x = goalFrame.M.dw.x();
00530 trajectory.points[pointIndex].accelerations[posIndex].angular.y = goalFrame.M.dw.y();
00531 trajectory.points[pointIndex].accelerations[posIndex].angular.z = goalFrame.M.dw.z();
00532 }
00533 }
00534 }
00535 }
00536
00537 if (valid)
00538 {
00539
00540 trajectory.refFrames = std::vector<std::string>(trajectory.nodes.size(), bases[0]);
00541 actionlib_msgs::GoalID goalId;
00542 goalId.stamp = trajectory.header.stamp;
00543 goalId.id = trajectory.header.frame_id;
00544 poseTrajectories[goalId] = trajectory;
00545 sendCartesianWaypoints(trajectory);
00546 }
00547 else
00548 {
00549 std::stringstream ss;
00550 ss << "invalid trajectory";
00551 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::WARN, ss.str());
00552 sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
00553 }
00554 }
00555
00556 void TrajectoryMonitor::addJointWaypoints(const trajectory_msgs::JointTrajectory& trajectory,
00557 const std::vector<r2_msgs::ReplanType>& replan)
00558 {
00559
00560 if (trajectory.joint_names.size() == 0 || trajectory.points.size() == 0)
00561 {
00562 sendJointWaypoints(trajectory, replan);
00563 return;
00564 }
00565
00566
00567 bool valid = true;
00568
00569 for (unsigned int i = 0; i < trajectory.points.size(); ++i)
00570 {
00571 if (trajectory.points[i].positions.size() != trajectory.joint_names.size())
00572 {
00573 valid = false;
00574 break;
00575 }
00576 }
00577
00578 if (valid)
00579 {
00580 actionlib_msgs::GoalID goalId;
00581 goalId.stamp = trajectory.header.stamp;
00582 goalId.id = trajectory.header.frame_id;
00583 jointTrajectories[goalId] = trajectory;
00584 sendJointWaypoints(trajectory, replan);
00585 }
00586 else
00587 {
00588 std::stringstream ss;
00589 ss << "invalid trajectory";
00590 NasaCommonLogging::Logger::log("gov.nasa.controllers.TrajectoryMonitor", log4cpp::Priority::WARN, ss.str());
00591 sendStatusMessage(trajectory.header.stamp, trajectory.header.frame_id, actionlib_msgs::GoalStatus::REJECTED, ss.str(), true);
00592 }
00593 }
00594
00595 void TrajectoryMonitor::sendCartesianWaypoints(const r2_msgs::PoseTrajectory& trajectory,
00596 const r2_msgs::ReplanType::_type_type& replan)
00597 {
00598 r2_msgs::PoseTrajectoryReplan traj;
00599 traj.header.stamp = ros::Time::now();
00600 traj.replan.type = replan;
00601 traj.trajectory = trajectory;
00602 writeCartesianWaypoints(traj);
00603 }
00604
00605 void TrajectoryMonitor::sendJointWaypoints(const trajectory_msgs::JointTrajectory& trajectory,
00606 const std::vector<r2_msgs::ReplanType>& replan)
00607 {
00608 r2_msgs::JointTrajectoryReplan traj;
00609 traj.header.stamp = ros::Time::now();
00610 traj.replan = replan;
00611 traj.trajectory = trajectory;
00612 writeJointWaypoints(traj);
00613 }
00614
00615