Message_cmd.cpp
Go to the documentation of this file.
1 
46 #include "roch_base/core/Message_cmd.h"
47 
48 
49 #include <cstring>
50 #include <iostream>
51 #include <typeinfo>
52 
53 using namespace std;
54 
55 #include "roch_base/core/Number.h"
56 // Conditions on the below to handle compiling for nonstandard hardware
57 #ifdef LOGGING_AVAIL
58 #include "roch_base/core/Logger.h"
59 #endif
60 #ifdef DEBUG_INFO
61 #include "roch_base/roch_hardware.h"//test for ROS_DEBUG_STREAM
62 #endif
63 namespace sawyer
64 {
65 
66  long CmdMessage::total_destroyed = 0;
67  long CmdMessage::total_sent = 0;
68 
69  CmdMessage::~CmdMessage()
70  {
71  ++total_destroyed;
72  if (!is_sent)
73  {
74 #ifdef LOGGING_AVAIL
75  CPR_WARN() << "Command message destroyed without being sent. Type: "
76  << "0x" << hex << getType() << dec
77  << ". Total unsent: " << (total_destroyed-total_sent) << endl;
78 #endif
79  }
80  else
81  {
82  total_sent++;
83  }
84  }
85 
86 
87  CmdProcessorReset::CmdProcessorReset() : CmdMessage()
88  {
89  setPayloadLength(2);
90  utob(getPayloadPointer(), 2, (uint16_t) 0x3A18);
91  setType(CMD_PROCESSOR_RESET);
92  makeValid();
93  }
94 
95  CmdProcessorReset::CmdProcessorReset(const CmdProcessorReset &other) : CmdMessage(other)
96  {
97  }
98 
99 
100  CmdRestoreSettings::CmdRestoreSettings(enum restoreFlags flags) : CmdMessage()
101  {
102 #ifdef DEBUG_INFO
103  ROS_DEBUG_STREAM("CmdRestoreSettings: flags:%x",(uint8_t) (flags));
104 #endif
105  setPayloadLength(3);
106 
107  utob(getPayloadPointer(), 2, (uint16_t) (0x3a18));
108  *getPayloadPointer(2) = (uint8_t) (flags);
109  setType(CMD_RESTORE_SETTINGS);
110 
111  makeValid();
112  }
113 
114  CmdRestoreSettings::CmdRestoreSettings(const CmdRestoreSettings &other) : CmdMessage(other)
115  {
116  }
117 
118 
119  CmdStoreSettings::CmdStoreSettings() : CmdMessage()
120  {
121  setPayloadLength(2);
122 
123  utob(getPayloadPointer(), 2, (uint16_t) (0x3a18));
124  setType(CMD_STORE_SETTINGS);
125 
126  makeValid();
127  }
128 
129  CmdStoreSettings::CmdStoreSettings(const CmdStoreSettings &other) : CmdMessage(other)
130  {
131  }
132 
133 
134  SetAckermannOutput::SetAckermannOutput(double steer, double throt, double brake) : CmdMessage()
135  {
136 #ifdef DEBUG_INFO
137  ROS_DEBUG_STREAM("SetAckermannOutput: steer:%.2lf, throt:%.2lf, brake:%.2lf",steer,throt,brake);
138 #endif
139  setPayloadLength(PAYLOAD_LEN);
140 
141  ftob(getPayloadPointer(STEERING), 2, steer, 100);
142  ftob(getPayloadPointer(THROTTLE), 2, throt, 100);
143  ftob(getPayloadPointer(BRAKE), 2, brake, 100);
144 
145  setType(SET_ACKERMANN_SETPT);
146  makeValid();
147  }
148 
149  SetAckermannOutput::SetAckermannOutput(const SetAckermannOutput &other) : CmdMessage(other)
150  {
151  }
152 
153 
154  SetDifferentialControl::SetDifferentialControl(
155  double p,
156  double i,
157  double d)
158  : CmdMessage()
159  {
160 #ifdef DEBUG_INFO
161  ROS_DEBUG_STREAM("SetDifferentialControl: P:%.2lf,I:%.2lf,D:%.2lf.", p, i, d);
162 #endif
163 
164  setPayloadLength(PAYLOAD_LEN);
165 
166  ftob(getPayloadPointer(LEFT_P), 2, p, 10000);
167  ftob(getPayloadPointer(LEFT_I), 2, i, 10000);
168  ftob(getPayloadPointer(LEFT_D), 2, d, 10000);
169 
170  ftob(getPayloadPointer(RIGHT_P), 2, p, 10000);
171  ftob(getPayloadPointer(RIGHT_I), 2, i, 10000);
172  ftob(getPayloadPointer(RIGHT_D), 2, d, 10000);
173 
174  setType(SET_DIFF_CTRL_CONSTS);
175  makeValid();
176  }
177 
178  SetDifferentialControl::SetDifferentialControl(
179  double left_p,
180  double left_i,
181  double left_d,
182  double right_p,
183  double right_i,
184  double right_d)
185  : CmdMessage()
186  {
187 #ifdef DEBUG_INFO
188  ROS_DEBUG_STREAM("SetDifferentialControl: left_p:%.2lf,left_i:%.2lf,left_d:%.2lf"
189  "right_p:%.2lf,right_i:%.2lf,right_d:%.2lf.", left_p,
190  left_i,
191  left_d,
192  right_p,
193  right_i,
194  right_d);
195 #endif
196  setPayloadLength(PAYLOAD_LEN);
197 
198  ftob(getPayloadPointer(LEFT_P), 2, left_p, 10000);
199  ftob(getPayloadPointer(LEFT_I), 2, left_i, 10000);
200  ftob(getPayloadPointer(LEFT_D), 2, left_d, 10000);
201 
202  ftob(getPayloadPointer(RIGHT_P), 2, right_p, 10000);
203  ftob(getPayloadPointer(RIGHT_I), 2, right_i, 10000);
204  ftob(getPayloadPointer(RIGHT_D), 2, right_d, 10000);
205 
206  setType(SET_DIFF_CTRL_CONSTS);
207  makeValid();
208  }
209 
210  SetDifferentialControl::SetDifferentialControl(const SetDifferentialControl &other)
211  : CmdMessage(other)
212  {
213  }
214 
215 
216  SetDifferentialOutput::SetDifferentialOutput(double left, double right) : CmdMessage()
217  {
218 #ifdef DEBUG_INFO
219  ROS_DEBUG_STREAM("SetDifferentialOutput: left:%.2lf,right:%.2lf",left,right);
220 #endif
221  setPayloadLength(PAYLOAD_LEN);
222  ftob(getPayloadPointer(LEFT), 2, left, 100);
223  ftob(getPayloadPointer(RIGHT), 2, right, 100);
224 
225  setType(SET_DIFF_WHEEL_SETPTS);
226  makeValid();
227  }
228 
229  SetDifferentialOutput::SetDifferentialOutput(const SetDifferentialOutput &other)
230  : CmdMessage(other)
231  {
232  }
233 
234 
235  SetDifferentialSpeed::SetDifferentialSpeed(double left_speed, double right_speed,
236  double left_accel, double right_accel)
237  : CmdMessage()
238  {
239  setPayloadLength(PAYLOAD_LEN);
240  ftob(getPayloadPointer(LEFT_SPEED), 2, left_speed, 100);
241  ftob(getPayloadPointer(LEFT_ACCEL), 2, left_accel, 100);
242  ftob(getPayloadPointer(RIGHT_SPEED), 2, right_speed, 100);
243  ftob(getPayloadPointer(RIGHT_ACCEL), 2, right_accel, 100);
244 #ifdef DEBUG_SEND_SPEED
245  ROS_DEBUG_STREAM("SetDifferentialSpeed: left_speed:%.2lf,right_speed:%.2lf,left_accel:%.2lf,right_accel:%.2lf",left_speed,right_speed,left_accel,right_accel);
246 #endif
247  setType(SET_DIFF_WHEEL_SPEEDS);
248  makeValid();
249  }
250 
251  SetDifferentialSpeed::SetDifferentialSpeed(const SetDifferentialSpeed &other) : CmdMessage(other)
252  {
253  }
254 
255 
256  SetGear::SetGear(uint8_t gear) : CmdMessage()
257  {
258 #ifdef DEBUG_INFO
259  ROS_DEBUG_STREAM("SetGear: gear:%x",gear);
260 #endif
261  setPayloadLength(1);
262  getPayloadPointer()[0] = gear;
263  setType(SET_GEAR_SETPOINT);
264  makeValid();
265  }
266 
267  SetGear::SetGear(const SetGear &other) : CmdMessage(other)
268  {
269  }
270 
271 
272  SetMaxAccel::SetMaxAccel(double max_fwd, double max_rev) : CmdMessage()
273  {
274  setPayloadLength(PAYLOAD_LEN);
275 
276  ftob(getPayloadPointer(MAX_FWD), 2, max_fwd, 100);
277  ftob(getPayloadPointer(MAX_REV), 2, max_rev, 100);
278 #ifdef DEBUG_INFO
279  ROS_DEBUG_STREAM("SetMaxAccel: max_fwd:%.2lf, max_rev:%.2lf",max_fwd,max_rev);
280 #endif
281  setType(SET_MAX_ACCEL);
282  makeValid();
283  }
284 
285  SetMaxAccel::SetMaxAccel(const SetMaxAccel &other) : CmdMessage(other)
286  {
287  }
288 
289 
290  SetMaxSpeed::SetMaxSpeed(double max_fwd, double max_rev) : CmdMessage()
291  {
292  setPayloadLength(PAYLOAD_LEN);
293 
294  ftob(getPayloadPointer(MAX_FWD), 2, max_fwd, 100);
295  ftob(getPayloadPointer(MAX_REV), 2, max_rev, 100);
296 #ifdef DEBUG_INFO
297  ROS_DEBUG_STREAM("SetMaxSpeed: max_fwd:%.2lf, max_rev:%.2lf",max_fwd,max_rev);
298 #endif
299  setType(SET_MAX_SPEED);
300  makeValid();
301  }
302 
303  SetMaxSpeed::SetMaxSpeed(const SetMaxSpeed &other) : CmdMessage(other)
304  {
305  }
306 
307 
308  SetPlatformName::SetPlatformName(const char *name) : CmdMessage()
309  {
310  size_t cpy_len = strlen(name);
311  size_t max_len = MAX_MSG_LENGTH - HEADER_LENGTH - CRC_LENGTH - 1 /* for size field */;
312  if (cpy_len > max_len) { cpy_len = max_len; }
313 #ifdef DEBUG_INFO
314  ROS_DEBUG_STREAM("SetPlatformName: name:%s",name);
315 #endif
316  setPayloadLength(cpy_len + 1);
317  getPayloadPointer()[0] = cpy_len;
318  memcpy(getPayloadPointer(1), name, cpy_len);
319 
320  setType(SET_PLATFORM_NAME);
321 
322  makeValid();
323  }
324 
325  SetPlatformName::SetPlatformName(const SetPlatformName &other) : CmdMessage(other)
326  {
327  }
328 
329 
330  SetPlatformTime::SetPlatformTime(uint32_t time) : CmdMessage()
331  {
332 #ifdef DEBUG_INFO
333  ROS_DEBUG_STREAM("SetPlatformTime: name:%d",time);
334 #endif
335  setPayloadLength(4);
336  utob(getPayloadPointer(), 4, time);
337  setType(SET_PLATFORM_TIME);
338  makeValid();
339  }
340 
341  SetPlatformTime::SetPlatformTime(const SetPlatformTime &other) : CmdMessage(other)
342  {
343  }
344 
345 
346  SetSafetySystem::SetSafetySystem(uint16_t flags) : CmdMessage()
347  {
348 #ifdef DEBUG_INFO
349  ROS_DEBUG_STREAM("SetSafetySystem: flags:%x",flags);
350 #endif
351  setPayloadLength(2);
352  utob(getPayloadPointer(), 2, flags);
353  setType(SET_SAFETY_SYSTEM);
354  makeValid();
355  }
356 
357  SetSafetySystem::SetSafetySystem(const SetSafetySystem &other) : CmdMessage(other)
358  {
359  }
360 
361 
362  SetTurn::SetTurn(double trans, double rad, double accel) : CmdMessage()
363  {
364  setPayloadLength(PAYLOAD_LEN);
365 #ifdef DEBUG_INFO
366  ROS_DEBUG_STREAM("SetTurn: rad:%.2lf, accel:%.2lf",trans,accel);
367 #endif
368 
369  ftob(getPayloadPointer(TRANSLATIONAL), 2, trans, 100);
370  ftob(getPayloadPointer(TURN_RADIUS), 2, rad, 100);
371  ftob(getPayloadPointer(TRANS_ACCEL), 2, accel, 100);
372 
373  setType(SET_TURN_SETPT);
374  makeValid();
375  }
376 
377  SetTurn::SetTurn(const SetTurn &other) : CmdMessage(other)
378  {
379  }
380 
381  SetWheelInfo::SetWheelInfo(double wheel_gauge, double wheel_diameter) : CmdMessage()
382  {
383  setPayloadLength(PAYLOAD_LEN);
384 #ifdef DEBUG_INFO
385  ROS_DEBUG_STREAM("Set Wheel gauge: %.2lf, wheel diameter: %.2lf .",wheel_gauge,wheel_diameter);
386 #endif
387 
388  ftob(getPayloadPointer(WHEEL_GAUGE), 2, wheel_gauge, 10000);
389  ftob(getPayloadPointer(WHEEL_DIAMETER), 2, wheel_diameter, 10000);
390 
391  setType(SET_WHEEL_INFO);
392  makeValid();
393  }
394 
395  SetWheelInfo::SetWheelInfo(const SetWheelInfo &other) : CmdMessage(other)
396  {
397  }
398 
399  SetVelocity::SetVelocity(double trans, double rot, double accel) : CmdMessage()
400  {
401  setPayloadLength(PAYLOAD_LEN);
402 #ifdef DEBUG_INFO
403  ROS_DEBUG_STREAM("SetVelocity: trans:%.2lf, rot:%.2lf, accel:%.2lf",trans,rot,accel);
404 #endif
405 
406  ftob(getPayloadPointer(TRANSLATIONAL), 2, trans, 100);
407  ftob(getPayloadPointer(ROTATIONAL), 2, rot, 100);
408  ftob(getPayloadPointer(TRANS_ACCEL), 2, accel, 100);
409 
410  setType(SET_VELOCITY_SETPT);
411  makeValid();
412  }
413 
414  SetVelocity::SetVelocity(const SetVelocity &other) : CmdMessage(other)
415  {
416  }
417 
418 
419 }; // namespace sawyer
420 
sawyer::SetWheelInfo::SetWheelInfo
SetWheelInfo(double wheel_gauge, double Wheel_diameter)
Definition: Message_cmd.cpp:381
roch_hardware.h
sawyer::Message::MAX_MSG_LENGTH
static const size_t MAX_MSG_LENGTH
Definition: Message.h:70
sawyer::Message::setType
void setType(uint16_t type)
Definition: Message.cpp:272
sawyer::SetDifferentialControl::SetDifferentialControl
SetDifferentialControl(double p, double i, double d)
Definition: Message_cmd.cpp:154
Message_cmd.h
sawyer::utob
void utob(void *dest, size_t dest_len, uint64_t src)
Definition: Number.cpp:55
sawyer::Message::getPayloadPointer
uint8_t * getPayloadPointer(size_t offset=0)
Definition: Message.cpp:218
sawyer::SetTurn::SetTurn
SetTurn(double trans, double rad, double accel)
Definition: Message_cmd.cpp:362
sawyer::SetMaxSpeed::SetMaxSpeed
SetMaxSpeed(double max_fwd, double max_rev)
Definition: Message_cmd.cpp:290
sawyer::SetDifferentialSpeed::SetDifferentialSpeed
SetDifferentialSpeed(double left_spd, double right_speed, double left_accel, double right_accel)
Definition: Message_cmd.cpp:235
sawyer::SetMaxAccel::SetMaxAccel
SetMaxAccel(double max_fwd, double max_rev)
Definition: Message_cmd.cpp:272
sawyer::Message::setPayloadLength
void setPayloadLength(uint8_t len)
Definition: Message.cpp:282
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
sawyer::ftob
void ftob(void *dest, size_t dest_len, double src, double scale)
Definition: Number.cpp:92
sawyer::Message::CRC_LENGTH
static const size_t CRC_LENGTH
Definition: Message.h:77
sawyer::SetPlatformName::SetPlatformName
SetPlatformName(const char *name)
Definition: Message_cmd.cpp:308
sawyer::Message::makeValid
void makeValid()
Definition: Message.cpp:372
sawyer::SetPlatformTime::SetPlatformTime
SetPlatformTime(uint32_t time)
Definition: Message_cmd.cpp:330
sawyer::SetAckermannOutput::SetAckermannOutput
SetAckermannOutput(double steering, double throt, double brake)
Definition: Message_cmd.cpp:134
sawyer::SetSafetySystem::SetSafetySystem
SetSafetySystem(uint16_t flags)
Definition: Message_cmd.cpp:346
sawyer::SetDifferentialOutput::SetDifferentialOutput
SetDifferentialOutput(double left, double right)
Definition: Message_cmd.cpp:216
CPR_WARN
#define CPR_WARN()
Definition: Logger.h:98
sawyer::CmdRestoreSettings::CmdRestoreSettings
CmdRestoreSettings(enum restoreFlags flags)
Definition: Message_cmd.cpp:100
sawyer::Message::HEADER_LENGTH
static const size_t HEADER_LENGTH
Definition: Message.h:80
sawyer::SetGear::SetGear
SetGear(uint8_t gear)
Definition: Message_cmd.cpp:256
sawyer::SetVelocity::SetVelocity
SetVelocity(double trans, double rot, double accel)
Definition: Message_cmd.cpp:399

roch_base
Author(s): Mike Purvis , Paul Bovbel , Chen
autogenerated on Mon Jun 10 2019 14:41:13