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;
218 double state_publish_rate = 50.0;
224 double action_monitor_rate = 20.0;
236 if (allow_partial_joints_goal_)
247 urdf::ModelSharedPtr
urdf =
getUrdf(root_nh,
"robot_description");
248 if (!urdf) {
return false;}
251 if (urdf_joints.empty()) {
return false;}
252 assert(n_joints == urdf_joints.size());
257 for (
unsigned int i = 0; i < n_joints; ++i)
280 "\n- Trajectory segment type: '" << hardware_interface::internal::demangledTypeName<SegmentImpl>() <<
"'");
318 assert(
joint_names_.size() == hold_trajectory_ptr_->size());
345 template <
class SegmentImpl,
class HardwareInterface>
358 time_data.
time = time;
359 time_data.
period = period;
377 if (curr_traj[i].end() == segment_it)
381 "Unexpected error: No trajectory defined at current time. Please contact the package maintainer.");
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->setAborted(rt_segment_goal->preallocated_result_);
413 else if (segment_it == --curr_traj[i].end())
425 if (inside_goal_tolerances)
442 rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
443 rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
455 current_active_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
456 current_active_goal->setSucceeded(current_active_goal->preallocated_result_);
457 current_active_goal.reset();
473 template <
class SegmentImpl,
class HardwareInterface>
479 options.error_string = error_string;
480 std::string error_string_tmp;
485 error_string_tmp =
"Can't accept new commands. Controller is not running.";
487 options.setErrorString(error_string_tmp);
493 error_string_tmp =
"Received null-pointer trajectory message, skipping.";
495 options.setErrorString(error_string_tmp);
509 if (msg->points.empty())
520 options.other_time_base = &next_update_uptime;
521 options.current_trajectory = curr_traj_ptr.get();
524 options.rt_goal_handle = gh;
532 *traj_ptr = initJointTrajectory<Trajectory>(*msg, next_update_time, options);
533 if (!traj_ptr->empty())
542 catch(
const std::exception& ex)
545 options.setErrorString(ex.what());
550 error_string_tmp =
"Unexpected exception caught when initializing trajectory from ROS message data.";
552 options.setErrorString(error_string_tmp);
559 template <
class SegmentImpl,
class HardwareInterface>
569 control_msgs::FollowJointTrajectoryResult result;
570 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
581 control_msgs::FollowJointTrajectoryResult result;
582 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
592 if (mapping_vector.empty())
595 control_msgs::FollowJointTrajectoryResult result;
596 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
603 std::string error_string;
607 rt_goal->preallocated_feedback_->joint_names =
joint_names_;
625 control_msgs::FollowJointTrajectoryResult result;
626 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
627 result.error_string = error_string;
632 template <
class SegmentImpl,
class HardwareInterface>
639 if (current_active_goal && current_active_goal->gh_ == gh)
649 ROS_DEBUG_NAMED(
name_,
"Canceling active action goal because cancel callback recieved from actionlib.");
652 current_active_goal->gh_.setCanceled();
656 template <
class SegmentImpl,
class HardwareInterface>
659 control_msgs::QueryTrajectoryState::Response& resp)
683 typename TrajectoryPerJoint::const_iterator segment_it =
sample(curr_traj[i], sample_time.
toSec(), state);
684 if (curr_traj[i].end() == segment_it)
690 response_point.position[i] = state.position[0];
691 response_point.velocity[i] = state.velocity[0];
692 response_point.acceleration[i] = state.acceleration[0];
697 resp.position = response_point.position;
698 resp.velocity = response_point.velocity;
699 resp.acceleration = response_point.acceleration;
704 template <
class SegmentImpl,
class HardwareInterface>
729 template <
class SegmentImpl,
class HardwareInterface>
734 ->setStartTime(time.
toSec())
741 template <
class SegmentImpl,
class HardwareInterface>
748 template <
class SegmentImpl,
class HardwareInterface>
755 template <
class SegmentImpl,
class HardwareInterface>
761 for (
unsigned int joint_index = 0; joint_index <
getNumberOfJoints(); ++joint_index)
783 template <
class SegmentImpl,
class HardwareInterface>
788 if (!current_active_goal)
793 current_active_goal->preallocated_feedback_->header.stamp =
time_data_.readFromRT()->time;
794 current_active_goal->preallocated_feedback_->desired.positions =
desired_state_.position;
795 current_active_goal->preallocated_feedback_->desired.velocities =
desired_state_.velocity;
796 current_active_goal->preallocated_feedback_->desired.accelerations =
desired_state_.acceleration;
797 current_active_goal->preallocated_feedback_->actual.positions =
current_state_.position;
798 current_active_goal->preallocated_feedback_->actual.velocities =
current_state_.velocity;
799 current_active_goal->preallocated_feedback_->error.positions =
state_error_.position;
800 current_active_goal->preallocated_feedback_->error.velocities =
state_error_.velocity;
801 current_active_goal->setFeedback( current_active_goal->preallocated_feedback_ );
805 template <
class SegmentImpl,
class HardwareInterface>
813 for (
unsigned int i = 0; i < number_of_joints; ++i)
815 default_joint_state.position[0]= default_state.position[i];
816 default_joint_state.velocity[0]= default_state.velocity[i];
817 Segment hold_segment(0.0, default_joint_state, 0.0, default_joint_state);
820 joint_segment.resize(1, hold_segment);
821 hold_traj->push_back(joint_segment);
ActionServerPtr action_server_
Segment::State current_state_
Preallocated workspace variable.
std::string getHardwareInterfaceType() const
void setActionFeedback()
Updates the pre-allocated feedback of the current active goal (if any) based on the current state val...
StateTolerances< Scalar > state_tolerance
#define ROS_DEBUG_STREAM_NAMED(name, args)
ros::NodeHandle controller_nh_
#define ROS_ERROR_STREAM_NAMED(name, args)
boost::shared_ptr< const Goal > getGoal() const
Segment::State state_error_
Preallocated workspace variable.
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
ros::Time last_state_publish_time_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
HwIfaceAdapter hw_iface_adapter_
Adapts desired trajectory state to HW interface.
static TrajectoryPtr createHoldTrajectory(const unsigned int &number_of_joints)
Returns a trajectory consisting of joint trajectories with one pre-allocated segment.
Segment::State desired_joint_state_
Preallocated workspace variable.
Builder creating a trajectory stopping the robot.
virtual void cancelCB(GoalHandle gh)
Trajectory::const_iterator sample(const Trajectory &trajectory, const typename Trajectory::value_type::Time &time, typename Trajectory::value_type::State &state)
Sample a trajectory at a specified time.
trajectory_msgs::JointTrajectory::ConstPtr JointTrajectoryConstPtr
bool init(std::vector< typename HardwareInterface::ResourceHandleType > &, ros::NodeHandle &)
unsigned int getNumberOfJoints() const
Returns the number of joints of the robot.
JointTrajectoryController()
void setRejected(const Result &result=Result(), const std::string &text=std::string(""))
std::vector< TrajectoryPerJoint > Trajectory
Segment::State desired_state_
Preallocated workspace variable.
Segment::Time stop_trajectory_duration_
Duration for stop ramp. If zero, the controller stops at the actual position.
ros::Time time
Time of last update cycle.
ros::Timer goal_handle_timer_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void update(const ros::Time &time, const ros::Duration &period)
bool verbose_
Hard coded verbose flag to help in debugging.
void starting(const ros::Time &time)
Holds the current position.
ros::Duration period
Period of last update cycle.
std::vector< urdf::JointConstSharedPtr > getUrdfJoints(const urdf::Model &urdf, const std::vector< std::string > &joint_names)
ros::Duration state_publisher_period_
realtime_tools::RealtimeServerGoalHandle< control_msgs::FollowJointTrajectoryAction > RealtimeGoalHandle
std::vector< Segment > TrajectoryPerJoint
virtual void trajectoryCommandCB(const JointTrajectoryConstPtr &msg)
realtime_tools::RealtimePublisher< control_msgs::JointTrajectoryControllerState > StatePublisher
void setAccepted(const std::string &text=std::string(""))
bool checkStateTolerancePerJoint(const State &state_error, const StateTolerances< typename State::Scalar > &state_tolerance, bool show_errors=false)
#define ROS_DEBUG_NAMED(name,...)
void setHoldPosition(const ros::Time &time, RealtimeGoalHandlePtr gh=RealtimeGoalHandlePtr())
Hold the current position.
Scalar goal_time_tolerance
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Type const & getType() const
void updateStates(const ros::Time &sample_time, const Trajectory *const traj)
Updates the states by sampling the specified trajectory for each joint at the specified sampling time...
std::vector< JointHandle > joints_
Handles to controlled joints.
void publishState(const ros::Time &time)
Publish current controller state at a throttled frequency.
virtual void goalCB(GoalHandle gh)
bool getParam(const std::string &key, std::string &s) const
StateTolerances< Scalar > goal_state_tolerance
Builder creating a trajectory "simply" holding (without motion) the specified position.
RealtimeGoalHandlePtr rt_active_goal_
Currently active action goal, if any.
virtual void updateFuncExtensionPoint(const Trajectory &curr_traj, const TimeData &time_data)
Allows derived classes to perform additional checks and to e.g. replace the newly calculated desired ...
bool init(HardwareInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
void updateCommand(const ros::Time &, const ros::Duration &, const State &, const State &)
#define ROS_WARN_STREAM(args)
StatePublisherPtr state_publisher_
TrajectoryPtr hold_trajectory_ptr_
Last hold trajectory values.
virtual void preemptActiveGoal()
Segment::State state_joint_error_
Preallocated workspace variable.
ros::Subscriber trajectory_command_sub_
ros::Time uptime
Controller uptime. Set to zero at every restart.
Class representing a multi-dimensional quintic spline segment with a start and end time...
ros::Duration action_monitor_period_
std::shared_ptr< Trajectory > TrajectoryPtr
virtual bool queryStateService(control_msgs::QueryTrajectoryState::Request &req, control_msgs::QueryTrajectoryState::Response &resp)
std::vector< unsigned int > mapping(const T &t1, const T &t2)
virtual bool updateTrajectoryCommand(const JointTrajectoryConstPtr &msg, RealtimeGoalHandlePtr gh, std::string *error_string=nullptr)
std::unique_ptr< TrajectoryBuilder< SegmentImpl > > hold_traj_builder_
realtime_tools::RealtimeBuffer< TimeData > time_data_
SegmentTolerances< Scalar > default_tolerances_
Default trajectory segment tolerances.
std::vector< std::string > joint_names_
Controlled joint names.
Options used when initializing a joint trajectory from ROS message data.
Trajectory segment tolerances per Joint.
const std::string & getNamespace() const
boost::dynamic_bitset successful_joint_traj_
Segment::State old_desired_state_
Preallocated workspace variable.
void stopping(const ros::Time &)
Cancels the active action goal, if any.
#define ROS_ERROR_NAMED(name,...)
boost::shared_ptr< Member > share_member(boost::shared_ptr< Enclosure > enclosure, Member &member)
urdf::ModelSharedPtr getUrdf(const ros::NodeHandle &nh, const std::string ¶m_name)
#define ROS_ERROR_STREAM(args)
std::string name_
Controller name.
#define ROS_DEBUG_STREAM_THROTTLE_NAMED(period, name, args)
std::string getLeafNamespace(const ros::NodeHandle &nh)
std::vector< std::string > getStrings(const ros::NodeHandle &nh, const std::string ¶m_name)
std::vector< bool > angle_wraparound_
Whether controlled joints wrap around or not.
def shortest_angular_distance(from_angle, to_angle)
actionlib::ActionServer< control_msgs::FollowJointTrajectoryAction > ActionServer
ros::ServiceServer query_state_service_
bool allow_partial_joints_goal_
TrajectoryBox curr_trajectory_box_
#define ROS_WARN_STREAM_NAMED(name, args)