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 #include <string> 00019 #include <vector> 00020 #include <math.h> 00021 00022 #include <ros/ros.h> 00023 00024 #include <actionlib/client/simple_action_client.h> 00025 #include <actionlib/server/simple_action_server.h> 00026 #include <actionlib/client/terminal_state.h> 00027 00028 #include <cob_lookat_action/LookAtAction.h> 00029 #include <control_msgs/FollowJointTrajectoryAction.h> 00030 #include <trajectory_msgs/JointTrajectory.h> 00031 #include <trajectory_msgs/JointTrajectoryPoint.h> 00032 00033 #include <tf/transform_listener.h> 00034 #include <tf/transform_datatypes.h> 00035 #include <tf_conversions/tf_kdl.h> 00036 #include <kdl_conversions/kdl_msg.h> 00037 #include <kdl_parser/kdl_parser.hpp> 00038 #include <kdl/chainfksolverpos_recursive.hpp> 00039 #include <kdl/chainiksolvervel_pinv.hpp> 00040 #include <kdl/chainiksolverpos_nr.hpp> 00041 00042 00043 class CobLookAtAction 00044 { 00045 protected: 00046 00047 ros::NodeHandle nh_; 00048 00049 actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> *fjt_ac_; 00050 actionlib::SimpleActionServer<cob_lookat_action::LookAtAction> *lookat_as_; 00051 std::string fjt_name_; 00052 std::string lookat_name_; 00053 cob_lookat_action::LookAtFeedback lookat_fb_; 00054 cob_lookat_action::LookAtResult lookat_res_; 00055 00056 std::vector<std::string> joint_names_; 00057 std::string chain_base_link_; 00058 std::string chain_tip_link_; 00059 00060 KDL::Chain chain_main_; 00061 boost::shared_ptr<KDL::ChainFkSolverPos_recursive> fk_solver_pos_; 00062 boost::shared_ptr<KDL::ChainIkSolverVel_pinv> ik_solver_vel_; 00063 boost::shared_ptr<KDL::ChainIkSolverPos_NR> ik_solver_pos_; 00064 00065 tf::TransformListener tf_listener_; 00066 00067 public: 00068 00069 CobLookAtAction(std::string action_name) : 00070 fjt_name_("joint_trajectory_controller/follow_joint_trajectory"), 00071 lookat_name_(action_name) 00072 {} 00073 00074 ~CobLookAtAction(void) {} 00075 00076 bool init(); 00077 void goalCB(const cob_lookat_action::LookAtGoalConstPtr &goal); 00078 };