manipulator_handler: ManipulatorHandler.cpp Source File

Go to the documentation of this file.
00001 
00002 #include <pluginlib/class_list_macros.h>
00003 
00004 #include <manipulator_handler/ManipulatorHandler.h>
00005 #include <v_repLib.h>
00006 #include <vrep_ros_plugin/access.h>
00007 
00008 
00009 #include <vrep_ros_plugin/ConsoleHandler.h>
00010 
00011 #include <rosconsole/macros_generated.h>
00012 
00013 ManipulatorHandler::ManipulatorHandler() : GenericObjectHandler(),
00014 _acquisitionFrequency(-1.0),
00015 _handleOfJoints(0),
00016 _numJoints(0),
00017 _defaultModeCtrl(CustomDataHeaders::TF_POSITION),
00018 _axle_lenght (0.331),
00019 _mb_radius(0.0970){
00020 }
00021 
00022 ManipulatorHandler::~ManipulatorHandler(){
00023 }
00024 
00025 unsigned int ManipulatorHandler::getObjectType() const {
00026     return CustomDataHeaders::MANIPULATOR_DATA_MAIN;
00027 }
00028 
00029 void ManipulatorHandler::synchronize(){
00030 
00031     // Remove # chars for compatibility with ROS
00032     _associatedObjectName = simGetObjectName(_associatedObjectID);
00033     std::string objectName(_associatedObjectName);
00034     std::replace( objectName.begin(), objectName.end(), '#', '_');
00035     _pub = _nh.advertise<sensor_msgs::JointState>(objectName+"/jointStatus", 1000);
00036 
00037 
00038 }
00039 
00040 void ManipulatorHandler::handleSimulation(){
00041     // called when the main script calls: simHandleModule
00042     if(!_initialized){
00043         _initialize();
00044     }
00045 
00046     ros::Time now = ros::Time::now();
00047 
00048     const simFloat currentSimulationTime = simGetSimulationTime();
00049 
00050     if ( ((currentSimulationTime - _lastPublishedStatus) >= 1.0/_acquisitionFrequency) ){
00051 
00052         sensor_msgs::JointState msg;
00053         msg.header.stamp = now;
00054         const unsigned int ndofs = _handleOfJoints.size();
00055         msg.name.resize(ndofs);
00056         msg.position.resize(ndofs);
00057         msg.velocity.resize(ndofs);
00058         for (uint jointIdx = 0; jointIdx < ndofs; ++jointIdx){
00059             msg.name[jointIdx] = _jointNames[jointIdx];
00060             simFloat temp;
00061             simGetJointPosition(_handleOfJoints[jointIdx],&temp);
00062             msg.position[jointIdx] = temp;
00063             simGetObjectFloatParameter(_handleOfJoints[jointIdx], 2012, &temp);
00064             msg.velocity[jointIdx] = temp;
00065         }
00066 
00067         _pub.publish(msg); // Publish the Joint status message in ROS
00068         _lastPublishedStatus = currentSimulationTime;
00069 
00070     }
00071 
00072     // Do the control
00073 
00074     // Set all joint velocities to zero if we haven't received any commands for a long time
00075     if (currentSimulationTime-_lastReceivedCmdTime > 0.1){
00076         for(uint jointIdx = 0; jointIdx < _handleOfJoints.size(); ++jointIdx){
00077             simSetJointTargetVelocity(_handleOfJoints[jointIdx], 0.0);
00078         }
00079         if (_lastReceivedCmdTime > 0 && (now-_lastPrintedMsg).toSec() >= 1){
00080             std::stringstream ss;
00081             ss << "- [" << _associatedObjectName << "] No command received since more than " << currentSimulationTime -_lastReceivedCmdTime << "s!" << std::endl;
00082             simAddStatusbarMessage(ss.str().c_str());
00083             //ConsoleHandler::printInConsole(ss);
00084             _lastPrintedMsg = now;
00085         }
00086         return;
00087     }
00088 
00089 
00090     for(uint jointIdx = 0; jointIdx < _handleOfJoints.size(); ++jointIdx){
00091         if (_jointCtrlMode[jointIdx] == CustomDataHeaders::TF_POSITION){
00092 
00093             ROS_INFO("Apply new position to joint %s: %f.", _jointNames[jointIdx].c_str(), _lastReceivedCmd.position[jointIdx]);
00094             //_tempjoint = _tempjoint+0.001;
00095             if(simSetJointTargetPosition(_handleOfJoints[jointIdx], _lastReceivedCmd.position[jointIdx])==-1)
00096                 // if(simSetJointTargetPosition(_handleOfJoints[jointIdx], _tempjoint)==-1)
00097             {
00098                 std::stringstream ss;
00099                 ss << "- [" << _associatedObjectName << "] Error setting position for joint "<<
00100                         jointIdx <<" (" << _jointNames[jointIdx] <<")." <<std::endl;
00101                 ConsoleHandler::printInConsole(ss);
00102             }
00103 
00104 
00105 
00106         } else if (_jointCtrlMode[jointIdx] == CustomDataHeaders::TF_VELOCITY ||_jointCtrlMode[jointIdx] == CustomDataHeaders::MOT_VELOCITY ){
00107 
00108             ROS_DEBUG("Apply new velocity to  joint %s: %f.", _jointNames[jointIdx].c_str(), _lastReceivedCmd.velocity[jointIdx]);
00109             if(simSetJointTargetVelocity(_handleOfJoints[jointIdx], _lastReceivedCmd.velocity[jointIdx])==-1){
00110                 std::stringstream ss;
00111                 ss << "- [" << _associatedObjectName << "] Error setting velocity for joint "<<
00112                         jointIdx <<" (" << _jointNames[jointIdx] <<")." <<std::endl;
00113                 ConsoleHandler::printInConsole(ss);
00114             }
00115 
00116         } else if (_jointCtrlMode[jointIdx] == CustomDataHeaders::TF_EFFORT ){
00117 
00118             ROS_DEBUG("Apply new torque/force to joint %s: %f.", _jointNames[jointIdx].c_str(), _lastReceivedCmd.effort[jointIdx]);
00119             if(simSetJointForce(_handleOfJoints[jointIdx], _lastReceivedCmd.effort[jointIdx])==-1){
00120                 std::stringstream ss;
00121                 ss << "- [" << _associatedObjectName << "] Error setting torque or force for joint "<<
00122                         jointIdx <<" (" << _jointNames[jointIdx] <<")." <<std::endl;
00123                 ConsoleHandler::printInConsole(ss);
00124             }
00125 
00126         } else if (_jointCtrlMode[jointIdx] == CustomDataHeaders::PASSIVE_MODE){
00127 
00128             ROS_DEBUG("Apply new position to joint %s: %f.", _jointNames[jointIdx].c_str(), _lastReceivedCmd.position[jointIdx]);
00129             //_tempjoint = _tempjoint+0.001;
00130             if(simSetJointPosition(_handleOfJoints[jointIdx], _lastReceivedCmd.position[jointIdx])==-1){
00131                 std::stringstream ss;
00132                 ss << "- [" << _associatedObjectName << "] Error setting position for joint "<<
00133                         jointIdx <<" (" << _jointNames[jointIdx] <<")." <<std::endl;
00134                 ConsoleHandler::printInConsole(ss);
00135             }
00136 
00137         }
00138 
00139     }
00140 
00141 }
00142 
00143 
00144 void ManipulatorHandler::_initialize(){
00145     if (_initialized)
00146         return;
00147 
00148     _numJoints = 0;
00149 
00150     // get some data from the main object
00151     std::vector<unsigned char> developerCustomData;
00152     getDeveloperCustomData(developerCustomData);
00153 
00154     std::vector<unsigned char> tempMainData;
00155     std::stringstream ss;
00156     std::string namectrlmode;
00157 
00158     if (CAccess::extractSerializationData(developerCustomData, CustomDataHeaders::MANIPULATOR_DATA_FREQ,tempMainData)){
00159         _acquisitionFrequency=CAccess::pop_float(tempMainData);
00160         if (_acquisitionFrequency > 0){
00161             ss << "- [" << _associatedObjectName << "] Publisher frequency: " << _acquisitionFrequency << "." << std::endl;
00162         } else {
00163             ss << "- [" << _associatedObjectName << "] Publisher frequency: simulation frequency."  << std::endl;
00164         }
00165     } else {
00166         ss << "- [" << _associatedObjectName << "] Joint status publisher frequency not specified. using simulation frequency as default."  << std::endl;
00167     }
00168 
00169     // We check if a default control mode is defined:
00170     if (CAccess::extractSerializationData(developerCustomData, CustomDataHeaders::MANIPULATOR_DATA_CTRL_MODE,tempMainData)){
00171         const int ctrlMode = CAccess::pop_int(tempMainData);
00172 
00173         ss << "- [" << _associatedObjectName << "] A Default Control Mode is defined: ";
00174 
00175         if (ctrlMode == (int)(CustomDataHeaders::TF_POSITION)){
00176             _defaultModeCtrl = CustomDataHeaders::TF_POSITION;
00177             ss << "Torque/Force control in POSITION (PID controller)." << std::endl;
00178             namectrlmode ="Torque/Force control in POSITION (PID controller)." ;
00179 
00180         } else if (ctrlMode == (int)(CustomDataHeaders::TF_VELOCITY)){
00181             _defaultModeCtrl = CustomDataHeaders::TF_VELOCITY;
00182             ss << "Torque/Force control in VELOCITY." << std::endl;
00183             namectrlmode ="Torque/Force control in VELOCITY." ;
00184 
00185         } else if (ctrlMode == (int)(CustomDataHeaders::TF_EFFORT)){
00186             _defaultModeCtrl = CustomDataHeaders::TF_EFFORT;
00187             ss << "Torque/Force control." << std::endl;
00188             namectrlmode ="Torque/Force control." ;
00189 
00190         } else if (ctrlMode == (int)(CustomDataHeaders::MOT_VELOCITY)){
00191             _defaultModeCtrl = CustomDataHeaders::MOT_VELOCITY;
00192             ss << "Motion control in VELOCITY." << std::endl;
00193             namectrlmode = "Motion control in VELOCITY." ;
00194 
00195         } else if (ctrlMode == (int)(CustomDataHeaders::PASSIVE_MODE)){
00196             _defaultModeCtrl = CustomDataHeaders::PASSIVE_MODE;
00197             ss << " Using control in Passive Mode." << std::endl;
00198             namectrlmode = "Control in Passive Mode." ;
00199 
00200         } else {
00201             _defaultModeCtrl = CustomDataHeaders::TF_POSITION;
00202             ss << " Invalid control mode specified. Using POSITION as default." << std::endl;
00203             namectrlmode = "Torque/Force control in POSITION (PID controller)." ;
00204         }
00205 
00206 
00207 
00208     } else {
00209         _defaultModeCtrl = CustomDataHeaders::TF_POSITION;
00210         ss << " Any control is mode specified. Using Torque/Force control in POSITION (PID controller) as default." << std::endl;
00211         namectrlmode = "Torque/Force control in POSITION (PID controller)."  ;
00212     }
00213 
00214 
00215 
00216 
00217 
00218     // search associated objects
00219     std::vector<int> toExplore;
00220     toExplore.push_back(_associatedObjectID); // We start exploration with the base of the manipulator
00221     while (toExplore.size()!=0)
00222     {
00223         int objHandle=toExplore[toExplore.size()-1];
00224         toExplore.pop_back();
00225         // 1. Add this object's children to the list to explore:
00226         int index=0;
00227         int childHandle=simGetObjectChild(objHandle,index++);
00228         ROS_DEBUG("__START___");
00229         while (childHandle!=-1) {
00230             toExplore.push_back(childHandle);
00231             ROS_DEBUG("Adding %s to exploration list.", simGetObjectName(childHandle));
00232             childHandle=simGetObjectChild(objHandle,index++);
00233         }
00234         ROS_DEBUG("__END___");
00235         // 2. Now check if this object has one of the tags we are looking for:
00236         // a. Get all the developer data attached to this scene object (this is custom data added by the developer):
00237         int buffSize=simGetObjectCustomDataLength(objHandle, CustomDataHeaders::DEVELOPER_DATA_HEADER);
00238         if (1) { // Yes there is some custom data written by us (the developer with the DEVELOPER_DATA_HEADER header)
00239             char* datBuff=new char[buffSize];
00240             simGetObjectCustomData(objHandle, CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
00241             std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
00242             delete[] datBuff;
00243             // b. From that retrieved data, try to extract sub-data with the searched tags:
00244             std::vector<unsigned char> tempMainData;
00245 
00246 
00247             //if (CAccess::extractSerializationData(developerCustomData, CustomDataHeaders::MANIPULATOR_DATA_JOINT,tempMainData))
00248             int ctrlMode = -1;
00249             if (CAccess::extractSerializationData(developerCustomData, CustomDataHeaders::MANIPULATOR_DATA_CTRL_MODE,tempMainData))
00250                 ctrlMode = CAccess::pop_int(tempMainData);
00251 
00252             // TODO: add support for spherical joints (they are always passive)
00253             if ((simGetJointType(objHandle) == sim_joint_revolute_subtype || simGetJointType(objHandle) == sim_joint_prismatic_subtype)
00254                     && ctrlMode != (int)(CustomDataHeaders::IGNORE_MODE)){
00255 
00256                 unsigned int  jointID = _numJoints;
00257                 _numJoints++;
00258                 ROS_DEBUG("Found %s. Id: %d", simGetObjectName(objHandle), jointID);
00259                 if (_handleOfJoints.size() < jointID+1){
00260                     _handleOfJoints.resize(jointID+1);
00261                     _jointNames.resize(jointID+1);
00262                     _jointCtrlMode.resize(jointID+1);
00263                 }
00264                 _handleOfJoints[jointID]=objHandle;
00265                 _jointNames[jointID]=simGetObjectName(objHandle);
00266                 ss << "- [" << simGetObjectName(objHandle) << "] Found Joint.";
00267 
00268                 if (CAccess::extractSerializationData(developerCustomData, CustomDataHeaders::MANIPULATOR_DATA_CTRL_MODE,tempMainData)){
00269                     const int ctrlMode = CAccess::pop_int(tempMainData);
00270 
00271                     if (ctrlMode == (int)(CustomDataHeaders::TF_POSITION)){
00272                         _jointCtrlMode[jointID] = CustomDataHeaders::TF_POSITION;
00273                         ss << " Using Torque/Force control in POSITION (PID controller)." << std::endl;
00274 
00275                     } else if (ctrlMode == (int)(CustomDataHeaders::TF_VELOCITY)){
00276                         _jointCtrlMode[jointID] = CustomDataHeaders::TF_VELOCITY;
00277                         ss << " Using Torque/Force control in VELOCITY." << std::endl;
00278 
00279                     } else if (ctrlMode == (int)(CustomDataHeaders::TF_EFFORT)){
00280                         _jointCtrlMode[jointID] = CustomDataHeaders::TF_EFFORT;
00281                         ss << "Torque/Force control." << std::endl;
00282 
00283                     } else if (ctrlMode == (int)(CustomDataHeaders::MOT_VELOCITY)){
00284                         _jointCtrlMode[jointID] = CustomDataHeaders::MOT_VELOCITY;
00285                         ss << " Using Motion control in VELOCITY." << std::endl;
00286 
00287                     } else if (ctrlMode == (int)(CustomDataHeaders::PASSIVE_MODE)){
00288                         _jointCtrlMode[jointID] = CustomDataHeaders::PASSIVE_MODE;
00289                         ss << " Using control in Passive Mode." << std::endl;
00290 
00291                     } else {
00292                         _jointCtrlMode[jointID] = _defaultModeCtrl;
00293                         ss << "  Using default control mode: " << namectrlmode << std::endl;
00294                     }
00295 
00296                 } else {
00297                     _jointCtrlMode[jointID] = _defaultModeCtrl;
00298                     ss << " Using default control mode: "  << namectrlmode << std::endl;
00299 
00300                     //  _jointCtrlMode[jointID] = CustomDataHeaders::VELOCITY;
00301                     // ss << " Control mode not specified. Using VELOCITY as default."  << std::endl;
00302 
00303                 }
00304 
00305                 if (_jointCtrlMode[jointID] == CustomDataHeaders::TF_POSITION){
00306                     simSetJointMode(_handleOfJoints[jointID], sim_jointmode_force, 0);
00307                     simSetObjectIntParameter(_handleOfJoints[jointID],2001,1);
00308 
00309                 } else if (_jointCtrlMode[jointID] == CustomDataHeaders::TF_VELOCITY){
00310                     simSetJointMode(_handleOfJoints[jointID], sim_jointmode_force, 0);
00311                     simSetObjectIntParameter(_handleOfJoints[jointID],2001,0);
00312 
00313                 } else if (_jointCtrlMode[jointID] == CustomDataHeaders::TF_EFFORT){
00314                     simSetJointMode(_handleOfJoints[jointID], sim_jointmode_force, 0);
00315                     simSetObjectIntParameter(_handleOfJoints[jointID],2001,0);
00316 
00317                 } else if (_jointCtrlMode[jointID] == CustomDataHeaders::MOT_VELOCITY){
00318                     simSetJointMode(_handleOfJoints[jointID], sim_jointmode_motion, 0);
00319                     simSetBooleanParameter(sim_boolparam_joint_motion_handling_enabled,1);
00320 
00321                     int childHandle = simGetObjectChild(_handleOfJoints[jointID],0);
00322                     simSetObjectIntParameter( childHandle,3003, 1); // Set the shape relative to the joint as STATIC
00323                     simSetObjectIntParameter( childHandle,3004, 0); // Set the shape relative to the joint as NOT RESPONSABLE
00324 
00325                 } else if (_jointCtrlMode[jointID] == CustomDataHeaders::PASSIVE_MODE){
00326                     simSetJointMode(_handleOfJoints[jointID], sim_jointmode_passive, 0);
00327                     int childHandle = simGetObjectChild(_handleOfJoints[jointID],0);
00328                     simSetObjectIntParameter( childHandle,3003, 1); // Set the shape relative to the joint as STATIC
00329                     simSetObjectIntParameter( childHandle,3004, 0); // Set the shape relative to the joint as NOT RESPONSABLE
00330                 }
00331 
00332             } else      {
00333                 ROS_DEBUG("Object %s will be ignored as requested.", simGetObjectName(objHandle));
00334             }
00335 
00336         }
00337     }
00338 
00339     std::string objectName(_associatedObjectName);
00340     std::replace( objectName.begin(), objectName.end(), '#', '_');
00341     _sub = _nh.subscribe(objectName+"/jointCommand", 1, &ManipulatorHandler::jointCommandCallback, this);
00342 
00343     // Subscriber for mobile robots
00344     _subVelMob = _nh.subscribe("/cmd_vel", 1, &ManipulatorHandler::VelMobCommandCallback, this);
00345 
00346     ss << "- [" << _associatedObjectName << "] Initialization done." << std::endl;
00347     ConsoleHandler::printInConsole(ss);
00348 
00349     _lastPublishedStatus = -1.0;
00350     _lastReceivedCmdTime = -1.0;
00351     _lastPrintedMsg = ros::Time(-1.0);
00352 
00353     _initialized=true;
00354 }
00355 
00356 
00357 void ManipulatorHandler::jointCommandCallback(const sensor_msgs::JointStateConstPtr& msg){
00358 
00359 
00360     const unsigned int nDofs = _handleOfJoints.size();
00361     if (msg->position.size()!=nDofs || msg->velocity.size()!=nDofs || msg->effort.size()!=nDofs){
00362         simSetLastError( _associatedObjectName.c_str(), "Received wrong command size.");
00363         ROS_ERROR("Received wrong command size (%ld, %ld, %ld) for manipulator which has %d dofs.",
00364                 msg->position.size(), msg->velocity.size(), msg->effort.size(), nDofs);
00365 
00366     } else {
00367         _lastReceivedCmd = *msg;
00368         _lastReceivedCmdTime = simGetSimulationTime();
00369     }
00370 }
00371 
00372 
00374 void ManipulatorHandler::VelMobCommandCallback(const geometry_msgs::TwistConstPtr& msg){
00375 
00376     geometry_msgs::Twist temp = *msg;
00377 
00378     _lastReceivedCmd.velocity.resize(2);
00379     _lastReceivedCmd.velocity[0]= (1/_mb_radius)*(temp.linear.x - _axle_lenght/2*temp.angular.z); // left
00380     _lastReceivedCmd.velocity[1]= (1/_mb_radius)*(temp.linear.x + _axle_lenght/2*temp.angular.z);  // rigth
00381 
00382     _lastReceivedCmdTime = simGetSimulationTime();
00383 
00384 }
00385 
00386 
00387 
00388 PLUGINLIB_EXPORT_CLASS(ManipulatorHandler, GenericObjectHandler)