iCub-main
skinContactList.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010-2011 RobotCub Consortium
3  * Author: Andrea Del Prete
4  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
5  *
6  */
7 
8 #include <iostream>
9 #include <sstream>
10 #include <iomanip>
11 #include <string>
12 
13 #include <yarp/os/ConnectionReader.h>
14 #include <yarp/os/ConnectionWriter.h>
15 
17 #include <iCub/ctrl/math.h>
18 
19 using namespace std;
20 using namespace yarp::os;
21 using namespace iCub::skinDynLib;
22 
23 
24 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
25 // CONSTRUCTORS
26 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
27 skinContactList::skinContactList()
28 :vector<skinContact>(){}
29 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
30 skinContactList::skinContactList(const size_type &n, const skinContact& value)
31 :vector<skinContact>(n, value){}
32 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
34 {
35  skinContactList res;
36  for(iterator it=begin(); it!=end(); it++)
37  if(it->getBodyPart() == bp)
38  res.push_back(*it);
39  return res;
40 }
41 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
42 map<BodyPart, skinContactList> skinContactList::splitPerBodyPart()
43 {
44  map<BodyPart, skinContactList> res;
45  for(iterator it=begin(); it!=end(); it++)
46  res[it->getBodyPart()].push_back(*it);
47  return res;
48 }
49 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
50 map<SkinPart, skinContactList> skinContactList::splitPerSkinPart()
51 {
52  map<SkinPart, skinContactList> res;
53  for(iterator it=begin(); it!=end(); it++)
54  res[it->getSkinPart()].push_back(*it);
55  return res;
56 }
57 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
58 // SERIALIZATION methods
59 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
60 bool skinContactList::read(ConnectionReader& connection)
61 {
62  // A skinContactList is represented as a list of list
63  // where each list is a skinContact
64  if(connection.expectInt32()!=BOTTLE_TAG_LIST)
65  return false;
66 
67  int listLength = connection.expectInt32();
68  if(listLength<0)
69  return false;
70  if(listLength!=size())
71  resize(listLength);
72 
73  for(iterator it=begin(); it!=end(); it++)
74  if(!it->read(connection))
75  return false;
76 
77  return !connection.isError();
78 }
79 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
80 bool skinContactList::write(ConnectionWriter& connection) const
81 {
82  // A skinContactList is represented as a list of list
83  // where each list is a skinContact
84  connection.appendInt32(BOTTLE_TAG_LIST);
85  connection.appendInt32(size());
86 
87  for(auto it=begin(); it!=end(); it++)
88  if(!it->write(connection))
89  return false;
90 
91  return !connection.isError();
92 }
93 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
95 {
96  dynContactList res(this->size());
97  const_iterator itSkin = begin();
98  for(dynContactList::iterator itDyn=res.begin(); itDyn!=res.end(); itDyn++)
99  {
100  *itDyn = (dynContact)(*itSkin);
101  itSkin++;
102  }
103  return res;
104 }
105 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
106 //bool skinContactList::fromDynContactList(dynContactList &l)
107 //{
108 // resize(l.size());
109 // for(unsigned int i=0; i<size(); i++)
110 // operator[](i) = *(dynamic_cast<skinContact*> (&l[i]));
111 // return true;
112 //}
113 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
114 string skinContactList::toString(const int &precision) const
115 {
116  stringstream ss;
117  for(const_iterator it=begin();it!=end();it++)
118  ss<<"- "<<it->toString(precision)<<";\n";
119  return ss.str();
120 }
121 
122 
Class representing a list of external contacts.
Class representing an external contact acting on a link of the robot body.
Definition: dynContact.h:52
Class representing a list of external contacts acting on the iCub' skin.
virtual skinContactList filterBodyPart(const BodyPart &bp)
Select all the contacts that have the specified body part.
virtual std::map< BodyPart, skinContactList > splitPerBodyPart()
Split the list in N lists dividing the contacts per body part.
virtual std::map< SkinPart, skinContactList > splitPerSkinPart()
Split the list in N lists dividing the contacts per skin part.
virtual bool read(yarp::os::ConnectionReader &connection)
virtual std::string toString(const int &precision=-1) const
Create a skinContactList starting from a dynContactList.
virtual bool write(yarp::os::ConnectionWriter &connection) const
Write skinContactList to a connection.
virtual dynContactList toDynContactList() const
Convert this skinContactList to a dynContactList casting all its elements to dynContact.
Class representing an external contact acting on the iCub' skin.
Definition: skinContact.h:50
int n