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;
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 }
00033
00034 void rightFingerCallback(const dynamixel_msgs::JointState::ConstPtr &msg) {
00035 _rightFingerInfo.second.position = msg->current_pos;
00036 _rightFingerInfo.second.velocity = msg->velocity;
00037 _rightFingerInfo.second.effort = msg->load;
00038 }
00039
00040 public:
00041
00042 Arm() : _jointStateInterface(), _posVelJointInterface(), _positionJointInterface(), _controller(&_jointStateInterface, &_posVelJointInterface) {
00043 _time = ros::Time::now();
00044 _first = true;
00045 std::string leftFingerPubTopic, leftFingerSubTopic, leftFingerJointName,
00046 rightFingerPubTopic, rightFingerSubTopic, rightFingerJointName;
00047 if(!_nodeHandle.getParam("left_finger_topic_pub", leftFingerPubTopic) ||
00048 !_nodeHandle.getParam("left_finger_topic_sub", leftFingerSubTopic) ||
00049 !_nodeHandle.getParam("left_finger_joint", leftFingerJointName) ||
00050 !_nodeHandle.getParam("right_finger_topic_pub", rightFingerPubTopic) ||
00051 !_nodeHandle.getParam("right_finger_topic_sub", rightFingerSubTopic) ||
00052 !_nodeHandle.getParam("right_finger_joint", rightFingerJointName)) {
00053 ROS_ERROR("[%s]: Invalid parameters", ros::this_node::getName().c_str());
00054 ros::shutdown();
00055 }
00056
00057
00058 _leftFingerCmd = _nodeHandle.advertise<std_msgs::Float64>(leftFingerPubTopic, 10);
00059 _rightFingerCmd = _nodeHandle.advertise<std_msgs::Float64>(rightFingerPubTopic, 10);
00060
00061 _leftFingerState = _nodeHandle.subscribe<dynamixel_msgs::JointState>(leftFingerSubTopic, 10, &Arm::leftFingerCallback, this);
00062 _rightFingerState = _nodeHandle.subscribe<dynamixel_msgs::JointState>(rightFingerSubTopic, 10, &Arm::rightFingerCallback, this);
00063
00064 _leftFingerInfo = std::pair<std::string, dynamixel_pro_controller::JointInfo_t>(leftFingerJointName, dynamixel_pro_controller::JointInfo_t());
00065 _rightFingerInfo = std::pair<std::string, dynamixel_pro_controller::JointInfo_t>(rightFingerJointName, dynamixel_pro_controller::JointInfo_t());
00066
00067 hardware_interface::JointStateHandle leftJointStateHandle(_leftFingerInfo.first,
00068 &_leftFingerInfo.second.position,
00069 &_leftFingerInfo.second.velocity,
00070 &_leftFingerInfo.second.effort );
00071
00072 hardware_interface::JointStateHandle rightJointStateHandle(_rightFingerInfo.first,
00073 &_rightFingerInfo.second.position,
00074 &_rightFingerInfo.second.velocity,
00075 &_rightFingerInfo.second.effort );
00076
00077 _jointStateInterface.registerHandle(leftJointStateHandle);
00078 _jointStateInterface.registerHandle(rightJointStateHandle);
00079
00080 hardware_interface::JointHandle leftJointHandle(_jointStateInterface.getHandle(_leftFingerInfo.first), &_leftFingerInfo.second.cmd_pos);
00081 hardware_interface::JointHandle rightJointHandle(_jointStateInterface.getHandle(_rightFingerInfo.first), &_rightFingerInfo.second.cmd_pos);
00082
00083 _positionJointInterface.registerHandle(leftJointHandle);
00084 _positionJointInterface.registerHandle(rightJointHandle);
00085
00086 registerInterface(&_jointStateInterface);
00087 registerInterface(&_posVelJointInterface);
00088 registerInterface(&_positionJointInterface);
00089 }
00090
00091 ros::Time getTime() {
00092 return ros::Time::now();
00093 }
00094
00095 ros::Duration getPeriod() {
00096 ros::Time now = ros::Time::now();
00097 ros::Duration period = now - _time;
00098 _time = now;
00099 return period;
00100 }
00101
00102 void read() {
00103 _controller.read();
00104 if(_first) {
00105 _leftFingerInfo.second.cmd_pos = _leftFingerInfo.second.position;
00106 _rightFingerInfo.second.cmd_pos = _rightFingerInfo.second.position;
00107 _first = false;
00108 }
00109 }
00110
00111 void write() {
00112 _controller.write();
00113
00114 std_msgs::Float64 leftMsg, rightMsg;
00115
00116 leftMsg.data = _leftFingerInfo.second.cmd_pos;
00117 rightMsg.data = _rightFingerInfo.second.cmd_pos;
00118
00119 _leftFingerCmd.publish(leftMsg);
00120 _rightFingerCmd.publish(rightMsg);
00121
00122 }
00123 };
00124
00125
00126 int main(int argc, char **argv) {
00127 ros::init(argc, argv, "arm_node");
00128 Arm arm;
00129 controller_manager::ControllerManager controllerManager(&arm);
00130 ros::AsyncSpinner asyncSpinner(1);
00131 asyncSpinner.start();
00132 ros::Rate loopRate(100);
00133
00134 while (ros::ok()) {
00135 arm.read();
00136 controllerManager.update(arm.getTime(), arm.getPeriod());
00137 arm.write();
00138 loopRate.sleep();
00139 }
00140
00141
00142 return 0;
00143 }