00001 /****************************************************************************** 00002 * \file manual_grasping_node.h 00003 * \brief Node for manual grasping 00004 * \author Zdenek Materna (imaterna@fit.vutbr.cz) 00005 * 00006 * $Id:$ 00007 * 00008 * Copyright (C) Brno University of Technology 00009 * 00010 * This file is part of software developed by dcgm-robotics@FIT group. 00011 * 00012 * Author: Zdenek Materna (imaterna@fit.vutbr.cz) 00013 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00014 * Date: dd/mm/2012 00015 * 00016 * This file is free software: you can redistribute it and/or modify 00017 * it under the terms of the GNU Lesser General Public License as published by 00018 * the Free Software Foundation, either version 3 of the License, or 00019 * (at your option) any later version. 00020 * 00021 * This file is distributed in the hope that it will be useful, 00022 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00023 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00024 * GNU Lesser General Public License for more details. 00025 * 00026 * You should have received a copy of the GNU Lesser General Public License 00027 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00028 */ 00029 00037 #pragma once 00038 #ifndef GRASPING_NODE_H 00039 #define GRASPING_NODE_H 00040 00041 #include <ros/ros.h> 00042 #include <string> 00043 #include <math.h> 00044 00045 #include <actionlib/server/simple_action_server.h> 00046 #include <actionlib/client/simple_action_client.h> 00047 00048 #include "srs_assisted_grasping/services_list.h" 00049 #include "srs_assisted_grasping/topics_list.h" 00050 00051 #include "srs_assisted_grasping_msgs/ReactiveGraspingAction.h" 00052 //#include <control_msgs/FollowJointTrajectoryAction.h> 00053 00054 #include "std_msgs/MultiArrayLayout.h" 00055 #include "std_msgs/MultiArrayDimension.h" 00056 #include "std_msgs/Int32MultiArray.h" 00057 #include "std_msgs/Float32MultiArray.h" 00058 00059 #include "trajectory_msgs/JointTrajectoryPoint.h" 00060 #include "pr2_controllers_msgs/JointTrajectoryControllerState.h" 00061 00062 #include "cob_srvs/SetOperationMode.h" 00063 #include "brics_actuator/JointVelocities.h" 00064 #include "schunk_sdh/TactileSensor.h" 00065 00066 #include "XmlRpcValue.h" 00067 #include <urdf/model.h> 00068 00069 /*#include <boost/units/io.hpp> 00070 #include <boost/units/systems/angle/degrees.hpp> 00071 #include <boost/units/conversion.hpp>*/ 00072 00073 namespace srs_assisted_grasping { 00074 00075 struct ReactiveGraspingParams { 00076 00077 ros::Duration default_time; 00078 double max_force; 00079 double max_velocity; 00080 double a_ramp; 00081 double d_ramp; 00082 double rate; 00083 00084 }; 00085 00086 struct Limits { 00087 00088 double min; 00089 double max; 00090 00091 }; 00092 00093 struct Joints { 00094 00095 00096 std::vector<std::string> joints; 00097 std::vector<Limits> limits; 00098 std::vector<bool> has_tactile_pad; 00099 std::vector<bool> is_static; 00100 00101 int num_of_tactile_pads; 00102 00103 std::vector< std::vector<double> > velocities; 00104 00105 }; 00106 00107 // accessed from callbacks 00108 struct FeedbackData { 00109 00110 //schunk_sdh::TactileSensor tactile_data; 00111 std::vector<int16_t> tactile_data; 00112 pr2_controllers_msgs::JointTrajectoryControllerState sdh_data; 00113 00114 ros::Time tactile_data_stamp; 00115 ros::Time sdh_data_stamp; 00116 00117 bool sdh_data_checked; 00118 00119 bool tactile_data_checked; 00120 00121 boost::mutex data_mutex; 00122 00123 }; 00124 00125 00126 class ReactiveGrasping { 00127 00128 public: 00129 00130 //void TactileDataCallback(const std_msgs::Float32MultiArray::ConstPtr & msg); 00131 void TactileDataCallback(const schunk_sdh::TactileSensor::ConstPtr & msg); 00132 void SdhStateCallback(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr & msg); 00133 00134 00135 00136 ReactiveGrasping(std::string name); 00137 ~ReactiveGrasping(void); 00138 00139 00140 00141 protected: 00142 00143 bool fatal_error_; 00144 00145 bool readParams(); 00146 00147 ros::ServiceClient sdh_mode_client_; // for setting SDH to right mode 00148 00149 ros::Subscriber tact_sub_; // for receiving tactile data 00150 ros::Subscriber state_sub_; // for receiving sdh state data 00151 00152 00153 actionlib::SimpleActionServer<srs_assisted_grasping_msgs::ReactiveGraspingAction> * server_; 00154 00155 ros::NodeHandle nh_; 00156 std::string action_name_; 00157 00158 00159 00160 00161 void execute(const srs_assisted_grasping_msgs::ReactiveGraspingGoalConstPtr &goal); 00162 00163 ros::Publisher vel_publisher_; 00164 00165 bool publish(std::vector<double> vel); 00166 void stop(); 00167 00168 00169 ReactiveGraspingParams params_; 00170 Joints joints_; 00171 FeedbackData feedback_data_; 00172 00173 bool setMode(std::string mode); 00174 00175 /*std_msgs::Float32MultiArray tactile_data_; 00176 pr2_controllers_msgs::JointTrajectoryControllerState sdh_data_;*/ 00177 00178 void copyData(); 00179 00180 00181 // these variable are accessed only from execute callback thread 00182 //schunk_sdh::TactileSensor tactile_data_; 00183 std::vector<int16_t> tactile_data_; 00184 trajectory_msgs::JointTrajectoryPoint sdh_data_act_; 00185 00186 std::vector<float> time_to_stop_; 00187 00188 private: 00189 00190 00191 00192 00193 }; 00194 00195 00196 } // namespace 00197 00198 00199 #endif /* GRASPING_NODE_H */