iCub-main
utils.h
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3 * Author: Carlo Ciliberto, Vadim Tikhanoff
4 * email: carlo.ciliberto@iit.it vadim.tikhanoff@iit.it
5 * website: www.robotcub.org
6 * Permission is granted to copy, distribute, and/or modify this program
7 * under the terms of the GNU General Public License, version 2 or any
8 * later version published by the Free Software Foundation.
9 *
10 * A copy of the license can be found at
11 * http://www.robotcub.org/icub/license/gpl.txt
12 *
13 * This program is distributed in the hope that it will be useful, but
14 * WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16 * Public License for more details
17 */
18 
19 #ifndef __VISUO_MOTOR_UTILS__
20 #define __VISUO_MOTOR_UTILS__
21 
22 #include <yarp/os/ResourceFinder.h>
23 #include <yarp/os/RpcClient.h>
24 #include <yarp/os/PortInfo.h>
25 #include <yarp/os/PortReport.h>
26 #include <yarp/os/Vocab.h>
27 #include <yarp/sig/Vector.h>
28 
29 #include <mutex>
30 #include <iostream>
31 #include <string>
32 
33 #define LEFT 0
34 #define RIGHT 1
35 #define ARM_IN_USE -1
36 #define ARM_MOST_SUITED -2
37 
38 using namespace std;
39 using namespace yarp::os;
40 using namespace yarp::sig;
41 
42 
43 class ObjectPropertiesCollectorPort: public RpcClient, public PortReport
44 {
45 private:
46  bool scheduleUpdate;
47 
48 public:
50  :scheduleUpdate(false)
51  {
52  this->setReporter(*this);
53  }
54 
55  void report(const PortInfo &info)
56  {
57  if(info.created && !info.incoming)
58  scheduleUpdate=true;
59  }
60 
61  bool getStereoPosition(const string &obj_name, Vector &stereo);
62  bool getCartesianPosition(const string &obj_name, Vector &x);
63  bool getKinematicOffsets(const string &obj_name, Vector *kinematic_offset);
64  bool setKinematicOffsets(const string &obj_name, const Vector *kinematic_offset);
65  bool getTableHeight(double &table_height);
66  bool setTableHeight(const double table_height);
67  bool setAction(const string &act_name, const Bottle *trajectory);
68  bool getAction(const string &act_name, Bottle *trajectory);
69 
71  {
72  if(scheduleUpdate)
73  {
74  scheduleUpdate=false;
75  return true;
76  }
77 
78  return false;
79  }
80 };
81 
82 
83 #include <stdio.h>
84 
85 class StereoTarget: public Vector
86 {
87 private:
88  mutex mtx;
89 
90 public:
91  void set(const Vector &stereo)
92  {
93  lock_guard<mutex> lck(mtx);
94  this->resize(stereo.size());
95  for(size_t i=0; i<stereo.size(); i++)
96  this->data()[i]=stereo[i];
97  }
98 
99  Vector get()
100  {
101  Vector stereo;
102  lock_guard<mutex> lck(mtx);
103  stereo.resize(this->size());
104  for(size_t i=0; i<this->size(); i++)
105  stereo[i]= this->data()[i];
106  this->clear();
107  return stereo;
108  }
109 };
110 
111 
112 
114 {
115 public:
117 
119 
120 public:
121  Initializer(ResourceFinder &rf)
122  {
123  string name=rf.find("name").asString();
124  port_opc.open("/"+name+"/OPC:io");
125  }
126 
127  bool interrupt()
128  {
129  port_opc.interrupt();
130  return true;
131  }
132 
133  bool close()
134  {
135  port_opc.close();
136  return true;
137  }
138 };
139 
140 
141 
142 
143 #endif
144 
145 
ObjectPropertiesCollectorPort
Definition: utils.h:43
Initializer::Initializer
Initializer(ResourceFinder &rf)
Definition: utils.h:121
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
ObjectPropertiesCollectorPort::ObjectPropertiesCollectorPort
ObjectPropertiesCollectorPort()
Definition: utils.h:49
ObjectPropertiesCollectorPort::isUpdateNeeded
bool isUpdateNeeded()
Definition: utils.h:70
StereoTarget
Definition: utils.h:85
Initializer
Definition: utils.h:113
ObjectPropertiesCollectorPort::report
void report(const PortInfo &info)
Definition: utils.h:55
data
@ data
Definition: ST_M1_dataType.h:64
StereoTarget::get
Vector get()
Definition: utils.h:99
Initializer::interrupt
bool interrupt()
Definition: utils.h:127
Initializer::port_opc
ObjectPropertiesCollectorPort port_opc
Definition: utils.h:118
x
x
Definition: compute_ekf_sym.m:21
Initializer::stereo_target
StereoTarget stereo_target
Definition: utils.h:116
StereoTarget::set
void set(const Vector &stereo)
Definition: utils.h:91
Initializer::close
bool close()
Definition: utils.h:133