arm_node.cpp
Go to the documentation of this file.
00001 //
00002 // Created by tom on 05/06/16.
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)) {/* parameters that must be instalize for the robot to work*/
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 }


robotican_manipulator_h
Author(s):
autogenerated on Fri Oct 27 2017 03:03:09