← Back to branch summary

~shadowrobot/sr-ros-interface/stable

« back to all changes in this revision

Viewing changes to sr_tactile_sensors/src/sr_gazebo_virtual_tactile_sensor.cpp

  • Committer: Ugo Cupcic
  • Date: 2011-11-21 16:34:38 UTC
  • mfrom: (1.1.314 shadow_robot)
  • Revision ID: ugo@shadowrobot.com-20111121163438-2g1oga47om300652
1.0.beta

Show diffs side-by-side

added added

removed removed

Lines of Context:
sr_tactile_sensors/src/sr_gazebo_virtual_tactile_sensor.cpp
38
38
  SrGazeboVirtualTactileSensor::SrGazeboVirtualTactileSensor(std::string name,
39
39
                                                             std::string gazebo_bumper_topic) :
40
40
    SrGenericTactileSensor(name, ""),
41
 
    touch_value(0.0)
 
41
    touch_value(0.0),
 
42
    touch_freshdata(false)
42
43
  {
43
44
    sub = nh.subscribe(gazebo_bumper_topic, 2, &SrGazeboVirtualTactileSensor::callback, this);
44
45
  }
49
50
  void SrGazeboVirtualTactileSensor::callback(const gazebo_plugins::ContactsState& msg)
50
51
  {
51
52
    double tmp_value;
52
 
    const ::geometry_msgs::Vector3& v = msg.states[0].wrenches[0].force;
 
53
    ::geometry_msgs::Vector3 v;
 
54
    v.x=0;
 
55
    v.y=0;
 
56
    v.z=0;
53
57
 
54
58
    ROS_INFO("Touch by %s", msg.header.frame_id.c_str());
55
59
    // Parse the message to retrieve the relevant touch pressure information
56
 
    // Currently just taking the first contact.
 
60
    // Currently the sum of all contacts.
57
61
    // More sophisticated analysis can be done to take the contact that is the most aligned with the distal link normal
 
62
    unsigned int nb_wrench = msg.states[0].wrenches.size();
 
63
    for (unsigned int i=0;i<nb_wrench;i++)
 
64
    {
 
65
        v.x=v.x+msg.states[0].wrenches[i].force.x;
 
66
        v.y=v.y+msg.states[0].wrenches[i].force.y;
 
67
        v.z=v.z+msg.states[0].wrenches[i].force.z;
 
68
    }
 
69
    v.x=v.x/nb_wrench;
 
70
    v.y=v.y/nb_wrench;
 
71
    v.z=v.z/nb_wrench;
58
72
    tmp_value = sqrt(pow(v.x, 2) + pow(v.y, 2) + pow(v.z, 2));
59
 
 
60
73
    touch_mutex.lock();
61
74
    touch_value = tmp_value;
 
75
    touch_freshdata =  true;
62
76
    touch_mutex.unlock();
63
77
  }
64
78
 
65
79
  double SrGazeboVirtualTactileSensor::get_touch_data()
66
80
  {
67
81
    double return_value;
68
 
 
69
82
    touch_mutex.lock();
70
 
    return_value = touch_value;
 
83
    if(touch_freshdata)
 
84
    {
 
85
        return_value = touch_value;
 
86
        touch_freshdata=false;
 
87
    }
 
88
    else
 
89
    {
 
90
        return_value= 0.0;
 
91
    }
71
92
    touch_mutex.unlock();
72
93
 
73
94
    return return_value;
82
103
  {
83
104
    std::vector<std::vector<std::string> > all_names = get_all_names();
84
105
 
85
 
    for( unsigned int i=0; i<5; ++i)
 
106
    for( unsigned int i=0; i< all_names[0].size() ; ++i)
86
107
    {
87
108
      tactile_sensors.push_back(
88
109
        boost::shared_ptr<SrGazeboVirtualTactileSensor>(