00001 /* 00002 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 00018 #ifndef COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_H 00019 #define COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_H 00020 00021 #include <vector> 00022 #include <string> 00023 #include <boost/shared_ptr.hpp> 00024 00025 #include <ros/ros.h> 00026 #include <tf/transform_listener.h> 00027 #include <tf/transform_broadcaster.h> 00028 #include <tf/transform_datatypes.h> 00029 00030 #include <actionlib/server/simple_action_server.h> 00031 #include <cob_cartesian_controller/CartesianControllerAction.h> 00032 00033 #include <cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h> 00034 #include <cob_cartesian_controller/cartesian_controller_data_types.h> 00035 #include <cob_cartesian_controller/cartesian_controller_utils.h> 00036 00037 typedef actionlib::SimpleActionServer<cob_cartesian_controller::CartesianControllerAction> SAS_CartesianControllerAction_t; 00038 00039 #define DEFAULT_CARTESIAN_TARGET "cartesian_target" 00040 00041 class CartesianController 00042 { 00043 public: 00044 bool initialize(); 00045 00046 // Main functions 00047 bool posePathBroadcaster(const geometry_msgs::PoseArray& cartesian_path); 00048 00049 // Helper function 00050 bool startTracking(); 00051 bool stopTracking(); 00052 00054 void goalCallback(); 00055 void preemptCallback(); 00056 void actionSuccess(const bool success, const std::string& message); 00057 void actionPreempt(const bool success, const std::string& message); 00058 void actionAbort(const bool success, const std::string& message); 00059 00060 cob_cartesian_controller::CartesianActionStruct acceptGoal(boost::shared_ptr<const cob_cartesian_controller::CartesianControllerGoal> goal); 00061 cob_cartesian_controller::MoveLinStruct convertMoveLin(const cob_cartesian_controller::MoveLin& move_lin_msg); 00062 cob_cartesian_controller::MoveCircStruct convertMoveCirc(const cob_cartesian_controller::MoveCirc& move_circ_msg); 00063 00064 private: 00065 ros::NodeHandle nh_; 00066 tf::TransformListener tf_listener_; 00067 tf::TransformBroadcaster tf_broadcaster_; 00068 00069 ros::ServiceClient start_tracking_; 00070 ros::ServiceClient stop_tracking_; 00071 bool tracking_; 00072 00073 double update_rate_; 00074 std::string root_frame_, chain_tip_link_, target_frame_; 00075 00077 std::string action_name_; 00078 boost::shared_ptr<SAS_CartesianControllerAction_t> as_; 00079 cob_cartesian_controller::CartesianControllerFeedback action_feedback_; 00080 cob_cartesian_controller::CartesianControllerResult action_result_; 00081 00082 CartesianControllerUtils utils_; 00083 boost::shared_ptr< TrajectoryInterpolator > trajectory_interpolator_; 00084 }; 00085 00086 #endif // COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_H