31 #ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_IMP_H 32 #define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_IMP_H 41 template <
class Enclosure,
class Member>
53 if (!nh.
getParam(param_name, xml_array))
56 return std::vector<std::string>();
58 if (xml_array.
getType() != XmlRpcValue::TypeArray)
60 ROS_ERROR_STREAM(
"The '" << param_name <<
"' parameter is not an array (namespace: " <<
62 return std::vector<std::string>();
65 std::vector<std::string> out;
66 for (
int i = 0; i < xml_array.
size(); ++i)
68 if (xml_array[i].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>(xml_array[i]));
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 (
unsigned int i = 0; i < joint_names.size(); ++i)
108 urdf::JointConstSharedPtr urdf_joint = urdf.getJoint(joint_names[i]);
111 out.push_back(urdf_joint);
115 ROS_ERROR_STREAM(
"Could not find joint '" << joint_names[i] <<
"' 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 < joints_.size(); ++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>
199 "The joint_trajectory_controller verbose flag is enabled. " 200 <<
"This flag breaks real-time safety and should only be " 201 <<
"used for debugging");
205 template <
class SegmentImpl,
class HardwareInterface>
210 using namespace internal;
219 double state_publish_rate = 50.0;
225 double action_monitor_rate = 20.0;
237 ROS_WARN(
"The 'hold_trajectory_duration' has been deprecated in favor of the 'stop_trajectory_duration' parameter. Please update your controller configuration.");
244 if (allow_partial_joints_goal_)
255 urdf::ModelSharedPtr
urdf =
getUrdf(root_nh,
"robot_description");
256 if (!urdf) {
return false;}
259 if (urdf_joints.empty()) {
return false;}
260 assert(n_joints == urdf_joints.size());
265 for (
unsigned int i = 0; i < n_joints; ++i)
286 "\n- Number of joints: " <<
joints_.size() <<
288 "\n- Trajectory segment type: '" << hardware_interface::internal::demangledTypeName<SegmentImpl>() <<
"'");
326 for (
unsigned int i = 0; i < n_joints; ++i)
330 Segment hold_segment(0.0, current_joint_state_, 0.0, current_joint_state_);
333 joint_segment.resize(1, hold_segment);
353 template <
class SegmentImpl,
class HardwareInterface>
364 time_data.
time = time;
365 time_data.
period = period;
378 for (
unsigned int i = 0; i <
joints_.size(); ++i)
385 if (curr_traj[i].end() == segment_it)
389 "Unexpected error: No trajectory defined at current time. Please contact the package maintainer.");
409 if (time_data.
uptime.
toSec() < segment_it->endTime())
420 rt_segment_goal->preallocated_result_->error_code =
421 control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
422 rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
427 else if (segment_it == --curr_traj[i].end())
439 if (inside_goal_tolerances)
456 rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
457 rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
469 current_active_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
470 current_active_goal->setSucceeded(current_active_goal->preallocated_result_);
471 current_active_goal.reset();
481 if (current_active_goal)
483 current_active_goal->preallocated_feedback_->header.stamp =
time_data_.readFromRT()->time;
484 current_active_goal->preallocated_feedback_->desired.positions =
desired_state_.position;
485 current_active_goal->preallocated_feedback_->desired.velocities =
desired_state_.velocity;
486 current_active_goal->preallocated_feedback_->desired.accelerations =
desired_state_.acceleration;
487 current_active_goal->preallocated_feedback_->actual.positions =
current_state_.position;
488 current_active_goal->preallocated_feedback_->actual.velocities =
current_state_.velocity;
489 current_active_goal->preallocated_feedback_->error.positions =
state_error_.position;
490 current_active_goal->preallocated_feedback_->error.velocities =
state_error_.velocity;
491 current_active_goal->setFeedback( current_active_goal->preallocated_feedback_ );
498 template <
class SegmentImpl,
class HardwareInterface>
504 options.error_string = error_string;
505 std::string error_string_tmp;
510 error_string_tmp =
"Can't accept new commands. Controller is not running.";
512 options.setErrorString(error_string_tmp);
518 error_string_tmp =
"Received null-pointer trajectory message, skipping.";
520 options.setErrorString(error_string_tmp);
534 if (msg->points.empty())
545 options.other_time_base = &next_update_uptime;
546 options.current_trajectory = curr_traj_ptr.get();
549 options.rt_goal_handle = gh;
557 *traj_ptr = initJointTrajectory<Trajectory>(*msg, next_update_time, options);
558 if (!traj_ptr->empty())
567 catch(
const std::exception& ex)
570 options.setErrorString(ex.what());
575 error_string_tmp =
"Unexpected exception caught when initializing trajectory from ROS message data.";
577 options.setErrorString(error_string_tmp);
584 template <
class SegmentImpl,
class HardwareInterface>
594 control_msgs::FollowJointTrajectoryResult result;
595 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
606 control_msgs::FollowJointTrajectoryResult result;
607 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
617 if (mapping_vector.empty())
620 control_msgs::FollowJointTrajectoryResult result;
621 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
628 std::string error_string;
632 rt_goal->preallocated_feedback_->joint_names =
joint_names_;
650 control_msgs::FollowJointTrajectoryResult result;
651 result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
652 result.error_string = error_string;
657 template <
class SegmentImpl,
class HardwareInterface>
664 if (current_active_goal && current_active_goal->gh_ == gh)
674 ROS_DEBUG_NAMED(
name_,
"Canceling active action goal because cancel callback recieved from actionlib.");
677 current_active_goal->gh_.setCanceled();
681 template <
class SegmentImpl,
class HardwareInterface>
684 control_msgs::QueryTrajectoryState::Response& resp)
705 for (
unsigned int i = 0; i <
joints_.size(); ++i)
708 typename TrajectoryPerJoint::const_iterator segment_it =
sample(curr_traj[i], sample_time.
toSec(), state);
709 if (curr_traj[i].end() == segment_it)
715 response_point.position[i] = state.position[0];
716 response_point.velocity[i] = state.velocity[0];
717 response_point.acceleration[i] = state.acceleration[0];
722 resp.position = response_point.position;
723 resp.velocity = response_point.velocity;
724 resp.acceleration = response_point.acceleration;
729 template <
class SegmentImpl,
class HardwareInterface>
754 template <
class SegmentImpl,
class HardwareInterface>
762 const unsigned int n_joints =
joints_.size();
768 for (
unsigned int i = 0; i < n_joints; ++i)
770 hold_start_state_.position[0] =
joints_[i].getPosition();
771 hold_start_state_.velocity[0] = 0.0;
772 hold_start_state_.acceleration[0] = 0.0;
773 (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_,
774 start_time, hold_start_state_);
776 (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh);
791 for (
unsigned int i = 0; i < n_joints; ++i)
797 hold_start_state_.acceleration[0] = 0.0;
801 hold_end_state_.acceleration[0] = 0.0;
803 (*hold_trajectory_ptr_)[i].front().
init(start_time, hold_start_state_,
804 end_time_2x, hold_end_state_);
807 (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_);
810 (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_,
811 end_time, hold_end_state_);
814 (*hold_trajectory_ptr_)[i].front().setGoalHandle(gh);
822 #endif // header guard ActionServerPtr action_server_
Segment::State current_state_
Preallocated workspace variable.
StateTolerances< Scalar > state_tolerance
#define ROS_DEBUG_STREAM_NAMED(name, args)
ros::NodeHandle controller_nh_
#define ROS_ERROR_STREAM_NAMED(name, args)
Segment::State state_error_
Preallocated workspace variable.
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.
Segment::State desired_joint_state_
Preallocated workspace variable.
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 &)
#define ROS_DEBUG_STREAM_THROTTLE_NAMED(rate, name, args)
JointTrajectoryController()
void setRejected(const Result &result=Result(), const std::string &text=std::string(""))
std::vector< TrajectoryPerJoint > Trajectory
Segment::State desired_state_
Preallocated workspace variable.
boost::shared_ptr< const Goal > getGoal() const
Segment::Time stop_trajectory_duration_
Duration for stop ramp. If zero, the controller stops at the actual position.
void init(const trajectory_msgs::JointTrajectoryPoint &point, const std::vector< Scalar > &position_offset=std::vector< Scalar >())
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)
Type const & getType() const
void update(const ros::Time &time, const ros::Duration &period)
bool verbose_
Hard coded verbose flag to help in debugging.
std::string getHardwareInterfaceType() const
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
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)
StateTolerances< Scalar > goal_state_tolerance
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
RealtimeGoalHandlePtr rt_active_goal_
Currently active action goal, if any.
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
const std::string & getNamespace() const
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_
virtual bool updateTrajectoryCommand(const JointTrajectoryConstPtr &msg, RealtimeGoalHandlePtr gh, std::string *error_string=0)
virtual bool queryStateService(control_msgs::QueryTrajectoryState::Request &req, control_msgs::QueryTrajectoryState::Response &resp)
std::vector< unsigned int > mapping(const T &t1, const T &t2)
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.
boost::dynamic_bitset successful_joint_traj_
void stopping(const ros::Time &)
Cancels the active action goal, if any.
bool getParam(const std::string &key, std::string &s) const
#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.
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)