@@ -40,9 +40,11 @@ class Driver
40
40
{
41
41
public:
42
42
Driver (const std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface,
43
- std::shared_ptr<rclcpp::Node> nh, const uint8_t & device_number, const std::string& device_name);
43
+ std::shared_ptr<rclcpp::Node> nh,
44
+ const uint8_t & device_number,
45
+ const std::string& device_name);
44
46
45
- void processMessage (const can_msgs::msg::Frame::SharedPtr received_msg);
47
+ void processMessage (const can_msgs::msg::Frame::SharedPtr received_msg);
46
48
47
49
double radPerSecToRpm () const ;
48
50
@@ -433,7 +435,7 @@ class Driver
433
435
*
434
436
* @return value of the set-point response.
435
437
*/
436
- double statusPositionGet ();
438
+ double statusPositionGet ();
437
439
438
440
std::string deviceName () const
439
441
{
@@ -449,9 +451,9 @@ class Driver
449
451
struct Field
450
452
{
451
453
uint8_t data[4 ];
452
- bool received;
454
+ bool received;
453
455
454
- float interpretFixed8x8 ()
456
+ float interpretFixed8x8 ()
455
457
{
456
458
return *(reinterpret_cast <int16_t *>(data)) / static_cast <float >(1 << 8 );
457
459
}
@@ -464,32 +466,32 @@ class Driver
464
466
465
467
private:
466
468
std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface_;
467
- std::shared_ptr<rclcpp::Node> nh_;
468
- uint8_t device_number_;
469
- std::string device_name_;
469
+ std::shared_ptr<rclcpp::Node> nh_;
470
+ uint8_t device_number_;
471
+ std::string device_name_;
470
472
471
- bool configured_;
472
- uint8_t state_;
473
+ bool configured_;
474
+ uint8_t state_;
473
475
474
- uint8_t control_mode_;
475
- double gain_p_;
476
- double gain_i_;
477
- double gain_d_;
478
- uint16_t encoder_cpr_;
479
- float gear_ratio_;
476
+ uint8_t control_mode_;
477
+ double gain_p_;
478
+ double gain_i_;
479
+ double gain_d_;
480
+ uint16_t encoder_cpr_;
481
+ float gear_ratio_;
480
482
481
483
/* *
482
484
* Helpers to generate data for CAN messages.
483
485
*/
484
486
can_msgs::msg::Frame::SharedPtr can_msg_;
485
- void sendId (const uint32_t id);
486
- void sendUint8 (const uint32_t id, const uint8_t value);
487
- void sendUint16 (const uint32_t id, const uint16_t value);
488
- void sendFixed8x8 (const uint32_t id, const float value);
489
- void sendFixed16x16 (const uint32_t id, const double value);
490
- can_msgs::msg::Frame getMsg (const uint32_t id);
491
- uint32_t getApi (const can_msgs::msg::Frame msg);
492
- uint32_t getDeviceNumber (const can_msgs::msg::Frame msg);
487
+ void sendId (const uint32_t id);
488
+ void sendUint8 (const uint32_t id, const uint8_t value);
489
+ void sendUint16 (const uint32_t id, const uint16_t value);
490
+ void sendFixed8x8 (const uint32_t id, const float value);
491
+ void sendFixed16x16 (const uint32_t id, const double value);
492
+ can_msgs::msg::Frame getMsg (const uint32_t id);
493
+ uint32_t getApi (const can_msgs::msg::Frame msg);
494
+ uint32_t getDeviceNumber (const can_msgs::msg::Frame msg);
493
495
494
496
/* *
495
497
* Comparing the raw bytes of the 16x16 fixed-point numbers
@@ -505,15 +507,15 @@ class Driver
505
507
*
506
508
* @return boolean if received is equal to expected.
507
509
*/
508
- bool verifyRaw8x8 (const uint8_t * received, const float expected);
510
+ bool verifyRaw8x8 (const uint8_t * received, const float expected);
509
511
510
- Field voltage_fields_[4 ];
511
- Field spd_fields_[7 ];
512
- Field vcomp_fields_[5 ];
513
- Field pos_fields_[7 ];
514
- Field ictrl_fields_[6 ];
515
- Field status_fields_[15 ];
516
- Field cfg_fields_[15 ];
512
+ Field voltage_fields_[4 ];
513
+ Field spd_fields_[7 ];
514
+ Field vcomp_fields_[5 ];
515
+ Field pos_fields_[7 ];
516
+ Field ictrl_fields_[6 ];
517
+ Field status_fields_[15 ];
518
+ Field cfg_fields_[15 ];
517
519
518
520
Field* voltageFieldForMessage (uint32_t api);
519
521
Field* spdFieldForMessage (uint32_t api);
0 commit comments