00001 #ifndef _WAM_TEST_H_ 00002 #define _WAM_TEST_H_ 00003 00004 #include <math.h> 00005 #include <Eigen/StdVector> 00006 #include <Eigen/Dense> 00007 #include "ros/this_node.h" 00008 #include "ros/names.h" 00009 #include "ros/node_handle.h" 00010 #include "ros/rate.h" 00011 00012 00013 // [publisher subscriber headers] 00014 #include "sensor_msgs/JointState.h" 00015 #include "geometry_msgs/Pose.h" 00016 #include "tf/tfMessage.h" 00017 #include "geometry_msgs/PoseStamped.h" 00018 00019 // [service client headers] 00020 #include "iri_wam_common_msgs/pose_move.h" 00021 #include "iri_wam_common_msgs/joints_move.h" 00022 #include <tf/transform_broadcaster.h> 00023 #include <tf/transform_listener.h> 00024 #include "tf/transform_datatypes.h" 00025 00026 class WamTest { 00027 public: 00028 WamTest(); 00029 00030 void mainLoop(); 00031 00032 private: 00033 bool gocenter; 00034 int numtargets; 00035 int targetid; 00036 std::vector<double> angles; 00037 ros::NodeHandle nh_; 00038 ros::Publisher tf_publisher; 00039 tf::TransformListener listener; 00040 tf::tfMessage tfMessage_msg; 00041 // std::vector<Eigen::Matrix4f> transforms; 00042 std::vector<Eigen::Matrix4f,Eigen::aligned_allocator<Eigen::Matrix4f> > transforms; 00043 00044 ros::Subscriber joint_states_subscriber; 00045 void joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg); 00046 ros::Subscriber pose_subscriber; 00047 void pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg); 00048 00049 ros::ServiceClient pose_move2_client; 00050 iri_wam_common_msgs::pose_move pose_move2_srv; 00051 ros::ServiceClient pose_move_client; 00052 iri_wam_common_msgs::pose_move pose_move_srv; 00053 ros::ServiceClient joint_move_client; 00054 iri_wam_common_msgs::joints_move joint_move_srv; 00055 00056 void calculate_error(std::string target); 00057 00058 00059 }; 00060 #endif