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


manipulator_handler
Author(s): Giovanni Claudio
autogenerated on Mon Apr 3 2017 04:03:52