Configurable xesc baudrate

vesc_driver/include/vesc_driver/vesc_interface.h

@@ -119,7 +119,7 @@ namespace vesc_driver {
 
         void setPosition(double position);
 
-        void start(const std::string &port);
+        void start(const std::string &port, const uint32_t &baudrate);
 
         void stop();
 
@@ -157,6 +157,7 @@ namespace vesc_driver {
         ErrorHandlerFunction error_handler_;
         serial::Serial serial_;
         std::string port_;
+        uint32_t baudrate_;
         std::mutex status_mutex_;
         // since multiple threads will call the send() function, we need a mutex.
         std::mutex serial_tx_mutex_;

vesc_driver/src/vesc_driver.cpp

@@ -56,7 +56,11 @@ namespace vesc_driver {
             pole_pairs = 1;
         }
 
-        vesc_.start(port);
+        int baudrate;
+        if (!private_nh.getParam("baudrate", baudrate)) {
+            baudrate = 115200;
+        }
+        vesc_.start(port, baudrate);
     }
 
 

vesc_driver/src/vesc_interface.cpp

@@ -98,6 +98,7 @@ namespace vesc_driver {
                 try {
                     serial_.setPort(port_);
                     serial_.open();
+                    serial_.setBaudrate(baudrate_);
                 } catch (std::exception &e) {
                     // retry later
                     sleep(1);
@@ -275,8 +276,9 @@ namespace vesc_driver {
         send(VescPacketSetPos(position));
     }
 
-    void VescInterface::start(const std::string &port) {
+    void VescInterface::start(const std::string &port, const uint32_t &baudrate) {
         port_ = port;
+        baudrate_ = baudrate;
         // start up a monitoring thread
         rx_thread_run_ = true;
         update_thread_run_ = true;

xesc_2040_driver/include/xesc_2040_driver/xesc_2040_interface.h

@@ -54,7 +54,7 @@ namespace xesc_2040_driver {
 
         void setDutyCycle(double duty_cycle);
 
-        void start(const std::string &port);
+        void start(const std::string &port, const uint32_t &baudrate);
 
         void stop();
 
@@ -95,6 +95,7 @@ namespace xesc_2040_driver {
         ErrorHandlerFunction error_handler_;
         serial::Serial serial_;
         std::string port_;
+        uint32_t baudrate_;
         std::mutex status_mutex_;
         // since multiple threads will call the send() function, we need a mutex.
         std::mutex serial_tx_mutex_;

xesc_2040_driver/src/xesc_2040_driver.cpp

@@ -20,12 +20,17 @@ xesc_2040_driver::Xesc2040Driver::Xesc2040Driver(ros::NodeHandle &nh, ros::NodeH
     float min_pcb_temp;
     float max_pcb_temp;
     std::string serial_port;
+    int baudrate;
 
     if(!private_nh.getParam("serial_port", serial_port)) {
         ROS_ERROR_STREAM("You need to provide parameter serial_port.");
         throw ros::InvalidParameterException("You need to provide parameter serial_port.");
     }
 
+    if (!private_nh.getParam("baudrate", baudrate)) {
+        baudrate = 115200;
+    }
+
     for(int i = 0; i < 8; i++) {
         int tmp;
         if (!private_nh.getParam(std::string("hall_table_")+std::to_string(i), tmp)) {
@@ -73,7 +78,7 @@ xesc_2040_driver::Xesc2040Driver::Xesc2040Driver(ros::NodeHandle &nh, ros::NodeH
                                     min_pcb_temp,
                                     max_pcb_temp);
 
-    xesc_interface->start(private_nh.param("serial_port", serial_port));
+    xesc_interface->start(private_nh.param("serial_port", serial_port), baudrate);
 }
 
 void xesc_2040_driver::Xesc2040Driver::getStatus(xesc_msgs::XescStateStamped &state_msg) {

xesc_2040_driver/src/xesc_2040_interface.cpp

@@ -36,7 +36,7 @@ namespace xesc_2040_driver {
                     status_.connection_state = XESC2040_CONNECTION_STATE::DISCONNECTED;
 
                     serial_.setPort(port_);
-                    serial_.setBaudrate(115200);
+                    serial_.setBaudrate(baudrate_);
                     auto to = serial::Timeout::simpleTimeout(100);
                     serial_.setTimeout(to);
                     serial_.open();
@@ -189,8 +189,9 @@ namespace xesc_2040_driver {
         send(reinterpret_cast<uint8_t *>(&controlPacket), sizeof(controlPacket));
     }
 
-    void Xesc2040Interface::start(const std::string &port) {
+    void Xesc2040Interface::start(const std::string &port, const uint32_t &baudrate) {
         port_ = port;
+        baudrate_ = baudrate;
         // start up a monitoring thread
         rx_thread_run_ = true;
         pthread_create(&rx_thread_handle_, NULL, &Xesc2040Interface::rx_thread_helper, this);