00001 00014 #ifndef CARL_INTERACTIVE_MANIPULATION_H_ 00015 #define CARL_INTERACTIVE_MANIPULATION_H_ 00016 00017 #include <ros/ros.h> 00018 #include <actionlib/client/simple_action_client.h> 00019 #include <boost/thread/recursive_mutex.hpp> 00020 #include <rail_manipulation_msgs/ArmAction.h> 00021 #include <rail_manipulation_msgs/PickupAction.h> 00022 #include <carl_safety/Error.h> 00023 #include <interactive_markers/interactive_marker_server.h> 00024 #include <interactive_markers/menu_handler.h> 00025 #include <rail_manipulation_msgs/GripperAction.h> 00026 #include <rail_manipulation_msgs/LiftAction.h> 00027 #include <rail_manipulation_msgs/SegmentedObjectList.h> 00028 #include <rail_pick_and_place_msgs/RemoveObject.h> 00029 #include <rail_segmentation/RemoveObject.h> 00030 #include <wpi_jaco_msgs/CartesianCommand.h> 00031 #include <wpi_jaco_msgs/EStop.h> 00032 #include <wpi_jaco_msgs/GetAngularPosition.h> 00033 #include <wpi_jaco_msgs/GetCartesianPosition.h> 00034 #include <wpi_jaco_msgs/JacoFK.h> 00035 #include <wpi_jaco_msgs/QuaternionToEuler.h> 00036 #include <sensor_msgs/JointState.h> 00037 #include <sensor_msgs/PointCloud.h> 00038 #include <sensor_msgs/point_cloud_conversion.h> 00039 #include <std_srvs/Empty.h> 00040 00041 //over current thresholds 00042 #define J1_THRESHOLD 7.0 00043 #define J2_THRESHOLD 25.5 00044 #define J3_THRESHOLD 14.0 00045 #define J4_THRESHOLD 5.0 00046 #define J5_THRESHOLD 5.0 00047 #define J6_THRESHOLD 3.5 00048 #define F1_THRESHOLD 1.5 00049 #define F2_THRESHOLD 1.5 00050 #define F3_THRESHOLD 1.5 00051 00060 class CarlInteractiveManipulation 00061 { 00062 00063 public: 00064 00068 CarlInteractiveManipulation(); 00069 00074 void processHandMarkerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00075 00080 //void processRecognizeMarkerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00081 00086 void processPickupMarkerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00087 00092 void processRemoveMarkerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00093 00098 void updateJoints(const sensor_msgs::JointState::ConstPtr& msg); 00099 00104 void segmentedObjectsCallback(const rail_manipulation_msgs::SegmentedObjectList::ConstPtr& objectList); 00105 00109 void clearSegmentedObjects(); 00110 00114 void updateMarkerPosition(); 00115 00116 private: 00117 00123 bool removeObjectMarker(int index); 00124 00128 void makeHandMarker(); 00129 00133 void sendStopCommand(); 00134 00138 void armCollisionRecovery(); 00139 00144 bool isArmRetracted(); 00145 00146 ros::NodeHandle n; 00147 00148 //messages 00149 ros::Publisher cartesianCmd; 00150 ros::Publisher safetyErrorPublisher; 00151 ros::Subscriber jointStateSubscriber; 00152 ros::Subscriber recognizedObjectsSubscriber; 00153 00154 //services 00155 ros::ServiceClient armAngularPositionClient; 00156 ros::ServiceClient armCartesianPositionClient; 00157 ros::ServiceClient armEStopClient; 00158 ros::ServiceClient eraseTrajectoriesClient; 00159 ros::ServiceClient jacoFkClient; 00160 ros::ServiceClient qeClient; 00161 // ros::ServiceClient pickupSegmentedClient; 00162 ros::ServiceClient removeObjectClient; 00163 ros::ServiceClient detachObjectsClient; 00164 00165 //actionlib 00166 actionlib::SimpleActionClient<rail_manipulation_msgs::GripperAction> acGripper; 00167 actionlib::SimpleActionClient<rail_manipulation_msgs::ArmAction> acArm; 00168 actionlib::SimpleActionClient<rail_manipulation_msgs::PickupAction> acPickup; 00169 //actionlib::SimpleActionClient<rail_manipulation_msgs::RecognizeAction> acRecognize; 00170 00171 boost::recursive_mutex api_mutex; 00172 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> imServer; 00173 interactive_markers::MenuHandler menuHandler; 00174 interactive_markers::MenuHandler objectMenuHandler; 00175 std::vector<interactive_markers::MenuHandler> recognizedMenuHandlers; 00176 std::vector<visualization_msgs::InteractiveMarker> segmentedObjects; 00177 rail_manipulation_msgs::SegmentedObjectList segmentedObjectList; 00178 std::vector<float> joints; 00179 std::vector<float> markerPose; 00180 std::vector<float> retractPos; //jaco arm retracted joint positions 00181 bool lockPose; 00182 bool movingArm; 00183 bool disableArmMarkerCommands; 00184 bool usingPickup; 00185 bool usingRecognition; 00186 ros::Time lastRetractedFeedback; 00187 }; 00188 00189 #endif