SampleSV.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
2 /*
3  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
4  * All rights reserved. This program is made available under the terms of the
5  * Eclipse Public License v1.0 which accompanies this distribution, and is
6  * available at http://www.eclipse.org/legal/epl-v10.html
7  * Contributors:
8  * National Institute of Advanced Industrial Science and Technology (AIST)
9  * General Robotix Inc.
10  */
19 #include "SampleSV.h"
20 
21 #include <iostream>
22 
23 #define DOF (4)
24 #define STEERING_ID 0
25 #define WHEEL_ID 1
26 
27 #define STEERING_FILE "etc/steer.dat"
28 
29 #define STEERING_P_GAIN 100.0
30 #define STEERING_D_GAIN 1.0
31 #define WHEEL_P_GAIN 100.0
32 #define WHEEL_D_GAIN 0.5
33 #define WHEEL_REF_VEL 6.28 // [rad/s]
34 
35 #define TIMESTEP 0.002
36 
37 namespace {
38  const bool CONTROLLER_BRIDGE_DEBUG = false;
39 }
40 
41 
42 // Module specification
43 // <rtc-template block="module_spec">
44 static const char* samplepd_spec[] =
45  {
46  "implementation_id", "SampleSV",
47  "type_name", "SampleSV",
48  "description", "Sample SV component",
49  "version", "0.1",
50  "vendor", "AIST",
51  "category", "Generic",
52  "activity_type", "DataFlowComponent",
53  "max_instance", "10",
54  "language", "C++",
55  "lang_type", "compile",
56  // Configuration variables
57 
58  ""
59  };
60 // </rtc-template>
61 
63  : RTC::DataFlowComponentBase(manager),
64  // <rtc-template block="initializer">
65  m_steerIn("steer", m_steer),
66  m_velIn("vel", m_vel),
67 #if 1
68  m_rangeIn("range", m_range),
69 #endif
70  m_torqueOut("torque", m_torque),
71 
72  // </rtc-template>
73  wheel_ref(0.0)
74 {
75  if( CONTROLLER_BRIDGE_DEBUG )
76  {
77  std::cout << "SampleSV::SampleSV" << std::endl;
78  }
79  // Registration: InPort/OutPort/Service
80  // <rtc-template block="registration">
81  // Set service provider to Ports
82 
83  // Set service consumers to Ports
84 
85  // Set CORBA Service Ports
86 
87  // </rtc-template>
88 }
89 
91 {
92  closeFiles();
93 }
94 
95 
96 RTC::ReturnCode_t SampleSV::onInitialize()
97 {
98  // <rtc-template block="bind_config">
99  // Bind variables and configuration variable
100  if( CONTROLLER_BRIDGE_DEBUG )
101  {
102  std::cout << "onInitialize" << std::endl;
103  }
104 
105  // Set InPort buffers
106  addInPort("steer", m_steerIn);
107  addInPort("vel", m_velIn);
108 #if 1
109  addInPort("range", m_rangeIn);
110 #endif
111 
112  // Set OutPort buffer
113  addOutPort("torque", m_torqueOut);
114  // </rtc-template>
115 
116  m_torque.data.length(DOF);
117  m_steer.data.length(2);
118  m_vel.data.length(2);
119  return RTC::RTC_OK;
120 }
121 
122 
123 
124 /*
125 RTC::ReturnCode_t SampleSV::onFinalize()
126 {
127  return RTC::RTC_OK;
128 }
129 */
130 
131 /*
132 RTC::ReturnCode_t SampleSV::onStartup(RTC::UniqueId ec_id)
133 {
134  return RTC::RTC_OK;
135 }
136 */
137 
138 /*
139 RTC::ReturnCode_t SampleSV::onShutdown(RTC::UniqueId ec_id)
140 {
141  return RTC::RTC_OK;
142 }
143 */
144 
145 RTC::ReturnCode_t SampleSV::onActivated(RTC::UniqueId ec_id)
146 {
147 
148  std::cout << "on Activated" << std::endl;
149  openFiles();
150  if(m_steerIn.isNew()){
151  m_steerIn.read();
152  }
153  if(m_velIn.isNew()){
154  m_velIn.read();
155  }
156  wheel_ref = 0.0;
157 
158  return RTC::RTC_OK;
159 }
160 
161 
162 RTC::ReturnCode_t SampleSV::onDeactivated(RTC::UniqueId ec_id)
163 {
164  std::cout << "on Deactivated" << std::endl;
165  closeFiles();
166  return RTC::RTC_OK;
167 }
168 
169 
170 
171 RTC::ReturnCode_t SampleSV::onExecute(RTC::UniqueId ec_id)
172 {
173  if( CONTROLLER_BRIDGE_DEBUG )
174  {
175  std::cout << "SampleSV::onExecute" << std::endl;
176  }
177 
178  if(m_steerIn.isNew()){
179  m_steerIn.read();
180  }
181  if( m_velIn.isNew() ){
182  m_velIn.read();
183  }
184 #if 1
185  if (m_rangeIn.isNew()){
186  m_rangeIn.read();
187  std::cout << "received range data(" << m_range.data.length() << ")" << std::endl;
188  }
189 #endif
190 
191  //double q_ref, dq_ref;
192  double dummy;
193  steer >> dummy; // skip time
194 
195  double steer_ref;
196  steer >> steer_ref;
197 
198  for(int i=0; i<DOF; i++) m_torque.data[i] = 0.0;
199 
203  m_torqueOut.write();
204 
205  return RTC::RTC_OK;
206 }
207 
208 
209 /*
210  RTC::ReturnCode_t SampleSV::onAborting(RTC::UniqueId ec_id)
211  {
212  return RTC::RTC_OK;
213  }
214 */
215 
216 /*
217  RTC::ReturnCode_t SampleSV::onError(RTC::UniqueId ec_id)
218  {
219  return RTC::RTC_OK;
220  }
221 */
222 
223 /*
224  RTC::ReturnCode_t SampleSV::onReset(RTC::UniqueId ec_id)
225  {
226  return RTC::RTC_OK;
227  }
228 */
229 
230 /*
231  RTC::ReturnCode_t SampleSV::onStateUpdate(RTC::UniqueId ec_id)
232  {
233  return RTC::RTC_OK;
234  }
235 */
236 
237 /*
238  RTC::ReturnCode_t SampleSV::onRateChanged(RTC::UniqueId ec_id)
239  {
240  return RTC::RTC_OK;
241  }
242 */
243 
245 {
246  steer.open( STEERING_FILE );
247  if (!steer.is_open())
248  std::cerr << STEERING_FILE <<" not opened" << std::endl;
249 }
250 
252 {
253  if( steer.is_open() )
254  {
255  steer.close();
256  steer.clear();
257  }
258 }
259 
260 
261 extern "C"
262 {
263 
265  {
267  manager->registerFactory(profile,
268  RTC::Create<SampleSV>,
269  RTC::Delete<SampleSV>);
270  }
271 
272 };
273 
png_infop png_charpp int png_charpp profile
Definition: png.h:2382
#define STEERING_FILE
Definition: SampleSV.cpp:27
SampleSV(RTC::Manager *manager)
Definition: SampleSV.cpp:62
DLL_EXPORT void SampleSVInit(RTC::Manager *manager)
Definition: SampleSV.cpp:264
void openFiles()
Definition: SampleSV.cpp:244
NewSample SV component.
#define STEERING_P_GAIN
Definition: SampleSV.cpp:29
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
Definition: SampleSV.cpp:162
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
Definition: SampleSV.cpp:145
std::ifstream steer
Definition: SampleSV.h:144
InPort< TimedDoubleSeq > m_steerIn
Definition: SampleSV.h:109
TimedDoubleSeq m_steer
Definition: SampleSV.h:108
#define WHEEL_P_GAIN
Definition: SampleSV.cpp:31
png_uint_32 i
Definition: png.h:2735
#define WHEEL_ID
Definition: SampleSV.cpp:25
#define WHEEL_D_GAIN
Definition: SampleSV.cpp:32
bool addOutPort(const char *name, OutPortBase &outport)
virtual RTC::ReturnCode_t onInitialize()
Definition: SampleSV.cpp:96
OutPort< TimedDoubleSeq > m_torqueOut
Definition: SampleSV.h:123
#define TIMESTEP
Definition: SampleSV.cpp:35
ExecutionContextHandle_t UniqueId
#define STEERING_ID
Definition: SampleSV.cpp:24
TimedDoubleSeq m_vel
Definition: SampleSV.h:111
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
Definition: SampleSV.cpp:171
InPort< TimedFloatSeq > m_rangeIn
Definition: SampleSV.h:115
void closeFiles()
Definition: SampleSV.cpp:251
TimedDoubleSeq m_torque
Definition: SampleSV.h:122
virtual bool isNew()
virtual bool write(DataType &value)
double wheel_ref
Definition: SampleSV.h:145
~SampleSV()
Definition: SampleSV.cpp:90
#define STEERING_D_GAIN
Definition: SampleSV.cpp:30
bool addInPort(const char *name, InPortBase &inport)
TimedFloatSeq m_range
Definition: SampleSV.h:114
static const char * samplepd_spec[]
Definition: SampleSV.cpp:44
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
#define DOF
Definition: SampleSV.cpp:23
#define WHEEL_REF_VEL
Definition: SampleSV.cpp:33
#define DLL_EXPORT
InPort< TimedDoubleSeq > m_velIn
Definition: SampleSV.h:112


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05