Go to the documentation of this file.
37 #ifndef JOINT_TRAJECTORY_ACTION_CONTROLLER_H__
38 #define JOINT_TRAJECTORY_ACTION_CONTROLLER_H__
42 #include <boost/scoped_ptr.hpp>
43 #include <boost/shared_ptr.hpp>
55 #include <control_msgs/FollowJointTrajectoryAction.h>
56 #include <trajectory_msgs/JointTrajectory.h>
57 #include <pr2_controllers_msgs/QueryTrajectoryState.h>
58 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
59 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
64 template <
class Action>
72 #if ((actionlib_VERSION_MAJOR == 1) && (actionlib_VERSION_MINOR < 12)) || (actionlib_VERSION_MAJOR < 1)
97 void setAborted(ResultConstPtr result = ResultConstPtr((Result*)NULL))
106 void setSucceeded(ResultConstPtr result = ResultConstPtr((Result*)NULL))
122 using namespace actionlib_msgs;
126 if (
req_abort_ && gs.status == GoalStatus::ACTIVE)
133 else if (
req_succeed_ && gs.status == GoalStatus::ACTIVE)
147 JointTolerance(
double _position = 0,
double _velocity = 0,
double _acceleration = 0) :
148 position(_position), velocity(_velocity), acceleration(_acceleration)
152 bool violated(
double p_err,
double v_err = 0,
double a_err = 0)
const
155 (position > 0 && fabs(p_err) > position) ||
156 (velocity > 0 && fabs(v_err) > velocity) ||
157 (acceleration > 0 && fabs(a_err) > acceleration);
192 std::vector<pr2_mechanism_model::JointState*>
joints_;
194 std::vector<control_toolbox::Pid>
pids_;
196 std::vector<control_toolbox::LimitedProxy>
proxies_;
212 void commandCB(
const trajectory_msgs::JointTrajectory::ConstPtr &msg);
215 bool queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req,
216 pr2_controllers_msgs::QueryTrajectoryState::Response &resp);
236 void commandTrajectory(
const trajectory_msgs::JointTrajectory::ConstPtr &traj,
240 void preemptActiveGoal();
274 std::vector<double> q, qd,
qdd;
279 static void sampleSplineWithTimeBounds(
const std::vector<double>& coefficients,
double duration,
double time,
280 double& position,
double& velocity,
double& acceleration);
actionlib::ServerGoalHandle< Action > GoalHandle
std::vector< boost::shared_ptr< filters::FilterChain< double > > > output_filters_
boost::shared_ptr< const Goal > getGoal() const
std::vector< control_toolbox::Pid > pids_
boost::shared_ptr< RTGoalHandle > rt_active_goal_
JointTolerance(double _position=0, double _velocity=0, double _acceleration=0)
void runNonRT(const ros::TimerEvent &te)
std::vector< double > coef
ResultPtr preallocated_result_
actionlib::ActionServer< control_msgs::FollowJointTrajectoryAction > FJTAS
std::vector< double > qdd
double default_goal_time_constraint_
boost::shared_ptr< Result > ResultPtr
ros::Subscriber sub_command_
std::vector< control_toolbox::LimitedProxy > proxies_
actionlib_msgs::GoalStatus getGoalStatus() const
ACTION_DEFINITION(Action)
RTServerGoalHandle< control_msgs::FollowJointTrajectoryAction > RTGoalHandleFollow
RTServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result=ResultPtr((Result *) NULL))
FJTAS::GoalHandle GoalHandleFollow
ResultConstPtr req_result_
boost::shared_ptr< Feedback > FeedbackPtr
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
bool violated(double p_err, double v_err=0, double a_err=0) const
std::vector< double > masses_
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
RTServerGoalHandle< pr2_controllers_msgs::JointTrajectoryAction > RTGoalHandle
ros::Timer goal_handle_timer_
realtime_tools::RealtimeBox< boost::shared_ptr< const SpecifiedTrajectory > > current_trajectory_box_
std::vector< JointTolerance > default_goal_tolerance_
boost::scoped_ptr< FJTAS > action_server_follow_
std::vector< Segment > SpecifiedTrajectory
void setSucceeded(ResultConstPtr result=ResultConstPtr((Result *) NULL))
std::vector< Spline > splines
std::vector< bool > proxies_enabled_
std::vector< JointTolerance > default_trajectory_tolerance_
boost::shared_ptr< RTGoalHandleFollow > rt_active_goal_follow_
boost::shared_ptr< RTGoalHandle > gh
pr2_mechanism_model::RobotState * robot_
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
boost::shared_ptr< RTGoalHandleFollow > gh_follow
std::vector< pr2_mechanism_model::JointState * > joints_
double goal_time_tolerance
void init(const M_string &remappings)
boost::scoped_ptr< JTAS > action_server_
JTAS::GoalHandle GoalHandle
std::vector< JointTolerance > trajectory_tolerance
FeedbackPtr preallocated_feedback_
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_controllers_msgs::JointTrajectoryControllerState > > controller_state_publisher_
void setAborted(ResultConstPtr result=ResultConstPtr((Result *) NULL))
actionlib::ActionServer< pr2_controllers_msgs::JointTrajectoryAction > JTAS
ros::ServiceServer serve_query_state_
std::vector< JointTolerance > goal_tolerance