Line data Source code
1 : /*
2 : * Copyright 2020 <copyright holder> <email>
3 : *
4 : * Licensed under the Apache License, Version 2.0 (the "License");
5 : * you may not use this file except in compliance with the License.
6 : * You may obtain a copy of the License at
7 : *
8 : * http://www.apache.org/licenses/LICENSE-2.0
9 : *
10 : * Unless required by applicable law or agreed to in writing, software
11 : * distributed under the License is distributed on an "AS IS" BASIS,
12 : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 : * See the License for the specific language governing permissions and
14 : * limitations under the License.
15 : */
16 :
17 : #include <ros_end_effector/GraspingActions/ActionGeneric.h>
18 :
19 :
20 296 : ROSEE::ActionGeneric::ActionGeneric () {}
21 :
22 24 : ROSEE::ActionGeneric::ActionGeneric(std::string actionName) : Action (actionName, Action::Type::Generic) {}
23 :
24 0 : ROSEE::ActionGeneric::ActionGeneric(std::string actionName, ROSEE::JointPos jointPos) : Action(actionName, Action::Type::Generic) {
25 :
26 : //HACK TODO now consider the position 0 as not used joint
27 0 : for (auto jp : jointPos) {
28 0 : bool zeros = std::all_of(jp.second.begin(), jp.second.end(), [](double i) { return i==0.0; });
29 0 : if (zeros) {
30 0 : jointsInvolvedCount.insert (std::make_pair (jp.first, 0) );
31 : } else {
32 0 : jointsInvolvedCount.insert (std::make_pair (jp.first, 1) );
33 :
34 : }
35 : }
36 :
37 0 : this->jointPos = jointPos;
38 :
39 0 : }
40 :
41 12 : ROSEE::ActionGeneric::ActionGeneric(std::string actionName, ROSEE::JointPos jointPos, JointsInvolvedCount jic) :
42 12 : Action(actionName, Action::Type::Generic) {
43 :
44 12 : if (jic.empty()) {
45 : //HACK TODO now consider the position 0 as not used joint
46 0 : for (auto jp : jointPos) {
47 0 : bool zeros = std::all_of(jp.second.begin(), jp.second.end(), [](double i) { return i==0.0; });
48 0 : if (zeros) {
49 0 : jointsInvolvedCount.insert (std::make_pair (jp.first, 0) );
50 : } else {
51 0 : jointsInvolvedCount.insert (std::make_pair (jp.first, 1) );
52 :
53 : }
54 : }
55 :
56 : } else {
57 12 : if ( ! ROSEE::Utils::keys_equal(jointPos, jic) ) {
58 0 : throw ROSEE::Utils::DifferentKeysException<ROSEE::JointPos, ROSEE::JointsInvolvedCount>(&jointPos, &jic);
59 : }
60 12 : this->jointsInvolvedCount = jic;
61 : }
62 :
63 12 : this->jointPos = jointPos;
64 :
65 12 : }
66 :
67 :
68 16 : ROSEE::ActionGeneric::ActionGeneric(std::string actionName, ROSEE::JointPos jointPos, JointsInvolvedCount jic,
69 20 : std::set<std::string> fingersInvolved) : Action(actionName, Action::Type::Generic) {
70 :
71 16 : if (jic.empty()) {
72 : //HACK TODO now consider the position 0 as not used joint
73 16 : for (auto jp : jointPos) {
74 16 : bool zeros = std::all_of(jp.second.begin(), jp.second.end(), [](double i) { return i==0.0; });
75 8 : if (zeros) {
76 0 : jointsInvolvedCount.insert (std::make_pair (jp.first, 0) );
77 : } else {
78 8 : jointsInvolvedCount.insert (std::make_pair (jp.first, 1) );
79 :
80 : }
81 : }
82 :
83 : } else {
84 :
85 8 : if ( ! ROSEE::Utils::keys_equal(jointPos, jic) ) {
86 4 : throw ROSEE::Utils::DifferentKeysException<ROSEE::JointPos, ROSEE::JointsInvolvedCount>(&jointPos, &jic);
87 : }
88 :
89 4 : this->jointsInvolvedCount = jic;
90 : }
91 :
92 12 : this->jointPos = jointPos;
93 :
94 12 : this->fingersInvolved = fingersInvolved;
95 12 : }
96 :
97 144 : ROSEE::JointPos ROSEE::ActionGeneric::getJointPos() const {
98 144 : return jointPos;
99 : }
100 :
101 22 : std::vector<ROSEE::JointPos> ROSEE::ActionGeneric::getAllJointPos() const {
102 22 : std::vector < JointPos> vect;
103 22 : vect.push_back ( jointPos ) ;
104 22 : return vect;
105 : }
106 :
107 19 : void ROSEE::ActionGeneric::emitYaml(YAML::Emitter& out) const {
108 :
109 19 : out << YAML::BeginMap << YAML::Key << name << YAML::Value << YAML::BeginMap ;
110 19 : out << YAML::Key << "Type" << YAML::Value << type;
111 19 : out << YAML::Key << "FingersInvolved" << YAML::Value << YAML::Flow << fingersInvolved;
112 19 : out << YAML::Key << "JointsInvolvedCount" << YAML::Value << YAML::BeginMap;
113 102 : for (const auto &jointCount : jointsInvolvedCount ) {
114 83 : out << YAML::Key << jointCount.first;
115 83 : out << YAML::Value << jointCount.second;
116 : }
117 19 : out << YAML::EndMap;
118 :
119 19 : out << YAML::Key << "JointPos" << YAML::Value << YAML::BeginMap;
120 102 : for (const auto &joint : jointPos) {
121 83 : out << YAML::Key << joint.first;
122 83 : out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
123 : }
124 19 : out << YAML::EndMap;
125 19 : out << YAML::EndMap;
126 19 : out << YAML::EndMap;
127 19 : }
128 :
129 :
130 209 : bool ROSEE::ActionGeneric::fillFromYaml(YAML::const_iterator yamlIt) {
131 :
132 209 : name = yamlIt->first.as<std::string>();
133 209 : type = ROSEE::Action::Type::Generic;
134 :
135 1045 : for (auto keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue ) {
136 :
137 1672 : std::string key = keyValue->first.as<std::string>();
138 :
139 836 : if ( key.compare ("FingersInvolved") == 0 ) {
140 : // if <not_inserted> tempVect is a empty vector
141 418 : auto tempVect = keyValue->second.as <std::vector <std::string> > ();
142 209 : fingersInvolved.insert ( tempVect.begin(), tempVect.end() );
143 :
144 627 : } else if ( key.compare ("Type") == 0 ) {
145 209 : if (ROSEE::Action::Type::Generic != static_cast<ROSEE::Action::Type> ( keyValue->second.as <unsigned int>() )) {
146 0 : std::cout << "[GENERIC ACTION::" << __func__ << "] Error, found type " << keyValue->second.as <unsigned int>()
147 0 : << "instead of generic type (" << ROSEE::Action::Type::Generic << ")" << std::endl;
148 0 : return false;
149 : }
150 209 : type = ROSEE::Action::Type::Generic;
151 :
152 418 : } else if ( key.compare ("JointsInvolvedCount") == 0 ) {
153 209 : jointsInvolvedCount = keyValue->second.as < JointsInvolvedCount >();
154 :
155 209 : } else if ( key.compare ("JointPos") == 0 ) {
156 209 : jointPos = keyValue->second.as < JointPos >();
157 :
158 : } else {
159 0 : std::cout << "[GENERIC ACTION::" << __func__ << "] Error, not known key " << key << std::endl;
160 0 : return false;
161 : }
162 : }
163 :
164 209 : if ( ! ROSEE::Utils::keys_equal(jointPos, jointsInvolvedCount) ) {
165 0 : throw ROSEE::Utils::DifferentKeysException<ROSEE::JointPos, ROSEE::JointsInvolvedCount>(&jointPos, &jointsInvolvedCount);
166 : }
167 :
168 209 : return true;
169 183 : }
170 :
171 :
172 :
173 :
174 :
|
