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
00125 template <class SegmentImpl, class HardwareInterface>
00126 class JointTrajectoryController : public controller_interface::Controller<HardwareInterface>
00127 {
00128 public:
00129
00130 JointTrajectoryController();
00131
00134 bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
00135
00136
00140 void starting(const ros::Time& time);
00141
00143 void stopping(const ros::Time& );
00144
00145 void update(const ros::Time& time, const ros::Duration& period);
00146
00147
00148 private:
00149
00150 struct TimeData
00151 {
00152 TimeData() : time(0.0), period(0.0), uptime(0.0) {}
00153
00154 ros::Time time;
00155 ros::Duration period;
00156 ros::Time uptime;
00157 };
00158
00159 typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> ActionServer;
00160 typedef boost::shared_ptr<ActionServer> ActionServerPtr;
00161 typedef ActionServer::GoalHandle GoalHandle;
00162 typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction> RealtimeGoalHandle;
00163 typedef boost::shared_ptr<RealtimeGoalHandle> RealtimeGoalHandlePtr;
00164 typedef trajectory_msgs::JointTrajectory::ConstPtr JointTrajectoryConstPtr;
00165 typedef realtime_tools::RealtimePublisher<control_msgs::JointTrajectoryControllerState> StatePublisher;
00166 typedef boost::scoped_ptr<StatePublisher> StatePublisherPtr;
00167
00168 typedef JointTrajectorySegment<SegmentImpl> Segment;
00169 typedef std::vector<Segment> Trajectory;
00170 typedef boost::shared_ptr<Trajectory> TrajectoryPtr;
00171 typedef realtime_tools::RealtimeBox<TrajectoryPtr> TrajectoryBox;
00172 typedef typename Segment::Scalar Scalar;
00173
00174 typedef HardwareInterfaceAdapter<HardwareInterface, typename Segment::State> HwIfaceAdapter;
00175 typedef typename HardwareInterface::ResourceHandleType JointHandle;
00176
00177 bool verbose_;
00178 std::string name_;
00179 std::vector<JointHandle> joints_;
00180 std::vector<bool> angle_wraparound_;
00181 std::vector<std::string> joint_names_;
00182 SegmentTolerances<Scalar> default_tolerances_;
00183 HwIfaceAdapter hw_iface_adapter_;
00184
00185 RealtimeGoalHandlePtr rt_active_goal_;
00186
00194 TrajectoryBox curr_trajectory_box_;
00195 TrajectoryPtr hold_trajectory_ptr_;
00196
00197 typename Segment::State current_state_;
00198 typename Segment::State desired_state_;
00199 typename Segment::State state_error_;
00200 typename Segment::State hold_start_state_;
00201 typename Segment::State hold_end_state_;
00202
00203 realtime_tools::RealtimeBuffer<TimeData> time_data_;
00204
00205 ros::Duration state_publisher_period_;
00206 ros::Duration action_monitor_period_;
00207
00208 typename Segment::Time stop_trajectory_duration_;
00209
00210
00211 ros::NodeHandle controller_nh_;
00212 ros::Subscriber trajectory_command_sub_;
00213 ActionServerPtr action_server_;
00214 ros::ServiceServer query_state_service_;
00215 StatePublisherPtr state_publisher_;
00216
00217 ros::Timer goal_handle_timer_;
00218 ros::Time last_state_publish_time_;
00219
00220 bool updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh);
00221 void trajectoryCommandCB(const JointTrajectoryConstPtr& msg);
00222 void goalCB(GoalHandle gh);
00223 void cancelCB(GoalHandle gh);
00224 void preemptActiveGoal();
00225 bool queryStateService(control_msgs::QueryTrajectoryState::Request& req,
00226 control_msgs::QueryTrajectoryState::Response& resp);
00227
00233 void publishState(const ros::Time& time);
00234
00242 void setHoldPosition(const ros::Time& time);
00243
00254 void checkPathTolerances(const typename Segment::State& state_error,
00255 const Segment& segment);
00256
00268 void checkGoalTolerances(const typename Segment::State& state_error,
00269 const Segment& segment);
00270
00271 };
00272
00273 }
00274
00275 #include <joint_trajectory_controller/joint_trajectory_controller_impl.h>
00276
00277 #endif // header guard