00001 #include "ros/ros.h" 00002 #include <hardware_interface/joint_command_interface.h> 00003 #include <hardware_interface/joint_state_interface.h> 00004 #include <hardware_interface/robot_hw.h> 00005 #include <sensor_msgs/JointState.h> 00006 #include <brics_actuator/JointVelocities.h> 00007 00008 #include <boost/thread.hpp> 00009 00010 class CobHWInterfaceTopics : public hardware_interface::RobotHW 00011 { 00012 public: 00013 CobHWInterfaceTopics(); 00014 void read(); 00015 void write(); 00016 void jointstates_callback(const sensor_msgs::JointState::ConstPtr& msg); 00017 00018 private: 00019 hardware_interface::JointStateInterface jnt_state_interface; 00020 hardware_interface::VelocityJointInterface jnt_vel_interface; 00021 00022 ros::NodeHandle nh; 00023 std::vector<std::string> joint_names; 00024 00025 ros::Publisher cmd_vel_pub; 00026 ros::Subscriber jointstates_sub; 00027 00028 00029 std::vector<double> pos; 00030 std::vector<double> vel; 00031 std::vector<double> eff; 00032 std::vector<double> cmd; 00033 boost::mutex mtx_; 00034 };