real_shadowhand.cpp
Go to the documentation of this file.
00001 
00027 #include <ros/ros.h>
00028 
00029 #include "sr_hand/hand/real_shadowhand.h"
00030 //our robot code
00031 #include <robot/config.h>
00032 #include <robot/robot.h>
00033 #include <robot/hand_protocol.h>
00034 #include <robot/hand.h>
00035 #ifdef FINGER
00036 #define LAST_HAND_JOINT NUM_FINGER_JOINTS
00037 #else
00038 #define LAST_HAND_JOINT START_OF_ARM
00039 #endif
00040 
00041 namespace shadowrobot
00042 {
00043   RealShadowhand::RealShadowhand() :
00044     SRArticulatedRobot()
00045   {
00046     /* We need to attach the program to the robot, or fail if we cannot. */
00047     if( robot_init() < 0 )
00048     {
00049       ROS_FATAL("Robot interface broken\n");
00050       ROS_BREAK();
00051     }
00052 
00053     /* We need to attach the program to the hand as well, or fail if we cannot. */
00054     if( hand_init() < 0 )
00055     {
00056       ROS_FATAL("Hand interface broken\n");
00057       ROS_BREAK();
00058     }
00059 
00060     initializeMap();
00061 
00062     self_test = boost::shared_ptr<self_test::TestRunner>(new self_test::TestRunner());
00063 
00064     self_test->add("Pretest", this, &RealShadowhand::pretest);
00065     self_test->add("Number of messages Received", this, &RealShadowhand::test_messages);
00066     self_test->add("Posttest", this, &RealShadowhand::posttest);
00067   }
00068 
00069   RealShadowhand::~RealShadowhand()
00070   {
00071   }
00072 
00073   void RealShadowhand::initializeMap()
00074   {
00075     joints_map_mutex.lock();
00076     parameters_map_mutex.lock();
00077 
00078     JointData tmpData;
00079 
00080     tmpData.position = 0.0;
00081     tmpData.target = 0.0;
00082     tmpData.temperature = 0.0;
00083     tmpData.current = 0.0;
00084     tmpData.force = 0.0;
00085     tmpData.flags = "";
00086 
00087     for( unsigned int i = 0; i < LAST_HAND_JOINT; i++ )
00088     {
00089       std::string name = hand_joints[i].joint_name;
00090       tmpData.jointIndex = i;
00091 
00092       joints_map[name] = tmpData;
00093 
00094       ROS_DEBUG("NAME[%d]: %s ", i, name.c_str());
00095     }
00096 
00097     parameters_map["d"] = PARAM_d;
00098     parameters_map["i"] = PARAM_i;
00099     parameters_map["p"] = PARAM_p;
00100     parameters_map["target"] = PARAM_target;
00101     parameters_map["sensor"] = PARAM_sensor;
00102 
00103     parameters_map["valve"] = PARAM_valve;
00104     parameters_map["dead"] = PARAM_deadband;
00105     parameters_map["deadband"] = PARAM_deadband;
00106     parameters_map["imax"] = PARAM_imax;
00107     parameters_map["offset"] = PARAM_output_offset;
00108     parameters_map["shift"] = PARAM_shift;
00109     parameters_map["max"] = PARAM_output_max;
00110 
00112     parameters_map["motor_maxforce"] = PARAM_motor_maxforce;
00113     parameters_map["motor_safeforce"] = PARAM_motor_safeforce;
00114 
00115     parameters_map["force_p"] = PARAM_force_p;
00116     parameters_map["force_i"] = PARAM_force_i;
00117     parameters_map["force_d"] = PARAM_force_d;
00118 
00119     parameters_map["force_imax"] = PARAM_force_imax;
00120     parameters_map["force_out_shift"] = PARAM_force_out_shift;
00121     parameters_map["force_deadband"] = PARAM_force_deadband;
00122     parameters_map["force_offset"] = PARAM_force_offset;
00123 
00124     parameters_map["sensor_imax"] = PARAM_sensor_imax;
00125     parameters_map["sensor_out_shift"] = PARAM_sensor_out_shift;
00126     parameters_map["sensor_deadband"] = PARAM_sensor_deadband;
00127     parameters_map["sensor_offset"] = PARAM_sensor_offset;
00128     parameters_map["max_temp"] = PARAM_max_temperature;
00129     parameters_map["max_temperature"] = PARAM_max_temperature;
00130     parameters_map["max_current"] = PARAM_max_current;
00131 
00132     parameters_map_mutex.unlock();
00133     joints_map_mutex.unlock();
00134   }
00135 
00136   short RealShadowhand::sendupdate( std::string joint_name, double target )
00137   {
00138     joints_map_mutex.lock();
00139 
00140     //search the sensor in the hash_map
00141     JointsMap::iterator iter = joints_map.find(joint_name);
00142 
00143     if( iter != joints_map.end() )
00144     {
00145       JointData tmpData = joints_map.find(joint_name)->second;
00146       int index_hand_joints = tmpData.jointIndex;
00147 
00148       //trim to the correct range
00149       if( target < hand_joints[index_hand_joints].min_angle )
00150         target = hand_joints[index_hand_joints].min_angle;
00151       if( target > hand_joints[index_hand_joints].max_angle )
00152         target = hand_joints[index_hand_joints].max_angle;
00153 
00154       //here we update the actual hand angles
00155       robot_update_sensor(&hand_joints[index_hand_joints].joint_target, target);
00156       joints_map_mutex.unlock();
00157       return 0;
00158     }
00159 
00160     ROS_DEBUG("Joint %s not found", joint_name.c_str());
00161 
00162     joints_map_mutex.unlock();
00163     return -1;
00164   }
00165 
00166   JointData RealShadowhand::getJointData( std::string joint_name )
00167   {
00168     joints_map_mutex.lock();
00169 
00170     JointsMap::iterator iter = joints_map.find(joint_name);
00171 
00172     //joint not found
00173     if( iter == joints_map.end() )
00174     {
00175       ROS_ERROR("Joint %s not found.", joint_name.c_str());
00176       JointData noData;
00177       noData.position = 0.0;
00178       noData.target = 0.0;
00179       noData.temperature = 0.0;
00180       noData.current = 0.0;
00181       noData.force = 0.0;
00182       noData.flags = "";
00183       noData.jointIndex = 0;
00184 
00185       joints_map_mutex.unlock();
00186       return noData;
00187     }
00188 
00189     //joint found
00190     JointData tmpData = iter->second;
00191     int index = tmpData.jointIndex;
00192 
00193     tmpData.position = robot_read_sensor(&hand_joints[index].position);
00194     tmpData.target = robot_read_sensor(&hand_joints[index].joint_target);
00195 
00196     //more information
00197     if( hand_joints[index].a.smartmotor.has_sensors )
00198     {
00199       tmpData.temperature = robot_read_sensor(&hand_joints[index].a.smartmotor.temperature);
00200       tmpData.current = robot_read_sensor(&hand_joints[index].a.smartmotor.current);
00201       tmpData.force = robot_read_sensor(&hand_joints[index].a.smartmotor.torque);
00202       tmpData.flags = "";
00203     }
00204 
00205     joints_map[joint_name] = tmpData;
00206 
00207     joints_map_mutex.unlock();
00208     return tmpData;
00209   }
00210 
00211   SRArticulatedRobot::JointsMap RealShadowhand::getAllJointsData()
00212   {
00213     //update the map for each joints
00214     for( JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it )
00215       getJointData(it->first);
00216 
00217     //hack for C6M2 with 4 fingers with coupling J1+J2=J0
00218     //copy J0/2 to J1 and J2 for FF MF RF LF
00219     joints_map_mutex.lock();
00220     
00221     std::string fingername("FF");
00222     
00223     JointsMap::iterator it_j0 = joints_map.find(fingername+"J0");
00224     JointsMap::iterator it_j1 = joints_map.find(fingername+"J1");
00225     JointsMap::iterator it_j2 = joints_map.find(fingername+"J2");
00226     it_j1->second.target = it_j0->second.target/2.0;
00227     it_j2->second.target = it_j0->second.target/2.0;
00228 
00229     fingername.assign("MF");
00230     it_j0 = joints_map.find(fingername+"J0");
00231     it_j1 = joints_map.find(fingername+"J1");
00232     it_j2 = joints_map.find(fingername+"J2");
00233     it_j1->second.target = it_j0->second.target/2.0;
00234     it_j2->second.target = it_j0->second.target/2.0;
00235 
00236     fingername.assign("RF");
00237     it_j0 = joints_map.find(fingername+"J0");
00238     it_j1 = joints_map.find(fingername+"J1");
00239     it_j2 = joints_map.find(fingername+"J2");
00240     it_j1->second.target = it_j0->second.target/2.0;
00241     it_j2->second.target = it_j0->second.target/2.0;
00242 
00243     fingername.assign("LF");
00244     it_j0 = joints_map.find(fingername+"J0");
00245     it_j1 = joints_map.find(fingername+"J1");
00246     it_j2 = joints_map.find(fingername+"J2");
00247     it_j1->second.target = it_j0->second.target/2.0;
00248     it_j2->second.target = it_j0->second.target/2.0;
00249     
00250     joints_map_mutex.unlock();
00251 
00252     JointsMap tmp = JointsMap(joints_map);
00253 
00254     //return the map
00255     return tmp;
00256   }
00257 
00258   short RealShadowhand::setContrl( std::string contrlr_name, JointControllerData ctrlr_data )
00259   {
00260     parameters_map_mutex.lock();
00261 
00262     struct controller_config myConfig;
00263     memset(&myConfig, 0, sizeof(myConfig));
00264 
00265     //set the nodename from contrlr_name
00266     myConfig.nodename = contrlr_name.c_str();
00267 
00268     controller_read_from_hardware(&myConfig);
00269     ROS_DEBUG("%s", controller_to_string(&myConfig));
00270 
00271     for( unsigned int i = 0; i < ctrlr_data.data.size(); ++i )
00272     {
00273       std::string name = ctrlr_data.data[i].name;
00274       ParametersMap::iterator iter = parameters_map.find(name);
00275 
00276       //parameter not found
00277       if( iter == parameters_map.end() )
00278       {
00279         ROS_ERROR("Parameter %s not found.", name.c_str());
00280         continue;
00281       }
00282 
00283       //parameter found
00284       controller_update_param(&myConfig, (controller_param)iter->second, ctrlr_data.data[i].value.c_str());
00285     }
00286 
00287     parameters_map_mutex.unlock();
00288 
00289     int result_ctrlr = controller_write_to_hardware(&myConfig);
00290     if( result_ctrlr )
00291     {
00292       ROS_ERROR("Failed to update contrlr (%s)", myConfig.nodename );
00293       return -1;
00294     }
00295 
00296     return 0;
00297   }
00298 
00299   JointControllerData RealShadowhand::getContrl( std::string contrlr_name )
00300   {
00301     struct controller_config myConfig;
00302     memset(&myConfig, 0, sizeof(myConfig));
00303 
00304     //set the nodename from contrlr_name
00305     myConfig.nodename = contrlr_name.c_str();
00306 
00307     controller_read_from_hardware(&myConfig);
00308 
00309     JointControllerData tmp_data;
00310 
00311     ROS_WARN("The get contrlr function is not implemented in the real shadowhand.");
00312 
00313     return tmp_data;
00314 
00315   }
00316 
00317   short RealShadowhand::setConfig( std::vector<std::string> myConfig )
00318   {
00319     ROS_WARN("The set config function is not implemented in the real shadowhand.");
00320 
00321     /*
00322       hand_protocol_config_t cfg;
00323       hand_protocol_get_config(cfg);
00324 
00325       //set the transmit rate value
00326       int value = msg->rate_value;
00327       cfg->u.palm.tx_freq[num]=value;
00328 
00329       //send the config to the palm.
00330       hand_protocol_set_config(cfg);
00331     */
00332 
00333     return 0;
00334   }
00335 
00336   void RealShadowhand::getConfig( std::string joint_name )
00337   {
00338     ROS_WARN("The get config function is not yet implement in the real shadow hand.");
00339   }
00340 
00341   std::vector<DiagnosticData> RealShadowhand::getDiagnostics()
00342   {
00343     //it's alright to do the tests when we're publishing the diagnostics
00344     self_test->checkTest();
00345 
00346     std::vector<DiagnosticData> returnVector;
00347 
00348     DiagnosticData tmpData;
00349     std::stringstream ss;
00350 
00351     //get the data from the hand
00352     for( unsigned int index = 0; index < START_OF_ARM; ++index )
00353     {
00354       tmpData.joint_name = std::string(hand_joints[index].joint_name);
00355       tmpData.level = 0;
00356 
00357       tmpData.position = robot_read_sensor(&hand_joints[index].position);
00358       tmpData.target = robot_read_sensor(&hand_joints[index].joint_target);
00359 
00360       //more information
00361       if( hand_joints[index].a.smartmotor.has_sensors )
00362       {
00363         //reads the number of received sensor msgs from the debug node.
00364         int res;
00365         if( *(&hand_joints[index].a.smartmotor.debug_nodename) != NULL)
00366         {
00367           std::string debug_node_name = *(&hand_joints[index].a.smartmotor.debug_nodename);
00368 
00369           //get all the debug values
00370           std::map<const std::string, const unsigned int>::const_iterator iter;
00371           for(iter = debug_values::names_and_offsets.begin();
00372               iter !=  debug_values::names_and_offsets.end(); ++iter)
00373           {
00374             struct sensor sensor_tmp;
00375 
00376             res = robot_channel_to_sensor(debug_node_name.c_str(), iter->second, &sensor_tmp);
00377             tmpData.debug_values[iter->first] = robot_read_incoming(&sensor_tmp);
00378           }
00379         }
00380         //      else
00381         //  ROS_ERROR_STREAM(tmpData.joint_name << ": no debug sensor" );
00382         //reads temperature current and force.
00383         tmpData.temperature = robot_read_sensor(&hand_joints[index].a.smartmotor.temperature);
00384         tmpData.current = robot_read_sensor(&hand_joints[index].a.smartmotor.current);
00385         tmpData.force = robot_read_sensor(&hand_joints[index].a.smartmotor.torque);
00386 
00387         //check for error_flags
00388 
00389         struct hand_protocol_flags fl;
00390         uint64_t uuid = robot_node_id(hand_joints[index].a.smartmotor.nodename);
00391         fl = hand_protocol_get_status_flags(uuid);
00392         if( fl.valid )
00393         {
00394           ss.clear();
00395           struct hand_protocol_flags_smart_motor f;
00396           f = fl.u.smart_motor;
00397 
00398           bool at_least_one_error_flag = false;
00399           if( f.nfault_pin )
00400           {
00401             at_least_one_error_flag = true;
00402             ss << "NFAULT ";
00403             ROS_WARN( "[%s]: NFAULT", hand_joints[index].joint_name );
00404           }
00405           if( f.temperature_cutout )
00406           {
00407             at_least_one_error_flag = true;
00408             ss << "TEMP ";
00409           }
00410           if( f.current_throttle )
00411           {
00412             at_least_one_error_flag = true;
00413             ss << "CURRENT ";
00414           }
00415           if( f.force_hard_limit )
00416           {
00417             at_least_one_error_flag = true;
00418             ss << "FORCE ";
00419           }
00420           if( hand_protocol_dead(uuid) )
00421           {
00422             at_least_one_error_flag = true;
00423             ss << "DEAD ";
00424           }
00425 
00426           //if a flag is up, then print a warning as well
00427           if( at_least_one_error_flag )
00428           {
00429             ROS_WARN( "[%s]: %s", hand_joints[index].joint_name, (ss.str()).c_str());
00430             tmpData.level = 1;
00431           }
00432 
00433           tmpData.flags = ss.str();
00434         }
00435       }
00436 
00437       returnVector.push_back(tmpData);
00438     }
00439     return returnVector;
00440   }
00441 
00443   //    TESTS      //
00445   void RealShadowhand::pretest(diagnostic_updater::DiagnosticStatusWrapper& status)
00446   {
00447     ROS_INFO("Preparing the environment to run self tests.");
00448 
00449     //lock all the mutexes to make sure we're not publishing other messages
00450     joints_map_mutex.lock();
00451     parameters_map_mutex.lock();
00452     controllers_map_mutex.lock();
00453 
00454     //TODO: set the palm transmit rate to max?
00455 
00456     sleep(1);
00457 
00458     status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Pretest completed successfully.");
00459   }
00460 
00461   void RealShadowhand::posttest(diagnostic_updater::DiagnosticStatusWrapper& status)
00462   {
00463     ROS_INFO("Restoring the environment after the self tests.");
00464 
00465     //test finished, unlocking all the mutexes
00466     joints_map_mutex.unlock();
00467     parameters_map_mutex.unlock();
00468     controllers_map_mutex.unlock();
00469 
00470     //TODO: reset the palm transmit rate to previous state?
00471 
00472     status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Postest completed successfully.");
00473   }
00474 
00475   void RealShadowhand::test_messages(diagnostic_updater::DiagnosticStatusWrapper& status)
00476   {
00477     ROS_WARN("Starting the test: Number of messages Received");
00478 
00479     std::pair<unsigned char, std::string> test_result;
00480     test_result.first = diagnostic_msgs::DiagnosticStatus::OK;
00481 
00482     for(unsigned int index_freq=0; index_freq < sr_self_tests::msgs_frequency_size; ++index_freq)
00483     {
00484       ros::Rate test_rate(sr_self_tests::msgs_frequency[index_freq]);
00485       for(unsigned int index_joint=0; index_joint < sr_self_tests::joints_to_test_size; ++index_joint)
00486       {
00487         std::pair<unsigned char, std::string> tmp_test_result;
00488         tmp_test_result = test_messages_routine(sr_self_tests::joints_to_test[index_joint], sr_self_tests::nb_targets_to_send, test_rate);
00489 
00490         if( tmp_test_result.first == diagnostic_msgs::DiagnosticStatus::ERROR)
00491           test_result.first = diagnostic_msgs::DiagnosticStatus::ERROR;
00492 
00493         std::stringstream ss;
00494         ss << "\n[" << sr_self_tests::msgs_frequency[index_freq] << "Hz]: ";
00495         ss << tmp_test_result.second;
00496 
00497         test_result.second += ss.str();
00498       }
00499     }
00500     status.summary(test_result.first, test_result.second);
00501   }
00502 
00503   std::pair<unsigned char, std::string> RealShadowhand::test_messages_routine(std::string joint_name, unsigned int repeat, ros::Rate rate)
00504   {
00505     std::pair<unsigned char, std::string> test_result;
00506 
00507     //id should be motor board number
00508     std::string ID = "1";
00509     self_test->setID(ID.c_str());
00510 
00511     unsigned int nb_msgs_sent = 0;
00512     unsigned int nb_msgs_received = 0;
00513 
00514     //sending lots of data to one joint
00515     JointsMap::iterator iter_joints_map = joints_map.find(joint_name);
00516 
00517     struct sensor sensor_msgs_received;
00518 
00519     if( iter_joints_map == joints_map.end() )
00520     {
00521       std::stringstream ss;
00522       ss << "No messages sent: couldn't find joint "<<joint_name;
00523 
00524       test_result.first = diagnostic_msgs::DiagnosticStatus::ERROR;
00525       test_result.second = ss.str();
00526       return test_result;
00527     }
00528 
00529     //OK joint found
00530     JointData tmpData = joints_map.find(joint_name)->second;
00531     int index_hand_joints = tmpData.jointIndex;
00532     float target =  hand_joints[index_hand_joints].min_angle;
00533 
00534     //testing a joint which doesn't have a smartmotor
00535     if( !hand_joints[index_hand_joints].a.smartmotor.has_sensors )
00536     {
00537       std::stringstream ss;
00538       ss << "No messages sent: joint["<<joint_name<<"] doesn't have any motor attached";
00539 
00540       test_result.first = diagnostic_msgs::DiagnosticStatus::ERROR;
00541       test_result.second = ss.str();
00542       return test_result;
00543     }
00544 
00545     ROS_DEBUG("Checking the current number of received messages");
00546 
00547     int res;
00548     std::string debug_node_name = *(&hand_joints[index_hand_joints].a.smartmotor.debug_nodename);
00549     std::map<const std::string, const unsigned int>::const_iterator iter_debug_values =  debug_values::names_and_offsets.find("Num sensor Msgs received");
00550 
00551     res = robot_channel_to_sensor(debug_node_name.c_str(), iter_debug_values->second, &sensor_msgs_received);
00552 
00553     //check the number of messages already received when starting the test
00554     nb_msgs_received = robot_read_incoming(&sensor_msgs_received) - nb_msgs_received;
00555 
00556     sleep(1);
00557 
00558     //check if no other messages have been received
00559     if( nb_msgs_received != robot_read_incoming(&sensor_msgs_received))
00560     {
00561       std::stringstream ss;
00562       ss <<  "New messages were received on the joint[" <<  joint_name.c_str() << "]." ;
00563       test_result.first = diagnostic_msgs::DiagnosticStatus::ERROR;
00564       test_result.second = ss.str();
00565 
00566       return test_result;
00567     }
00568     //ok still the same number of messages
00569 
00570     ROS_DEBUG("Sending lots of messages.");
00571 
00572     for(; nb_msgs_sent < repeat; ++nb_msgs_sent)
00573     {
00574       //send values to the sensor
00575       robot_update_sensor(&hand_joints[index_hand_joints].joint_target, target);
00576       rate.sleep();
00577       ROS_DEBUG_STREAM("msg "<< nb_msgs_sent<< "/"<<sr_self_tests::nb_targets_to_send);
00578     }
00579 
00580     ROS_DEBUG("Done sending the messages.");
00581     //wait for all the messages to be received?
00582     sleep(0.5);
00583 
00584     ROS_DEBUG("Reading the number of received messages.");
00585     //compute the number of messages received during the test
00586     nb_msgs_received = robot_read_incoming(&sensor_msgs_received) - nb_msgs_received;
00587 
00588     if( nb_msgs_sent == nb_msgs_received)
00589     {
00590       std::stringstream ss;
00591       ss <<  nb_msgs_sent << " messages sent, all received";
00592       test_result.first = diagnostic_msgs::DiagnosticStatus::OK;
00593       test_result.second = ss.str();
00594       return test_result;
00595     }
00596     else
00597     {
00598       std::stringstream ss;
00599       ss << nb_msgs_sent << " messages sent, "<<nb_msgs_received << " messages received";
00600       test_result.first = diagnostic_msgs::DiagnosticStatus::ERROR;
00601       test_result.second = ss.str();
00602       return test_result;
00603     }
00604   }
00605 } //end namespace
00606 
00607 /* For the emacs weenies in the crowd.
00608    Local Variables:
00609    c-basic-offset: 2
00610    End:
00611 */


sr_hand
Author(s): Ugo Cupcic / ugo@shadowrobot.com, contact@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:44:02