Controller for executing joint-space trajectories on a group of joints. More...

#include <joint_trajectory_controller.h>

Inheritance diagram for joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >:

Classes
struct	TimeData
Public Member Functions
	JointTrajectoryController ()
Non Real-Time Safe Functions
bool	init (HardwareInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Real-Time Safe Functions
void	starting (const ros::Time &time)
	Holds the current position.
void	stopping (const ros::Time &time)
	Cancels the active action goal, if any.
void	update (const ros::Time &time, const ros::Duration &period)
Private Types
typedef actionlib::ActionServer < control_msgs::FollowJointTrajectoryAction >	ActionServer
typedef boost::shared_ptr < ActionServer >	ActionServerPtr
typedef ActionServer::GoalHandle	GoalHandle
typedef HardwareInterfaceAdapter < HardwareInterface, typename Segment::State >	HwIfaceAdapter
typedef HardwareInterface::ResourceHandleType	JointHandle
typedef trajectory_msgs::JointTrajectory::ConstPtr	JointTrajectoryConstPtr
typedef realtime_tools::RealtimeServerGoalHandle < control_msgs::FollowJointTrajectoryAction >	RealtimeGoalHandle
typedef boost::shared_ptr < RealtimeGoalHandle >	RealtimeGoalHandlePtr
typedef Segment::Scalar	Scalar
typedef JointTrajectorySegment < SegmentImpl >	Segment
typedef realtime_tools::RealtimePublisher < control_msgs::JointTrajectoryControllerState >	StatePublisher
typedef boost::scoped_ptr < StatePublisher >	StatePublisherPtr
typedef std::vector< Segment >	Trajectory
typedef realtime_tools::RealtimeBox < TrajectoryPtr >	TrajectoryBox
typedef boost::shared_ptr < Trajectory >	TrajectoryPtr
Private Member Functions
void	cancelCB (GoalHandle gh)
void	checkGoalTolerances (const typename Segment::State &state_error, const Segment &segment)
	Check goal tolerances.
void	checkPathTolerances (const typename Segment::State &state_error, const Segment &segment)
	Check path tolerances.
void	goalCB (GoalHandle gh)
void	preemptActiveGoal ()
void	publishState (const ros::Time &time)
	Publish current controller state at a throttled frequency.
bool	queryStateService (control_msgs::QueryTrajectoryState::Request &req, control_msgs::QueryTrajectoryState::Response &resp)
void	setHoldPosition (const ros::Time &time)
	Hold the current position.
void	trajectoryCommandCB (const JointTrajectoryConstPtr &msg)
bool	updateTrajectoryCommand (const JointTrajectoryConstPtr &msg, RealtimeGoalHandlePtr gh)
Private Attributes
ros::Duration	action_monitor_period_
ActionServerPtr	action_server_
std::vector< bool >	angle_wraparound_
	Whether controlled joints wrap around or not.
ros::NodeHandle	controller_nh_
TrajectoryBox	curr_trajectory_box_
Segment::State	current_state_
	Preallocated workspace variable.
SegmentTolerances< Scalar >	default_tolerances_
	Default trajectory segment tolerances.
Segment::State	desired_state_
	Preallocated workspace variable.
ros::Timer	goal_handle_timer_
Segment::State	hold_end_state_
	Preallocated workspace variable.
Segment::State	hold_start_state_
	Preallocated workspace variable.
TrajectoryPtr	hold_trajectory_ptr_
	Last hold trajectory values.
HwIfaceAdapter	hw_iface_adapter_
	Adapts desired trajectory state to HW interface.
std::vector< std::string >	joint_names_
	Controlled joint names.
std::vector< JointHandle >	joints_
	Handles to controlled joints.
ros::Time	last_state_publish_time_
std::string	name_
	Controller name.
ros::ServiceServer	query_state_service_
RealtimeGoalHandlePtr	rt_active_goal_
	Currently active action goal, if any.
Segment::State	state_error_
	Preallocated workspace variable.
StatePublisherPtr	state_publisher_
ros::Duration	state_publisher_period_
Segment::Time	stop_trajectory_duration_
realtime_tools::RealtimeBuffer < TimeData >	time_data_
ros::Subscriber	trajectory_command_sub_
bool	verbose_
	Hard coded verbose flag to help in debugging.

Detailed Description

template<class SegmentImpl, class HardwareInterface>
class joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >

Controller for executing joint-space trajectories on a group of joints.

Note:: Non-developer documentation and usage instructions can be found in the package's ROS wiki page.

Template Parameters:

SegmentImpl

Trajectory segment representation to use. The type must comply with the following structure:

 class FooSegment
 {
 public:
   // Required types
   typedef double                 Scalar; // Scalar can be anything convertible to double
   typedef Scalar                 Time;
   typedef PosVelAccState<Scalar> State;

   // Default constructor
   FooSegment();

   // Constructor from start and end states (boundary conditions)
   FooSegment(const Time&  start_time,
              const State& start_state,
              const Time&  end_time,
              const State& end_state);

   // Start and end states initializer (the guts of the above constructor)
   // May throw std::invalid_argument if parameters are invalid
   void init(const Time&  start_time,
             const State& start_state,
             const Time&  end_time,
             const State& end_state);

   // Sampler (realtime-safe)
   void sample(const Time& time, State& state) const;

   // Accesors (realtime-safe)
   Time startTime()    const;
   Time endTime()      const;
   unsigned int size() const;
 };

HardwareInterface Controller hardware interface. Currently hardware_interface::PositionJointInterface and hardware_interface::EffortJointInterface are supported out-of-the-box.

Definition at line 125 of file joint_trajectory_controller.h.

Member Typedef Documentation

template<class SegmentImpl , class HardwareInterface >

typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::ActionServer [private]

Definition at line 158 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef boost::shared_ptr<ActionServer> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::ActionServerPtr [private]

Definition at line 159 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef ActionServer::GoalHandle joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::GoalHandle [private]

Definition at line 160 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef HardwareInterfaceAdapter<HardwareInterface, typename Segment::State> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::HwIfaceAdapter [private]

Definition at line 173 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef HardwareInterface::ResourceHandleType joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::JointHandle [private]

Definition at line 174 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef trajectory_msgs::JointTrajectory::ConstPtr joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::JointTrajectoryConstPtr [private]

Definition at line 163 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::RealtimeGoalHandle [private]

Definition at line 161 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef boost::shared_ptr<RealtimeGoalHandle> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::RealtimeGoalHandlePtr [private]

Definition at line 162 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef Segment::Scalar joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::Scalar [private]

Definition at line 171 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef JointTrajectorySegment<SegmentImpl> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::Segment [private]

Definition at line 167 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef realtime_tools::RealtimePublisher<control_msgs::JointTrajectoryControllerState> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::StatePublisher [private]

Definition at line 164 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef boost::scoped_ptr<StatePublisher> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::StatePublisherPtr [private]

Definition at line 165 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef std::vector<Segment> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::Trajectory [private]

Definition at line 168 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef realtime_tools::RealtimeBox<TrajectoryPtr> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::TrajectoryBox [private]

Definition at line 170 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

typedef boost::shared_ptr<Trajectory> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::TrajectoryPtr [private]

Definition at line 169 of file joint_trajectory_controller.h.

Constructor & Destructor Documentation

template<class SegmentImpl , class HardwareInterface >

joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::JointTrajectoryController ( )

Definition at line 238 of file joint_trajectory_controller_impl.h.

Member Function Documentation

template<class SegmentImpl , class HardwareInterface >

void joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::cancelCB ( GoalHandle gh ) [private]

Definition at line 583 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

void joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::checkGoalTolerances	(	const typename Segment::State &	state_error,
		const Segment &	segment
	)		`[inline, private]`

Check goal tolerances.

If goal tolerances are fulfilled, the currently active action goal will be considered successful. If they are violated, the action goal will be aborted.

Parameters:

state_error	Error between the current and desired trajectory states.
segment	Currently active trajectory segment.

Precondition:: segment is associated to the active goal handle.

Definition at line 200 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

void joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::checkPathTolerances	(	const typename Segment::State &	state_error,
		const Segment &	segment
	)		`[inline, private]`

Check path tolerances.

If path tolerances are violated, the currently active action goal will be aborted.

Parameters:

state_error	Error between the current and desired trajectory states.
segment	Currently active trajectory segment.

Precondition:: segment is associated to the active goal handle.

Definition at line 184 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

void joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::goalCB ( GoalHandle gh ) [private]

Definition at line 527 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

bool joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::init	(	HardwareInterface *	hw,
		ros::NodeHandle &	root_nh,
		ros::NodeHandle &	controller_nh
	)		`[virtual]`

Reimplemented from controller_interface::Controller< HardwareInterface >.

Definition at line 244 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

void joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::preemptActiveGoal ( ) [inline, private]

Definition at line 169 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

void joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::publishState ( const ros::Time & time ) [private]

Publish current controller state at a throttled frequency.

Note:: This method is realtime-safe and is meant to be called from update, as it shares data with it without any locking.

Definition at line 646 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

bool joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::queryStateService	(	control_msgs::QueryTrajectoryState::Request &	req,
		control_msgs::QueryTrajectoryState::Response &	resp
	)		`[private]`

Definition at line 607 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

void joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::setHoldPosition ( const ros::Time & time ) [private]

Hold the current position.

Substitutes the current trajectory with a single-segment one going from the current position and velocity to the current position and zero velocity.

Note:: This method is realtime-safe.

Definition at line 671 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

void joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::starting ( const ros::Time & time ) [inline, virtual]

Holds the current position.

Reimplemented from controller_interface::ControllerBase.

Definition at line 134 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

void joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::stopping ( const ros::Time & time ) [inline, virtual]

Cancels the active action goal, if any.

Reimplemented from controller_interface::ControllerBase.

Definition at line 154 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

void joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::trajectoryCommandCB ( const JointTrajectoryConstPtr & msg ) [inline, private]

Definition at line 161 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

void joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::update	(	const ros::Time &	time,
		const ros::Duration &	period
	)		`[virtual]`

Implements controller_interface::ControllerBase.

Definition at line 374 of file joint_trajectory_controller_impl.h.

template<class SegmentImpl , class HardwareInterface >

bool joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::updateTrajectoryCommand	(	const JointTrajectoryConstPtr &	msg,
		RealtimeGoalHandlePtr	gh
	)		`[private]`

Definition at line 450 of file joint_trajectory_controller_impl.h.

Member Data Documentation

template<class SegmentImpl , class HardwareInterface >

ros::Duration joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::action_monitor_period_ [private]

Definition at line 205 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

ActionServerPtr joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::action_server_ [private]

Definition at line 212 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

std::vector<bool> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::angle_wraparound_ [private]

Whether controlled joints wrap around or not.

Definition at line 179 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

ros::NodeHandle joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::controller_nh_ [private]

Definition at line 210 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

TrajectoryBox joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::curr_trajectory_box_ [private]

Thread-safe container with a smart pointer to trajectory currently being followed. Can be either a hold trajectory or a trajectory received from a ROS message.

We store the hold trajectory in a separate class member because the starting(time) method must be realtime-safe. The (single segment) hold trajectory is preallocated at initialization time and its size is kept unchanged.

Definition at line 193 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

Segment::State joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::current_state_ [private]

Preallocated workspace variable.

Definition at line 196 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

SegmentTolerances<Scalar> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::default_tolerances_ [private]

Default trajectory segment tolerances.

Definition at line 181 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >

Segment::State joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::desired_state_ [private]

Preallocated workspace variable.