Go to the documentation of this file.00001
00002
00003
00004
00005 #include <ros/ros.h>
00006 #include <std_msgs/Float64.h>
00007 #include <dynamixel_msgs/JointState.h>
00008 #include <controller_manager/controller_manager.h>
00009 #include <robotican_hardware_interface/dynamixel_pro_controller.h>
00010 #include <hardware_interface/robot_hw.h>
00011
00012 class Arm : public hardware_interface::RobotHW {
00013 private:
00014 ros::Time _time;
00015 ros::NodeHandle _nodeHandle;
00016 hardware_interface::JointStateInterface _jointStateInterface;
00017 hardware_interface::PosVelJointInterface _posVelJointInterface;
00018 hardware_interface::PositionJointInterface _positionJointInterface;
00019 dynamixel_pro_controller::DynamixelProController _controller;
00020 ros::Publisher _leftFingerCmd;
00021 ros::Publisher _rightFingerCmd;
00022 ros::Subscriber _leftFingerState;
00023 ros::Subscriber _rightFingerState;
00024 std::pair<std::string, dynamixel_pro_controller::JointInfo_t> _leftFingerInfo;
00025 std::pair<std::string, dynamixel_pro_controller::JointInfo_t> _rightFingerInfo;
00026 bool _first[2];
00027
00028 void leftFingerCallback(const dynamixel_msgs::JointState::ConstPtr &msg) {
00029 _leftFingerInfo.second.position = msg->current_pos;
00030 _leftFingerInfo.second.velocity = msg->velocity;
00031 _leftFingerInfo.second.effort = msg->load;
00032 if(!_first[1]) {
00033 _leftFingerInfo.second.cmd_pos = _leftFingerInfo.second.position;
00034 _first[1] = true;
00035 }
00036 }
00037 void rightFingerCallback(const dynamixel_msgs::JointState::ConstPtr &msg) {
00038 _rightFingerInfo.second.position = msg->current_pos;
00039 _rightFingerInfo.second.velocity = msg->velocity;
00040 _rightFingerInfo.second.effort = msg->load;
00041 if(!_first[0]) {
00042 _rightFingerInfo.second.cmd_pos = _rightFingerInfo.second.position;
00043 _first[0] = true;
00044 }
00045 }
00046 public:
00047
00048 Arm() : _jointStateInterface(), _posVelJointInterface(), _positionJointInterface(), _controller(&_jointStateInterface, &_posVelJointInterface) {
00049 _first[0] = _first[1] = false;
00050 _time = ros::Time::now();
00051 std::string leftFingerPubTopic, leftFingerSubTopic, leftFingerJointName,
00052 rightFingerPubTopic, rightFingerSubTopic, rightFingerJointName;
00053 if(!_nodeHandle.getParam("left_finger_topic_pub", leftFingerPubTopic) ||
00054 !_nodeHandle.getParam("left_finger_topic_sub", leftFingerSubTopic) ||
00055 !_nodeHandle.getParam("left_finger_joint", leftFingerJointName) ||
00056 !_nodeHandle.getParam("right_finger_topic_pub", rightFingerPubTopic) ||
00057 !_nodeHandle.getParam("right_finger_topic_sub", rightFingerSubTopic) ||
00058 !_nodeHandle.getParam("right_finger_joint", rightFingerJointName)) {
00059 ROS_ERROR("[%s]: Invalid parameters", ros::this_node::getName().c_str());
00060 ros::shutdown();
00061 }
00062
00063
00064 _leftFingerCmd = _nodeHandle.advertise<std_msgs::Float64>(leftFingerPubTopic, 10);
00065 _rightFingerCmd = _nodeHandle.advertise<std_msgs::Float64>(rightFingerPubTopic, 10);
00066
00067 _leftFingerState = _nodeHandle.subscribe<dynamixel_msgs::JointState>(leftFingerSubTopic, 10, &Arm::leftFingerCallback, this);
00068 _rightFingerState = _nodeHandle.subscribe<dynamixel_msgs::JointState>(rightFingerSubTopic, 10, &Arm::rightFingerCallback, this);
00069
00070 _leftFingerInfo = std::pair<std::string, dynamixel_pro_controller::JointInfo_t>(leftFingerJointName, dynamixel_pro_controller::JointInfo_t());
00071 _rightFingerInfo = std::pair<std::string, dynamixel_pro_controller::JointInfo_t>(rightFingerJointName, dynamixel_pro_controller::JointInfo_t());
00072
00073 hardware_interface::JointStateHandle leftJointStateHandle(_leftFingerInfo.first,
00074 &_leftFingerInfo.second.position,
00075 &_leftFingerInfo.second.velocity,
00076 &_leftFingerInfo.second.effort );
00077
00078 hardware_interface::JointStateHandle rightJointStateHandle(_rightFingerInfo.first,
00079 &_rightFingerInfo.second.position,
00080 &_rightFingerInfo.second.velocity,
00081 &_rightFingerInfo.second.effort );
00082
00083 _jointStateInterface.registerHandle(leftJointStateHandle);
00084 _jointStateInterface.registerHandle(rightJointStateHandle);
00085
00086 hardware_interface::JointHandle leftJointHandle(_jointStateInterface.getHandle(_leftFingerInfo.first), &_leftFingerInfo.second.cmd_pos);
00087 hardware_interface::JointHandle rightJointHandle(_jointStateInterface.getHandle(_rightFingerInfo.first), &_rightFingerInfo.second.cmd_pos);
00088
00089 _positionJointInterface.registerHandle(leftJointHandle);
00090 _positionJointInterface.registerHandle(rightJointHandle);
00091
00092 registerInterface(&_jointStateInterface);
00093 registerInterface(&_posVelJointInterface);
00094 registerInterface(&_positionJointInterface);
00095 }
00096
00097 ros::Time getTime() {
00098 return ros::Time::now();
00099 }
00100
00101 ros::Duration getPeriod() {
00102 ros::Time now = ros::Time::now();
00103 ros::Duration period = now - _time;
00104 _time = now;
00105 return period;
00106 }
00107
00108 void read() {
00109 _controller.read();
00110 }
00111
00112 void write() {
00113 _controller.write();
00114 std_msgs::Float64 leftMsg, rightMsg;
00115
00116 if(_first[0] && _first[1]) {
00117 leftMsg.data = _leftFingerInfo.second.cmd_pos;
00118 rightMsg.data = _rightFingerInfo.second.cmd_pos;
00119
00120 _leftFingerCmd.publish(leftMsg);
00121 _rightFingerCmd.publish(rightMsg);
00122 }
00123
00124 }
00125 };
00126
00127
00128 int main(int argc, char **argv) {
00129 ros::init(argc, argv, "arm_node");
00130 Arm arm;
00131 controller_manager::ControllerManager controllerManager(&arm);
00132 ros::AsyncSpinner asyncSpinner(2);
00133 asyncSpinner.start();
00134 ros::Rate loopRate(100);
00135
00136 while (ros::ok()) {
00137 arm.read();
00138 controllerManager.update(arm.getTime(), arm.getPeriod());
00139 arm.write();
00140 loopRate.sleep();
00141 }
00142
00143
00144 return 0;
00145 }