Classes | Public Member Functions | Private Types | Private Member Functions | Private Attributes
joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface > Class Template Reference

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 >:
Inheritance graph
[legend]

List of all members.

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 &)
 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< SegmentTrajectory
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< Scalardefault_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< JointHandlejoints_
 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:
SegmentImplTrajectory 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;
 };
HardwareInterfaceController hardware interface. Currently hardware_interface::PositionJointInterface, hardware_interface::VelocityJointInterface, and hardware_interface::EffortJointInterface are supported out-of-the-box.

Definition at line 126 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 159 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 160 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 161 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 174 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 175 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 164 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 162 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 163 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 172 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 168 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 165 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 166 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 169 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 171 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 170 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_errorError between the current and desired trajectory states.
segmentCurrently 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_errorError between the current and desired trajectory states.
segmentCurrently 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]
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 ) [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]
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 206 of file joint_trajectory_controller.h.

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

Definition at line 213 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 180 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 211 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 194 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 197 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 182 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.

Definition at line 198 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
ros::Timer joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::goal_handle_timer_ [private]

Definition at line 217 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
Segment::State joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::hold_end_state_ [private]

Preallocated workspace variable.

Definition at line 201 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
Segment::State joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::hold_start_state_ [private]

Preallocated workspace variable.

Definition at line 200 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
TrajectoryPtr joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::hold_trajectory_ptr_ [private]

Last hold trajectory values.

Definition at line 195 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
HwIfaceAdapter joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::hw_iface_adapter_ [private]

Adapts desired trajectory state to HW interface.

Definition at line 183 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
std::vector<std::string> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::joint_names_ [private]

Controlled joint names.

Definition at line 181 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
std::vector<JointHandle> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::joints_ [private]

Handles to controlled joints.

Definition at line 179 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
ros::Time joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::last_state_publish_time_ [private]

Definition at line 218 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
std::string joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::name_ [private]

Controller name.

Definition at line 178 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
ros::ServiceServer joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::query_state_service_ [private]

Definition at line 214 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
RealtimeGoalHandlePtr joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::rt_active_goal_ [private]

Currently active action goal, if any.

Definition at line 185 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
Segment::State joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::state_error_ [private]

Preallocated workspace variable.

Definition at line 199 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
StatePublisherPtr joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::state_publisher_ [private]

Definition at line 215 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
ros::Duration joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::state_publisher_period_ [private]

Definition at line 205 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
Segment::Time joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::stop_trajectory_duration_ [private]

Definition at line 208 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
realtime_tools::RealtimeBuffer<TimeData> joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::time_data_ [private]

Definition at line 203 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
ros::Subscriber joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::trajectory_command_sub_ [private]

Definition at line 212 of file joint_trajectory_controller.h.

template<class SegmentImpl , class HardwareInterface >
bool joint_trajectory_controller::JointTrajectoryController< SegmentImpl, HardwareInterface >::verbose_ [private]

Hard coded verbose flag to help in debugging.

Definition at line 177 of file joint_trajectory_controller.h.


The documentation for this class was generated from the following files:


joint_trajectory_controller
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Sat Aug 13 2016 04:20:51