robotican_h_manipulator_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;
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)) {/* parameters that must be instalize for the robot to work*/
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 }


robotican_h_manipulator
Author(s):
autogenerated on Tue Jul 12 2016 04:41:20