Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00028
00030
00031 #ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_H
00032 #define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_H
00033
00034
00035 #include <cassert>
00036 #include <iterator>
00037 #include <stdexcept>
00038 #include <string>
00039
00040
00041 #include <boost/shared_ptr.hpp>
00042 #include <boost/scoped_ptr.hpp>
00043
00044
00045 #include <ros/node_handle.h>
00046
00047
00048 #include <urdf/model.h>
00049
00050
00051 #include <control_msgs/FollowJointTrajectoryAction.h>
00052 #include <control_msgs/JointTrajectoryControllerState.h>
00053 #include <control_msgs/QueryTrajectoryState.h>
00054 #include <trajectory_msgs/JointTrajectory.h>
00055
00056
00057 #include <actionlib/server/action_server.h>
00058
00059
00060 #include <realtime_tools/realtime_box.h>
00061 #include <realtime_tools/realtime_buffer.h>
00062 #include <realtime_tools/realtime_publisher.h>
00063
00064
00065 #include <realtime_tools/realtime_server_goal_handle.h>
00066 #include <controller_interface/controller.h>
00067 #include <hardware_interface/joint_command_interface.h>
00068 #include <hardware_interface/internal/demangle_symbol.h>
00069
00070
00071 #include <trajectory_interface/trajectory_interface.h>
00072
00073 #include <joint_trajectory_controller/joint_trajectory_segment.h>
00074 #include <joint_trajectory_controller/init_joint_trajectory.h>
00075 #include <joint_trajectory_controller/hardware_interface_adapter.h>
00076
00077 namespace joint_trajectory_controller
00078 {
00079
00124 template <class SegmentImpl, class HardwareInterface>
00125 class JointTrajectoryController : public controller_interface::Controller<HardwareInterface>
00126 {
00127 public:
00128
00129 JointTrajectoryController();
00130
00133 bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
00134
00135
00139 void starting(const ros::Time& time);
00140
00142 void stopping(const ros::Time& time);
00143
00144 void update(const ros::Time& time, const ros::Duration& period);
00145
00146
00147 private:
00148
00149 struct TimeData
00150 {
00151 TimeData() : time(0.0), period(0.0), uptime(0.0) {}
00152
00153 ros::Time time;
00154 ros::Duration period;
00155 ros::Time uptime;
00156 };
00157
00158 typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> ActionServer;
00159 typedef boost::shared_ptr<ActionServer> ActionServerPtr;
00160 typedef ActionServer::GoalHandle GoalHandle;
00161 typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction> RealtimeGoalHandle;
00162 typedef boost::shared_ptr<RealtimeGoalHandle> RealtimeGoalHandlePtr;
00163 typedef trajectory_msgs::JointTrajectory::ConstPtr JointTrajectoryConstPtr;
00164 typedef realtime_tools::RealtimePublisher<control_msgs::JointTrajectoryControllerState> StatePublisher;
00165 typedef boost::scoped_ptr<StatePublisher> StatePublisherPtr;
00166
00167 typedef JointTrajectorySegment<SegmentImpl> Segment;
00168 typedef std::vector<Segment> Trajectory;
00169 typedef boost::shared_ptr<Trajectory> TrajectoryPtr;
00170 typedef realtime_tools::RealtimeBox<TrajectoryPtr> TrajectoryBox;
00171 typedef typename Segment::Scalar Scalar;
00172
00173 typedef HardwareInterfaceAdapter<HardwareInterface, typename Segment::State> HwIfaceAdapter;
00174 typedef typename HardwareInterface::ResourceHandleType JointHandle;
00175
00176 bool verbose_;
00177 std::string name_;
00178 std::vector<JointHandle> joints_;
00179 std::vector<bool> angle_wraparound_;
00180 std::vector<std::string> joint_names_;
00181 SegmentTolerances<Scalar> default_tolerances_;
00182 HwIfaceAdapter hw_iface_adapter_;
00183
00184 RealtimeGoalHandlePtr rt_active_goal_;
00185
00193 TrajectoryBox curr_trajectory_box_;
00194 TrajectoryPtr hold_trajectory_ptr_;
00195
00196 typename Segment::State current_state_;
00197 typename Segment::State desired_state_;
00198 typename Segment::State state_error_;
00199 typename Segment::State hold_start_state_;
00200 typename Segment::State hold_end_state_;
00201
00202 realtime_tools::RealtimeBuffer<TimeData> time_data_;
00203
00204 ros::Duration state_publisher_period_;
00205 ros::Duration action_monitor_period_;
00206
00207 typename Segment::Time stop_trajectory_duration_;
00208
00209
00210 ros::NodeHandle controller_nh_;
00211 ros::Subscriber trajectory_command_sub_;
00212 ActionServerPtr action_server_;
00213 ros::ServiceServer query_state_service_;
00214 StatePublisherPtr state_publisher_;
00215
00216 ros::Timer goal_handle_timer_;
00217 ros::Time last_state_publish_time_;
00218
00219 bool updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh);
00220 void trajectoryCommandCB(const JointTrajectoryConstPtr& msg);
00221 void goalCB(GoalHandle gh);
00222 void cancelCB(GoalHandle gh);
00223 void preemptActiveGoal();
00224 bool queryStateService(control_msgs::QueryTrajectoryState::Request& req,
00225 control_msgs::QueryTrajectoryState::Response& resp);
00226
00232 void publishState(const ros::Time& time);
00233
00241 void setHoldPosition(const ros::Time& time);
00242
00253 void checkPathTolerances(const typename Segment::State& state_error,
00254 const Segment& segment);
00255
00267 void checkGoalTolerances(const typename Segment::State& state_error,
00268 const Segment& segment);
00269
00270 };
00271
00272 }
00273
00274 #include <joint_trajectory_controller/joint_trajectory_controller_impl.h>
00275
00276 #endif // header guard