joint_trajectory_action_controller.h
Go to the documentation of this file.
1 /*
2  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
3  * Copyright (C) 2010 University of Osnabrück
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18  *
19  * joint_trajectory_action_controller.h
20  *
21  * Created on: 07.12.2010
22  * Author: Martin Günther <mguenthe@uos.de>
23  */
24 
25 #ifndef JOINT_TRAJECTORY_ACTION_CONTROLLER_H__
26 #define JOINT_TRAJECTORY_ACTION_CONTROLLER_H__
27 
28 #include <vector>
29 
30 #include <ros/node_handle.h>
31 
34 
35 #include <trajectory_msgs/JointTrajectory.h>
36 #include <control_msgs/QueryTrajectoryState.h>
37 #include <control_msgs/JointTrajectoryControllerState.h>
38 
39 #include <control_msgs/JointTrajectoryAction.h>
40 #include <control_msgs/FollowJointTrajectoryAction.h>
41 
42 #include <katana/AbstractKatana.h>
45 
46 namespace katana
47 {
48 
50 {
51 
52 // Action typedefs for the original PR2 specific joint trajectory action
55 
56 // Action typedefs for the new follow joint trajectory action
58 
59 public:
62 
64  void update();
65 
66 private:
67  // additional control_msgs::FollowJointTrajectoryResult
68  enum { PREEMPT_REQUESTED = -1000 };
69 
70  // robot and joint state
71  std::vector<std::string> joints_; // controlled joints, same order as expected by the KNI (index 0 = first motor, ...)
73 
74  // parameters
75 // double goal_time_constraint_;
77  std::vector<double> goal_constraints_;
78 
79  // subscriber to "command" topic
80  void commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg);
82 
83  // "query_state" service
84  bool queryStateService(control_msgs::QueryTrajectoryState::Request &req,
85  control_msgs::QueryTrajectoryState::Response &resp);
87 
88  // publisher to "state" topic
90 
91  // action servers
94  void executeCB(const JTAS::GoalConstPtr &goal);
95  void executeCBFollow(const FJTAS::GoalConstPtr &goal);
96  int executeCommon(const trajectory_msgs::JointTrajectory &trajectory, boost::function<bool ()> isPreemptRequested);
97 
99 
100  boost::shared_ptr<SpecifiedTrajectory> calculateTrajectory(const trajectory_msgs::JointTrajectory &msg);
101 
102  bool validJoints(std::vector<std::string> new_joint_names);
103 
104  std::vector<int> makeJointsLookup(const trajectory_msgs::JointTrajectory &msg);
105 
106  bool validTrajectory(const SpecifiedTrajectory &traj);
107 
108  bool goalReached();
109 
110  bool allJointsStopped();
111 };
112 }
113 
114 #endif /* JOINT_TRAJECTORY_ACTION_CONTROLLER_H_ */
actionlib::SimpleActionClient< control_msgs::JointTrajectoryAction > JTAC
int executeCommon(const trajectory_msgs::JointTrajectory &trajectory, boost::function< bool()> isPreemptRequested)
bool queryStateService(control_msgs::QueryTrajectoryState::Request &req, control_msgs::QueryTrajectoryState::Response &resp)
boost::shared_ptr< SpecifiedTrajectory > calculateTrajectory(const trajectory_msgs::JointTrajectory &msg)
actionlib::SimpleActionServer< control_msgs::FollowJointTrajectoryAction > FJTAS
void commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
std::vector< int > makeJointsLookup(const trajectory_msgs::JointTrajectory &msg)
std::vector< Segment > SpecifiedTrajectory
actionlib::SimpleActionServer< control_msgs::JointTrajectoryAction > JTAS
JointTrajectoryActionController(boost::shared_ptr< AbstractKatana > katana)
boost::shared_ptr< SpecifiedTrajectory > current_trajectory_
bool validJoints(std::vector< std::string > new_joint_names)


katana
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:25