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