← Back to branch summary

~ugocupcic/sr-ros-interface/manipulation

« back to all changes in this revision

Viewing changes to shadow_robot/sr_hand/src/hand/real_shadowhand.cpp

  • Committer: Ugo Cupcic
  • Date: 2011-05-23 15:44:05 UTC
  • mfrom: (119.1.67 sr-ros-interface)
  • Revision ID: ugo@shadowrobot.com-20110523154405-kwvv543sy9wxehzi
Huge merge from trunk. Lots of changes.

Show diffs side-by-side

added added

removed removed

Lines of Context:
shadow_robot/sr_hand/src/hand/real_shadowhand.cpp
3
3
 * @author Ugo Cupcic <ugo@shadowrobot.com>, Contact <contact@shadowrobot.com>
4
4
 * @date   Tue May 25 17:50:42 2010
5
5
 *
 
6
*
 
7
* Copyright 2011 Shadow Robot Company Ltd.
 
8
*
 
9
* This program is free software: you can redistribute it and/or modify it
 
10
* under the terms of the GNU General Public License as published by the Free
 
11
* Software Foundation, either version 2 of the License, or (at your option)
 
12
* any later version.
 
13
*
 
14
* This program is distributed in the hope that it will be useful, but WITHOUT
 
15
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 
16
* FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 
17
* more details.
 
18
*
 
19
* You should have received a copy of the GNU General Public License along
 
20
* with this program.  If not, see <http://www.gnu.org/licenses/>.
 
21
*
6
22
 * @brief
7
23
 *
8
24
 *
18
34
 
