00001 #ifndef CARL_MOVEIT_H_ 00002 #define CARL_MOVEIT_H_ 00003 00004 //CPP 00005 #include <boost/thread/recursive_mutex.hpp> 00006 00007 //ROS 00008 #include <ros/ros.h> 00009 #include <actionlib/server/simple_action_server.h> 00010 #include <actionlib/client/simple_action_client.h> 00011 #include <control_msgs/FollowJointTrajectoryAction.h> 00012 #include <moveit/robot_state/conversions.h> 00013 #include <moveit/move_group_interface/move_group.h> 00014 #include <moveit/planning_scene_interface/planning_scene_interface.h> 00015 #include <moveit/robot_model_loader/robot_model_loader.h> 00016 #include <moveit/robot_model/robot_model.h> 00017 #include <moveit/robot_state/robot_state.h> 00018 #include <moveit_msgs/CollisionObject.h> 00019 #include <moveit_msgs/DisplayTrajectory.h> 00020 #include <moveit_msgs/GetPositionIK.h> 00021 #include <geometry_msgs/PoseStamped.h> 00022 #include <geometry_msgs/Twist.h> 00023 #include <pcl_ros/point_cloud.h> 00024 #include <rail_manipulation_msgs/CallIK.h> 00025 #include <rail_manipulation_msgs/CartesianPath.h> 00026 #include <rail_manipulation_msgs/MoveToJointPoseAction.h> 00027 #include <rail_manipulation_msgs/MoveToPoseAction.h> 00028 #include <rail_manipulation_msgs/SegmentedObjectList.h> 00029 #include <sensor_msgs/JointState.h> 00030 #include <sensor_msgs/point_cloud_conversion.h> 00031 #include <std_msgs/Bool.h> 00032 #include <std_srvs/Empty.h> 00033 #include <tf/transform_listener.h> 00034 #include <tf2_ros/transform_listener.h> 00035 #include <wpi_jaco_msgs/AngularCommand.h> 00036 00037 //PCL 00038 #include <pcl/point_types.h> 00039 #include <pcl/point_cloud.h> 00040 #include <pcl/common/common.h> 00041 #include <pcl/common/transforms.h> 00042 00043 #define NUM_JACO_JOINTS 6 00044 #define SCENE_OBJECT_DST_THRESHOLD 0.2 00045 00046 class CarlMoveIt 00047 { 00048 00049 public: 00050 00051 CarlMoveIt(); 00052 00053 ~CarlMoveIt(); 00054 00055 private: 00056 ros::NodeHandle n; 00057 ros::Subscriber armJointStateSubscriber; 00058 ros::Subscriber cartesianControlSubscriber; 00059 ros::Subscriber armHomedSubscriber; 00060 ros::Subscriber recognizedObjectsSubscriber; 00061 ros::Publisher angularCmdPublisher; 00062 ros::Publisher trajectoryVisPublisher; 00063 ros::ServiceServer cartesianPathServer; 00064 ros::ServiceServer ikServer; 00065 ros::ServiceServer attachObjectServer; 00066 ros::ServiceServer detachObjectServer; 00067 ros::ServiceClient ikClient; 00068 ros::ServiceClient clearOctomapClient; 00069 00070 actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> armTrajectoryClient; 00071 actionlib::SimpleActionServer<rail_manipulation_msgs::MoveToPoseAction> moveToPoseServer; 00072 actionlib::SimpleActionServer<rail_manipulation_msgs::MoveToJointPoseAction> moveToJointPoseServer; 00073 00074 tf::TransformListener tf; 00075 tf2_ros::Buffer tfBuffer; 00076 tf2_ros::TransformListener tfListener; 00077 00078 move_group_interface::MoveGroup *armGroup; 00079 move_group_interface::PlanningSceneInterface *planningSceneInterface; 00080 00081 boost::recursive_mutex api_mutex; 00082 00083 sensor_msgs::JointState jointState; 00084 rail_manipulation_msgs::SegmentedObjectList objectList; //the last received object list 00085 std::vector<std::string> attachedObjects; //the names of the objects (in the planning scene) attached to the robot 00086 std::vector<std::string> unattachedObjects; //the names of the objects (in the planning scene) not attached to the robot 00087 00088 void moveToPose(const rail_manipulation_msgs::MoveToPoseGoalConstPtr &goal); 00089 00090 void moveToJointPose(const rail_manipulation_msgs::MoveToJointPoseGoalConstPtr &goal); 00091 00092 bool cartesianPathCallback(rail_manipulation_msgs::CartesianPath::Request &req, rail_manipulation_msgs::CartesianPath::Response &res); 00093 00094 bool ikCallback(rail_manipulation_msgs::CallIK::Request &req, rail_manipulation_msgs::CallIK::Response &res); 00095 00096 moveit_msgs::GetPositionIK::Response callIK(geometry_msgs::PoseStamped pose); 00097 00098 void armJointStatesCallback(const sensor_msgs::JointState &msg); 00099 00100 void cartesianControlCallback(const geometry_msgs::Twist &msg); 00101 00102 void armHomedCallback(const std_msgs::Bool &msg); 00103 00104 void recognizedObjectsCallback(const rail_manipulation_msgs::SegmentedObjectList &msg); 00105 00106 bool attachClosestSceneObject(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); 00107 00108 bool detachSceneObjects(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); 00109 }; 00110 00111 #endif