openrover_driver.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 
3 #include <fcntl.h>
4 #include <termios.h>
5 #include <ctime>
6 #include <vector>
7 #include <string>
8 #include <cmath>
9 #include <fstream>
10 #include <iostream>
11 #include <sys/ioctl.h>
12 
15 #include "std_msgs/Int32.h"
16 #include "std_msgs/Int32MultiArray.h"
17 #include "std_msgs/Float32MultiArray.h"
18 #include "geometry_msgs/Twist.h"
19 #include <std_msgs/Bool.h>
20 #include "nav_msgs/Odometry.h"
21 #include "rr_openrover_driver_msgs/RawRrOpenroverDriverFastRateData.h"
22 #include "rr_openrover_driver_msgs/RawRrOpenroverDriverMedRateData.h"
23 #include "rr_openrover_driver_msgs/RawRrOpenroverDriverSlowRateData.h"
24 #include "rr_openrover_driver_msgs/SmartBatteryStatus.h"
25 
27 
28 namespace openrover
29 {
31  : nh_(nh)
32  , nh_priv_(nh_priv)
33  , port_("/dev/ttyUSB0")
34  , serial_baud_rate_(57600)
35  , use_legacy_(false)
36  , fast_rate_hz_(10.0) // can increase to 60Hz for TX2
37  , medium_rate_hz_(2.0)
38  , slow_rate_hz_(1.0)
39  , motor_speeds_commanded_{ MOTOR_NEUTRAL, MOTOR_NEUTRAL, MOTOR_NEUTRAL } // default motor commands to neutral
40  , timeout_(0.2) // in seconds
44  , is_serial_coms_open_(false)
46  , pidGains_(40, 100, 0)
53  , e_stop_on_(false)
57  , l_pid_csv_file_("")
58  , r_pid_csv_file_("")
59 {
60  ROS_INFO("Initializing openrover driver.");
61 }
62 
64 {
65  if (!setupRobotParams())
66  {
67  ROS_WARN("Failed to setup Robot parameters.");
68  return false;
69  }
70 
71  ROS_INFO("%f, %f, %f", pidGains_.Kp, pidGains_.Ki, pidGains_.Kd);
72 
73  if (l_fs_.is_open()) {
75  }
76 
77  if (r_fs_.is_open()) {
79  }
80 
83 
84  serial_fast_buffer_.reserve(5 * FAST_SIZE); // reserve space for 5 sets of FAST rate data
85  serial_medium_buffer_.reserve(5 * MEDIUM_SIZE); // reserve space for 5 sets of Medium rate data
86  serial_slow_buffer_.reserve(5 * SLOW_SIZE); // reserve space for 5 sets of Slow rate data
87  serial_fan_buffer_.reserve(5); // reserve space for 5 sets of Fan commands
88 
89  ROS_INFO("Creating Publishers and Subscribers");
90  // WallTimers simplify the timing of updating parameters by reloading serial buffers at specified rates.
91  // without them the serial buffers will never be loaded with new commands
96 
97 
98  if (!(nh_priv_.getParam("use_legacy", use_legacy_)))
99  {
100  ROS_WARN("Failed to retrieve drive_type from parameter.Defaulting to %s", use_legacy_ ? "true" : "false");
101  }
102 
103  if(use_legacy_) {
104  fast_rate_pub = nh_priv_.advertise<rr_openrover_driver_msgs::RawRrOpenroverDriverFastRateData>("raw_fast_rate_data",
105  1);
106  medium_rate_pub = nh_priv_.advertise<rr_openrover_driver_msgs::RawRrOpenroverDriverMedRateData>("raw_med_rate_data",
107  1);
108  slow_rate_pub = nh_priv_.advertise<rr_openrover_driver_msgs::RawRrOpenroverDriverSlowRateData>("raw_slow_rate_data",
109  1);
110  }
111  battery_status_a_pub = nh_priv_.advertise<rr_openrover_driver_msgs::SmartBatteryStatus>("battery_status_a", 1);
112  battery_status_b_pub = nh_priv_.advertise<rr_openrover_driver_msgs::SmartBatteryStatus>("battery_status_b", 1);
113  battery_state_of_charge_pub = nh_priv_.advertise<std_msgs::Int32>("battery_state_of_charge", 1);
114  odom_enc_pub = nh_priv_.advertise<nav_msgs::Odometry>("odom_encoder", 1);
115  is_charging_pub = nh_priv_.advertise<std_msgs::Bool>("charging", 1);
116 
117  motor_speeds_pub = nh_priv_.advertise<std_msgs::Int32MultiArray>("motor_speeds_commanded", 1);
118  vel_calc_pub = nh_priv_.advertise<std_msgs::Float32MultiArray>("vel_calc_pub", 1);
119 
120  cmd_vel_sub = nh_priv_.subscribe("/cmd_vel/managed", 1, &OpenRover::cmdVelCB, this);
121  fan_speed_sub = nh_priv_.subscribe("/rr_openrover_driver/fan_speed", 1, &OpenRover::fanSpeedCB, this);
122  e_stop_sub = nh_priv_.subscribe("/soft_estop/enable", 1, &OpenRover::eStopCB, this);
123  e_stop_reset_sub = nh_priv_.subscribe("/soft_estop/reset", 1, &OpenRover::eStopResetCB, this);
124 
125  return true;
126 }
127 
129 { // Get ROS params and save them to class variables
130 
131  if (!(nh_priv_.getParam("port", port_)))
132  {
133  ROS_WARN("Failed to retrieve port from parameter server.Defaulting to %s", port_.c_str());
134  }
135 
136  if (!(openComs()))
137  {
138  is_serial_coms_open_ = false;
139  ROS_ERROR("Failed to start serial communication.");
140  return false;
141  }
142 
143  if (!(nh_priv_.getParam("fast_data_rate", fast_rate_hz_)))
144  {
145  ROS_WARN("Failed to retrieve fast_data_rate from parameter. Defaulting to 10");
146  }
147 
148  if (!(nh_priv_.getParam("medium_data_rate", medium_rate_hz_)))
149  {
150  ROS_WARN("Failed to retrieve medium_data_rate from parameter. Defaulting to 2");
151  }
152 
153  if (!(nh_priv_.getParam("slow_data_rate", slow_rate_hz_)))
154  {
155  ROS_WARN("Failed to retrieve slow_data_rate from parameter. Defaulting to 1");
156  }
157 
158 
159  if (!(nh_priv_.getParam("closed_loop_control_on", closed_loop_control_on_)))
160  {
161  ROS_WARN("Failed to retrieve closed_loop_control_on from parameter server. Defaulting to off.");
162  }
163 
164  if (!(nh_priv_.getParam("timeout", timeout_)))
165  {
166  ROS_WARN("Failed to retrieve timeout from parameter server. Defaulting to %f s", timeout_);
167  }
168 
169  if (!(nh_priv_.getParam("total_weight", total_weight_)))
170  {
171  ROS_WARN("Failed to retrieve total_weight from parameter server. Defaulting to %f lbs", total_weight_);
172  }
173 
174  if (!(nh_priv_.getParam("drive_type", drive_type_)))
175  {
176  ROS_WARN("Failed to retrieve drive_type from parameter.Defaulting to %s", drive_type_.c_str());
177  }
178 
179  if (!(nh_priv_.getParam("Kp", pidGains_.Kp)))
180  {
181  ROS_WARN("Failed to retrieve Kp from parameter.Defaulting to %f", pidGains_.Kp);
182  }
183  else{
184  ROS_INFO("Kp: %f", pidGains_.Kp);
185  }
186 
187  if (!(nh_priv_.getParam("Ki", pidGains_.Ki)))
188  {
189  ROS_WARN("Failed to retrieve Ki from parameter.Defaulting to %f", pidGains_.Ki);
190  }
191  else{
192  ROS_INFO("Ki: %f", pidGains_.Ki);
193  }
194 
195  if (!(nh_priv_.getParam("Kd", pidGains_.Kd)))
196  {
197  ROS_WARN("Failed to retrieve Kd from parameter.Defaulting to %f", pidGains_.Kd);
198  }
199  else{
200  ROS_INFO("Kd: %f", pidGains_.Kd);
201  }
202 
203  if (!(nh_priv_.getParam("left_motor_pid_csv", l_pid_csv_file_)))
204  {
205  ROS_INFO("Not logging left motor PID");
206  }
207  else{
208  ROS_INFO("Recording left motor PID data to %s", l_pid_csv_file_.c_str());
209  }
210 
211  if (!(nh_priv_.getParam("right_motor_pid_csv", r_pid_csv_file_)))
212  {
213  ROS_INFO("Not logging right motor PID");
214  }
215  else{
216  ROS_INFO("Recording right motor PID data to %s", r_pid_csv_file_.c_str());
217  }
218 
219  if (!l_pid_csv_file_.empty()){
220  l_fs_.open(l_pid_csv_file_, std::ofstream::out);
221  if(!l_fs_.is_open()){
222  ROS_WARN("Could not open file: %s", l_pid_csv_file_.c_str());
223  }
224  }
225 
226  if (!r_pid_csv_file_.empty()){
227  r_fs_.open(r_pid_csv_file_, std::ofstream::out);
228  if(!r_fs_.is_open()){
229  ROS_WARN("Could not open file: %s", r_pid_csv_file_.c_str());
230  }
231  }
232 
233  if (drive_type_ == (std::string) "2wd")
234  {
235  ROS_INFO("2wd parameters loaded.");
240 
243 
246  }
247  else if (drive_type_ == (std::string) "4wd")
248  {
249  ROS_INFO("4wd parameters loaded.");
254 
257 
260  }
261  else if (drive_type_ == (std::string) "flippers")
262  {
263  ROS_INFO("flipper parameters loaded.");
268 
271 
274  }
275  else
276  {
277  ROS_WARN("Unclear ROS param drive_type. Defaulting to flippers params.");
282 
285 
288  }
289 
290  if (!(nh_priv_.getParam("traction_factor", odom_traction_factor_)))
291  {
292  ROS_WARN("Failed to retrieve traction_factor from parameter. Defaulting to %f", odom_traction_factor_);
293  odom_traction_factor_ = 0.61;
294  }
295 
296  if (!(nh_priv_.getParam("odom_covariance_0", odom_covariance_0_)))
297  {
298  ROS_WARN("Failed to retrieve odom_covariance_0 from parameter. Defaulting to 0.01");
299  odom_covariance_0_ = 0.01;
300  }
301 
302  if (!(nh_priv_.getParam("odom_covariance_35", odom_covariance_35_)))
303  {
304  ROS_WARN("Failed to retrieve odom_covariance_35 from parameter. Defaulting to 0.03");
305  odom_covariance_35_ = 0.03;
306  }
307 
308  ROS_INFO("Openrover parameters loaded:");
309  ROS_INFO("port: %s", port_.c_str());
310  ROS_INFO("drive_type: %s", drive_type_.c_str());
311  ROS_INFO("timeout: %f s", timeout_);
312  ROS_INFO("closed_loop_control_on: %i", closed_loop_control_on_);
313  ROS_INFO("total_weight: %f kg", total_weight_);
314  ROS_INFO("traction_factor: %f", odom_traction_factor_);
315  ROS_INFO("odom_covariance_0: %f", odom_covariance_0_);
316  ROS_INFO("odom_covariance_35: %f", odom_covariance_35_);
317  ROS_INFO("fast_data_rate: %f hz", fast_rate_hz_);
318  ROS_INFO("medium_data_rate: %f hz", medium_rate_hz_);
319  ROS_INFO("slow_data_rate: %f hz", slow_rate_hz_);
320 
321  return true;
322 }
323 
325 {
327  {
328  for (int i = 0; i < SLOW_SIZE; i++)
329  {
331  }
333  }
334  return;
335 }
336 
338 {
340  {
341  for (int i = 0; i < MEDIUM_SIZE; i++)
342  {
344  }
346  }
347  return;
348 }
349 
351 {
353  {
354  for (int i = 0; i < FAST_SIZE; i++)
355  {
356  // Fill buffer with all the param2's defined as fast data
357  // by the ROBOT_DATA_INDEX_FAST array
359  }
361  }
362  else
363  {
364  ROS_WARN_DELAYED_THROTTLE(5, "Fast data rate too high. Consider reducing fast_data_rate param");
365  }
366  return;
367 }
368 
370 { // Timer goes off when a command isn't received soon enough. Sets motors to neutral values
374  left_vel_commanded_ = 0.0;
375  right_vel_commanded_ = 0.0;
376 
377  return;
378 }
379 
380 void OpenRover::fanSpeedCB(const std_msgs::Int32::ConstPtr& msg)
381 {
382  if (is_serial_coms_open_ && (serial_fan_buffer_.size() == 0))
383  {
384  serial_fan_buffer_.push_back(msg->data);
385  }
386  // ROS_DEBUG("Fan Buffer size is %i, new data is %i", serial_fan_buffer_.size(), msg->data);
387  return;
388 }
389 
390 void OpenRover::cmdVelCB(const geometry_msgs::Twist::ConstPtr& msg)
391 { // converts from cmd_vel (m/s and radians/s) into motor speed commands
392  cmd_vel_commanded_ = *msg;
393  float left_motor_speed, right_motor_speed;
394  int flipper_motor_speed;
395  int motor_speed_deadband_scaled;
396  double turn_rate = msg->angular.z;
397  double linear_rate = msg->linear.x;
398  double flipper_rate = msg->angular.y;
399  static bool prev_e_stop_state_ = false;
400 
401  double diff_vel_commanded = turn_rate / odom_angular_coef_ / odom_traction_factor_;
402 
403  right_vel_commanded_ = linear_rate + 0.5 * diff_vel_commanded;
404  left_vel_commanded_ = linear_rate - 0.5 * diff_vel_commanded;
405 
407 
408  if (e_stop_on_)
409  {
410  if (!prev_e_stop_state_)
411  {
412  prev_e_stop_state_ = true;
413  ROS_WARN("Openrover driver - Soft e-stop on.");
414  }
418  return;
419  }
420  else
421  {
422  if (prev_e_stop_state_)
423  {
424  prev_e_stop_state_ = false;
425  ROS_INFO("Openrover driver - Soft e-stop off.");
426  }
427  }
428 
429  flipper_motor_speed = ((int)round(flipper_rate * motor_speed_flipper_coef_) + 125) % 250;
430 
431  motor_speeds_commanded_[FLIPPER_MOTOR_INDEX_] = (unsigned char)flipper_motor_speed;
432 
434  return;
435 }
436 
437 void OpenRover::eStopCB(const std_msgs::Bool::ConstPtr& msg)
438 {
439  static bool prev_e_stop_state_ = false;
440 
441  // e-stop only trigger on the rising edge of the signal and only deactivates when reset
442  if(msg->data && !prev_e_stop_state_)
443  {
444  e_stop_on_ = true;
445  }
446 
447  prev_e_stop_state_ = msg->data;
448  return;
449 }
450 
451 void OpenRover::eStopResetCB(const std_msgs::Bool::ConstPtr& msg)
452 {
453  if(msg->data)
454  {
455  e_stop_on_ = false;
456  }
457  return;
458 }
459 
460 void OpenRover::publishOdometry(float left_vel, float right_vel)
461 { // convert encoder readings to real world values and publish as Odometry
462  static double left_dist = 0;
463  static double right_dist = 0;
464  static double pos_x = 0;
465  static double pos_y = 0;
466  static double theta = 0;
467  static double past_time = 0;
468  double net_vel = 0;
469  double diff_vel = 0;
470  double alpha = 0;
471  double dt = 0;
472  tf2::Quaternion q_new;
473 
474  ros::Time ros_now_time = ros::Time::now();
475  double now_time = ros_now_time.toSec();
476 
477  nav_msgs::Odometry odom_msg;
478 
479  dt = now_time - past_time;
480  past_time = now_time;
481 
482  if (past_time != 0)
483  {
484  left_dist += left_vel * dt;
485  right_dist += right_vel * dt;
486 
487  net_vel = 0.5 * (left_vel + right_vel);
488  diff_vel = right_vel - left_vel;
489 
490  alpha = odom_angular_coef_ * diff_vel * odom_traction_factor_;
491 
492  pos_x = pos_x + net_vel * cos(theta) * dt;
493  pos_y = pos_y + net_vel * sin(theta) * dt;
494  theta = (theta + alpha * dt);
495 
496  q_new.setRPY(0, 0, theta);
497  tf2::convert(q_new, odom_msg.pose.pose.orientation);
498  }
499 
500  odom_msg.header.stamp = ros_now_time;
501  odom_msg.header.frame_id = "odom";
502  odom_msg.child_frame_id = "base_link";
503 
504  odom_msg.twist.twist.linear.x = net_vel;
505  odom_msg.twist.twist.angular.z = alpha;
506 
507  // If not moving, trust the encoders completely
508  // otherwise set them to the ROS param
509  if (net_vel == 0 && alpha == 0)
510  {
511  odom_msg.twist.covariance[0] = odom_covariance_0_ / 1e3;
512  odom_msg.twist.covariance[7] = odom_covariance_0_ / 1e3;
513  odom_msg.twist.covariance[35] = odom_covariance_35_ / 1e6;
514  }
515  else
516  {
517  odom_msg.twist.covariance[0] = odom_covariance_0_;
518  odom_msg.twist.covariance[7] = odom_covariance_0_;
519  odom_msg.twist.covariance[35] = odom_covariance_35_;
520  }
521 
522  odom_msg.pose.pose.position.x = pos_x;
523  odom_msg.pose.pose.position.y = pos_y;
524 
525  odom_enc_pub.publish(odom_msg);
526  return;
527 }
528 
530 { // Update to publish from OdomControl
531  static ros::Time ros_start_time = ros::Time::now();
532  ros::Time ros_now_time = ros::Time::now();
533  double run_time = (ros_now_time - ros_start_time).toSec();
534  std_msgs::Float32MultiArray vel_vec;
535 
536  vel_vec.data.push_back(left_vel_filtered_);
537  vel_vec.data.push_back(left_vel_measured_);
538  vel_vec.data.push_back(left_vel_commanded_);
539  vel_vec.data.push_back(right_vel_filtered_);
540  vel_vec.data.push_back(right_vel_measured_);
541  vel_vec.data.push_back(right_vel_commanded_);
542 
543  vel_vec.data.push_back(motor_speeds_commanded_[LEFT_MOTOR_INDEX_]);
544  vel_vec.data.push_back(motor_speeds_commanded_[RIGHT_MOTOR_INDEX_]);
545 
546  vel_calc_pub.publish(vel_vec);
547 
548  return;
549 }
550 
552 {
553  rr_openrover_driver_msgs::RawRrOpenroverDriverFastRateData msg;
554 
555  msg.header.stamp = ros::Time::now();
556  msg.header.frame_id = "";
557 
558  msg.left_motor = robot_data_[i_ENCODER_INTERVAL_MOTOR_LEFT];
559  msg.right_motor = robot_data_[i_ENCODER_INTERVAL_MOTOR_RIGHT];
560  msg.flipper_motor = robot_data_[i_ENCODER_INTERVAL_MOTOR_FLIPPER];
561  if(use_legacy_) {
562  fast_rate_pub.publish(msg);
563  }
565  return;
566 }
567 
569 {
570  rr_openrover_driver_msgs::RawRrOpenroverDriverMedRateData med_msg;
571  std_msgs::Bool is_charging_msg;
572 
573  med_msg.header.stamp = ros::Time::now();
574  med_msg.header.frame_id = "";
575 
576  med_msg.reg_pwr_total_current = robot_data_[i_REG_PWR_TOTAL_CURRENT];
577  med_msg.reg_motor_fb_rpm_left = robot_data_[i_REG_MOTOR_FB_RPM_LEFT];
578  med_msg.reg_motor_fb_rpm_right = robot_data_[i_REG_MOTOR_FB_RPM_RIGHT];
579  med_msg.reg_flipper_fb_position_pot1 = robot_data_[i_REG_FLIPPER_FB_POSITION_POT1];
580  med_msg.reg_flipper_fb_position_pot2 = robot_data_[i_REG_FLIPPER_FB_POSITION_POT2];
581  med_msg.reg_motor_fb_current_left = robot_data_[i_REG_MOTOR_FB_CURRENT_LEFT];
582  med_msg.reg_motor_fb_current_right = robot_data_[i_REG_MOTOR_FB_CURRENT_RIGHT];
583  med_msg.reg_motor_charger_state = robot_data_[i_REG_MOTOR_CHARGER_STATE];
584  med_msg.reg_power_a_current = robot_data_[i_REG_POWER_A_CURRENT];
585  med_msg.reg_power_b_current = robot_data_[i_REG_POWER_B_CURRENT];
586  med_msg.reg_motor_flipper_angle = robot_data_[i_REG_MOTOR_FLIPPER_ANGLE];
587  med_msg.battery_current_a = robot_data_[i_BATTERY_CURRENT_A];
588  med_msg.battery_current_b = robot_data_[i_BATTERY_CURRENT_B];
589 
590  if (robot_data_[i_REG_MOTOR_CHARGER_STATE] == 0xDADA)
591  {
592  is_charging_ = true;
593  is_charging_msg.data = true;
594  is_charging_pub.publish(is_charging_msg);
595  }
596  else
597  {
598  is_charging_ = false;
599  is_charging_msg.data = false;
600  is_charging_pub.publish(is_charging_msg);
601  }
602 
603  if(use_legacy_) {
604  medium_rate_pub.publish(med_msg);
605  }
606  publish_med_rate_values_ = false;
607  return;
608 }
609 
610 rr_openrover_driver_msgs::SmartBatteryStatus interpret_battery_status(uint16_t bits)
611 {
612  rr_openrover_driver_msgs::SmartBatteryStatus status_msg;
613  status_msg.over_charged_alarm = bool(bits & 0x8000);
614  status_msg.terminate_charge_alarm = bool(bits & 0x4000);
615  status_msg.over_temp_alarm = bool(bits & 0x1000);
616  status_msg.terminate_discharge_alarm = bool(bits & 0x0800);
617  status_msg.remaining_capacity_alarm = bool(bits & 0x0200);
618  status_msg.remaining_time_alarm = bool(bits & 0x0100);
619  status_msg.initialized = bool(bits & 0x0080);
620  status_msg.discharging = bool(bits & 0x0040);
621  status_msg.fully_charged = bool(bits & 0x0020);
622  status_msg.fully_discharged = bool(bits & 0x0010);
623  return status_msg;
624 }
625 
627 {
628  rr_openrover_driver_msgs::RawRrOpenroverDriverSlowRateData slow_msg;
629  rr_openrover_driver_msgs::SmartBatteryStatus batteryStatusA;
630 
631  slow_msg.header.stamp = ros::Time::now();
632  slow_msg.header.frame_id = "";
633 
634  slow_msg.reg_motor_fault_flag_left = robot_data_[i_REG_MOTOR_FAULT_FLAG_LEFT];
635  slow_msg.reg_motor_temp_left = robot_data_[i_REG_MOTOR_TEMP_LEFT];
636  slow_msg.reg_motor_temp_right = robot_data_[i_REG_MOTOR_TEMP_RIGHT];
637  slow_msg.reg_power_bat_voltage_a = robot_data_[i_REG_POWER_BAT_VOLTAGE_A];
638  slow_msg.reg_power_bat_voltage_b = robot_data_[i_REG_POWER_BAT_VOLTAGE_B];
639  slow_msg.reg_robot_rel_soc_a = robot_data_[i_REG_ROBOT_REL_SOC_A];
640  slow_msg.reg_robot_rel_soc_b = robot_data_[i_REG_ROBOT_REL_SOC_B];
641  slow_msg.battery_mode_a = robot_data_[i_BATTERY_MODE_A];
642  slow_msg.battery_mode_b = robot_data_[i_BATTERY_MODE_B];
643  slow_msg.battery_temp_a = robot_data_[i_BATTERY_TEMP_A];
644  slow_msg.battery_temp_b = robot_data_[i_BATTERY_TEMP_B];
645  slow_msg.battery_voltage_a = robot_data_[i_BATTERY_VOLTAGE_A];
646  slow_msg.battery_voltage_b = robot_data_[i_BATTERY_VOLTAGE_B];
647  slow_msg.buildno = robot_data_[i_BUILDNO];
648 
651 
652  std_msgs::Int32 soc;
655 
656  if(use_legacy_) {
657  slow_rate_pub.publish(slow_msg);
658  }
660  return;
661 }
662 
664 {
665  std_msgs::Int32MultiArray motor_speeds_msg;
666  motor_speeds_msg.data.clear();
667  motor_speeds_msg.data.push_back(motor_speeds_commanded_[LEFT_MOTOR_INDEX_]);
668  motor_speeds_msg.data.push_back(motor_speeds_commanded_[RIGHT_MOTOR_INDEX_]);
669  motor_speeds_msg.data.push_back(motor_speeds_commanded_[FLIPPER_MOTOR_INDEX_]);
670 
671  motor_speeds_pub.publish(motor_speeds_msg);
672  return;
673 }
674 
676 { // sends serial commands stored in the 3 buffers in order of speed with fast getting highest priority
677  unsigned char param1;
678  unsigned char param2;
679  static double past_time = 0;
680 
681  while ((serial_fast_buffer_.size() > 0) || (serial_medium_buffer_.size() > 0) || (serial_slow_buffer_.size() > 0) ||
682  (serial_fan_buffer_.size() > 0))
683  {
684  // Fast data gets highest priority from being first in this if statement
685  // If the CPU running the driver can only process 60 commands / second and the fast
686  // data rate is set to 60hz, no other data will be gathered and the medium and slow Buffers
687  // will fill up and issue a warning.
688  if (serial_fast_buffer_.size() > 0)
689  {
690  param1 = 10;
691  param2 = serial_fast_buffer_.back();
692  serial_fast_buffer_.pop_back();
693  ROS_DEBUG("Its fast data's turn to be sent: %i", param2);
694  }
695  else if (serial_fan_buffer_.size() > 0)
696  {
697  param1 = 20;
698  param2 = serial_fan_buffer_.back();
699  serial_fan_buffer_.pop_back();
700  ROS_DEBUG("Its fan speed's turn to be sent: %i", param2);
701  }
702  else if (serial_medium_buffer_.size() > 0)
703  {
704  param1 = 10;
705  param2 = serial_medium_buffer_.back();
706  serial_medium_buffer_.pop_back();
707  ROS_DEBUG("Its medium data's turn to be sent: %i", param2);
708  }
709  else if (serial_slow_buffer_.size() > 0)
710  {
711  param1 = 10;
712  param2 = serial_slow_buffer_.back();
713  serial_slow_buffer_.pop_back();
714  ROS_DEBUG("Its slow data's turn to be sent: %i", param2);
715  }
716  else
717  {
718  param2 = 0;
719  param1 = 0;
720  }
721 
722  // Check param1 to determine what communication to the robot is required
723  try
724  {
725  if (param1 == 10) // Param1==10 requests data with index of param2
726  {
727  updateRobotData(param2);
728  }
729  else if (param1 == 20)
730  { // param1==20 means sending fan speed of param2
731  setParameterData(param1, param2);
732  }
733  else if (param1 == 250)
734  { // param1==250 means calibrating flipper DO NOT USE OFTEN
735  setParameterData(param1, param2);
736  }
737  else if (param1 == 0)
738  { // param1==0 means buffers are empty and doesn't need to do anything
739  }
740  else
741  {
742  throw std::string("Unknown param1. Removing parameter from buffer");
743  }
744  }
745  catch (std::string s)
746  {
747  throw;
748  }
749  catch (...)
750  {
751  throw;
752  }
753 
754  // If one of the buffers are empty, publish the values
755  if ((serial_fast_buffer_.size() == 0) && publish_fast_rate_values_)
756  {
757  ros::Time ros_now_time = ros::Time::now();
758  double now_time = ros_now_time.toSec();
759 
760  double dt = now_time - past_time;
761  past_time = now_time;
763  updateMeasuredVelocities(); // Update openrover measured velocities based on latest encoder readings
764 
769 
772 
773  publishOdometry(left_vel_measured_, right_vel_measured_); // Publish new calculated odometry
774  publishWheelVels(); // call after publishOdometry()
776  }
777  else if ((serial_medium_buffer_.size() == 0) && publish_med_rate_values_)
778  {
780  }
781  else if ((serial_slow_buffer_.size() == 0) && publish_slow_rate_values_)
782  {
784  }
785 
786  // Checks timers and subscribers
787  ros::spinOnce();
788  }
789 
790  if ((serial_fast_buffer_.size() == 0) && publish_fast_rate_values_)
791  {
793  }
794 
795  if ((serial_medium_buffer_.size() == 0) && publish_med_rate_values_)
796  {
797  publish_med_rate_values_ = false;
798  }
799 
800  if ((serial_slow_buffer_.size() == 0) && publish_slow_rate_values_)
801  {
803  }
804 
805  return;
806 }
807 
809 {
812 
813  // Bound left_encoder readings to range of normal operation.
814  if (left_enc < ENCODER_MIN)
815  {
816  left_vel_measured_ = 0;
817  }
818  else if (left_enc > ENCODER_MAX)
819  {
820  left_vel_measured_ = 0;
821  }
822  else if (motor_speeds_commanded_[LEFT_MOTOR_INDEX_] > MOTOR_NEUTRAL) // this sets direction of measured
823  {
825  }
826  else
827  {
829  }
830 
831  // Bound right_encoder readings to range of normal operation.
832  if (right_enc < ENCODER_MIN)
833  {
835  }
836  else if (right_enc > ENCODER_MAX)
837  {
839  }
840  else if (motor_speeds_commanded_[RIGHT_MOTOR_INDEX_] > MOTOR_NEUTRAL) // this sets direction of measured
841  {
843  }
844  else
845  {
847  }
848 
849  return;
850 }
851 
853 {
854  try
855  {
856  int data = getParameterData(param);
857  if (data < 0) // check if val is good (not negative) and if not, push param back to buffer
858  {
859  throw;
860  }
861 
862  robot_data_[param] = data;
863  }
864  catch (std::string s)
865  {
866  char str_ex[50];
867  sprintf(str_ex, "Failed to update param %i. ", param);
868  throw std::string(str_ex) + s;
869  }
870  return;
871 }
872 
873 bool OpenRover::sendCommand(int param1, int param2)
874 {
875  unsigned char write_buffer[SERIAL_OUT_PACKAGE_LENGTH];
876 
877  write_buffer[0] = SERIAL_START_BYTE;
878  write_buffer[1] = (unsigned char)motor_speeds_commanded_[LEFT_MOTOR_INDEX_]; // left motor
879  write_buffer[2] = (unsigned char)motor_speeds_commanded_[RIGHT_MOTOR_INDEX_]; // right motor
880  write_buffer[3] = (unsigned char)motor_speeds_commanded_[FLIPPER_MOTOR_INDEX_]; // flipper
881  write_buffer[4] = (unsigned char)param1; // Param 1: 10 to get data, 240 for low speed mode
882  write_buffer[5] = (unsigned char)param2; // Param 2:
883  // Calculate Checksum
884  write_buffer[6] =
885  (char)255 - (write_buffer[1] + write_buffer[2] + write_buffer[3] + write_buffer[4] + write_buffer[5]) % 255;
886 
887  if (!(serial_port_fd_ > 0))
888  {
889  ROS_INFO("Serial communication failed. Attempting to restart.");
890  if (!(openComs()))
891  {
892  ROS_WARN("Failed to restart serial communication.");
893  }
894  }
895 
897  {
898  char str_ex[50];
899  sprintf(str_ex, "Failed to send command: %02x,%02x,%02x,%02x,%02x,%02x,%02x", write_buffer[0], write_buffer[1],
900  write_buffer[2], write_buffer[3], write_buffer[4], write_buffer[5], write_buffer[6]);
901  throw std::string(str_ex);
902  }
903 
904  return true;
905 }
906 
908 { // only used after a send command with param1==10
909  unsigned char read_buffer[1];
910  unsigned char start_byte_read, checksum, read_checksum, data1, data2, dataNO;
911  int data;
912  if (!(serial_port_fd_ > 0))
913  {
914  ROS_INFO("Serial communication failed. Attempting to restart.");
915  if (!(openComs()))
916  {
917  ROS_WARN("Failed to restart serial communication.");
918  }
919  }
920 
921  int bits_read = read(serial_port_fd_, read_buffer, 1);
922  start_byte_read = read_buffer[0];
923 
924  read(serial_port_fd_, read_buffer, 1); // get param
925  dataNO = read_buffer[0];
926 
927  read(serial_port_fd_, read_buffer, 1); // get data1
928  data1 = read_buffer[0];
929 
930  read(serial_port_fd_, read_buffer, 1); // get data2
931  data2 = read_buffer[0];
932 
933  read(serial_port_fd_, read_buffer, 1); // get checksum
934  read_checksum = read_buffer[0];
935 
936  checksum = 255 - (dataNO + data1 + data2) % 255;
937 
938  if (!(SERIAL_START_BYTE == start_byte_read))
939  {
940  char str_ex[50];
941  sprintf(str_ex, "Received bad start byte. Received: %02x", start_byte_read);
942  tcflush(serial_port_fd_, TCIOFLUSH); // flush received buffer
943  throw std::string(str_ex);
944  }
945  else if (checksum != read_checksum)
946  {
947  char str_ex[50];
948  sprintf(str_ex, "Received bad CRC. Received: %02x,%02x,%02x,%02x,%02x", start_byte_read, dataNO, data1, data2,
949  read_checksum);
950  tcflush(serial_port_fd_, TCIOFLUSH); // flush received buffer
951  throw std::string(str_ex);
952  }
953  data = (data1 << 8) + data2;
954  return data;
955 }
956 
957 bool OpenRover::setParameterData(int param1, int param2)
958 {
959  try
960  {
961  if (!sendCommand(param1, param2))
962  {
963  throw;
964  }
965 
966  return true;
967  }
968  catch (std::string s)
969  {
970  std::string s2("setParameterData() failed. ");
971  throw(s2 + s);
972  }
973 }
974 
976 {
977  int data;
978 
979  try
980  {
981  if (!sendCommand(10, param))
982  {
983  throw;
984  }
985 
986  data = readCommand();
987 
988  if (0 > data)
989  {
990  throw;
991  }
992  return data;
993  }
994  catch (std::string s)
995  {
996  std::string s2("getParameterData() failed. "); // %i. ", param);
997  throw(s2 + s);
998  }
999 }
1000 
1002 {
1003  ROS_INFO("Opening serial port");
1004  struct termios serial_port_fd__options;
1005 
1006  serial_port_fd_ = ::open(port_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
1007  if (serial_port_fd_ < 0)
1008  {
1009  ROS_ERROR("Failed to open port: %s", strerror(errno));
1010  return false;
1011  }
1012  if (0 > fcntl(serial_port_fd_, F_SETFL, 0))
1013  {
1014  ROS_ERROR("Failed to set port descriptor: %s", strerror(errno));
1015  return false;
1016  }
1017  if (0 > tcgetattr(serial_port_fd_, &serial_port_fd__options))
1018  {
1019  ROS_ERROR("Failed to fetch port attributes: %s", strerror(errno));
1020  return false;
1021  }
1022  if (0 > cfsetispeed(&serial_port_fd__options, B57600))
1023  {
1024  ROS_ERROR("Failed to set input baud: %s", strerror(errno));
1025  return false;
1026  }
1027  if (0 > cfsetospeed(&serial_port_fd__options, B57600))
1028  {
1029  ROS_ERROR("Failed to set output baud: %s", strerror(errno));
1030  return false;
1031  }
1032 
1033  serial_port_fd__options.c_cflag |= (CREAD | CLOCAL | CS8);
1034  serial_port_fd__options.c_cflag &= ~(PARODD | CRTSCTS | CSTOPB | PARENB);
1035  serial_port_fd__options.c_iflag &= ~(IUCLC | IXANY | IMAXBEL | IXON | IXOFF | IUTF8 | ICRNL | INPCK); // input modes
1036  serial_port_fd__options.c_oflag |= (NL0 | CR0 | TAB0 | BS0 | VT0 | FF0);
1037  serial_port_fd__options.c_oflag &=
1038  ~(OPOST | ONLCR | OLCUC | OCRNL | ONOCR | ONLRET | OFILL | OFDEL | NL1 | CR1 | CR2 | TAB3 | BS1 | VT1 | FF1);
1039  serial_port_fd__options.c_lflag |= (NOFLSH);
1040  serial_port_fd__options.c_lflag &= ~(ICANON | IEXTEN | TOSTOP | ISIG | ECHOPRT | ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE);
1041  serial_port_fd__options.c_cc[VINTR] = 0x03; // INTR Character
1042  serial_port_fd__options.c_cc[VQUIT] = 0x1C; // QUIT Character
1043  serial_port_fd__options.c_cc[VERASE] = 0x7F; // ERASE Character
1044  serial_port_fd__options.c_cc[VKILL] = 0x15; // KILL Character
1045  serial_port_fd__options.c_cc[VEOF] = 0x04; // EOF Character
1046  serial_port_fd__options.c_cc[VTIME] = 0x01; // Timeout in 0.1s of serial read
1047  serial_port_fd__options.c_cc[VMIN] = 0; // SERIAL_IN_PACKAGE_LENGTH; //Min Number of bytes to read
1048  serial_port_fd__options.c_cc[VSWTC] = 0x00;
1049  serial_port_fd__options.c_cc[VSTART] = SERIAL_START_BYTE; // START Character
1050  serial_port_fd__options.c_cc[VSTOP] = 0x13; // STOP character
1051  serial_port_fd__options.c_cc[VSUSP] = 0x1A; // SUSP character
1052  serial_port_fd__options.c_cc[VEOL] = 0x00; // EOL Character
1053  serial_port_fd__options.c_cc[VREPRINT] = 0x12;
1054  serial_port_fd__options.c_cc[VDISCARD] = 0x0F;
1055  serial_port_fd__options.c_cc[VWERASE] = 0x17;
1056  serial_port_fd__options.c_cc[VLNEXT] = 0x16;
1057  serial_port_fd__options.c_cc[VEOL2] = 0x00;
1058 
1059  if (0 > tcsetattr(serial_port_fd_, TCSANOW, &serial_port_fd__options))
1060  {
1061  ROS_ERROR("Failed to set port attributes: %s", strerror(errno));
1062  return false;
1063  }
1064  ::ioctl(serial_port_fd_, TIOCEXCL); // turn on exclusive mode
1065 
1066  ROS_INFO("Serial port opened");
1067  is_serial_coms_open_ = true;
1068  tcflush(serial_port_fd_, TCIOFLUSH); // flush received buffer
1069 
1070  return true;
1071 }
1072 
1073 } // namespace openrover
1074 
1075 int main(int argc, char* argv[])
1076 {
1077  // Create ROS node
1078  ros::init(argc, argv, "rr_openrover_driver_node");
1079 
1080  ros::NodeHandle nh("");
1081  ros::NodeHandle nh_priv("~");
1082  openrover::OpenRover openrover(nh, nh_priv);
1083  /* if( !nh )
1084  {
1085  ROS_FATAL( "Failed to initialize NodeHandle" );
1086  ros::shutdown( );
1087  return -1;
1088  }
1089  if( !nh_priv )
1090  {
1091  ROS_FATAL( "Failed to initialize private NodeHandle" );
1092  delete nh;
1093  ros::shutdown( );
1094  return -2;
1095  }
1096  if( !openrover )
1097  {
1098  ROS_FATAL( "Failed to initialize driver" );
1099  delete nh_priv;
1100  delete nh;
1101  ros::shutdown( );
1102  return -3;
1103  }
1104  */
1105  if (!openrover.start())
1106  {
1107  ROS_FATAL("Failed to start the openrover driver");
1109  }
1110 
1111  ros::Rate loop_rate(openrover::LOOP_RATE);
1112 
1113  while (ros::ok())
1114  {
1115  try
1116  {
1117  ros::spinOnce();
1118  // Process Serial Buffers
1119  openrover.serialManager();
1120  loop_rate.sleep(); // sleeping greatly reduces CPU
1121  }
1122  catch (std::string s)
1123  {
1124  ROS_ERROR("%s", s.c_str());
1125  }
1126  catch (...)
1127  {
1128  ROS_ERROR("Unknown Exception occurred");
1129  }
1130  }
1131  /*
1132  delete openrover;
1133  delete nh_priv;
1134  delete nh;
1135  */
1136 
1137  return 0;
1138 }
void robotDataMediumCB(const ros::WallTimerEvent &e)
void robotDataSlowCB(const ros::WallTimerEvent &e)
msg
const int i_REG_MOTOR_FB_CURRENT_LEFT
Definition: constants.hpp:69
const int i_BATTERY_VOLTAGE_B
Definition: constants.hpp:101
int main(int argc, char *argv[])
const int i_REG_MOTOR_FLIPPER_ANGLE
Definition: constants.hpp:88
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
std::vector< unsigned char > serial_fast_buffer_
Definition: openrover.hpp:133
void robotDataFastCB(const ros::WallTimerEvent &e)
const int MOTOR_SPEED_ANGULAR_COEF_4WD_HS
Definition: constants.hpp:28
#define ROS_FATAL(...)
const int MOTOR_SPEED_LINEAR_COEF_F_HS
Definition: constants.hpp:17
std::vector< unsigned char > serial_slow_buffer_
Definition: openrover.hpp:135
const int LOOP_RATE
Definition: constants.hpp:8
const float ODOM_AXLE_TRACK_F
Definition: constants.hpp:13
void eStopCB(const std_msgs::Bool::ConstPtr &msg)
const int i_REG_MOTOR_TEMP_RIGHT
Definition: constants.hpp:73
void publish(const boost::shared_ptr< M > &message) const
const int i_REG_POWER_BAT_VOLTAGE_A
Definition: constants.hpp:75
ros::NodeHandle & nh_priv_
Definition: openrover.hpp:67
const int i_ENCODER_INTERVAL_MOTOR_RIGHT
Definition: constants.hpp:78
OdomControl left_controller_
Definition: openrover.hpp:29
const int MEDIUM_SIZE
Definition: constants.hpp:129
const int i_REG_ROBOT_REL_SOC_A
Definition: constants.hpp:81
const float ODOM_AXLE_TRACK_2WD
Definition: constants.hpp:34
const int LEFT_MOTOR_INDEX_
Definition: openrover.hpp:99
std::string r_pid_csv_file_
Definition: openrover.hpp:55
ros::Publisher is_charging_pub
Definition: openrover.hpp:78
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
const int i_REG_MOTOR_CHARGER_STATE
Definition: constants.hpp:83
const int i_BATTERY_VOLTAGE_A
Definition: constants.hpp:100
void eStopResetCB(const std_msgs::Bool::ConstPtr &msg)
const float ODOM_ENCODER_COEF_2WD
Definition: constants.hpp:33
ros::Publisher battery_status_a_pub
Definition: openrover.hpp:85
const int i_BATTERY_MODE_A
Definition: constants.hpp:95
XmlRpcServer s
int motor_speeds_commanded_[3]
Definition: openrover.hpp:98
const float ODOM_ENCODER_COEF_F
Definition: constants.hpp:12
std::string drive_type_
Definition: openrover.hpp:61
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const int ENCODER_MIN
Definition: constants.hpp:50
ros::WallTimer medium_timer
Definition: openrover.hpp:71
const int i_ENCODER_INTERVAL_MOTOR_FLIPPER
Definition: constants.hpp:79
data
#define ROS_WARN_DELAYED_THROTTLE(rate,...)
ros::Publisher slow_rate_pub
Definition: openrover.hpp:84
void updateRobotData(int parameter)
bool publish_fast_rate_values_
Definition: openrover.hpp:46
const int MOTOR_SPEED_ANGULAR_COEF_F_HS
Definition: constants.hpp:18
const float ODOM_TRACTION_FACTOR_2WD
Definition: constants.hpp:36
const int i_REG_FLIPPER_FB_POSITION_POT2
Definition: constants.hpp:67
const int ROBOT_DATA_INDEX_FAST[]
Definition: constants.hpp:105
#define ROS_WARN(...)
bool publish_slow_rate_values_
Definition: openrover.hpp:48
const float ODOM_ANGULAR_COEF_4WD
Definition: constants.hpp:24
const int i_REG_MOTOR_FAULT_FLAG_LEFT
Definition: constants.hpp:71
const int RIGHT_MOTOR_INDEX_
Definition: openrover.hpp:100
const int ENCODER_MAX
Definition: constants.hpp:49
OdomControl right_controller_
Definition: openrover.hpp:30
ros::Publisher battery_status_b_pub
Definition: openrover.hpp:85
void timeoutCB(const ros::WallTimerEvent &e)
const int i_BATTERY_CURRENT_B
Definition: constants.hpp:103
const int i_REG_FLIPPER_FB_POSITION_POT1
Definition: constants.hpp:66
std::string l_pid_csv_file_
Definition: openrover.hpp:54
const int MOTOR_SPEED_MAX
Definition: constants.hpp:55
const int SERIAL_OUT_PACKAGE_LENGTH
Definition: constants.hpp:6
const float ODOM_AXLE_TRACK_4WD
Definition: constants.hpp:23
const int MOTOR_FLIPPER_COEF
Definition: constants.hpp:51
ros::Subscriber fan_speed_sub
Definition: openrover.hpp:89
std::ofstream l_fs_
Definition: openrover.hpp:56
ros::Publisher battery_state_of_charge_pub
Definition: openrover.hpp:86
const int FAST_SIZE
Definition: constants.hpp:128
ros::Publisher fast_rate_pub
Definition: openrover.hpp:82
const int i_BATTERY_MODE_B
Definition: constants.hpp:96
void fanSpeedCB(const std_msgs::Int32::ConstPtr &msg)
std::vector< unsigned char > serial_fan_buffer_
Definition: openrover.hpp:136
const int i_REG_POWER_B_CURRENT
Definition: constants.hpp:87
#define ROS_INFO(...)
const int i_BATTERY_TEMP_A
Definition: constants.hpp:97
const int i_REG_ROBOT_REL_SOC_B
Definition: constants.hpp:82
void start(bool use_control, PidGains pid_gains, int max, int min)
const int MOTOR_NEUTRAL
Definition: constants.hpp:54
const int i_BATTERY_TEMP_B
Definition: constants.hpp:98
ROSCPP_DECL bool ok()
const int i_ENCODER_INTERVAL_MOTOR_LEFT
Definition: constants.hpp:77
const int MOTOR_SPEED_ANGULAR_COEF_2WD_HS
Definition: constants.hpp:39
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
ros::Subscriber e_stop_reset_sub
Definition: openrover.hpp:91
ros::Publisher vel_calc_pub
Definition: openrover.hpp:80
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const int i_REG_POWER_BAT_VOLTAGE_B
Definition: constants.hpp:76
rr_openrover_driver_msgs::SmartBatteryStatus interpret_battery_status(uint16_t bits)
const int i_REG_MOTOR_FB_RPM_RIGHT
Definition: constants.hpp:65
const float ODOM_TRACTION_FACTOR_F
Definition: constants.hpp:15
bool sleep()
ros::Subscriber e_stop_sub
Definition: openrover.hpp:90
const int i_BATTERY_STATUS_A
Definition: constants.hpp:93
const int i_REG_MOTOR_TEMP_LEFT
Definition: constants.hpp:72
unsigned char run(bool e_stop_on, bool control_on, double commanded_vel, double measured_vel, double dt)
ros::WallTimer slow_timer
Definition: openrover.hpp:72
const float ODOM_ENCODER_COEF_4WD
Definition: constants.hpp:22
ros::Subscriber cmd_vel_sub
Definition: openrover.hpp:88
const int MOTOR_SPEED_LINEAR_COEF_4WD_HS
Definition: constants.hpp:27
ros::Publisher medium_rate_pub
Definition: openrover.hpp:83
ROSCPP_DECL void requestShutdown()
std::vector< unsigned char > serial_medium_buffer_
Definition: openrover.hpp:134
const int ROBOT_DATA_INDEX_SLOW[]
Definition: constants.hpp:119
const int ROBOT_DATA_INDEX_MEDIUM[]
Definition: constants.hpp:110
const int i_REG_PWR_TOTAL_CURRENT
Definition: constants.hpp:63
const float ODOM_ANGULAR_COEF_2WD
Definition: constants.hpp:35
bool getParam(const std::string &key, std::string &s) const
ros::WallTimer timeout_timer
Definition: openrover.hpp:73
const int SLOW_SIZE
Definition: constants.hpp:130
const int i_REG_POWER_A_CURRENT
Definition: constants.hpp:85
static Time now()
void convert(const A &a, B &b)
ros::WallTimer fast_timer
Definition: openrover.hpp:70
bool sendCommand(int param1, int param2)
const int MOTOR_DEADBAND
Definition: constants.hpp:53
ros::Publisher odom_enc_pub
Definition: openrover.hpp:76
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
const float ODOM_TRACTION_FACTOR_4WD
Definition: constants.hpp:25
OpenRover(ros::NodeHandle &nh, ros::NodeHandle &nh_priv)
int getParameterData(int parameter)
const int i_REG_MOTOR_FB_CURRENT_RIGHT
Definition: constants.hpp:70
bool setParameterData(int param1, int param2)
const int MOTOR_SPEED_MIN
Definition: constants.hpp:56
ros::Publisher motor_speeds_pub
Definition: openrover.hpp:79
const float ODOM_ANGULAR_COEF_F
Definition: constants.hpp:14
const int i_REG_MOTOR_FB_RPM_LEFT
Definition: constants.hpp:64
ROSCPP_DECL void spinOnce()
const int MOTOR_SPEED_LINEAR_COEF_2WD_HS
Definition: constants.hpp:38
const int i_BATTERY_STATUS_B
Definition: constants.hpp:94
void publishOdometry(float left_vel, float right_vel)
const int FLIPPER_MOTOR_INDEX_
Definition: openrover.hpp:101
geometry_msgs::Twist cmd_vel_commanded_
Definition: openrover.hpp:131
std::string port_
Definition: openrover.hpp:60
#define ROS_ERROR(...)
void cmdVelCB(const geometry_msgs::Twist::ConstPtr &msg)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
const unsigned char SERIAL_START_BYTE
Definition: constants.hpp:5
const int i_BATTERY_CURRENT_A
Definition: constants.hpp:102
std::ofstream r_fs_
Definition: openrover.hpp:57
const int i_BUILDNO
Definition: constants.hpp:84
#define ROS_DEBUG(...)


rr_openrover_driver
Author(s): Jack Kilian
autogenerated on Thu Sep 10 2020 03:38:38