31 #ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_H 32 #define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_H 41 #include <boost/shared_ptr.hpp> 42 #include <boost/scoped_ptr.hpp> 43 #include <boost/dynamic_bitset.hpp> 52 #include <control_msgs/FollowJointTrajectoryAction.h> 53 #include <control_msgs/JointTrajectoryControllerState.h> 54 #include <control_msgs/QueryTrajectoryState.h> 55 #include <trajectory_msgs/JointTrajectory.h> 126 template <
class SegmentImpl,
class HardwareInterface>
178 typedef typename HardwareInterface::ResourceHandleType
JointHandle;
225 virtual bool updateTrajectoryCommand(
const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh, std::string* error_string = 0);
227 virtual void goalCB(GoalHandle gh);
228 virtual void cancelCB(GoalHandle gh);
231 control_msgs::QueryTrajectoryState::Response& resp);
256 #endif // header guard ActionServerPtr action_server_
ActionServer::GoalHandle GoalHandle
Segment::State current_state_
Preallocated workspace variable.
boost::scoped_ptr< StatePublisher > StatePublisherPtr
ros::NodeHandle controller_nh_
Segment::State state_error_
Preallocated workspace variable.
ros::Time last_state_publish_time_
HwIfaceAdapter hw_iface_adapter_
Adapts desired trajectory state to HW interface.
Segment::State desired_joint_state_
Preallocated workspace variable.
Controller for executing joint-space trajectories on a group of joints.
virtual void cancelCB(GoalHandle gh)
trajectory_msgs::JointTrajectory::ConstPtr JointTrajectoryConstPtr
JointTrajectoryController()
boost::shared_ptr< ActionServer > ActionServerPtr
HardwareInterface::ResourceHandleType JointHandle
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_
boost::shared_ptr< RealtimeGoalHandle > RealtimeGoalHandlePtr
JointTrajectorySegment< SegmentImpl > Segment
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.
ros::Duration state_publisher_period_
realtime_tools::RealtimeServerGoalHandle< control_msgs::FollowJointTrajectoryAction > RealtimeGoalHandle
std::vector< Segment > TrajectoryPerJoint
boost::shared_ptr< TrajectoryPerJoint > TrajectoryPerJointPtr
virtual void trajectoryCommandCB(const JointTrajectoryConstPtr &msg)
realtime_tools::RealtimePublisher< control_msgs::JointTrajectoryControllerState > StatePublisher
void setHoldPosition(const ros::Time &time, RealtimeGoalHandlePtr gh=RealtimeGoalHandlePtr())
Hold the current position.
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)
boost::shared_ptr< Trajectory > TrajectoryPtr
RealtimeGoalHandlePtr rt_active_goal_
Currently active action goal, if any.
bool init(HardwareInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
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_
HardwareInterfaceAdapter< HardwareInterface, typename Segment::State > HwIfaceAdapter
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)
realtime_tools::RealtimeBuffer< TimeData > time_data_
SegmentTolerances< Scalar > default_tolerances_
Default trajectory segment tolerances.
std::vector< std::string > joint_names_
Controlled joint names.
realtime_tools::RealtimeBox< TrajectoryPtr > TrajectoryBox
boost::dynamic_bitset successful_joint_traj_
void stopping(const ros::Time &)
Cancels the active action goal, if any.
std::string name_
Controller name.
Trajectory segment tolerances.
std::vector< bool > angle_wraparound_
Whether controlled joints wrap around or not.
actionlib::ActionServer< control_msgs::FollowJointTrajectoryAction > ActionServer
ros::ServiceServer query_state_service_
bool allow_partial_joints_goal_
TrajectoryBox curr_trajectory_box_