00001 /* 00002 * jaco_arm_driver.h 00003 * 00004 * Created on: Feb 26, 2013 00005 * Modified on: June 25, 2013 00006 * Author: mdedonato, Clearpath Robotics 00007 * 00008 */ 00009 00010 #ifndef JACO_DRIVER_JACO_ARM_H 00011 #define JACO_DRIVER_JACO_ARM_H 00012 00013 #include <ros/ros.h> 00014 00015 #include <geometry_msgs/PoseStamped.h> 00016 #include <geometry_msgs/TwistStamped.h> 00017 #include <tf/tf.h> 00018 #include <tf/transform_listener.h> 00019 #include <sensor_msgs/JointState.h> 00020 00021 #include <jaco_msgs/Stop.h> 00022 #include <jaco_msgs/Start.h> 00023 #include <jaco_msgs/HomeArm.h> 00024 #include <jaco_msgs/JointVelocity.h> 00025 #include <jaco_msgs/FingerPosition.h> 00026 #include <jaco_msgs/JointAngles.h> 00027 00028 #include <time.h> 00029 #include <math.h> 00030 #include <vector> 00031 00032 #include "kinova/KinovaTypes.h" 00033 #include "jaco_driver/jaco_comm.h" 00034 #include "jaco_driver/jaco_api.h" 00035 00036 00037 namespace jaco 00038 { 00039 00040 class JacoArm 00041 { 00042 public: 00043 JacoArm(JacoComm& arm, const ros::NodeHandle &node_handle); 00044 ~JacoArm(); 00045 00046 void jointVelocityCallback(const jaco_msgs::JointVelocityConstPtr& joint_vel); 00047 void cartesianVelocityCallback(const geometry_msgs::TwistStampedConstPtr& cartesian_vel); 00048 00049 bool stopServiceCallback(jaco_msgs::Stop::Request &req, jaco_msgs::Stop::Response &res); 00050 bool startServiceCallback(jaco_msgs::Start::Request &req, jaco_msgs::Start::Response &res); 00051 bool homeArmServiceCallback(jaco_msgs::HomeArm::Request &req, jaco_msgs::HomeArm::Response &res); 00052 00053 private: 00054 void positionTimer(const ros::TimerEvent&); 00055 void cartesianVelocityTimer(const ros::TimerEvent&); 00056 void jointVelocityTimer(const ros::TimerEvent&); 00057 void statusTimer(const ros::TimerEvent&); 00058 00059 void publishJointAngles(void); 00060 void publishToolPosition(void); 00061 void publishFingerPosition(void); 00062 00063 tf::TransformListener tf_listener_; 00064 ros::NodeHandle node_handle_; 00065 JacoComm &jaco_comm_; 00066 00067 // Publishers, subscribers, services 00068 ros::Subscriber joint_velocity_subscriber_; 00069 ros::Subscriber cartesian_velocity_subscriber_; 00070 00071 ros::Publisher joint_angles_publisher_; 00072 ros::Publisher tool_position_publisher_; 00073 ros::Publisher finger_position_publisher_; 00074 ros::Publisher joint_state_publisher_; 00075 00076 ros::ServiceServer stop_service_; 00077 ros::ServiceServer start_service_; 00078 ros::ServiceServer homing_service_; 00079 00080 // Timers for control loops 00081 ros::Timer status_timer_; 00082 ros::Timer cartesian_vel_timer_; 00083 ros::Timer joint_vel_timer_; 00084 00085 // Parameters 00086 double status_interval_seconds_; 00087 double joint_vel_timeout_seconds_; 00088 double cartesian_vel_timeout_seconds_; 00089 double joint_vel_interval_seconds_; 00090 double cartesian_vel_interval_seconds_; 00091 00092 // State tracking or utility members 00093 bool cartesian_vel_timer_flag_; 00094 bool joint_vel_timer_flag_; 00095 00096 AngularInfo joint_velocities_; 00097 CartesianInfo cartesian_velocities_; 00098 00099 ros::Time last_joint_vel_cmd_time_; 00100 ros::Time last_cartesian_vel_cmd_time_; 00101 }; 00102 00103 } // namespace jaco 00104 #endif // JACO_DRIVER_JACO_ARM_H