Go to the documentation of this file.
41 #include <boost/shared_ptr.hpp>
42 #include <boost/dynamic_bitset.hpp>
51 #include <control_msgs/FollowJointTrajectoryAction.h>
52 #include <control_msgs/JointTrajectoryControllerState.h>
53 #include <control_msgs/QueryTrajectoryState.h>
54 #include <trajectory_msgs/JointTrajectory.h>
127 template <
class SegmentImpl,
class HardwareInterface>
179 typedef typename HardwareInterface::ResourceHandleType
JointHandle;
236 control_msgs::QueryTrajectoryState::Response& resp);
Trajectory segment tolerances.
std::unique_ptr< StatePublisher > StatePublisherPtr
std::vector< Segment > TrajectoryPerJoint
TrajectoryPtr hold_trajectory_ptr_
Last hold trajectory values.
HardwareInterface::ResourceHandleType JointHandle
trajectory_msgs::JointTrajectory::ConstPtr JointTrajectoryConstPtr
realtime_tools::RealtimePublisher< control_msgs::JointTrajectoryControllerState > StatePublisher
virtual void goalCB(GoalHandle gh)
actionlib::ActionServer< control_msgs::FollowJointTrajectoryAction > ActionServer
void setActionFeedback()
Updates the pre-allocated feedback of the current active goal (if any) based on the current state val...
std::vector< JointHandle > joints_
Handles to controlled joints.
ActionServer::GoalHandle GoalHandle
realtime_tools::RealtimeBuffer< TimeData > time_data_
RealtimeGoalHandlePtr rt_active_goal_
Currently active action goal, if any.
ros::Time time
Time of last update cycle.
Segment::State desired_state_
Preallocated workspace variable.
HwIfaceAdapter hw_iface_adapter_
Adapts desired trajectory state to HW interface.
ros::Time uptime
Controller uptime. Set to zero at every restart.
virtual bool queryStateService(control_msgs::QueryTrajectoryState::Request &req, control_msgs::QueryTrajectoryState::Response &resp)
ros::Timer goal_handle_timer_
ros::ServiceServer query_state_service_
JointTrajectorySegment< SegmentImpl > Segment
void starting(const ros::Time &time)
Holds the current position.
TrajectoryBox curr_trajectory_box_
boost::dynamic_bitset successful_joint_traj_
void setHoldPosition(const ros::Time &time, RealtimeGoalHandlePtr gh=RealtimeGoalHandlePtr())
Hold the current position.
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...
Class representing a multi-dimensional quintic spline segment with a start and end time.
std::shared_ptr< Trajectory > TrajectoryPtr
void publishState(const ros::Time &time)
Publish current controller state at a throttled frequency.
ros::Duration period
Period of last update cycle.
void update(const ros::Time &time, const ros::Duration &period)
SegmentTolerances< Scalar > default_tolerances_
Default trajectory segment tolerances.
std::vector< std::string > joint_names_
Controlled joint names.
virtual void trajectoryCommandCB(const JointTrajectoryConstPtr &msg)
Segment::State state_joint_error_
Preallocated workspace variable.
std::vector< TrajectoryPerJoint > Trajectory
HardwareInterfaceAdapter< HardwareInterface, typename Segment::State > HwIfaceAdapter
bool init(HardwareInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
std::shared_ptr< TrajectoryPerJoint > TrajectoryPerJointPtr
std::shared_ptr< ActionServer > ActionServerPtr
ros::NodeHandle controller_nh_
virtual bool updateTrajectoryCommand(const JointTrajectoryConstPtr &msg, RealtimeGoalHandlePtr gh, std::string *error_string=nullptr)
realtime_tools::RealtimeServerGoalHandle< control_msgs::FollowJointTrajectoryAction > RealtimeGoalHandle
ros::Duration state_publisher_period_
Segment::State desired_joint_state_
Preallocated workspace variable.
void stopping(const ros::Time &)
Cancels the active action goal, if any.
Controller for executing joint-space trajectories on a group of joints.
static TrajectoryPtr createHoldTrajectory(const unsigned int &number_of_joints)
Returns a trajectory consisting of joint trajectories with one pre-allocated segment.
Segment::State state_error_
Preallocated workspace variable.
virtual void preemptActiveGoal()
JointTrajectoryController()
virtual void cancelCB(GoalHandle gh)
ros::Subscriber trajectory_command_sub_
boost::shared_ptr< RealtimeGoalHandle > RealtimeGoalHandlePtr
unsigned int getNumberOfJoints() const
Returns the number of joints of the robot.
Segment::State old_desired_state_
Preallocated workspace variable.
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 ...
Segment::State current_state_
Preallocated workspace variable.
bool allow_partial_joints_goal_
ros::Time last_state_publish_time_
std::vector< bool > angle_wraparound_
Whether controlled joints wrap around or not.
ActionServerPtr action_server_
std::unique_ptr< TrajectoryBuilder< SegmentImpl > > hold_traj_builder_
ros::Duration action_monitor_period_
realtime_tools::RealtimeBox< TrajectoryPtr > TrajectoryBox
StatePublisherPtr state_publisher_
std::string name_
Controller name.
Segment::Time stop_trajectory_duration_
Duration for stop ramp. If zero, the controller stops at the actual position.
bool verbose_
Hard coded verbose flag to help in debugging.