iCub-main
main.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 // Copyright (C) 2015 Istituto Italiano di Tecnologia - iCub Facility
3 // Author: Marco Randazzo <marco.randazzo@iit.it>
4 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
5 
6 // *******************************
7 // THIS MODULE IS EXPERIMENTAL!!!!
8 // *******************************
9 
10 #include <stdio.h>
11 #include <yarp/os/Network.h>
12 #include <yarp/os/Property.h>
13 #include <yarp/os/Time.h>
14 #include <yarp/dev/PolyDriver.h>
15 #include <yarp/os/BufferedPort.h>
16 #include <yarp/os/RFModule.h>
17 #include "positionDirectThread.h"
18 #include <yarp/os/Log.h>
19 #include <yarp/os/LogStream.h>
20 #include <string.h>
21 
22 //default rate for the control loop
23 const int CONTROL_PERIOD=10;
24 
25 using namespace yarp::os;
26 using namespace yarp::dev;
27 
28 class VelControlModule: public RFModule {
29 private:
30  PolyDriver driver;
32  char partName[255];
33 
34  Port rpc_port;
35 
36 public:
37  virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
38  {
39  reply.clear();
40  if (command.get(0).isString())
41  {
42  if (command.get(0).asString()=="help")
43  {
44  reply.addVocab32("many");
45  reply.addString("Available commands:");
46  reply.addString("currently nothing");
47  return true;
48  }
49  else if (command.get(0).asString()=="***")
50  {
51  return true;
52  }
53  }
54  reply.addString("Unknown command");
55  return true;
56  }
57 
58  virtual bool configure(yarp::os::ResourceFinder &rf)
59  {
60  Property options;
61  options.fromString(rf.toString());
62  char robotName[255];
63  Bottle *jointsList=0;
64  std::string moduleName = "directPositionControl";
65 
66  if (options.check("help"))
67  {
68  yInfo() << "Available options:";
69  yInfo() << "--robot <robot name>";
70  yInfo() << "--name <local_module_name>";
71  yInfo() << "--part <part_name>";
72  yInfo() << "--joints ""(x x x)"" ";
73  yInfo() << "--joints_limiter <deg>";
74  yInfo() << "--target_limiter <deg>";
75  yInfo() << "--period <s>";
76  return false;
77  }
78 
79  options.put("device", "remote_controlboard");
80  if(options.check("robot"))
81  strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
82  else
83  strncpy(robotName, "icub", sizeof(robotName));
84 
85  if (options.check("name"))
86  {
87  moduleName = options.find("name").asString();
88  }
89 
90  if(options.check("part"))
91  {
92  snprintf(partName, sizeof(partName), "%s", options.find("part").asString().c_str());
93 
94  char tmp[800];
95  snprintf(tmp, sizeof(tmp), "/%s/%s/%s/client", moduleName.c_str(), robotName, partName);
96  options.put("local",tmp);
97 
98  snprintf(tmp, sizeof(tmp), "/%s/%s", robotName, partName);
99  options.put("remote", tmp);
100 
101  snprintf(tmp, sizeof(tmp), "/%s/%s/rpc", moduleName.c_str(), partName);
102  rpc_port.open(tmp);
103 
104  options.put("carrier", "tcp");
105 
106  attach(rpc_port);
107  }
108  else
109  {
110  yError("Please specify part (e.g. --part head)\n");
111  return false;
112  }
113 
114  if(options.check("joints"))
115  {
116  jointsList = options.find("joints").asList();
117  if (jointsList==0) yError("Unable to parts 'joints' parameter\n");
118  }
119  else
120  {
121  yError("Please specify the joints to control (e.g. --joints ""(0 1 2)"" ");
122  return false;
123  }
124 
125 
126  double joints_limiter = 2.0; //deg
127  double target_limiter = 1.0; //deg
128  if (options.check("joints_limiter"))
129  {
130  joints_limiter = options.find("joints_limiter").asFloat64();
131  }
132  if (options.check("target_limiter"))
133  {
134  target_limiter = options.find("target_limiter").asFloat64();
135  }
136 
137  yDebug() << "joints_limiter:" << joints_limiter;
138  yDebug() << "target_limiter" << target_limiter;
139 
140  //opening the device driver
141  if (!driver.open(options))
142  {
143  yError("Error opening device, check parameters\n");
144  return false;
145  }
146 
148  int period = CONTROL_PERIOD;
149  if(options.check("period"))
150  period = options.find("period").asInt32();
151 
152  yInfo("control rate is %d ms",period);
153 
154  pThread=new positionDirectControlThread(period);
155  pThread->init(&driver, moduleName, partName, robotName, jointsList);
156  pThread->joints_limiter = joints_limiter;
157  pThread->target_limiter = target_limiter;
158  pThread->start();
159 
160  return true;
161  }
162 
163  virtual bool close()
164  {
165  yInfo("Closing module [%s]\n", partName);
166  pThread->stop();
167  pThread->halt();
168  delete pThread;
169  yInfo("Thread [%s] stopped\n", partName);
170 
171  driver.close();
172  rpc_port.close();
173 
174  yInfo("Module [%s] closed\n", partName);
175  return true;
176  }
177 
179  {
180  return true;
181  }
182 
183 };
184 
185 int main(int argc, char *argv[])
186 {
187  Network yarp;
188  VelControlModule mod;
189  ResourceFinder rf;
190 
191  rf.configure(argc, argv);
192  if (mod.configure(rf)==false) return 1;
193  mod.runModule();
194  yInfo("Main returning\n");
195  return 0;
196 
197 }
python-motor-control.tmp
tmp
Definition: python-motor-control.py:43
CONTROL_PERIOD
const int CONTROL_PERIOD
Definition: main.cpp:23
positionDirectThread.h
VelControlModule::respond
virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition: main.cpp:37
main
int main(int argc, char *argv[])
Definition: main.cpp:31
VelControlModule::updateModule
bool updateModule()
Definition: main.cpp:178
positionDirectControlThread::init
bool init(yarp::dev::PolyDriver *d, std::string moduleName, std::string partName, std::string robotName, yarp::os::Bottle *jointsList)
Definition: positionDirectThread.cpp:131
VelControlModule
Definition: main.cpp:28
yarp::dev
Definition: DebugInterfaces.h:52
VelControlModule::close
virtual bool close()
Definition: main.cpp:163
positionDirectControlThread::halt
void halt()
Definition: positionDirectThread.cpp:235
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
positionDirectControlThread::joints_limiter
double joints_limiter
Definition: positionDirectThread.h:53
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
scripting.argc
argc
Definition: scripting.py:184
positionDirectControlThread::target_limiter
double target_limiter
Definition: positionDirectThread.h:54
positionDirectControlThread
Definition: positionDirectThread.h:19
VelControlModule::configure
virtual bool configure(yarp::os::ResourceFinder &rf)
Definition: main.cpp:58