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 #include <ros/ros.h>
00033 #include <actionlib/server/action_server.h>
00034
00035 #include <nav_msgs/Odometry.h>
00036 #include <trajectory_msgs/JointTrajectory.h>
00037 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00038 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00039 #include <pr2_base_trajectory_action/pr2_base_trajectory_action.h>
00040
00041 #include <sstream>
00042 #include "angles/angles.h"
00043
00044 int main(int argc, char** argv)
00045 {
00046 ros::init(argc, argv, "base_trajectory_action_node");
00047 ros::NodeHandle node;
00048 BaseTrajectoryActionController ac(node);
00049
00050 ros::Rate r(40.0);
00051 while(ros::ok()) {
00052 ros::spinOnce();
00053 ac.update();
00054 r.sleep();
00055 }
00056
00057 return 0;
00058 }
00059
00060 static inline void generatePowers(int n, double x, double* powers)
00061 {
00062 powers[0] = 1.0;
00063 for (int i=1; i<=n; i++)
00064 {
00065 powers[i] = powers[i-1]*x;
00066 }
00067 }
00068
00069 static void getQuinticSplineCoefficients(double start_pos, double start_vel, double start_acc,
00070 double end_pos, double end_vel, double end_acc, double time, std::vector<double>& coefficients)
00071 {
00072 coefficients.resize(6);
00073
00074 if (time == 0.0)
00075 {
00076 coefficients[0] = end_pos;
00077 coefficients[1] = end_vel;
00078 coefficients[2] = 0.5*end_acc;
00079 coefficients[3] = 0.0;
00080 coefficients[4] = 0.0;
00081 coefficients[5] = 0.0;
00082 }
00083 else
00084 {
00085 double T[6];
00086 generatePowers(5, time, T);
00087
00088 coefficients[0] = start_pos;
00089 coefficients[1] = start_vel;
00090 coefficients[2] = 0.5*start_acc;
00091 coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
00092 12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
00093 coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
00094 16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
00095 coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
00096 6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
00097 }
00098 }
00099
00103 static void sampleQuinticSpline(const std::vector<double>& coefficients, double time,
00104 double& position, double& velocity, double& acceleration)
00105 {
00106
00107 double t[6];
00108 generatePowers(5, time, t);
00109
00110 position = t[0]*coefficients[0] +
00111 t[1]*coefficients[1] +
00112 t[2]*coefficients[2] +
00113 t[3]*coefficients[3] +
00114 t[4]*coefficients[4] +
00115 t[5]*coefficients[5];
00116
00117 velocity = t[0]*coefficients[1] +
00118 2.0*t[1]*coefficients[2] +
00119 3.0*t[2]*coefficients[3] +
00120 4.0*t[3]*coefficients[4] +
00121 5.0*t[4]*coefficients[5];
00122
00123 acceleration = 2.0*t[0]*coefficients[2] +
00124 6.0*t[1]*coefficients[3] +
00125 12.0*t[2]*coefficients[4] +
00126 20.0*t[3]*coefficients[5];
00127 }
00128
00129 static void getCubicSplineCoefficients(double start_pos, double start_vel,
00130 double end_pos, double end_vel, double time, std::vector<double>& coefficients)
00131 {
00132 coefficients.resize(4);
00133
00134 if (time == 0.0)
00135 {
00136 coefficients[0] = end_pos;
00137 coefficients[1] = end_vel;
00138 coefficients[2] = 0.0;
00139 coefficients[3] = 0.0;
00140 }
00141 else
00142 {
00143 double T[4];
00144 generatePowers(3, time, T);
00145
00146 coefficients[0] = start_pos;
00147 coefficients[1] = start_vel;
00148 coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
00149 coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
00150 }
00151 }
00152
00153
00154 BaseTrajectoryActionController::BaseTrajectoryActionController(const ros::NodeHandle &nh)
00155 : node_(nh)
00156 {
00157 joints_.push_back(&x_joint_);
00158 joints_.push_back(&y_joint_);
00159 joints_.push_back(&z_joint_);
00160
00161 init();
00162 }
00163
00164 BaseTrajectoryActionController::~BaseTrajectoryActionController()
00165 {
00166 pub_command_.shutdown();
00167 sub_odom_.shutdown();
00168 action_server_.reset();
00169 }
00170
00171 bool BaseTrajectoryActionController::init()
00172 {
00173 using namespace XmlRpc;
00174
00175
00176 std::string ns = std::string("joint_trajectory_action");
00177 node_.param(ns+"/x_joint_name", x_joint_.name, std::string("base_link_x"));
00178 node_.param(ns+"/y_joint_name", y_joint_.name, std::string("base_link_y"));
00179 node_.param(ns+"/rotational_joint_name", z_joint_.name,
00180 std::string("base_link_pan"));
00181
00182 node_.param(ns+"/use_pid", use_pid, true);
00183 node_.param(ns+"/goal_threshold", goal_threshold_, 0.01);
00184 node_.param(ns+"/stopped_velocity_tolerance", stopped_velocity_tolerance_, 0.01);
00185 node_.param(ns+"/goal_time_constraint", goal_time_constraint_, 0.0);
00186
00187 for (size_t i = 0; i < joints_.size(); ++i)
00188 {
00189 node_.param(ns + "/" + joints_[i]->name + "/max_velocity",
00190 joints_[i]->max_abs_velocity_, 0.2);
00191 ROS_INFO("%s %f", ns.c_str(), joints_[i]->max_abs_velocity_);
00192 }
00193
00194
00195 boost::shared_ptr<SpecifiedTrajectory> traj_ptr(new SpecifiedTrajectory(1));
00196 SpecifiedTrajectory &traj = *traj_ptr;
00197 traj[0].start_time = ros::Time::now().toSec();
00198 traj[0].duration = 0.0;
00199 traj[0].splines.resize(joints_.size());
00200 for (size_t j = 0; j < joints_.size(); ++j)
00201 traj[0].splines[j].coef[0] = 0.0;
00202 current_trajectory_ = traj_ptr;
00203
00204 pub_command_ = node_.advertise<geometry_msgs::Twist>("command", 10);
00205 sub_odom_ = node_.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&BaseTrajectoryActionController::odomCB, this, _1));
00206
00207 q.resize(joints_.size());
00208 qd.resize(joints_.size());
00209 qdd.resize(joints_.size());
00210 e.resize(joints_.size());
00211 ed.resize(joints_.size());
00212 edd.resize(joints_.size());
00213
00214 action_server_.reset(new JTAS(node_, "joint_trajectory_action",
00215 boost::bind(&BaseTrajectoryActionController::goalCB, this, _1),
00216 boost::bind(&BaseTrajectoryActionController::cancelCB, this, _1)));
00217
00218 ROS_DEBUG("base traj controller initialized;");
00219
00220 return true;
00221 }
00222
00223 void BaseTrajectoryActionController::starting()
00224 {
00225 last_time_ = ros::Time::now();
00226
00227
00228 boost::shared_ptr<SpecifiedTrajectory> hold_ptr(new SpecifiedTrajectory(1));
00229 SpecifiedTrajectory &hold = *hold_ptr;
00230 hold[0].start_time = last_time_.toSec() - 0.001;
00231 hold[0].duration = 0.0;
00232 hold[0].splines.resize(joints_.size());
00233 for (size_t j = 0; j < joints_.size(); ++j)
00234 hold[0].splines[j].coef[0] = joints_[j]->position_;
00235
00236 current_trajectory_ = hold_ptr;
00237 }
00238
00239 void BaseTrajectoryActionController::update()
00240 {
00241 ros::Time time = ros::Time::now();
00242 ros::Duration dt = time - last_time_;
00243 last_time_ = time;
00244
00245 if(active_goal_ == NULL) return;
00246
00247 boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
00248 traj_ptr = current_trajectory_;
00249 if (!traj_ptr)
00250 ROS_FATAL("The current trajectory can never be null");
00251
00252
00253 const SpecifiedTrajectory &traj = *traj_ptr;
00254
00255
00256
00257
00258 int seg = -1;
00259 while (seg + 1 < (int)traj.size() &&
00260 traj[seg+1].start_time < time.toSec())
00261 {
00262 ++seg;
00263 }
00264
00265 if (seg == -1)
00266 {
00267 if (traj.size() == 0)
00268 ROS_ERROR("No segments in the trajectory");
00269 else
00270 ROS_ERROR("No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
00271 return;
00272 }
00273
00274
00275
00276 for (size_t i = 0; i < q.size(); ++i)
00277 {
00278 sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
00279 time.toSec() - traj[seg].start_time,
00280 q[i], qd[i], qdd[i]);
00281 }
00282
00283
00284 std::vector<double> error(joints_.size());
00285 for (size_t i = 0; i < joints_.size(); ++i)
00286 {
00287
00288 if(robot_time_ == ros::Time(0)) continue;
00289
00290 sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
00291 robot_time_.toSec() - traj[seg].start_time,
00292 e[i], ed[i], edd[i]);
00293 if(joints_[i] == &z_joint_) {
00294 error[i] = angles::shortest_angular_distance(joints_[i]->position_, e[i]);
00295 } else {
00296 error[i] = e[i] - joints_[i]->position_;
00297 }
00298
00299 joints_[i]->commanded_effort_ = error[i] * 1.0;
00300 }
00301
00302
00303
00304
00305 if (traj[seg].gh && traj[seg].gh == active_goal_)
00306 {
00307 ros::Time end_time(traj[seg].start_time + traj[seg].duration);
00308 ROS_DEBUG("time: %f -> end: %f", time.toSec(), end_time.toSec());
00309 ROS_DEBUG("seg = %d, tarj.size() = %ld", seg, traj.size());
00310 if (time <= end_time)
00311 {
00312
00313 for (size_t j = 0; j < joints_.size(); ++j)
00314 {
00315 }
00316 }
00317 else if (seg == (int)traj.size() - 1)
00318 {
00319
00320 bool inside_goal_constraints = true;
00321 for (size_t i = 0; i < joints_.size() && inside_goal_constraints; ++i)
00322 {
00323
00324 if (fabs(qd[i]) < 1e-6)
00325 {
00326 ROS_DEBUG("qd[%ld] = %f", i, qd[i]);
00327 if (fabs(joints_[i]->velocity_) > stopped_velocity_tolerance_) {
00328 ROS_WARN("fabs(joints_[%ld]->velocity_) = %f > %f (stopped_velocity_tolerance_)",
00329 i, joints_[i]->velocity_, stopped_velocity_tolerance_);
00330 inside_goal_constraints = false;
00331 }
00332 }
00333 }
00334
00335 for (size_t i = 0; i < joints_.size() && inside_goal_constraints; ++i)
00336 {
00337 if(fabs(error[i]) > goal_threshold_) {
00338 ROS_WARN("fabs(error[%ld]) = %f > %f (goal_threshold)",
00339 i, fabs(error[i]), goal_threshold_);
00340 inside_goal_constraints = false;
00341 }
00342 }
00343
00344 if (inside_goal_constraints)
00345 {
00346 traj[seg].gh->setSucceeded();
00347 active_goal_ = boost::shared_ptr<GoalHandle>((GoalHandle*)NULL);
00348 ROS_DEBUG("base_traj success");
00349
00350 }
00351 else if (time < end_time + ros::Duration(goal_time_constraint_))
00352 {
00353
00354 }
00355 else
00356 {
00357
00358 traj[seg].gh->setAborted();
00359 active_goal_ = boost::shared_ptr<GoalHandle>((GoalHandle*)NULL);
00360 ROS_WARN("base_traj aborted");
00361 }
00362 }
00363
00364
00365 for (size_t j = 0; j < joints_.size(); ++j) {
00366 joints_[j]->commanded_velocity_ = qd[j] + (use_pid ? joints_[j]->commanded_effort_ : 0);
00367 if(joints_[j]->max_abs_velocity_ < fabs(joints_[j]->commanded_velocity_))
00368 {
00369 joints_[j]->commanded_velocity_ *=
00370 joints_[j]->max_abs_velocity_/fabs(joints_[j]->commanded_velocity_);
00371 ROS_WARN("joint(%s) violates its velocity limit.", joints_[j]->name.c_str());
00372 }
00373 }
00374
00375
00376 geometry_msgs::Twist vel;
00377 double vx,vy,theta;
00378 vx = joints_[0]->commanded_velocity_;
00379 vy = joints_[1]->commanded_velocity_;
00380 theta = joints_[2]->position_;
00381 vel.linear.x = vx * cos(theta) + vy * sin(theta);
00382 vel.linear.y = vy * cos(theta) - vx * sin(theta);
00383 vel.angular.z = joints_[2]->commanded_velocity_;
00384 pub_command_.publish(vel);
00385
00386 ROS_DEBUG("pos = %f %f %f", joints_[0]->position_,joints_[1]->position_,joints_[2]->position_);
00387 ROS_DEBUG("vel = %f %f %f", vel.linear.x, vel.linear.y, vel.angular.z);
00388 }
00389
00390 }
00391
00392 void BaseTrajectoryActionController::commandTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &msg, boost::shared_ptr<GoalHandle> gh)
00393 {
00394 ros::Time time = last_time_ + ros::Duration(0.01);
00395 ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf",
00396 time.toSec(), msg->header.stamp.toSec());
00397
00398 boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
00399 SpecifiedTrajectory &new_traj = *new_traj_ptr;
00400
00401
00402
00403 if (msg->points.empty())
00404 {
00405 starting();
00406 return;
00407 }
00408
00409
00410
00411 std::vector<int> lookup(joints_.size(), -1);
00412 for (size_t j = 0; j < joints_.size(); ++j)
00413 {
00414 for (size_t k = 0; k < msg->joint_names.size(); ++k)
00415 {
00416 if (msg->joint_names[k] == joints_[j]->name)
00417 {
00418 lookup[j] = k;
00419 break;
00420 }
00421 }
00422
00423 if (lookup[j] == -1)
00424 {
00425 ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j]->name.c_str());
00426 return;
00427 }
00428 }
00429
00430
00431
00432 boost::shared_ptr<const SpecifiedTrajectory> prev_traj_ptr;
00433 prev_traj_ptr = current_trajectory_;
00434 if (!prev_traj_ptr)
00435 {
00436 ROS_FATAL("The current trajectory can never be null");
00437 return;
00438 }
00439 const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;
00440
00441
00442
00443
00444 int first_useful = -1;
00445 while (first_useful + 1 < (int)prev_traj.size() &&
00446 prev_traj[first_useful + 1].start_time <= time.toSec())
00447 {
00448 ++first_useful;
00449 }
00450
00451
00452 int last_useful = -1;
00453 double msg_start_time;
00454 if (msg->header.stamp == ros::Time(0.0))
00455 msg_start_time = time.toSec();
00456 else
00457 msg_start_time = msg->header.stamp.toSec();
00458
00459
00460
00461
00462
00463 while (last_useful + 1 < (int)prev_traj.size() &&
00464 prev_traj[last_useful + 1].start_time < msg_start_time)
00465 {
00466 ++last_useful;
00467 }
00468
00469 if (last_useful < first_useful)
00470 first_useful = last_useful;
00471
00472
00473 for (int i = std::max(first_useful,0); i <= last_useful; ++i)
00474 {
00475 new_traj.push_back(prev_traj[i]);
00476 }
00477
00478
00479
00480 if (new_traj.size() == 0)
00481 new_traj.push_back(prev_traj[prev_traj.size() - 1]);
00482
00483
00484
00485
00486 Segment &last = new_traj[new_traj.size() - 1];
00487 std::vector<double> prev_positions(joints_.size());
00488 std::vector<double> prev_velocities(joints_.size());
00489 std::vector<double> prev_accelerations(joints_.size());
00490
00491 double t = (msg->header.stamp == ros::Time(0.0) ? time.toSec() : msg->header.stamp.toSec())
00492 - last.start_time;
00493 ROS_DEBUG("Initial conditions at %.3f for new set of splines:", t);
00494 for (size_t i = 0; i < joints_.size(); ++i)
00495 {
00496 sampleSplineWithTimeBounds(last.splines[i].coef, last.duration,
00497 t,
00498 prev_positions[i], prev_velocities[i], prev_accelerations[i]);
00499 ROS_DEBUG(" %.2lf, %.2lf, %.2lf (%s)", prev_positions[i], prev_velocities[i],
00500 prev_accelerations[i], joints_[i]->name.c_str());
00501 }
00502
00503
00504
00505 std::vector<double> positions;
00506 std::vector<double> velocities;
00507 std::vector<double> accelerations;
00508
00509 std::vector<double> durations(msg->points.size());
00510 if (msg->points.size() > 0)
00511 durations[0] = msg->points[0].time_from_start.toSec();
00512 for (size_t i = 1; i < msg->points.size(); ++i)
00513 durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec();
00514
00515
00516 std::vector<double> wrap(joints_.size(), 0.0);
00517 if (msg->points[0].positions.empty()) {
00518 ROS_ERROR("First point of trajectory has no positions");
00519 return;
00520 }
00521 ROS_DEBUG("wrap:");
00522 for (size_t j = 0; j < joints_.size(); ++j)
00523 {
00524 if (joints_[j] == &z_joint_)
00525 {
00526 double dist = angles::shortest_angular_distance(prev_positions[j], msg->points[0].positions[lookup[j]]);
00527 wrap[j] = (prev_positions[j] + dist) - msg->points[0].positions[lookup[j]];
00528 ROS_DEBUG(" %.2lf - %s bc dist(%.2lf, %.2lf) = %.2lf", wrap[j], joints_[j]->name.c_str(),
00529 prev_positions[j], msg->points[0].positions[lookup[j]], dist);
00530 }
00531 }
00532
00533 for (size_t i = 0; i < msg->points.size(); ++i)
00534 {
00535 Segment seg;
00536
00537 if (msg->header.stamp == ros::Time(0.0))
00538 seg.start_time = (time + msg->points[i].time_from_start).toSec() - durations[i];
00539 else
00540 seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i];
00541 seg.duration = durations[i];
00542 seg.gh = gh;
00543 seg.splines.resize(joints_.size());
00544
00545
00546
00547 if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joints_.size())
00548 {
00549 ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size());
00550 return;
00551 }
00552 if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joints_.size())
00553 {
00554 ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size());
00555 return;
00556 }
00557 if (msg->points[i].positions.size() != joints_.size())
00558 {
00559 ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size());
00560 return;
00561 }
00562
00563
00564
00565 accelerations.resize(msg->points[i].accelerations.size());
00566 velocities.resize(msg->points[i].velocities.size());
00567 positions.resize(msg->points[i].positions.size());
00568 for (size_t j = 0; j < joints_.size(); ++j)
00569 {
00570 if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[lookup[j]];
00571 if (!velocities.empty()) velocities[j] = msg->points[i].velocities[lookup[j]];
00572 if (!positions.empty()) positions[j] = msg->points[i].positions[lookup[j]] + wrap[j];
00573 }
00574
00575
00576
00577 for (size_t j = 0; j < joints_.size(); ++j)
00578 {
00579 if (prev_accelerations.size() > 0 && accelerations.size() > 0)
00580 {
00581 getQuinticSplineCoefficients(
00582 prev_positions[j], prev_velocities[j], prev_accelerations[j],
00583 positions[j], velocities[j], accelerations[j],
00584 durations[i],
00585 seg.splines[j].coef);
00586 }
00587 else if (prev_velocities.size() > 0 && velocities.size() > 0)
00588 {
00589 getCubicSplineCoefficients(
00590 prev_positions[j], prev_velocities[j],
00591 positions[j], velocities[j],
00592 durations[i],
00593 seg.splines[j].coef);
00594 seg.splines[j].coef.resize(6, 0.0);
00595 }
00596 else
00597 {
00598 seg.splines[j].coef[0] = prev_positions[j];
00599 if (durations[i] == 0.0)
00600 seg.splines[j].coef[1] = 0.0;
00601 else
00602 seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
00603 seg.splines[j].coef[2] = 0.0;
00604 seg.splines[j].coef[3] = 0.0;
00605 seg.splines[j].coef[4] = 0.0;
00606 seg.splines[j].coef[5] = 0.0;
00607 }
00608 }
00609
00610
00611
00612 new_traj.push_back(seg);
00613
00614
00615
00616 prev_positions = positions;
00617 prev_velocities = velocities;
00618 prev_accelerations = accelerations;
00619 }
00620
00621
00622
00623
00624
00625 if (!new_traj_ptr)
00626 {
00627 ROS_ERROR("The new trajectory was null!");
00628 return;
00629 }
00630
00631 current_trajectory_ = new_traj_ptr;
00632 ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size());
00633 #if 0
00634 for (size_t i = 0; i < std::min((size_t)20,new_traj.size()); ++i)
00635 {
00636 ROS_DEBUG("Segment %2ld: %.3lf for %.3lf", i, new_traj[i].start_time, new_traj[i].duration);
00637 for (size_t j = 0; j < new_traj[i].splines.size(); ++j)
00638 {
00639 ROS_DEBUG(" %.2lf %.2lf %.2lf %.2lf , %.2lf %.2lf(%s)",
00640 new_traj[i].splines[j].coef[0],
00641 new_traj[i].splines[j].coef[1],
00642 new_traj[i].splines[j].coef[2],
00643 new_traj[i].splines[j].coef[3],
00644 new_traj[i].splines[j].coef[4],
00645 new_traj[i].splines[j].coef[5],
00646 joints_[j]->name.c_str());
00647 }
00648 }
00649 #endif
00650 }
00651
00652 void BaseTrajectoryActionController::odomCB(const nav_msgs::Odometry::ConstPtr& msg)
00653 {
00654 robot_time_ = msg->header.stamp;
00655 x_joint_.position_ = msg->pose.pose.position.x;
00656 y_joint_.position_ = msg->pose.pose.position.y;
00657 z_joint_.position_ = atan2(msg->pose.pose.orientation.z,
00658 msg->pose.pose.orientation.w) * 2;
00659
00660
00661
00662 }
00663
00664 void BaseTrajectoryActionController::sampleSplineWithTimeBounds(
00665 const std::vector<double>& coefficients, double duration, double time,
00666 double& position, double& velocity, double& acceleration)
00667 {
00668 if (time < 0)
00669 {
00670 double _;
00671 sampleQuinticSpline(coefficients, 0.0, position, _, _);
00672 velocity = 0;
00673 acceleration = 0;
00674 }
00675 else if (time > duration)
00676 {
00677 double _;
00678 sampleQuinticSpline(coefficients, duration, position, _, _);
00679 velocity = 0;
00680 acceleration = 0;
00681 }
00682 else
00683 {
00684 sampleQuinticSpline(coefficients, time,
00685 position, velocity, acceleration);
00686 }
00687 }
00688
00689 static bool setsEqual(const std::vector<std::string> &a, const std::vector<std::string> &b)
00690 {
00691 if (a.size() != b.size())
00692 return false;
00693
00694 for (size_t i = 0; i < a.size(); ++i)
00695 {
00696 if (count(b.begin(), b.end(), a[i]) != 1)
00697 return false;
00698 }
00699 for (size_t i = 0; i < b.size(); ++i)
00700 {
00701 if (count(a.begin(), a.end(), b[i]) != 1)
00702 return false;
00703 }
00704
00705 return true;
00706 }
00707
00708 template <class Enclosure, class Member>
00709 static boost::shared_ptr<Member> share_member(boost::shared_ptr<Enclosure> enclosure, Member &member)
00710 {
00711 actionlib::EnclosureDeleter<Enclosure> d(enclosure);
00712 boost::shared_ptr<Member> p(&member, d);
00713 return p;
00714 }
00715
00716
00717 void BaseTrajectoryActionController::goalCB(GoalHandle gh)
00718 {
00719 ROS_DEBUG("goalCB");
00720
00721 std::vector<std::string> joint_names(joints_.size());
00722 for (size_t j = 0; j < joints_.size(); ++j)
00723 joint_names[j] = joints_[j]->name;
00724
00725
00726 if (!setsEqual(joint_names, gh.getGoal()->trajectory.joint_names))
00727 {
00728 ROS_ERROR("Joints on incoming goal don't match our joints");
00729 gh.setRejected();
00730 return;
00731 }
00732
00733
00734 if (active_goal_)
00735 {
00736
00737 active_goal_->setCanceled();
00738 }
00739
00740 starting();
00741
00742 gh.setAccepted();
00743
00744
00745 active_goal_ = boost::shared_ptr<GoalHandle>(new GoalHandle(gh));
00746 commandTrajectory(share_member(gh.getGoal(),gh.getGoal()->trajectory), active_goal_);
00747 }
00748
00749 void BaseTrajectoryActionController::cancelCB(GoalHandle gh)
00750 {
00751 if (active_goal_ && *active_goal_ == gh)
00752 {
00753 trajectory_msgs::JointTrajectory::Ptr empty(new trajectory_msgs::JointTrajectory);
00754 empty->joint_names.resize(joints_.size());
00755 for (size_t j = 0; j < joints_.size(); ++j)
00756 empty->joint_names[j] = joints_[j]->name;
00757 commandTrajectory(empty);
00758
00759
00760 active_goal_->setCanceled();
00761 active_goal_ = boost::shared_ptr<GoalHandle>((GoalHandle*)NULL);
00762 }
00763 }