19
35
namespace shadowrobot
20
36
{
21
 
RealShadowhand::RealShadowhand() :
 
37
  RealShadowhand::RealShadowhand() :
22
38
    SRArticulatedRobot()
23
 
{
 
39
  {
24
40
    /* We need to attach the program to the robot, or fail if we cannot. */
25
41
    if( robot_init() < 0 )
26
42
    {
27
 
        ROS_FATAL("Robot interface broken\n");
28
 
        ROS_BREAK();
 
43
      ROS_FATAL("Robot interface broken\n");
 
44
      ROS_BREAK();
29
45
    }
30
46
 
31
47
    /* We need to attach the program to the hand as well, or fail if we cannot. */
32
48
    if( hand_init() < 0 )
33
49
    {
34
 
        ROS_FATAL("Hand interface broken\n");
35
 
        ROS_BREAK();
 
50
      ROS_FATAL("Hand interface broken\n");
 
51
      ROS_BREAK();
36
52
    }
37
53
 
38
54
    initializeMap();
39
 
}
40
 
 
41
 
RealShadowhand::~RealShadowhand()
42
 
{
43
 
}
44
 
 
45
 
void RealShadowhand::initializeMap()
46
 
{
 
55
 
 
56
    self_test = boost::shared_ptr<self_test::TestRunner>(new self_test::TestRunner());
 
57
 
 
58
    self_test->add("Pretest", this, &RealShadowhand::pretest);
 
59
    self_test->add("Number of messages Received", this, &RealShadowhand::test_messages);
 
60
    self_test->add("Posttest", this, &RealShadowhand::posttest);
 
61
  }
 
62
 
 
63
  RealShadowhand::~RealShadowhand()
 
64
  {
 
65
  }
 
66
 
 
67
  void RealShadowhand::initializeMap()
 
68
  {
47
69
    joints_map_mutex.lock();
48
70
    parameters_map_mutex.lock();
49
71
 
56
78
    tmpData.force = 0.0;
57
79
    tmpData.flags = "";
58
80
 
59
 
    for( unsigned int i = 0; i < NUM_HAND_JOINTS + 4; i++ )
 
81
    ROS_DEBUG_STREAM("START OF ARM "<< START_OF_ARM);
 
82
 
 
83
    for( unsigned int i = 0; i < START_OF_ARM; i++ )
60
84
    {
61
 
        std::string name = hand_joints[i].joint_name;
62
 
        tmpData.jointIndex = i;
63
 
 
64
 
        joints_map[name] = tmpData;
65
 
 
66
 
        ROS_INFO("NAME[%d]: %s ", i, name.c_str());
 
85
      std::string name = hand_joints[i].joint_name;
 
86
      tmpData.jointIndex = i;
 
87
 
 
88
      joints_map[name] = tmpData;
 
89
 
 
90
      ROS_DEBUG("NAME[%d]: %s ", i, name.c_str());
67
91
    }
68
92
 
69
93
    parameters_map["d"] = PARAM_d;
103
127
 
104
128
    parameters_map_mutex.unlock();
105
129
    joints_map_mutex.unlock();
106
 
}
 
130
  }
107
131
 
108
 
short RealShadowhand::sendupdate( std::string joint_name, double target )
109
 
{
 
132
  short RealShadowhand::sendupdate( std::string joint_name, double target )
 
133
  {
110
134
    joints_map_mutex.lock();
111
135
 
112
136
    //search the sensor in the hash_map
114
138
 
115
139
    if( iter != joints_map.end() )
116
140
    {
117
 
        JointData tmpData = joints_map.find(joint_name)->second;
118
 
        int index_hand_joints = tmpData.jointIndex;
119
 
 
120
 
        //trim to the correct range
121
 
        if( target < hand_joints[index_hand_joints].min_angle )
122
 
            target = hand_joints[index_hand_joints].min_angle;
123
 
        if( target > hand_joints[index_hand_joints].max_angle )
124
 
            target = hand_joints[index_hand_joints].max_angle;
125
 
 
126
 
        //here we update the actual hand angles
127
 
        robot_update_sensor(&hand_joints[index_hand_joints].joint_target, target);
128
 
        joints_map_mutex.unlock();
129
 
        return 0;
 
141
      JointData tmpData = joints_map.find(joint_name)->second;
 
142
      int index_hand_joints = tmpData.jointIndex;
 
143
 
 
144
      //trim to the correct range
 
145
      if( target < hand_joints[index_hand_joints].min_angle )
 
146
        target = hand_joints[index_hand_joints].min_angle;
 
147
      if( target > hand_joints[index_hand_joints].max_angle )
 
148
        target = hand_joints[index_hand_joints].max_angle;
 
149
 
 
150
      //here we update the actual hand angles
 
151
      robot_update_sensor(&hand_joints[index_hand_joints].joint_target, target);
 
152
      joints_map_mutex.unlock();
 
153
      return 0;
130
154
    }
131
155
 
132
156
    ROS_DEBUG("Joint %s not found", joint_name.c_str());
133
157
 
134
158
    joints_map_mutex.unlock();
135
159
    return -1;
136
 
}
 
160
  }
137
161
 
138
 
JointData RealShadowhand::getJointData( std::string joint_name )
139
 
{
 
162
  JointData RealShadowhand::getJointData( std::string joint_name )
 
163
  {
140
164
    joints_map_mutex.lock();
141
165
 
142
166
    JointsMap::iterator iter = joints_map.find(joint_name);
144
168
    //joint not found
145
169
    if( iter == joints_map.end() )
146
170
    {
147
 
        ROS_ERROR("Joint %s not found.", joint_name.c_str());
148
 
        JointData noData;
149
 
        noData.position = 0.0;
150
 
        noData.target = 0.0;
151
 
        noData.temperature = 0.0;
152
 
        noData.current = 0.0;
153
 
        noData.force = 0.0;
154
 
        noData.flags = "";
155
 
        noData.jointIndex = 0;
 
171
      ROS_ERROR("Joint %s not found.", joint_name.c_str());
 
172
      JointData noData;
 
173
      noData.position = 0.0;
 
174
      noData.target = 0.0;
 
175
      noData.temperature = 0.0;
 
176
      noData.current = 0.0;
 
177
      noData.force = 0.0;
 
178
      noData.flags = "";
 
179
      noData.jointIndex = 0;
156
180
 
157
 
        joints_map_mutex.unlock();
158
 
        return noData;
 
181
      joints_map_mutex.unlock();
 
182
      return noData;
159
183
    }
160
184
 
161
185
    //joint found
168
192
    //more information
169
193
    if( hand_joints[index].a.smartmotor.has_sensors )
170
194
    {
171
 
        tmpData.temperature = robot_read_sensor(&hand_joints[index].a.smartmotor.temperature);
172
 
        tmpData.current = robot_read_sensor(&hand_joints[index].a.smartmotor.current);
173
 
        tmpData.force = robot_read_sensor(&hand_joints[index].a.smartmotor.torque);
174
 
        tmpData.flags = "";
 
195
      tmpData.temperature = robot_read_sensor(&hand_joints[index].a.smartmotor.temperature);
 
196
      tmpData.current = robot_read_sensor(&hand_joints[index].a.smartmotor.current);
 
197
      tmpData.force = robot_read_sensor(&hand_joints[index].a.smartmotor.torque);
 
198
      tmpData.flags = "";
175
199
    }
176
200
 
177
201
    joints_map[joint_name] = tmpData;
178
202
 
179
203
    joints_map_mutex.unlock();
180
204
    return tmpData;
181
 
}
 
205
  }
182
206
 
183
 
SRArticulatedRobot::JointsMap RealShadowhand::getAllJointsData()
184
 
{
 
207
  SRArticulatedRobot::JointsMap RealShadowhand::getAllJointsData()
 
208
  {
185
209
    //update the map for each joints
186
210
    for( JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it )
187
 
        getJointData(it->first);
 
211
      getJointData(it->first);
188
212
 
189
213
    JointsMap tmp = JointsMap(joints_map);
190
214
 
191
215
    //return the map
192
216
    return tmp;
193
 
}
 
217
  }
194
218
 
195
 
short RealShadowhand::setContrl( std::string contrlr_name, JointControllerData ctrlr_data )
196
 
{
 
219
  short RealShadowhand::setContrl( std::string contrlr_name, JointControllerData ctrlr_data )
 
220
  {
197
221
    parameters_map_mutex.lock();
198
222
 
199
223
    struct controller_config myConfig;
207
231
 
208
232
    for( unsigned int i = 0; i < ctrlr_data.data.size(); ++i )
209
233
    {
210
 
        std::string name = ctrlr_data.data[i].name;
211
 
        ParametersMap::iterator iter = parameters_map.find(name);
212
 
 
213
 
        //parameter not found
214
 
        if( iter == parameters_map.end() )
215
 
        {
216
 
            ROS_ERROR("Parameter %s not found.", name.c_str());
217
 
            continue;
218
 
        }
219
 
 
220
 
        //parameter found
221
 
        controller_update_param(&myConfig, (controller_param)iter->second, ctrlr_data.data[i].value.c_str());
 
234
      std::string name = ctrlr_data.data[i].name;
 
235
      ParametersMap::iterator iter = parameters_map.find(name);
 
236
 
 
237
      //parameter not found
 
238
      if( iter == parameters_map.end() )
 
239
      {
 
240
        ROS_ERROR("Parameter %s not found.", name.c_str());
 
241
        continue;
 
242
      }
 
243
 
 
244
      //parameter found
 
245
      controller_update_param(&myConfig, (controller_param)iter->second, ctrlr_data.data[i].value.c_str());
222
246
    }
223
247
 
224
248
    parameters_map_mutex.unlock();
226
250
    int result_ctrlr = controller_write_to_hardware(&myConfig);
227
251
    if( result_ctrlr )
228
252
    {
229
 
        ROS_ERROR("Failed to update contrlr (%s)", myConfig.nodename );
230
 
        return -1;
 
253
      ROS_ERROR("Failed to update contrlr (%s)", myConfig.nodename );
 
254
      return -1;
231
255
    }
232
256
 
233
257
    return 0;
234
 
}
 
258
  }
235
259
 
236
 
JointControllerData RealShadowhand::getContrl( std::string contrlr_name )
237
 
{
 
260
  JointControllerData RealShadowhand::getContrl( std::string contrlr_name )
 
261
  {
238
262
    struct controller_config myConfig;
239
263
    memset(&myConfig, 0, sizeof(myConfig));
240
264
 
249
273
 
250
274
    return tmp_data;
251
275
 
252
 
}
 
276
  }
253
277
 
254
 
short RealShadowhand::setConfig( std::vector<std::string> myConfig )
255
 
{
 
278
  short RealShadowhand::setConfig( std::vector<std::string> myConfig )
 
279
  {
256
280
    ROS_WARN("The set config function is not implemented in the real shadowhand.");
257
281
 
258
282
    /*
259
 
     hand_protocol_config_t cfg;
260
 
     hand_protocol_get_config(cfg);
261
 
 
262
 
     //set the transmit rate value
263
 
     int value = msg->rate_value;
264
 
     cfg->u.palm.tx_freq[num]=value;
265
 
 
266
 
     //send the config to the palm.
267
 
     hand_protocol_set_config(cfg);
268
 
     */
 
283
      hand_protocol_config_t cfg;
 
284
      hand_protocol_get_config(cfg);
 
285
 
 
286
      //set the transmit rate value
 
287
      int value = msg->rate_value;
 
288
      cfg->u.palm.tx_freq[num]=value;
 
289
 
 
290
      //send the config to the palm.
 
291
      hand_protocol_set_config(cfg);
 
292
    */
269
293
 
270
294
    return 0;
271
 
}
 
295
  }
272
296
 
273
 
void RealShadowhand::getConfig( std::string joint_name )
274
 
{
 
297
  void RealShadowhand::getConfig( std::string joint_name )
 
298
  {
275
299
    ROS_WARN("The get config function is not yet implement in the real shadow hand.");
276
 
}
277
 
 
278
 
/*
279
 
 char* ShadowhandDiagnosticer::get_setpoint_name(uint16_t s_num)
280
 
 {
281
 
 //  struct sensor temp;
282
 
 static char name_buff[256];
283
 
 
284
 
 snprintf(name_buff, 256, "%s.%d", "smart_motor_setpoints", s_num);
285
 
 
286
 
 //TODO: get the sensor name
287
 
 if (0==robot_channel_to_sensor("smart_motor_setpoints", s_num, &temp))
288
 
 {
289
 
 const char *name = robot_sensor_calibration_name(&temp);
290
 
 return (char*)name;
291
 
 }
292
 
 return name_buff;
293
 
 }*/
294
 
 
295
 
std::vector<DiagnosticData> RealShadowhand::getDiagnostics()
296
 
{
 
300
  }
 
301
 
 
302
  std::vector<DiagnosticData> RealShadowhand::getDiagnostics()
 
303
  {
 
304
    //it's alright to do the tests when we're publishing the diagnostics
 
305
    self_test->checkTest();
 
306
 
297
307
    std::vector<DiagnosticData> returnVector;
298
308
 
299
309
    DiagnosticData tmpData;
300
 
 
301
 
    //concatenate the flags in a stringstream
302
310
    std::stringstream ss;
303
311
 
304
312
    //get the data from the hand
305
313
    for( unsigned int index = 0; index < START_OF_ARM; ++index )
306
314
    {
307
 
        tmpData.joint_name = std::string(hand_joints[index].joint_name);
308
 
        tmpData.level = 0;
309
 
 
310
 
        tmpData.position = robot_read_sensor(&hand_joints[index].position);
311
 
        tmpData.target = robot_read_sensor(&hand_joints[index].joint_target);
312
 
 
313
 
        //more information
314
 
        if( hand_joints[index].a.smartmotor.has_sensors )
315
 
        {
316
 
            tmpData.temperature = robot_read_sensor(&hand_joints[index].a.smartmotor.temperature);
317
 
            tmpData.current = robot_read_sensor(&hand_joints[index].a.smartmotor.current);
318
 
            tmpData.force = robot_read_sensor(&hand_joints[index].a.smartmotor.torque);
319
 
 
320
 
            //check for error_flags
321
 
            uint64_t uuid = robot_node_id(hand_joints[index].a.smartmotor.nodename);
322
 
            struct hand_protocol_flags fl;
323
 
            fl = hand_protocol_get_status_flags(uuid);
324
 
            if( fl.valid )
325
 
            {
326
 
                struct hand_protocol_flags_smart_motor f;
327
 
                f = fl.u.smart_motor;
328
 
 
329
 
                //empty the stringstream
330
 
                ss.str("");
331
 
 
332
 
                bool at_least_one_error_flag = false;
333
 
                if( f.nfault_pin )
334
 
                {
335
 
                    at_least_one_error_flag = true;
336
 
                    ss << "NFAULT ";
337
 
                    ROS_WARN( "[%s]: NFAULT", hand_joints[index].joint_name );
338
 
                }
339
 
                if( f.temperature_cutout )
340
 
                {
341
 
                    at_least_one_error_flag = true;
342
 
                    ss << "TEMP ";
343
 
                }
344
 
                if( f.current_throttle )
345
 
                {
346
 
                    at_least_one_error_flag = true;
347
 
                    ss << "CURRENT ";
348
 
                }
349
 
                if( f.force_hard_limit )
350
 
                {
351
 
                    at_least_one_error_flag = true;
352
 
                    ss << "FORCE ";
353
 
                }
354
 
                if( hand_protocol_dead(uuid) )
355
 
                {
356
 
                    at_least_one_error_flag = true;
357
 
                    ss << "DEAD ";
358
 
                }
359
 
 
360
 
                //set the message flags
361
 
                tmpData.flags = ss.str();
362
 
                //if a flag is up, then print a warning as well
363
 
                if( at_least_one_error_flag )
364
 
                {
365
 
                    ROS_WARN( "[%s]: %s", hand_joints[index].joint_name, (ss.str()).c_str());
366
 
                    tmpData.level = 1;
367
 
                }
368
 
            }
369
 
        }
370
 
 
371
 
        returnVector.push_back(tmpData);
 
315
      tmpData.joint_name = std::string(hand_joints[index].joint_name);
 
316
      tmpData.level = 0;
 
317
 
 
318
      tmpData.position = robot_read_sensor(&hand_joints[index].position);
 
319
      tmpData.target = robot_read_sensor(&hand_joints[index].joint_target);
 
320
 
 
321
      //more information
 
322
      if( hand_joints[index].a.smartmotor.has_sensors )
 
323
      {
 
324
        //reads the number of received sensor msgs from the debug node.
 
325
        int res;
 
326
        if( *(&hand_joints[index].a.smartmotor.debug_nodename) != NULL)
 
327
        {
 
328
          std::string debug_node_name = *(&hand_joints[index].a.smartmotor.debug_nodename);
 
329
 
 
330
          //get all the debug values
 
331
          std::map<const std::string, const unsigned int>::const_iterator iter;
 
332
          for(iter = debug_values::names_and_offsets.begin();
 
333
              iter !=  debug_values::names_and_offsets.end(); ++iter)
 
334
          {
 
335
            struct sensor sensor_tmp;
 
336
 
 
337
            res = robot_channel_to_sensor(debug_node_name.c_str(), iter->second, &sensor_tmp);
 
338
            tmpData.debug_values[iter->first] = robot_read_incoming(&sensor_tmp);
 
339
          }
 
340
        }
 
341
        //      else
 
342
        //  ROS_ERROR_STREAM(tmpData.joint_name << ": no debug sensor" );
 
343
        //reads temperature current and force.
 
344
        tmpData.temperature = robot_read_sensor(&hand_joints[index].a.smartmotor.temperature);
 
345
        tmpData.current = robot_read_sensor(&hand_joints[index].a.smartmotor.current);
 
346
        tmpData.force = robot_read_sensor(&hand_joints[index].a.smartmotor.torque);
 
347
 
 
348
        //check for error_flags
 
349
 
 
350
        struct hand_protocol_flags fl;
 
351
        uint64_t uuid = robot_node_id(hand_joints[index].a.smartmotor.nodename);
 
352
        fl = hand_protocol_get_status_flags(uuid);
 
353
        if( fl.valid )
 
354
        {
 
355
          ss.clear();
 
356
          struct hand_protocol_flags_smart_motor f;
 
357
          f = fl.u.smart_motor;
 
358
 
 
359
          bool at_least_one_error_flag = false;
 
360
          if( f.nfault_pin )
 
361
          {
 
362
            at_least_one_error_flag = true;
 
363
            ss << "NFAULT ";
 
364
            ROS_WARN( "[%s]: NFAULT", hand_joints[index].joint_name );
 
365
          }
 
366
          if( f.temperature_cutout )
 
367
          {
 
368
            at_least_one_error_flag = true;
 
369
            ss << "TEMP ";
 
370
          }
 
371
          if( f.current_throttle )
 
372
          {
 
373
            at_least_one_error_flag = true;
 
374
            ss << "CURRENT ";
 
375
          }
 
376
          if( f.force_hard_limit )
 
377
          {
 
378
            at_least_one_error_flag = true;
 
379
            ss << "FORCE ";
 
380
          }
 
381
          if( hand_protocol_dead(uuid) )
 
382
          {
 
383
            at_least_one_error_flag = true;
 
384
            ss << "DEAD ";
 
385
          }
 
386
 
 
387
          //if a flag is up, then print a warning as well
 
388
          if( at_least_one_error_flag )
 
389
          {
 
390
            ROS_WARN( "[%s]: %s", hand_joints[index].joint_name, (ss.str()).c_str());
 
391
            tmpData.level = 1;
 
392
          }
 
393
 
 
394
          tmpData.flags = ss.str();
 
395
        }
 
396
      }
 
397
 
 
398
      returnVector.push_back(tmpData);
372
399
    }
373
 
 
374
400
    return returnVector;
375
 
}
 
401
  }
 
402
 
 
403
  ///////////////////
 
404
  //    TESTS      //
 
405
  ///////////////////
 
406
  void RealShadowhand::pretest(diagnostic_updater::DiagnosticStatusWrapper& status)
 
407
  {
 
408
    ROS_INFO("Preparing the environment to run self tests.");
 
409
 
 
410
    //lock all the mutexes to make sure we're not publishing other messages
 
411
    joints_map_mutex.lock();
 
412
    parameters_map_mutex.lock();
 
413
    controllers_map_mutex.lock();
 
414
 
 
415
    //TODO: set the palm transmit rate to max?
 
416
 
 
417
    sleep(1);
 
418
 
 
419
    status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Pretest completed successfully.");
 
420
  }
 
421
 
 
422
  void RealShadowhand::posttest(diagnostic_updater::DiagnosticStatusWrapper& status)
 
423
  {
 
424
    ROS_INFO("Restoring the environment after the self tests.");
 
425
 
 
426
    //test finished, unlocking all the mutexes
 
427
    joints_map_mutex.unlock();
 
428
    parameters_map_mutex.unlock();
 
429
    controllers_map_mutex.unlock();
 
430
 
 
431
    //TODO: reset the palm transmit rate to previous state?
 
432
 
 
433
    status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Postest completed successfully.");
 
434
  }
 
435
 
 
436
  void RealShadowhand::test_messages(diagnostic_updater::DiagnosticStatusWrapper& status)
 
437
  {
 
438
    ROS_WARN("Starting the test: Number of messages Received");
 
439
 
 
440
    std::pair<unsigned char, std::string> test_result;
 
441
    test_result.first = diagnostic_msgs::DiagnosticStatus::OK;
 
442
 
 
443
    for(unsigned int index_freq=0; index_freq < sr_self_tests::msgs_frequency_size; ++index_freq)
 
444
    {
 
445
      ros::Rate test_rate(sr_self_tests::msgs_frequency[index_freq]);
 
446
      for(unsigned int index_joint=0; index_joint < sr_self_tests::joints_to_test_size; ++index_joint)
 
447
      {
 
448
        std::pair<unsigned char, std::string> tmp_test_result;
 
449
        tmp_test_result = test_messages_routine(sr_self_tests::joints_to_test[index_joint], sr_self_tests::nb_targets_to_send, test_rate);
 
450
 
 
451
        if( tmp_test_result.first == diagnostic_msgs::DiagnosticStatus::ERROR)
 
452
          test_result.first = diagnostic_msgs::DiagnosticStatus::ERROR;
 
453
 
 
454
        std::stringstream ss;
 
455
        ss << "\n[" << sr_self_tests::msgs_frequency[index_freq] << "Hz]: ";
 
456
        ss << tmp_test_result.second;
 
457
 
 
458
        test_result.second += ss.str();
 
459
      }
 
460
    }
 
461
    status.summary(test_result.first, test_result.second);
 
462
  }
 
463
 
 
464
  std::pair<unsigned char, std::string> RealShadowhand::test_messages_routine(std::string joint_name, unsigned int repeat, ros::Rate rate)
 
465
  {
 
466
    std::pair<unsigned char, std::string> test_result;
 
467
 
 
468
    //id should be motor board number
 
469
    std::string ID = "1";
 
470
    self_test->setID(ID.c_str());
 
471
 
 
472
    unsigned int nb_msgs_sent = 0;
 
473
    unsigned int nb_msgs_received = 0;
 
474
 
 
475
    //sending lots of data to one joint
 
476
    JointsMap::iterator iter_joints_map = joints_map.find(joint_name);
 
477
 
 
478
    struct sensor sensor_msgs_received;
 
479
 
 
480
    if( iter_joints_map == joints_map.end() )
 
481
    {
 
482
      std::stringstream ss;
 
483
      ss << "No messages sent: couldn't find joint "<<joint_name;
 
484
 
 
485
      test_result.first = diagnostic_msgs::DiagnosticStatus::ERROR;
 
486
      test_result.second = ss.str();
 
487
      return test_result;
 
488
    }
 
489
 
 
490
    //OK joint found
 
491
    JointData tmpData = joints_map.find(joint_name)->second;
 
492
    int index_hand_joints = tmpData.jointIndex;
 
493
    float target =  hand_joints[index_hand_joints].min_angle;
 
494
 
 
495
    //testing a joint which doesn't have a smartmotor
 
496
    if( !hand_joints[index_hand_joints].a.smartmotor.has_sensors )
 
497
    {
 
498
      std::stringstream ss;
 
499
      ss << "No messages sent: joint["<<joint_name<<"] doesn't have any motor attached";
 
500
 
 
501
      test_result.first = diagnostic_msgs::DiagnosticStatus::ERROR;
 
502
      test_result.second = ss.str();
 
503
      return test_result;
 
504
    }
 
505
 
 
506
    ROS_DEBUG("Checking the current number of received messages");
 
507
 
 
508
    int res;
 
509
    std::string debug_node_name = *(&hand_joints[index_hand_joints].a.smartmotor.debug_nodename);
 
510
    std::map<const std::string, const unsigned int>::const_iterator iter_debug_values =  debug_values::names_and_offsets.find("Num sensor Msgs received");
 
511
 
 
512
    res = robot_channel_to_sensor(debug_node_name.c_str(), iter_debug_values->second, &sensor_msgs_received);
 
513
 
 
514
    //check the number of messages already received when starting the test
 
515
    nb_msgs_received = robot_read_incoming(&sensor_msgs_received) - nb_msgs_received;
 
516
 
 
517
    sleep(1);
 
518
 
 
519
    //check if no other messages have been received
 
520
    if( nb_msgs_received != robot_read_incoming(&sensor_msgs_received))
 
521
    {
 
522
      std::stringstream ss;
 
523
      ss <<  "New messages were received on the joint[" <<  joint_name.c_str() << "]." ;
 
524
      test_result.first = diagnostic_msgs::DiagnosticStatus::ERROR;
 
525
      test_result.second = ss.str();
 
526
 
 
527
      return test_result;
 
528
    }
 
529
    //ok still the same number of messages
 
530
 
 
531
    ROS_DEBUG("Sending lots of messages.");
 
532
 
 
533
    for(; nb_msgs_sent < repeat; ++nb_msgs_sent)
 
534
    {
 
535
      //send values to the sensor
 
536
      robot_update_sensor(&hand_joints[index_hand_joints].joint_target, target);
 
537
      rate.sleep();
 
538
      ROS_DEBUG_STREAM("msg "<< nb_msgs_sent<< "/"<<sr_self_tests::nb_targets_to_send);
 
539
    }
 
540
 
 
541
    ROS_DEBUG("Done sending the messages.");
 
542
    //wait for all the messages to be received?
 
543
    sleep(0.5);
 
544
 
 
545
    ROS_DEBUG("Reading the number of received messages.");
 
546
    //compute the number of messages received during the test
 
547
    nb_msgs_received = robot_read_incoming(&sensor_msgs_received) - nb_msgs_received;
 
548
 
 
549
    if( nb_msgs_sent == nb_msgs_received)
 
550
    {
 
551
      std::stringstream ss;
 
552
      ss <<  nb_msgs_sent << " messages sent, all received";
 
553
      test_result.first = diagnostic_msgs::DiagnosticStatus::OK;
 
554
      test_result.second = ss.str();
 
555
      return test_result;
 
556
    }
 
557
    else
 
558
    {
 
559
      std::stringstream ss;
 
560
      ss << nb_msgs_sent << " messages sent, "<<nb_msgs_received << " messages received";
 
561
      test_result.first = diagnostic_msgs::DiagnosticStatus::ERROR;
 
562
      test_result.second = ss.str();
 
563
      return test_result;
 
564
    }
 
565
  }
376
566
} //end namespace
 
567
 
 
568
/* For the emacs weenies in the crowd.
 
569
   Local Variables:
 
570
   c-basic-offset: 2
 
571
   End:
 
572
*/