Go to the documentation of this file.00001
00002
00003
00004
00005 #ifndef ROBOTICAN_CONTROLLERS_JOINT_WITH_EFFORT_TRAJECTORY_CONTROLLER_H_H
00006 #define ROBOTICAN_CONTROLLERS_JOINT_WITH_EFFORT_TRAJECTORY_CONTROLLER_H_H
00007
00008
00009 #include <cassert>
00010 #include <iterator>
00011 #include <stdexcept>
00012 #include <string>
00013
00014
00015 #include <boost/shared_ptr.hpp>
00016 #include <boost/scoped_ptr.hpp>
00017
00018
00019 #include <ros/node_handle.h>
00020
00021
00022 #include <urdf/model.h>
00023
00024
00025 #include <control_msgs/FollowJointTrajectoryAction.h>
00026 #include <control_msgs/JointTrajectoryControllerState.h>
00027 #include <control_msgs/QueryTrajectoryState.h>
00028 #include <trajectory_msgs/JointTrajectory.h>
00029
00030
00031 #include <actionlib/server/action_server.h>
00032
00033
00034 #include <realtime_tools/realtime_box.h>
00035 #include <realtime_tools/realtime_buffer.h>
00036 #include <realtime_tools/realtime_publisher.h>
00037
00038
00039 #include <realtime_tools/realtime_server_goal_handle.h>
00040 #include <controller_interface/controller.h>
00041 #include <hardware_interface/joint_command_interface.h>
00042 #include <hardware_interface/internal/demangle_symbol.h>
00043
00044
00045 #include <trajectory_interface/trajectory_interface.h>
00046
00047 #include <joint_trajectory_controller/joint_trajectory_segment.h>
00048 #include <joint_trajectory_controller/init_joint_trajectory.h>
00049 #include <joint_trajectory_controller/hardware_interface_adapter.h>
00050
00051 using namespace joint_trajectory_controller;
00052
00053 namespace joint_with_effort_trajectory_controller
00054 {
00055
00101 template <class SegmentImpl, class HardwareInterface>
00102 class JointTrajectoryController : public controller_interface::Controller<HardwareInterface>
00103 {
00104 public:
00105
00106 JointTrajectoryController();
00107
00110 bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
00111
00112
00116 void starting(const ros::Time& time);
00117
00119 void stopping(const ros::Time& );
00120
00121 void update(const ros::Time& time, const ros::Duration& period);
00122
00123
00124 private:
00125
00126 struct TimeData
00127 {
00128 TimeData() : time(0.0), period(0.0), uptime(0.0) {}
00129
00130 ros::Time time;
00131 ros::Duration period;
00132 ros::Time uptime;
00133 };
00134
00135 typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> ActionServer;
00136 typedef boost::shared_ptr<ActionServer> ActionServerPtr;
00137 typedef ActionServer::GoalHandle GoalHandle;
00138 typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction> RealtimeGoalHandle;
00139 typedef boost::shared_ptr<RealtimeGoalHandle> RealtimeGoalHandlePtr;
00140 typedef trajectory_msgs::JointTrajectory::ConstPtr JointTrajectoryConstPtr;
00141 typedef realtime_tools::RealtimePublisher<control_msgs::JointTrajectoryControllerState> StatePublisher;
00142 typedef boost::scoped_ptr<StatePublisher> StatePublisherPtr;
00143
00144 typedef JointTrajectorySegment<SegmentImpl> Segment;
00145 typedef std::vector<Segment> Trajectory;
00146 typedef boost::shared_ptr<Trajectory> TrajectoryPtr;
00147 typedef realtime_tools::RealtimeBox<TrajectoryPtr> TrajectoryBox;
00148 typedef typename Segment::Scalar Scalar;
00149
00150 typedef HardwareInterfaceAdapter<HardwareInterface, typename Segment::State> HwIfaceAdapter;
00151 typedef typename HardwareInterface::ResourceHandleType JointHandle;
00152
00153 bool verbose_;
00154 std::string name_;
00155 std::vector<JointHandle> joints_;
00156 std::vector<bool> angle_wraparound_;
00157 std::vector<std::string> joint_names_;
00158 SegmentTolerances<Scalar> default_tolerances_;
00159 HwIfaceAdapter hw_iface_adapter_;
00160
00161 RealtimeGoalHandlePtr rt_active_goal_;
00162
00170 TrajectoryBox curr_trajectory_box_;
00171 TrajectoryPtr hold_trajectory_ptr_;
00172
00173 typename Segment::State current_state_;
00174 typename Segment::State desired_state_;
00175 typename Segment::State state_error_;
00176 typename Segment::State hold_start_state_;
00177 typename Segment::State hold_end_state_;
00178
00179 realtime_tools::RealtimeBuffer<TimeData> time_data_;
00180
00181 ros::Duration state_publisher_period_;
00182 ros::Duration action_monitor_period_;
00183
00184 typename Segment::Time stop_trajectory_duration_;
00185
00186
00187 ros::NodeHandle controller_nh_;
00188 ros::Subscriber trajectory_command_sub_;
00189 ActionServerPtr action_server_;
00190 ros::ServiceServer query_state_service_;
00191 StatePublisherPtr state_publisher_;
00192
00193 ros::Timer goal_handle_timer_;
00194 ros::Time last_state_publish_time_;
00195 double _currentEffort;
00196 double _goalEffort;
00197 bool updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh);
00198 void trajectoryCommandCB(const JointTrajectoryConstPtr& msg);
00199 void goalCB(GoalHandle gh);
00200 void cancelCB(GoalHandle gh);
00201 void preemptActiveGoal();
00202 bool queryStateService(control_msgs::QueryTrajectoryState::Request& req,
00203 control_msgs::QueryTrajectoryState::Response& resp);
00204
00210 void publishState(const ros::Time& time);
00211
00219 void setHoldPosition(const ros::Time& time);
00220
00231 void checkPathTolerances(const typename Segment::State& state_error,
00232 const Segment& segment);
00233
00245 void checkGoalTolerances(const typename Segment::State& state_error,
00246 const Segment& segment);
00247
00248 };
00249
00250 }
00251
00252 #include <robotican_controllers/joint_with_effort_trajectory_controller_impl.h>
00253
00254 #endif //ROBOTICAN_CONTROLLERS_JOINT_WITH_EFFORT_TRAJECTORY_CONTROLLER_H_H