joint_trajectory_action_controller.h
Go to the documentation of this file.
00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2010  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * joint_trajectory_action_controller.h
00020  *
00021  *  Created on: 07.12.2010
00022  *      Author: Martin Günther <mguenthe@uos.de>
00023  */
00024 
00025 #ifndef JOINT_TRAJECTORY_ACTION_CONTROLLER_H__
00026 #define JOINT_TRAJECTORY_ACTION_CONTROLLER_H__
00027 
00028 #include <vector>
00029 
00030 #include <ros/node_handle.h>
00031 
00032 #include <actionlib/server/simple_action_server.h>
00033 #include <actionlib/client/simple_action_client.h>
00034 
00035 #include <trajectory_msgs/JointTrajectory.h>
00036 #include <pr2_controllers_msgs/QueryTrajectoryState.h>
00037 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00038 
00039 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00040 #include <control_msgs/FollowJointTrajectoryAction.h>
00041 
00042 #include <katana/AbstractKatana.h>
00043 #include <katana/SpecifiedTrajectory.h>
00044 #include <katana/spline_functions.h>
00045 
00046 namespace katana
00047 {
00048 
00049 class JointTrajectoryActionController
00050 {
00051 
00052 // Action typedefs for the original PR2 specific joint trajectory action
00053 typedef actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction> JTAS;
00054 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> JTAC;
00055 
00056 // Action typedefs for the new follow joint trajectory action
00057 typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> FJTAS;
00058 
00059 public:
00060   JointTrajectoryActionController(boost::shared_ptr<AbstractKatana> katana);
00061   virtual ~JointTrajectoryActionController();
00062 
00063   void reset_trajectory_and_stop();
00064   void update();
00065 
00066 private:
00067   // additional control_msgs::FollowJointTrajectoryResult
00068   enum { PREEMPT_REQUESTED = -1000 };
00069 
00070   // robot and joint state
00071   std::vector<std::string> joints_; // controlled joints, same order as expected by the KNI (index 0 = first motor, ...)
00072   boost::shared_ptr<AbstractKatana> katana_;
00073 
00074   // parameters
00075 //  double goal_time_constraint_;
00076   double stopped_velocity_tolerance_;
00077   std::vector<double> goal_constraints_;
00078 
00079   // subscriber to "command" topic
00080   void commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg);
00081   ros::Subscriber sub_command_;
00082 
00083   // "query_state" service
00084   bool queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req,
00085                          pr2_controllers_msgs::QueryTrajectoryState::Response &resp);
00086   ros::ServiceServer serve_query_state_;
00087 
00088   // publisher to "state" topic
00089   ros::Publisher controller_state_publisher_;
00090 
00091   // action servers
00092   JTAS action_server_;
00093   FJTAS action_server_follow_;
00094   void executeCB(const JTAS::GoalConstPtr &goal);
00095   void executeCBFollow(const FJTAS::GoalConstPtr &goal);
00096   int executeCommon(const trajectory_msgs::JointTrajectory &trajectory, boost::function<bool ()> isPreemptRequested);
00097 
00098   boost::shared_ptr<SpecifiedTrajectory> current_trajectory_;
00099 
00100   boost::shared_ptr<SpecifiedTrajectory> calculateTrajectory(const trajectory_msgs::JointTrajectory &msg);
00101 
00102   bool validJoints(std::vector<std::string> new_joint_names);
00103 
00104   std::vector<int> makeJointsLookup(const trajectory_msgs::JointTrajectory &msg);
00105 
00106   bool validTrajectory(const SpecifiedTrajectory &traj);
00107 
00108   bool goalReached();
00109 
00110   bool allJointsStopped();
00111 };
00112 }
00113 
00114 #endif /* JOINT_TRAJECTORY_ACTION_CONTROLLER_H_ */


katana
Author(s): Martin Günther
autogenerated on Mon Oct 6 2014 10:46:04