40 template <
class Enclosure,
class Member>
52 if (!nh.
getParam(param_name, xml_array))
55 return std::vector<std::string>();
57 if (xml_array.
getType() != XmlRpcValue::TypeArray)
59 ROS_ERROR_STREAM(
"The '" << param_name <<
"' parameter is not an array (namespace: " <<
61 return std::vector<std::string>();
64 std::vector<std::string> out;
65 for (
int i = 0; i < xml_array.
size(); ++i)
68 if (elem.
getType() != XmlRpcValue::TypeString)
70 ROS_ERROR_STREAM(
"The '" << param_name <<
"' parameter contains a non-string element (namespace: " <<
72 return std::vector<std::string>();
74 out.push_back(
static_cast<std::string
>(elem));
85 if (nh.
getParam(param_name, urdf_str))
87 if (!
urdf->initString(urdf_str))
89 ROS_ERROR_STREAM(
"Failed to parse URDF contained in '" << param_name <<
"' parameter (namespace: " <<
91 return urdf::ModelSharedPtr();
95 else if (!
urdf->initParam(
"robot_description"))
97 ROS_ERROR_STREAM(
"Failed to parse URDF contained in '" << param_name <<
"' parameter");
98 return urdf::ModelSharedPtr();
105 std::vector<urdf::JointConstSharedPtr> out;
106 for (
const auto& joint_name : joint_names)
108 urdf::JointConstSharedPtr urdf_joint =
urdf.getJoint(joint_name);
111 out.push_back(urdf_joint);
115 ROS_ERROR_STREAM(
"Could not find joint '" << joint_name <<
"' in URDF model.");
116 return std::vector<urdf::JointConstSharedPtr>();
125 std::size_t
id = complete_ns.find_last_of(
"/");
126 return complete_ns.substr(
id + 1);
131 template <
class SegmentImpl,
class HardwareInterface>
137 time_data.
time = time;
139 time_data_.initRT(time_data);
142 for (
unsigned int i = 0; i < getNumberOfJoints(); ++i)
144 desired_state_.position[i] = joints_[i].getPosition();
145 desired_state_.velocity[i] = joints_[i].getVelocity();
149 setHoldPosition(time_data.
uptime);
152 last_state_publish_time_ = time_data.
uptime;
155 hw_iface_adapter_.starting(time_data.
uptime);
158 template <
class SegmentImpl,
class HardwareInterface>
165 template <
class SegmentImpl,
class HardwareInterface>
170 if (update_ok) {preemptActiveGoal();}
173 template <
class SegmentImpl,
class HardwareInterface>
180 if (current_active_goal)
183 rt_active_goal_.reset();
184 current_active_goal->gh_.setCanceled();
188 template <
class SegmentImpl,
class HardwareInterface>
198 "The joint_trajectory_controller verbose flag is enabled. "
199 <<
"This flag breaks real-time safety and should only be "
200 <<
"used for debugging");
204 template <
class SegmentImpl,
class HardwareInterface>
209 using namespace internal;
212 controller_nh_ = controller_nh;
218 double state_publish_rate = 50.0;
219 controller_nh_.getParam(
"state_publish_rate", state_publish_rate);
221 state_publisher_period_ =
ros::Duration(1.0 / state_publish_rate);
224 double action_monitor_rate = 20.0;
225 controller_nh_.getParam(
"action_monitor_rate", action_monitor_rate);
226 action_monitor_period_ =
ros::Duration(1.0 / action_monitor_rate);
227 ROS_DEBUG_STREAM_NAMED(name_,
"Action status changes will be monitored at " << action_monitor_rate <<
"Hz.");
230 stop_trajectory_duration_ = 0.0;
231 controller_nh_.getParam(
"stop_trajectory_duration", stop_trajectory_duration_);
235 controller_nh_.param<
bool>(
"allow_partial_joints_goal", allow_partial_joints_goal_,
false);
236 if (allow_partial_joints_goal_)
238 ROS_DEBUG_NAMED(name_,
"Goals with partial set of joints are allowed");
242 joint_names_ =
getStrings(controller_nh_,
"joints");
243 if (joint_names_.empty()) {
return false;}
244 const unsigned int n_joints = joint_names_.size();
247 urdf::ModelSharedPtr
urdf =
getUrdf(root_nh,
"robot_description");
248 if (!
urdf) {
return false;}
250 std::vector<urdf::JointConstSharedPtr> urdf_joints =
getUrdfJoints(*
urdf, joint_names_);
251 if (urdf_joints.empty()) {
return false;}
252 assert(n_joints == urdf_joints.size());
255 joints_.resize(n_joints);
256 angle_wraparound_.resize(n_joints);
257 for (
unsigned int i = 0; i < n_joints; ++i)
260 try {joints_[i] = hw->getHandle(joint_names_[i]);}
264 this->getHardwareInterfaceType() <<
"'.");
269 angle_wraparound_[i] = urdf_joints[i]->type == urdf::Joint::CONTINUOUS;
270 const std::string not_if = angle_wraparound_[i] ?
"" :
"non-";
272 ROS_DEBUG_STREAM_NAMED(name_,
"Found " << not_if <<
"continuous joint '" << joint_names_[i] <<
"' in '" <<
273 this->getHardwareInterfaceType() <<
"'.");
276 assert(joints_.size() == angle_wraparound_.size());
278 "\n- Number of joints: " << getNumberOfJoints() <<
279 "\n- Hardware interface type: '" << this->getHardwareInterfaceType() <<
"'" <<
280 "\n- Trajectory segment type: '" << hardware_interface::internal::demangledTypeName<SegmentImpl>() <<
"'");
284 default_tolerances_ = getSegmentTolerances<Scalar>(tol_nh, joint_names_);
287 hw_iface_adapter_.init(joints_, controller_nh_);
293 state_publisher_.reset(
new StatePublisher(controller_nh_,
"state", 1));
296 action_server_.reset(
297 new ActionServer(controller_nh_,
"follow_joint_trajectory",
300 action_server_->start();
303 query_state_service_ = controller_nh_.advertiseService(
"query_state",
315 successful_joint_traj_ = boost::dynamic_bitset<>(getNumberOfJoints());
317 hold_trajectory_ptr_ = createHoldTrajectory(n_joints);
318 assert(joint_names_.size() == hold_trajectory_ptr_->size());
320 if (stop_trajectory_duration_ == 0.0)
330 state_publisher_->lock();
331 state_publisher_->msg_.joint_names = joint_names_;
332 state_publisher_->msg_.desired.positions.resize(n_joints);
333 state_publisher_->msg_.desired.velocities.resize(n_joints);
334 state_publisher_->msg_.desired.accelerations.resize(n_joints);
335 state_publisher_->msg_.actual.positions.resize(n_joints);
336 state_publisher_->msg_.actual.velocities.resize(n_joints);
337 state_publisher_->msg_.error.positions.resize(n_joints);
338 state_publisher_->msg_.error.velocities.resize(n_joints);
339 state_publisher_->unlock();
345 template <
class SegmentImpl,
class HardwareInterface>
351 curr_trajectory_box_.get(curr_traj_ptr);
354 old_time_data_ = *(time_data_.readFromRT());
358 time_data.
time = time;
359 time_data.
period = period;
360 time_data.
uptime = old_time_data_.uptime + period;
361 time_data_.writeFromNonRT(time_data);
371 updateStates(time_data.
uptime, curr_traj_ptr.get());
374 for (
unsigned int i = 0; i < getNumberOfJoints(); ++i)
376 typename TrajectoryPerJoint::const_iterator segment_it =
sample(curr_traj[i], time_data.
uptime.
toSec(), desired_joint_state_);
377 if (curr_traj[i].end() == segment_it)
381 "Unexpected error: No trajectory defined at current time. Please contact the package maintainer.");
386 state_joint_error_.position[0] = state_error_.position[i];
387 state_joint_error_.velocity[0] = state_error_.velocity[i];
388 state_joint_error_.acceleration[0] = state_error_.acceleration[i];
392 if (rt_segment_goal && rt_segment_goal == rt_active_goal_)
395 if (time_data.
uptime.
toSec() < segment_it->endTime())
406 rt_segment_goal->preallocated_result_->error_code =
407 control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
408 rt_segment_goal->preallocated_result_->error_string = joint_names_[i] +
" path error " + std::to_string( state_joint_error_.position[0] );
409 rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
412 rt_active_goal_.reset();
413 successful_joint_traj_.reset();
416 else if (segment_it == --curr_traj[i].end())
422 const ros::Time uptime = time_data_.readFromRT()->uptime;
428 if (inside_goal_tolerances)
430 successful_joint_traj_[i] = 1;
445 rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
446 rt_segment_goal->preallocated_result_->error_string = joint_names_[i] +
" goal error " + std::to_string( state_joint_error_.position[0] );
447 rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
450 rt_active_goal_.reset();
451 successful_joint_traj_.reset();
459 if (current_active_goal && successful_joint_traj_.count() == getNumberOfJoints())
461 current_active_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
462 current_active_goal->setSucceeded(current_active_goal->preallocated_result_);
463 current_active_goal.reset();
464 rt_active_goal_.reset();
465 successful_joint_traj_.reset();
468 updateFuncExtensionPoint(curr_traj, time_data);
471 hw_iface_adapter_.updateCommand(time_data.
uptime, time_data.
period,
472 desired_state_, state_error_);
476 publishState(time_data.
uptime);
479 template <
class SegmentImpl,
class HardwareInterface>
486 std::string error_string_tmp;
489 if (!this->isRunning())
491 error_string_tmp =
"Can't accept new commands. Controller is not running.";
493 options.setErrorString(error_string_tmp);
499 error_string_tmp =
"Received null-pointer trajectory message, skipping.";
501 options.setErrorString(error_string_tmp);
506 TimeData* time_data = time_data_.readFromRT();
515 if (msg->points.empty())
517 setHoldPosition(time_data->
uptime, gh);
524 curr_trajectory_box_.get(curr_traj_ptr);
526 options.other_time_base = &next_update_uptime;
527 options.current_trajectory = curr_traj_ptr.get();
528 options.joint_names = &joint_names_;
529 options.angle_wraparound = &angle_wraparound_;
530 options.rt_goal_handle = gh;
531 options.default_tolerances = &default_tolerances_;
532 options.allow_partial_joints_goal = allow_partial_joints_goal_;
538 *traj_ptr = initJointTrajectory<Trajectory>(*msg, next_update_time, options);
539 if (!traj_ptr->empty())
541 curr_trajectory_box_.set(traj_ptr);
548 catch(
const std::exception& ex)
551 options.setErrorString(ex.what());
556 error_string_tmp =
"Unexpected exception caught when initializing trajectory from ROS message data.";
558 options.setErrorString(error_string_tmp);
565 template <
class SegmentImpl,
class HardwareInterface>
572 if (!this->isRunning())
574 ROS_ERROR_NAMED(name_,
"Can't accept new action goals. Controller is not running.");
575 control_msgs::FollowJointTrajectoryResult result;
576 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
582 if (!allow_partial_joints_goal_)
584 if (gh.
getGoal()->trajectory.joint_names.size() != joint_names_.size())
586 ROS_ERROR_NAMED(name_,
"Joints on incoming goal don't match the controller joints.");
587 control_msgs::FollowJointTrajectoryResult result;
588 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
596 std::vector<unsigned int> mapping_vector =
mapping(gh.
getGoal()->trajectory.joint_names, joint_names_);
598 if (mapping_vector.empty())
600 ROS_ERROR_NAMED(name_,
"Joints on incoming goal don't match the controller joints.");
601 control_msgs::FollowJointTrajectoryResult result;
602 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
609 std::string error_string;
613 rt_goal->preallocated_feedback_->joint_names = joint_names_;
620 rt_active_goal_ = rt_goal;
623 goal_handle_timer_ = controller_nh_.createTimer(action_monitor_period_,
624 &RealtimeGoalHandle::runNonRealtime,
626 goal_handle_timer_.start();
631 control_msgs::FollowJointTrajectoryResult result;
632 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
633 result.error_string = error_string;
638 template <
class SegmentImpl,
class HardwareInterface>
645 if (current_active_goal && current_active_goal->gh_ == gh)
648 rt_active_goal_.reset();
651 const ros::Time uptime = time_data_.readFromRT()->uptime;
654 setHoldPosition(uptime);
655 ROS_DEBUG_NAMED(name_,
"Canceling active action goal because cancel callback recieved from actionlib.");
658 current_active_goal->gh_.setCanceled();
662 template <
class SegmentImpl,
class HardwareInterface>
665 control_msgs::QueryTrajectoryState::Response& resp)
668 if (!this->isRunning())
670 ROS_ERROR_NAMED(name_,
"Can't sample trajectory. Controller is not running.");
675 TimeData* time_data = time_data_.readFromRT();
681 curr_trajectory_box_.get(curr_traj_ptr);
686 for (
unsigned int i = 0; i < getNumberOfJoints(); ++i)
689 typename TrajectoryPerJoint::const_iterator segment_it =
sample(curr_traj[i], sample_time.
toSec(), state);
690 if (curr_traj[i].end() == segment_it)
696 response_point.position[i] = state.position[0];
697 response_point.velocity[i] = state.velocity[0];
698 response_point.acceleration[i] = state.acceleration[0];
702 resp.name = joint_names_;
703 resp.position = response_point.position;
704 resp.velocity = response_point.velocity;
705 resp.acceleration = response_point.acceleration;
710 template <
class SegmentImpl,
class HardwareInterface>
715 if (!state_publisher_period_.isZero() && last_state_publish_time_ + state_publisher_period_ < time)
717 if (state_publisher_ && state_publisher_->trylock())
719 last_state_publish_time_ += state_publisher_period_;
721 state_publisher_->msg_.header.stamp = time_data_.readFromRT()->time;
722 state_publisher_->msg_.desired.positions = desired_state_.position;
723 state_publisher_->msg_.desired.velocities = desired_state_.velocity;
724 state_publisher_->msg_.desired.accelerations = desired_state_.acceleration;
725 state_publisher_->msg_.desired.time_from_start =
ros::Duration(desired_state_.time_from_start);
726 state_publisher_->msg_.actual.positions = current_state_.position;
727 state_publisher_->msg_.actual.velocities = current_state_.velocity;
728 state_publisher_->msg_.actual.time_from_start =
ros::Duration(current_state_.time_from_start);
729 state_publisher_->msg_.error.positions = state_error_.position;
730 state_publisher_->msg_.error.velocities = state_error_.velocity;
731 state_publisher_->msg_.error.time_from_start =
ros::Duration(state_error_.time_from_start);
733 state_publisher_->unlockAndPublish();
738 template <
class SegmentImpl,
class HardwareInterface>
743 ->setStartTime(time.
toSec())
745 ->buildTrajectory(hold_trajectory_ptr_.get());
746 hold_traj_builder_->reset();
747 curr_trajectory_box_.set(hold_trajectory_ptr_);
750 template <
class SegmentImpl,
class HardwareInterface>
757 template <
class SegmentImpl,
class HardwareInterface>
761 return joints_.size();
764 template <
class SegmentImpl,
class HardwareInterface>
768 old_desired_state_ = desired_state_;
770 for (
unsigned int joint_index = 0; joint_index < getNumberOfJoints(); ++joint_index)
772 const auto segment =
sample( (*traj)[joint_index], sample_time.
toSec(), desired_joint_state_);
774 current_state_.position[joint_index] = joints_[joint_index].getPosition();
775 current_state_.velocity[joint_index] = joints_[joint_index].getVelocity();
778 desired_state_.position[joint_index] = desired_joint_state_.position[0];
779 desired_state_.velocity[joint_index] = desired_joint_state_.velocity[0];
780 desired_state_.acceleration[joint_index] = desired_joint_state_.acceleration[0];
783 state_error_.velocity[joint_index] = desired_joint_state_.velocity[0] - current_state_.velocity[joint_index];
784 state_error_.acceleration[joint_index] = 0.0;
786 if (joint_index == 0)
788 const auto time_from_start = segment->timeFromStart();
789 current_state_.time_from_start = sample_time.
toSec() - segment->startTime() + time_from_start;
790 desired_state_.time_from_start = time_from_start;
791 state_error_.time_from_start = desired_state_.time_from_start - current_state_.time_from_start;
796 template <
class SegmentImpl,
class HardwareInterface>
801 if (!current_active_goal)
806 current_active_goal->preallocated_feedback_->header.stamp = time_data_.readFromRT()->time;
807 current_active_goal->preallocated_feedback_->desired.positions = desired_state_.position;
808 current_active_goal->preallocated_feedback_->desired.velocities = desired_state_.velocity;
809 current_active_goal->preallocated_feedback_->desired.accelerations = desired_state_.acceleration;
810 current_active_goal->preallocated_feedback_->desired.time_from_start =
ros::Duration(desired_state_.time_from_start);
811 current_active_goal->preallocated_feedback_->actual.positions = current_state_.position;
812 current_active_goal->preallocated_feedback_->actual.velocities = current_state_.velocity;
813 current_active_goal->preallocated_feedback_->actual.time_from_start =
ros::Duration(current_state_.time_from_start);
814 current_active_goal->preallocated_feedback_->error.positions = state_error_.position;
815 current_active_goal->preallocated_feedback_->error.velocities = state_error_.velocity;
816 current_active_goal->preallocated_feedback_->error.time_from_start =
ros::Duration(state_error_.time_from_start);
817 current_active_goal->setFeedback( current_active_goal->preallocated_feedback_ );
821 template <
class SegmentImpl,
class HardwareInterface>
829 for (
unsigned int i = 0; i < number_of_joints; ++i)
831 default_joint_state.position[0]= default_state.position[i];
832 default_joint_state.velocity[0]= default_state.velocity[i];
833 Segment hold_segment(0.0, default_joint_state, 0.0, default_joint_state);
836 joint_segment.resize(1, hold_segment);
837 hold_traj->push_back(joint_segment);