RangeDataViewer.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include "RangeDataViewer.h"
11 
12 #if __cplusplus >= 201103L
13 using std::isinf;
14 #endif
15 
16 // Module specification
17 // <rtc-template block="module_spec">
18 static const char* cameraimageviewercomponent_spec[] =
19  {
20  "implementation_id", "RangeDataViewer",
21  "type_name", "RangeDataViewer",
22  "description", "range data viewer component",
23  "version", HRPSYS_PACKAGE_VERSION,
24  "vendor", "AIST",
25  "category", "example",
26  "activity_type", "DataFlowComponent",
27  "max_instance", "10",
28  "language", "C++",
29  "lang_type", "compile",
30  // Configuration variables
31  "conf.default.maxRange", "2.0",
32 
33 
34  ""
35  };
36 // </rtc-template>
37 
39  : RTC::DataFlowComponentBase(manager),
40  // <rtc-template block="initializer">
41  m_rangeIn("rangeIn", m_range),
42  // </rtc-template>
43  m_cvImage(NULL),
44  dummy(0)
45 {
46 }
47 
49 {
50 }
51 
52 
53 
54 RTC::ReturnCode_t RangeDataViewer::onInitialize()
55 {
56  std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
57  // <rtc-template block="bind_config">
58  // Bind variables and configuration variable
59  bindParameter("maxRange", m_maxRange, "2.0");
60 
61  // </rtc-template>
62 
63  // Registration: InPort/OutPort/Service
64  // <rtc-template block="registration">
65  // Set InPort buffers
66  addInPort("rangeIn", m_rangeIn);
67 
68  // Set OutPort buffer
69 
70  // Set service provider to Ports
71 
72  // Set service consumers to Ports
73 
74  // Set CORBA Service Ports
75 
76  // </rtc-template>
77 
79 
80  return RTC::RTC_OK;
81 }
82 
83 
84 
85 /*
86  RTC::ReturnCode_t RangeDataViewer::onFinalize()
87  {
88  return RTC::RTC_OK;
89  }
90 */
91 
92 /*
93  RTC::ReturnCode_t RangeDataViewer::onStartup(RTC::UniqueId ec_id)
94  {
95  return RTC::RTC_OK;
96  }
97 */
98 
99 /*
100  RTC::ReturnCode_t RangeDataViewer::onShutdown(RTC::UniqueId ec_id)
101  {
102  return RTC::RTC_OK;
103  }
104 */
105 
106 #define WSIZE 640
108 {
109  std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
110  m_cvImage = cvCreateImage(cvSize(WSIZE, WSIZE), IPL_DEPTH_8U, 3);
111  cvNamedWindow("Range",CV_WINDOW_AUTOSIZE);
112  return RTC::RTC_OK;
113 }
114 
116 {
117  std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
118  if (m_cvImage) {
119  cvReleaseImage(&m_cvImage);
120  m_cvImage = NULL;
121  }
122  cvDestroyWindow("Range");
123  return RTC::RTC_OK;
124 }
125 
127 {
128  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
129 
130  if (1/*m_rangeIn.isNew()*/){
131  do {
132  m_rangeIn.read();
133  }while(m_rangeIn.isNew());
134 
135  cvSetZero(m_cvImage);
136 
137  CvPoint center = cvPoint(WSIZE/2,WSIZE/2);
138  CvScalar green = cvScalar(0,255,0);
139  double th,d,x,y;
140 #if 0
141  std::cout << "minAngle:" << m_range.config.minAngle << std::endl;
142  std::cout << "maxAngle:" << m_range.config.maxAngle << std::endl;
143  std::cout << "angularRes:" << m_range.config.angularRes << std::endl;
144  std::cout << "minRange:" << m_range.config.minRange << std::endl;
145  std::cout << "maxRange:" << m_range.config.maxRange << std::endl;
146  std::cout << "rangeRes:" << m_range.config.rangeRes << std::endl;
147  std::cout << "frequency:" << m_range.config.frequency << std::endl;
148  std::cout << "ndata = " << m_range.ranges.length() << std::endl;
149 #endif
150  for (unsigned int i=0; i<m_range.ranges.length(); i++){
151  d = m_range.ranges[i];
152  if (isinf(d)) continue;
153  th = m_range.config.minAngle + m_range.config.angularRes*i;
154  x = -d*sin(th)/m_maxRange*WSIZE/2 + WSIZE/2;
155  y = -d*cos(th)/m_maxRange*WSIZE/2 + WSIZE/2;
156  cvLine(m_cvImage, center, cvPoint(x, y), green, 1, 8, 0);
157  }
158 
159  cvShowImage("Range",m_cvImage);
160  cvWaitKey(10);
161  }
162 
163  return RTC::RTC_OK;
164 }
165 
166 /*
167  RTC::ReturnCode_t RangeDataViewer::onAborting(RTC::UniqueId ec_id)
168  {
169  return RTC::RTC_OK;
170  }
171 */
172 
173 /*
174  RTC::ReturnCode_t RangeDataViewer::onError(RTC::UniqueId ec_id)
175  {
176  return RTC::RTC_OK;
177  }
178 */
179 
180 /*
181  RTC::ReturnCode_t RangeDataViewer::onReset(RTC::UniqueId ec_id)
182  {
183  return RTC::RTC_OK;
184  }
185 */
186 
187 /*
188  RTC::ReturnCode_t RangeDataViewer::onStateUpdate(RTC::UniqueId ec_id)
189  {
190  return RTC::RTC_OK;
191  }
192 */
193 
194 /*
195  RTC::ReturnCode_t RangeDataViewer::onRateChanged(RTC::UniqueId ec_id)
196  {
197  return RTC::RTC_OK;
198  }
199 */
200 
201 
202 
203 extern "C"
204 {
205 
207  {
209  manager->registerFactory(profile,
210  RTC::Create<RangeDataViewer>,
211  RTC::Delete<RangeDataViewer>);
212  }
213 
214 };
215 
216 
ComponentProfile m_profile
d
png_infop png_charpp int png_charpp profile
void green()
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
null component
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
png_uint_32 i
coil::Properties & getProperties()
ExecutionContextHandle_t UniqueId
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
RangeDataViewer(RTC::Manager *manager)
Constructor.
static const char * cameraimageviewercomponent_spec[]
prop
virtual bool isNew()
void RangeDataViewerInit(RTC::Manager *manager)
InPort< RangeData > m_rangeIn
IplImage * m_cvImage
virtual RTC::ReturnCode_t onInitialize()
#define WSIZE
bool addInPort(const char *name, InPortBase &inport)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
virtual ~RangeDataViewer()
Destructor.
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20