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/ActionSingleJointMultipleTips.h>
18 :
19 8 : ROSEE::ActionSingleJointMultipleTips::ActionSingleJointMultipleTips() :
20 8 : ActionPrimitive ( "singleJointMultipleTips", 1, ROSEE::ActionPrimitive::Type::SingleJointMultipleTips ) {}
21 :
22 0 : ROSEE::ActionSingleJointMultipleTips::ActionSingleJointMultipleTips(std::string actionName, unsigned int nFingers ) :
23 0 : ActionPrimitive ( actionName, nFingers, 1, ROSEE::ActionPrimitive::Type::SingleJointMultipleTips ) {}
24 :
25 1 : ROSEE::ActionSingleJointMultipleTips::ActionSingleJointMultipleTips (std::string actionName, std::vector<std::string> fingers, std::string jointName,
26 1 : JointPos jpFurther, JointPos jpNearer) :
27 1 : ActionPrimitive ( actionName, fingers.size(), 1, ROSEE::ActionPrimitive::Type::SingleJointMultipleTips ) {
28 :
29 1 : this->jointInvolved = jointName;
30 1 : fingersInvolved.insert (fingers.begin(), fingers.end());
31 :
32 1 : this->jointPosFurther = jpFurther;
33 1 : this->jointPosNearer = jpNearer;
34 :
35 : // still need to do this, that can be done in costructor because we know that only one joint will be used, per definition of this action
36 2 : for (auto it : jpFurther ) {
37 1 : jointsInvolvedCount.insert (std::make_pair (it.first, 0) );
38 : }
39 1 : jointsInvolvedCount.at (jointName) = 1;
40 :
41 1 : }
42 :
43 0 : std::vector<ROSEE::JointPos> ROSEE::ActionSingleJointMultipleTips::getAllJointPos() const {
44 :
45 0 : std::vector<JointPos> vect;
46 0 : vect.push_back (jointPosFurther);
47 0 : vect.push_back (jointPosNearer);
48 0 : return vect;
49 : }
50 :
51 5 : ROSEE::JointPos ROSEE::ActionSingleJointMultipleTips::getJointPos() const {
52 5 : return jointPosFurther;
53 : }
54 :
55 0 : ROSEE::JointPos ROSEE::ActionSingleJointMultipleTips::getJointPosFurther() const {
56 0 : return jointPosFurther;
57 : }
58 :
59 0 : ROSEE::JointPos ROSEE::ActionSingleJointMultipleTips::getJointPosNearer() const {
60 0 : return jointPosNearer;
61 : }
62 :
63 0 : std::string ROSEE::ActionSingleJointMultipleTips::getJointName() const {
64 0 : return jointInvolved;
65 : }
66 :
67 10 : std::set<std::string> ROSEE::ActionSingleJointMultipleTips::getKeyElements() const {
68 10 : std::set <std::string> set;
69 10 : set.insert(jointInvolved);
70 10 : return set;
71 : }
72 :
73 0 : void ROSEE::ActionSingleJointMultipleTips::print() const {
74 :
75 0 : std::stringstream output;
76 0 : output << "ActionName: " << name << std::endl;
77 :
78 0 : output << "FingersInvolved: [";
79 0 : for (auto fingName : fingersInvolved){
80 0 : output << fingName << ", " ;
81 : }
82 0 : output.seekp (-2, output.cur); //to remove the last comma (and space)
83 0 : output << "]" << std::endl;
84 :
85 0 : output << "Joint which moves the tips: " << jointInvolved << std::endl;
86 :
87 0 : output << "JointPos Further from 0:" << std::endl;
88 0 : output << jointPosFurther;
89 :
90 0 : output << "JointPos Nearer to 0:" << std::endl;
91 0 : output << jointPosNearer;
92 :
93 0 : output << std::endl;
94 :
95 0 : std::cout << output.str();
96 :
97 0 : }
98 :
99 1 : void ROSEE::ActionSingleJointMultipleTips::emitYaml(YAML::Emitter& out) const {
100 :
101 1 : out << YAML::Key << jointInvolved;
102 1 : out << YAML::Value << YAML::BeginMap;
103 :
104 1 : out << YAML::Key << "PrimitiveType" << YAML::Value << primitiveType;
105 1 : out << YAML::Key << "ActionName" << YAML::Value << name;
106 1 : out << YAML::Key << "FingersInvolved" << YAML::Value << YAML::Flow << fingersInvolved;
107 :
108 1 : out << YAML::Key << "JointPosFurther" << YAML::Value << YAML::BeginMap;
109 2 : for (const auto &joint : jointPosFurther) {
110 1 : out << YAML::Key << joint.first;
111 1 : out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
112 : }
113 1 : out << YAML::EndMap;
114 :
115 1 : out << YAML::Key << "JointPosNearer" << YAML::Value << YAML::BeginMap;
116 2 : for (const auto &joint : jointPosNearer) {
117 1 : out << YAML::Key << joint.first;
118 1 : out << YAML::Value << YAML::Flow << joint.second; //vector of double is emitted like Seq
119 : }
120 1 : out << YAML::EndMap;
121 :
122 1 : out << YAML::EndMap;
123 :
124 1 : }
125 :
126 :
127 :
128 8 : bool ROSEE::ActionSingleJointMultipleTips::fillFromYaml(YAML::const_iterator yamlIt) {
129 :
130 8 : jointInvolved = yamlIt->first.as < std::string > ();
131 :
132 48 : for ( YAML::const_iterator keyValue = yamlIt->second.begin(); keyValue != yamlIt->second.end(); ++keyValue) {
133 80 : std::string key = keyValue->first.as<std::string>();
134 :
135 40 : if ( key.compare ("ActionName") == 0 ){
136 8 : name = keyValue->second.as < std::string > ();
137 :
138 32 : } else if (key.compare("FingersInvolved") == 0) {
139 16 : std::vector <std::string> fingInvolvedVect = keyValue->second.as <std::vector < std::string >> ();
140 24 : for (const auto &it : fingInvolvedVect) {
141 16 : fingersInvolved.insert(it);
142 : }
143 :
144 24 : } else if (key.compare ("JointPosNearer") == 0) {
145 8 : jointPosNearer = keyValue->second.as <JointPos>();
146 :
147 16 : } else if (key.compare ("JointPosFurther") == 0) {
148 8 : jointPosFurther = keyValue->second.as <JointPos>();
149 :
150 8 : } else if (key.compare ("PrimitiveType") == 0) {
151 : ROSEE::ActionPrimitive::Type parsedType = static_cast<ROSEE::ActionPrimitive::Type> (
152 8 : keyValue->second.as <unsigned int>() );
153 8 : if (parsedType != primitiveType ) {
154 0 : std::cerr << "[ERROR ActionSingleJointMultipleTips::" << __func__ << " parsed a type " << parsedType <<
155 0 : " but this object has primitive type " << primitiveType << std::endl;
156 0 : return false;
157 : }
158 :
159 : } else {
160 : std::cerr << "[ERROR ActionSingleJointMultipleTips::" << __func__ << "not know key " << key <<
161 0 : " found in the yaml file" << std::endl;
162 0 : return false;
163 : }
164 : }
165 :
166 : // we have also to fill this structure, that is not present on yaml file because redundant
167 16 : for (auto it : jointPosFurther ) {
168 8 : jointsInvolvedCount.insert (std::make_pair (it.first, 0) );
169 : }
170 8 : jointsInvolvedCount.at (jointInvolved) = 1;
171 :
172 8 : nFingersInvolved = fingersInvolved.size();
173 :
174 8 : return true;
175 :
176 183 : }
177 :
178 :
179 :
180 :
|
