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
00028
00030
00031 #ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_IMP_H
00032 #define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_IMP_H
00033
00034
00035 namespace joint_trajectory_controller
00036 {
00037
00038 namespace internal
00039 {
00040
00041 template <class Enclosure, class Member>
00042 inline boost::shared_ptr<Member> share_member(boost::shared_ptr<Enclosure> enclosure, Member &member)
00043 {
00044 actionlib::EnclosureDeleter<Enclosure> d(enclosure);
00045 boost::shared_ptr<Member> p(&member, d);
00046 return p;
00047 }
00048
00049 std::vector<std::string> getStrings(const ros::NodeHandle& nh, const std::string& param_name)
00050 {
00051 using namespace XmlRpc;
00052 XmlRpcValue xml_array;
00053 if (!nh.getParam(param_name, xml_array))
00054 {
00055 ROS_ERROR_STREAM("Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
00056 return std::vector<std::string>();
00057 }
00058 if (xml_array.getType() != XmlRpcValue::TypeArray)
00059 {
00060 ROS_ERROR_STREAM("The '" << param_name << "' parameter is not an array (namespace: " <<
00061 nh.getNamespace() << ").");
00062 return std::vector<std::string>();
00063 }
00064
00065 std::vector<std::string> out;
00066 for (int i = 0; i < xml_array.size(); ++i)
00067 {
00068 if (xml_array[i].getType() != XmlRpcValue::TypeString)
00069 {
00070 ROS_ERROR_STREAM("The '" << param_name << "' parameter contains a non-string element (namespace: " <<
00071 nh.getNamespace() << ").");
00072 return std::vector<std::string>();
00073 }
00074 out.push_back(static_cast<std::string>(xml_array[i]));
00075 }
00076 return out;
00077 }
00078
00079 boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
00080 {
00081 boost::shared_ptr<urdf::Model> urdf(new urdf::Model);
00082
00083 std::string urdf_str;
00084
00085 if (nh.getParam(param_name, urdf_str))
00086 {
00087 if (!urdf->initString(urdf_str))
00088 {
00089 ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
00090 nh.getNamespace() << ").");
00091 return boost::shared_ptr<urdf::Model>();
00092 }
00093 }
00094
00095 else if (!urdf->initParam("robot_description"))
00096 {
00097 ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter");
00098 return boost::shared_ptr<urdf::Model>();
00099 }
00100 return urdf;
00101 }
00102
00103 typedef boost::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
00104 std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
00105 {
00106 std::vector<UrdfJointConstPtr> out;
00107 for (unsigned int i = 0; i < joint_names.size(); ++i)
00108 {
00109 UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
00110 if (urdf_joint)
00111 {
00112 out.push_back(urdf_joint);
00113 }
00114 else
00115 {
00116 ROS_ERROR_STREAM("Could not find joint '" << joint_names[i] << "' in URDF model.");
00117 return std::vector<UrdfJointConstPtr>();
00118 }
00119 }
00120 return out;
00121 }
00122
00123 std::string getLeafNamespace(const ros::NodeHandle& nh)
00124 {
00125 const std::string complete_ns = nh.getNamespace();
00126 std::size_t id = complete_ns.find_last_of("/");
00127 return complete_ns.substr(id + 1);
00128 }
00129
00130 }
00131
00132 template <class SegmentImpl, class HardwareInterface>
00133 inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
00134 starting(const ros::Time& time)
00135 {
00136
00137 TimeData time_data;
00138 time_data.time = time;
00139 time_data.uptime = ros::Time(0.0);
00140 time_data_.initRT(time_data);
00141
00142
00143 setHoldPosition(time_data.uptime);
00144
00145
00146 last_state_publish_time_ = time_data.uptime;
00147
00148
00149 hw_iface_adapter_.starting(time_data.uptime);
00150 }
00151
00152 template <class SegmentImpl, class HardwareInterface>
00153 inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
00154 stopping(const ros::Time& time)
00155 {
00156 preemptActiveGoal();
00157 }
00158
00159 template <class SegmentImpl, class HardwareInterface>
00160 inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
00161 trajectoryCommandCB(const JointTrajectoryConstPtr& msg)
00162 {
00163 const bool update_ok = updateTrajectoryCommand(msg, RealtimeGoalHandlePtr());
00164 if (update_ok) {preemptActiveGoal();}
00165 }
00166
00167 template <class SegmentImpl, class HardwareInterface>
00168 inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
00169 preemptActiveGoal()
00170 {
00171 RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
00172
00173
00174 if (current_active_goal)
00175 {
00176
00177 rt_active_goal_.reset();
00178 current_active_goal->gh_.setCanceled();
00179 }
00180 }
00181
00182 template <class SegmentImpl, class HardwareInterface>
00183 inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
00184 checkPathTolerances(const typename Segment::State& state_error,
00185 const Segment& segment)
00186 {
00187 const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle();
00188 const SegmentTolerances<Scalar>& tolerances = segment.getTolerances();
00189 if (!checkStateTolerance(state_error, tolerances.state_tolerance))
00190 {
00191 rt_segment_goal->preallocated_result_->error_code =
00192 control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
00193 rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
00194 rt_active_goal_.reset();
00195 }
00196 }
00197
00198 template <class SegmentImpl, class HardwareInterface>
00199 inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
00200 checkGoalTolerances(const typename Segment::State& state_error,
00201 const Segment& segment)
00202 {
00203
00204 const ros::Time uptime = time_data_.readFromRT()->uptime;
00205
00206
00207 const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle();
00208 const SegmentTolerances<Scalar>& tolerances = segment.getTolerances();
00209 const bool inside_goal_tolerances = checkStateTolerance(state_error, tolerances.goal_state_tolerance);
00210
00211 if (inside_goal_tolerances)
00212 {
00213 rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
00214 rt_segment_goal->setSucceeded(rt_segment_goal->preallocated_result_);
00215 rt_active_goal_.reset();
00216 }
00217 else if (uptime.toSec() < segment.endTime() + tolerances.goal_time_tolerance)
00218 {
00219
00220 }
00221 else
00222 {
00223 if (verbose_)
00224 {
00225 ROS_ERROR_STREAM_NAMED(name_,"Goal tolerances failed");
00226
00227 checkStateTolerance(state_error, tolerances.goal_state_tolerance, true);
00228 }
00229
00230 rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
00231 rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
00232 rt_active_goal_.reset();
00233 }
00234 }
00235
00236 template <class SegmentImpl, class HardwareInterface>
00237 JointTrajectoryController<SegmentImpl, HardwareInterface>::
00238 JointTrajectoryController()
00239 : verbose_(false),
00240 hold_trajectory_ptr_(new Trajectory)
00241 {}
00242
00243 template <class SegmentImpl, class HardwareInterface>
00244 bool JointTrajectoryController<SegmentImpl, HardwareInterface>::init(HardwareInterface* hw,
00245 ros::NodeHandle& root_nh,
00246 ros::NodeHandle& controller_nh)
00247 {
00248 using namespace internal;
00249
00250
00251 controller_nh_ = controller_nh;
00252
00253
00254 name_ = getLeafNamespace(controller_nh_);
00255
00256
00257 double state_publish_rate = 50.0;
00258 controller_nh_.getParam("state_publish_rate", state_publish_rate);
00259 ROS_DEBUG_STREAM_NAMED(name_, "Controller state will be published at " << state_publish_rate << "Hz.");
00260 state_publisher_period_ = ros::Duration(1.0 / state_publish_rate);
00261
00262
00263 double action_monitor_rate = 20.0;
00264 controller_nh_.getParam("action_monitor_rate", action_monitor_rate);
00265 action_monitor_period_ = ros::Duration(1.0 / action_monitor_rate);
00266 ROS_DEBUG_STREAM_NAMED(name_, "Action status changes will be monitored at " << action_monitor_rate << "Hz.");
00267
00268
00269 stop_trajectory_duration_ = 0.0;
00270 if (!controller_nh_.getParam("stop_trajectory_duration", stop_trajectory_duration_))
00271 {
00272
00273 if (controller_nh_.getParam("hold_trajectory_duration", stop_trajectory_duration_))
00274 {
00275 ROS_WARN("The 'hold_trajectory_duration' has been deprecated in favor of the 'stop_trajectory_duration' parameter. Please update your controller configuration.");
00276 }
00277 }
00278 ROS_DEBUG_STREAM_NAMED(name_, "Stop trajectory has a duration of " << stop_trajectory_duration_ << "s.");
00279
00280
00281 joint_names_ = getStrings(controller_nh_, "joints");
00282 if (joint_names_.empty()) {return false;}
00283 const unsigned int n_joints = joint_names_.size();
00284
00285
00286 boost::shared_ptr<urdf::Model> urdf = getUrdf(root_nh, "robot_description");
00287 if (!urdf) {return false;}
00288
00289 std::vector<UrdfJointConstPtr> urdf_joints = getUrdfJoints(*urdf, joint_names_);
00290 if (urdf_joints.empty()) {return false;}
00291 assert(n_joints == urdf_joints.size());
00292
00293
00294 joints_.resize(n_joints);
00295 angle_wraparound_.resize(n_joints);
00296 for (unsigned int i = 0; i < n_joints; ++i)
00297 {
00298
00299 try {joints_[i] = hw->getHandle(joint_names_[i]);}
00300 catch (...)
00301 {
00302 ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << joint_names_[i] << "' in '" <<
00303 this->getHardwareInterfaceType() << "'.");
00304 return false;
00305 }
00306
00307
00308 angle_wraparound_[i] = urdf_joints[i]->type == urdf::Joint::CONTINUOUS;
00309 const std::string not_if = angle_wraparound_[i] ? "" : "non-";
00310
00311 ROS_DEBUG_STREAM_NAMED(name_, "Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" <<
00312 this->getHardwareInterfaceType() << "'.");
00313 }
00314
00315 assert(joints_.size() == angle_wraparound_.size());
00316 ROS_DEBUG_STREAM_NAMED(name_, "Initialized controller '" << name_ << "' with:" <<
00317 "\n- Number of joints: " << joints_.size() <<
00318 "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'" <<
00319 "\n- Trajectory segment type: '" << hardware_interface::internal::demangledTypeName<SegmentImpl>() << "'");
00320
00321
00322 ros::NodeHandle tol_nh(controller_nh_, "constraints");
00323 default_tolerances_ = getSegmentTolerances<Scalar>(tol_nh, joint_names_);
00324
00325
00326 hw_iface_adapter_.init(joints_, controller_nh_);
00327
00328
00329 trajectory_command_sub_ = controller_nh_.subscribe("command", 1, &JointTrajectoryController::trajectoryCommandCB, this);
00330
00331
00332 state_publisher_.reset(new StatePublisher(controller_nh_, "state", 1));
00333
00334
00335 action_server_.reset(new ActionServer(controller_nh_, "follow_joint_trajectory",
00336 boost::bind(&JointTrajectoryController::goalCB, this, _1),
00337 boost::bind(&JointTrajectoryController::cancelCB, this, _1),
00338 false));
00339 action_server_->start();
00340
00341
00342 query_state_service_ = controller_nh_.advertiseService("query_state",
00343 &JointTrajectoryController::queryStateService,
00344 this);
00345
00346
00347 current_state_ = typename Segment::State(n_joints);
00348 desired_state_ = typename Segment::State(n_joints);
00349 state_error_ = typename Segment::State(n_joints);
00350 hold_start_state_ = typename Segment::State(n_joints);
00351 hold_end_state_ = typename Segment::State(n_joints);
00352
00353 Segment hold_segment(0.0, current_state_, 0.0, current_state_);
00354 hold_trajectory_ptr_->resize(1, hold_segment);
00355
00356 {
00357 state_publisher_->lock();
00358 state_publisher_->msg_.joint_names = joint_names_;
00359 state_publisher_->msg_.desired.positions.resize(n_joints);
00360 state_publisher_->msg_.desired.velocities.resize(n_joints);
00361 state_publisher_->msg_.desired.accelerations.resize(n_joints);
00362 state_publisher_->msg_.actual.positions.resize(n_joints);
00363 state_publisher_->msg_.actual.velocities.resize(n_joints);
00364 state_publisher_->msg_.error.positions.resize(n_joints);
00365 state_publisher_->msg_.error.velocities.resize(n_joints);
00366 state_publisher_->unlock();
00367 }
00368
00369 return true;
00370 }
00371
00372 template <class SegmentImpl, class HardwareInterface>
00373 void JointTrajectoryController<SegmentImpl, HardwareInterface>::
00374 update(const ros::Time& time, const ros::Duration& period)
00375 {
00376
00377 TrajectoryPtr curr_traj_ptr;
00378 curr_trajectory_box_.get(curr_traj_ptr);
00379 Trajectory& curr_traj = *curr_traj_ptr;
00380
00381
00382 TimeData time_data;
00383 time_data.time = time;
00384 time_data.period = period;
00385 time_data.uptime = time_data_.readFromRT()->uptime + period;
00386 time_data_.writeFromNonRT(time_data);
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397 typename Trajectory::const_iterator segment_it = sample(curr_traj, time_data.uptime.toSec(), desired_state_);
00398 if (curr_traj.end() == segment_it)
00399 {
00400
00401 ROS_ERROR_NAMED(name_,
00402 "Unexpected error: No trajectory defined at current time. Please contact the package maintainer.");
00403 return;
00404 }
00405
00406
00407 for (unsigned int i = 0; i < joints_.size(); ++i)
00408 {
00409 current_state_.position[i] = joints_[i].getPosition();
00410 current_state_.velocity[i] = joints_[i].getVelocity();
00411
00412
00413 state_error_.position[i] = desired_state_.position[i] - current_state_.position[i];
00414 state_error_.velocity[i] = desired_state_.velocity[i] - current_state_.velocity[i];
00415 state_error_.acceleration[i] = 0.0;
00416 }
00417
00418
00419 const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle();
00420 if (rt_segment_goal && rt_segment_goal == rt_active_goal_)
00421 {
00422
00423 if (time_data.uptime.toSec() < segment_it->endTime())
00424 {
00425
00426 checkPathTolerances(state_error_,
00427 *segment_it);
00428 }
00429 else if (segment_it == --curr_traj.end())
00430 {
00431 if (verbose_)
00432 ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segement, checking goal tolerances");
00433
00434
00435 checkGoalTolerances(state_error_,
00436 *segment_it);
00437 }
00438 }
00439
00440
00441 hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period,
00442 desired_state_, state_error_);
00443
00444
00445 publishState(time_data.uptime);
00446 }
00447
00448 template <class SegmentImpl, class HardwareInterface>
00449 bool JointTrajectoryController<SegmentImpl, HardwareInterface>::
00450 updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh)
00451 {
00452 typedef InitJointTrajectoryOptions<Trajectory> Options;
00453
00454
00455 if (!this->isRunning())
00456 {
00457 ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
00458 return false;
00459 }
00460
00461 if (!msg)
00462 {
00463 ROS_WARN_NAMED(name_, "Received null-pointer trajectory message, skipping.");
00464 return false;
00465 }
00466
00467
00468 TimeData* time_data = time_data_.readFromRT();
00469
00470
00471 const ros::Time next_update_time = time_data->time + time_data->period;
00472
00473
00474 ros::Time next_update_uptime = time_data->uptime + time_data->period;
00475
00476
00477 if (msg->points.empty())
00478 {
00479 setHoldPosition(time_data->uptime);
00480 ROS_DEBUG_NAMED(name_, "Empty trajectory command, stopping.");
00481 return true;
00482 }
00483
00484
00485 TrajectoryPtr curr_traj_ptr;
00486 curr_trajectory_box_.get(curr_traj_ptr);
00487
00488 Options options;
00489 options.other_time_base = &next_update_uptime;
00490 options.current_trajectory = curr_traj_ptr.get();
00491 options.joint_names = &joint_names_;
00492 options.angle_wraparound = &angle_wraparound_;
00493 options.rt_goal_handle = gh;
00494 options.default_tolerances = &default_tolerances_;
00495
00496
00497 try
00498 {
00499 TrajectoryPtr traj_ptr(new Trajectory);
00500 *traj_ptr = initJointTrajectory<Trajectory>(*msg, next_update_time, options);
00501 if (!traj_ptr->empty())
00502 {
00503 curr_trajectory_box_.set(traj_ptr);
00504 }
00505 else
00506 {
00507
00508 return false;
00509 }
00510 }
00511 catch(const std::invalid_argument& ex)
00512 {
00513 ROS_ERROR_STREAM_NAMED(name_, ex.what());
00514 return false;
00515 }
00516 catch(...)
00517 {
00518 ROS_ERROR_NAMED(name_, "Unexpected exception caught when initializing trajectory from ROS message data.");
00519 return false;
00520 }
00521
00522 return true;
00523 }
00524
00525 template <class SegmentImpl, class HardwareInterface>
00526 void JointTrajectoryController<SegmentImpl, HardwareInterface>::
00527 goalCB(GoalHandle gh)
00528 {
00529 ROS_DEBUG_STREAM_NAMED(name_,"Recieved new action goal");
00530
00531
00532 if (!this->isRunning())
00533 {
00534 ROS_ERROR_NAMED(name_, "Can't accept new action goals. Controller is not running.");
00535 control_msgs::FollowJointTrajectoryResult result;
00536 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
00537 gh.setRejected(result);
00538 return;
00539 }
00540
00541
00542 using internal::permutation;
00543 std::vector<unsigned int> permutation_vector = permutation(joint_names_, gh.getGoal()->trajectory.joint_names);
00544
00545 if (permutation_vector.empty())
00546 {
00547 ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints.");
00548 control_msgs::FollowJointTrajectoryResult result;
00549 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
00550 gh.setRejected(result);
00551 return;
00552 }
00553
00554
00555 RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
00556 const bool update_ok = updateTrajectoryCommand(internal::share_member(gh.getGoal(), gh.getGoal()->trajectory),
00557 rt_goal);
00558
00559 if (update_ok)
00560 {
00561
00562 preemptActiveGoal();
00563 gh.setAccepted();
00564 rt_active_goal_ = rt_goal;
00565
00566
00567 goal_handle_timer_ = controller_nh_.createTimer(action_monitor_period_,
00568 &RealtimeGoalHandle::runNonRealtime,
00569 rt_goal);
00570 goal_handle_timer_.start();
00571 }
00572 else
00573 {
00574
00575 control_msgs::FollowJointTrajectoryResult result;
00576 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
00577 gh.setRejected(result);
00578 }
00579 }
00580
00581 template <class SegmentImpl, class HardwareInterface>
00582 void JointTrajectoryController<SegmentImpl, HardwareInterface>::
00583 cancelCB(GoalHandle gh)
00584 {
00585 RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
00586
00587
00588 if (current_active_goal && current_active_goal->gh_ == gh)
00589 {
00590
00591 rt_active_goal_.reset();
00592
00593
00594 const ros::Time uptime = time_data_.readFromRT()->uptime;
00595
00596
00597 setHoldPosition(uptime);
00598 ROS_DEBUG_NAMED(name_, "Canceling active action goal because cancel callback recieved from actionlib.");
00599
00600
00601 current_active_goal->gh_.setCanceled();
00602 }
00603 }
00604
00605 template <class SegmentImpl, class HardwareInterface>
00606 bool JointTrajectoryController<SegmentImpl, HardwareInterface>::
00607 queryStateService(control_msgs::QueryTrajectoryState::Request& req,
00608 control_msgs::QueryTrajectoryState::Response& resp)
00609 {
00610
00611 if (!this->isRunning())
00612 {
00613 ROS_ERROR_NAMED(name_, "Can't sample trajectory. Controller is not running.");
00614 return false;
00615 }
00616
00617
00618 TimeData* time_data = time_data_.readFromRT();
00619 const ros::Duration time_offset = req.time - time_data->time;
00620 const ros::Time sample_time = time_data->uptime + time_offset;
00621
00622
00623 TrajectoryPtr curr_traj_ptr;
00624 curr_trajectory_box_.get(curr_traj_ptr);
00625 Trajectory& curr_traj = *curr_traj_ptr;
00626
00627 typename Segment::State state;
00628 typename Trajectory::const_iterator segment_it = sample(curr_traj, sample_time.toSec(), state);
00629 if (curr_traj.end() == segment_it)
00630 {
00631 ROS_ERROR_STREAM_NAMED(name_, "Requested sample time preceeds trajectory start time.");
00632 return false;
00633 }
00634
00635
00636 resp.name = joint_names_;
00637 resp.position = state.position;
00638 resp.velocity = state.velocity;
00639 resp.acceleration = state.acceleration;
00640
00641 return true;
00642 }
00643
00644 template <class SegmentImpl, class HardwareInterface>
00645 void JointTrajectoryController<SegmentImpl, HardwareInterface>::
00646 publishState(const ros::Time& time)
00647 {
00648
00649 if (!state_publisher_period_.isZero() && last_state_publish_time_ + state_publisher_period_ < time)
00650 {
00651 if (state_publisher_ && state_publisher_->trylock())
00652 {
00653 last_state_publish_time_ += state_publisher_period_;
00654
00655 state_publisher_->msg_.header.stamp = time_data_.readFromRT()->time;
00656 state_publisher_->msg_.desired.positions = desired_state_.position;
00657 state_publisher_->msg_.desired.velocities = desired_state_.velocity;
00658 state_publisher_->msg_.desired.accelerations = desired_state_.acceleration;
00659 state_publisher_->msg_.actual.positions = current_state_.position;
00660 state_publisher_->msg_.actual.velocities = current_state_.velocity;
00661 state_publisher_->msg_.error.positions = state_error_.position;
00662 state_publisher_->msg_.error.velocities = state_error_.velocity;
00663
00664 state_publisher_->unlockAndPublish();
00665 }
00666 }
00667 }
00668
00669 template <class SegmentImpl, class HardwareInterface>
00670 void JointTrajectoryController<SegmentImpl, HardwareInterface>::
00671 setHoldPosition(const ros::Time& time)
00672 {
00673
00674
00675
00676
00677
00678
00679 assert(1 == hold_trajectory_ptr_->size());
00680
00681 const typename Segment::Time start_time = time.toSec();
00682 const typename Segment::Time end_time = time.toSec() + stop_trajectory_duration_;
00683 const typename Segment::Time end_time_2x = time.toSec() + 2.0 * stop_trajectory_duration_;
00684
00685
00686 const unsigned int n_joints = joints_.size();
00687 for (unsigned int i = 0; i < n_joints; ++i)
00688 {
00689 hold_start_state_.position[i] = joints_[i].getPosition();
00690 hold_start_state_.velocity[i] = joints_[i].getVelocity();
00691 hold_start_state_.acceleration[i] = 0.0;
00692
00693 hold_end_state_.position[i] = joints_[i].getPosition();
00694 hold_end_state_.velocity[i] = -joints_[i].getVelocity();
00695 hold_end_state_.acceleration[i] = 0.0;
00696 }
00697 hold_trajectory_ptr_->front().init(start_time, hold_start_state_,
00698 end_time_2x, hold_end_state_);
00699
00700
00701 hold_trajectory_ptr_->front().sample(end_time, hold_end_state_);
00702
00703
00704 hold_trajectory_ptr_->front().init(start_time, hold_start_state_,
00705 end_time, hold_end_state_);
00706
00707 curr_trajectory_box_.set(hold_trajectory_ptr_);
00708 }
00709
00710 }
00711
00712 #endif // header guard