11 #include <sys/ioctl.h> 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" 33 , port_(
"/dev/ttyUSB0")
34 , serial_baud_rate_(57600)
37 , medium_rate_hz_(2.0)
60 ROS_INFO(
"Initializing openrover driver.");
67 ROS_WARN(
"Failed to setup Robot parameters.");
73 if (
l_fs_.is_open()) {
77 if (
r_fs_.is_open()) {
89 ROS_INFO(
"Creating Publishers and Subscribers");
100 ROS_WARN(
"Failed to retrieve drive_type from parameter.Defaulting to %s",
use_legacy_ ?
"true" :
"false");
133 ROS_WARN(
"Failed to retrieve port from parameter server.Defaulting to %s",
port_.c_str());
139 ROS_ERROR(
"Failed to start serial communication.");
145 ROS_WARN(
"Failed to retrieve fast_data_rate from parameter. Defaulting to 10");
150 ROS_WARN(
"Failed to retrieve medium_data_rate from parameter. Defaulting to 2");
155 ROS_WARN(
"Failed to retrieve slow_data_rate from parameter. Defaulting to 1");
161 ROS_WARN(
"Failed to retrieve closed_loop_control_on from parameter server. Defaulting to off.");
166 ROS_WARN(
"Failed to retrieve timeout from parameter server. Defaulting to %f s",
timeout_);
171 ROS_WARN(
"Failed to retrieve total_weight from parameter server. Defaulting to %f lbs",
total_weight_);
176 ROS_WARN(
"Failed to retrieve drive_type from parameter.Defaulting to %s",
drive_type_.c_str());
205 ROS_INFO(
"Not logging left motor PID");
213 ROS_INFO(
"Not logging right motor PID");
221 if(!
l_fs_.is_open()){
228 if(!
r_fs_.is_open()){
263 ROS_INFO(
"flipper parameters loaded.");
277 ROS_WARN(
"Unclear ROS param drive_type. Defaulting to flippers params.");
298 ROS_WARN(
"Failed to retrieve odom_covariance_0 from parameter. Defaulting to 0.01");
304 ROS_WARN(
"Failed to retrieve odom_covariance_35 from parameter. Defaulting to 0.03");
308 ROS_INFO(
"Openrover parameters loaded:");
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;
410 if (!prev_e_stop_state_)
412 prev_e_stop_state_ =
true;
413 ROS_WARN(
"Openrover driver - Soft e-stop on.");
422 if (prev_e_stop_state_)
424 prev_e_stop_state_ =
false;
425 ROS_INFO(
"Openrover driver - Soft e-stop off.");
439 static bool prev_e_stop_state_ =
false;
442 if(msg->data && !prev_e_stop_state_)
447 prev_e_stop_state_ = msg->data;
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;
475 double now_time = ros_now_time.
toSec();
477 nav_msgs::Odometry odom_msg;
479 dt = now_time - past_time;
480 past_time = now_time;
484 left_dist += left_vel * dt;
485 right_dist += right_vel * dt;
487 net_vel = 0.5 * (left_vel + right_vel);
488 diff_vel = right_vel - left_vel;
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);
496 q_new.
setRPY(0, 0, theta);
500 odom_msg.header.stamp = ros_now_time;
501 odom_msg.header.frame_id =
"odom";
502 odom_msg.child_frame_id =
"base_link";
504 odom_msg.twist.twist.linear.x = net_vel;
505 odom_msg.twist.twist.angular.z = alpha;
509 if (net_vel == 0 && alpha == 0)
522 odom_msg.pose.pose.position.x = pos_x;
523 odom_msg.pose.pose.position.y = pos_y;
533 double run_time = (ros_now_time - ros_start_time).toSec();
534 std_msgs::Float32MultiArray vel_vec;
553 rr_openrover_driver_msgs::RawRrOpenroverDriverFastRateData
msg;
556 msg.header.frame_id =
"";
570 rr_openrover_driver_msgs::RawRrOpenroverDriverMedRateData med_msg;
571 std_msgs::Bool is_charging_msg;
574 med_msg.header.frame_id =
"";
593 is_charging_msg.data =
true;
599 is_charging_msg.data =
false;
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);
628 rr_openrover_driver_msgs::RawRrOpenroverDriverSlowRateData slow_msg;
629 rr_openrover_driver_msgs::SmartBatteryStatus batteryStatusA;
632 slow_msg.header.frame_id =
"";
665 std_msgs::Int32MultiArray motor_speeds_msg;
666 motor_speeds_msg.data.clear();
677 unsigned char param1;
678 unsigned char param2;
679 static double past_time = 0;
693 ROS_DEBUG(
"Its fast data's turn to be sent: %i", param2);
700 ROS_DEBUG(
"Its fan speed's turn to be sent: %i", param2);
707 ROS_DEBUG(
"Its medium data's turn to be sent: %i", param2);
714 ROS_DEBUG(
"Its slow data's turn to be sent: %i", param2);
729 else if (param1 == 20)
733 else if (param1 == 250)
737 else if (param1 == 0)
742 throw std::string(
"Unknown param1. Removing parameter from buffer");
745 catch (std::string
s)
758 double now_time = ros_now_time.
toSec();
760 double dt = now_time - past_time;
761 past_time = now_time;
864 catch (std::string
s)
867 sprintf(str_ex,
"Failed to update param %i. ", param);
868 throw std::string(str_ex) + s;
881 write_buffer[4] = (
unsigned char)param1;
882 write_buffer[5] = (
unsigned char)param2;
885 (char)255 - (write_buffer[1] + write_buffer[2] + write_buffer[3] + write_buffer[4] + write_buffer[5]) % 255;
889 ROS_INFO(
"Serial communication failed. Attempting to restart.");
892 ROS_WARN(
"Failed to restart serial communication.");
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);
909 unsigned char read_buffer[1];
910 unsigned char start_byte_read, checksum, read_checksum, data1, data2, dataNO;
914 ROS_INFO(
"Serial communication failed. Attempting to restart.");
917 ROS_WARN(
"Failed to restart serial communication.");
922 start_byte_read = read_buffer[0];
925 dataNO = read_buffer[0];
928 data1 = read_buffer[0];
931 data2 = read_buffer[0];
934 read_checksum = read_buffer[0];
936 checksum = 255 - (dataNO + data1 + data2) % 255;
941 sprintf(str_ex,
"Received bad start byte. Received: %02x", start_byte_read);
943 throw std::string(str_ex);
945 else if (checksum != read_checksum)
948 sprintf(str_ex,
"Received bad CRC. Received: %02x,%02x,%02x,%02x,%02x", start_byte_read, dataNO, data1, data2,
951 throw std::string(str_ex);
953 data = (data1 << 8) + data2;
968 catch (std::string
s)
970 std::string s2(
"setParameterData() failed. ");
994 catch (std::string
s)
996 std::string s2(
"getParameterData() failed. ");
1004 struct termios serial_port_fd__options;
1007 if (serial_port_fd_ < 0)
1009 ROS_ERROR(
"Failed to open port: %s", strerror(errno));
1012 if (0 > fcntl(serial_port_fd_, F_SETFL, 0))
1014 ROS_ERROR(
"Failed to set port descriptor: %s", strerror(errno));
1017 if (0 > tcgetattr(serial_port_fd_, &serial_port_fd__options))
1019 ROS_ERROR(
"Failed to fetch port attributes: %s", strerror(errno));
1022 if (0 > cfsetispeed(&serial_port_fd__options, B57600))
1024 ROS_ERROR(
"Failed to set input baud: %s", strerror(errno));
1027 if (0 > cfsetospeed(&serial_port_fd__options, B57600))
1029 ROS_ERROR(
"Failed to set output baud: %s", strerror(errno));
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);
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;
1042 serial_port_fd__options.c_cc[VQUIT] = 0x1C;
1043 serial_port_fd__options.c_cc[VERASE] = 0x7F;
1044 serial_port_fd__options.c_cc[VKILL] = 0x15;
1045 serial_port_fd__options.c_cc[VEOF] = 0x04;
1046 serial_port_fd__options.c_cc[VTIME] = 0x01;
1047 serial_port_fd__options.c_cc[VMIN] = 0;
1048 serial_port_fd__options.c_cc[VSWTC] = 0x00;
1050 serial_port_fd__options.c_cc[VSTOP] = 0x13;
1051 serial_port_fd__options.c_cc[VSUSP] = 0x1A;
1052 serial_port_fd__options.c_cc[VEOL] = 0x00;
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;
1059 if (0 > tcsetattr(serial_port_fd_, TCSANOW, &serial_port_fd__options))
1061 ROS_ERROR(
"Failed to set port attributes: %s", strerror(errno));
1064 ::ioctl(serial_port_fd_, TIOCEXCL);
1068 tcflush(serial_port_fd_, TCIOFLUSH);
1078 ros::init(argc, argv,
"rr_openrover_driver_node");
1105 if (!openrover.
start())
1107 ROS_FATAL(
"Failed to start the openrover driver");
1122 catch (std::string
s)
1128 ROS_ERROR(
"Unknown Exception occurred");
void robotDataMediumCB(const ros::WallTimerEvent &e)
void robotDataSlowCB(const ros::WallTimerEvent &e)
const int i_REG_MOTOR_FB_CURRENT_LEFT
const int i_BATTERY_VOLTAGE_B
int main(int argc, char *argv[])
const int i_REG_MOTOR_FLIPPER_ANGLE
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
bool is_serial_coms_open_
std::vector< unsigned char > serial_fast_buffer_
void robotDataFastCB(const ros::WallTimerEvent &e)
const int MOTOR_SPEED_ANGULAR_COEF_4WD_HS
const int MOTOR_SPEED_LINEAR_COEF_F_HS
std::vector< unsigned char > serial_slow_buffer_
const float ODOM_AXLE_TRACK_F
void eStopCB(const std_msgs::Bool::ConstPtr &msg)
const int i_REG_MOTOR_TEMP_RIGHT
void publish(const boost::shared_ptr< M > &message) const
const int i_REG_POWER_BAT_VOLTAGE_A
void publishFastRateData()
ros::NodeHandle & nh_priv_
const int i_ENCODER_INTERVAL_MOTOR_RIGHT
OdomControl left_controller_
const int i_REG_ROBOT_REL_SOC_A
const float ODOM_AXLE_TRACK_2WD
void publishMotorSpeeds()
const int LEFT_MOTOR_INDEX_
std::string r_pid_csv_file_
ros::Publisher is_charging_pub
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
const int i_BATTERY_VOLTAGE_A
void eStopResetCB(const std_msgs::Bool::ConstPtr &msg)
const float ODOM_ENCODER_COEF_2WD
ros::Publisher battery_status_a_pub
const int i_BATTERY_MODE_A
int motor_speeds_commanded_[3]
const float ODOM_ENCODER_COEF_F
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void updateMeasuredVelocities()
ros::WallTimer medium_timer
int motor_speed_flipper_coef_
const int i_ENCODER_INTERVAL_MOTOR_FLIPPER
#define ROS_WARN_DELAYED_THROTTLE(rate,...)
ros::Publisher slow_rate_pub
double right_vel_commanded_
void updateRobotData(int parameter)
bool publish_fast_rate_values_
const int MOTOR_SPEED_ANGULAR_COEF_F_HS
const float ODOM_TRACTION_FACTOR_2WD
const int i_REG_FLIPPER_FB_POSITION_POT2
const int ROBOT_DATA_INDEX_FAST[]
bool publish_slow_rate_values_
const float ODOM_ANGULAR_COEF_4WD
const int i_REG_MOTOR_FAULT_FLAG_LEFT
const int RIGHT_MOTOR_INDEX_
double left_vel_commanded_
double right_vel_filtered_
OdomControl right_controller_
ros::Publisher battery_status_b_pub
void timeoutCB(const ros::WallTimerEvent &e)
int motor_speed_angular_coef_
const int i_BATTERY_CURRENT_B
int motor_speed_linear_coef_
const int i_REG_FLIPPER_FB_POSITION_POT1
std::string l_pid_csv_file_
const int MOTOR_SPEED_MAX
const int SERIAL_OUT_PACKAGE_LENGTH
const float ODOM_AXLE_TRACK_4WD
const int MOTOR_FLIPPER_COEF
ros::Subscriber fan_speed_sub
ros::Publisher battery_state_of_charge_pub
void publishSlowRateData()
int motor_speed_deadband_
ros::Publisher fast_rate_pub
const int i_BATTERY_MODE_B
void fanSpeedCB(const std_msgs::Int32::ConstPtr &msg)
std::vector< unsigned char > serial_fan_buffer_
const int i_REG_POWER_B_CURRENT
const int i_BATTERY_TEMP_A
const int i_REG_ROBOT_REL_SOC_B
double velocity_filtered_
void start(bool use_control, PidGains pid_gains, int max, int min)
const int i_BATTERY_TEMP_B
float odom_traction_factor_
const int i_ENCODER_INTERVAL_MOTOR_LEFT
const int MOTOR_SPEED_ANGULAR_COEF_2WD_HS
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
ros::Subscriber e_stop_reset_sub
ros::Publisher vel_calc_pub
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const int i_REG_POWER_BAT_VOLTAGE_B
rr_openrover_driver_msgs::SmartBatteryStatus interpret_battery_status(uint16_t bits)
const int i_REG_MOTOR_FB_RPM_RIGHT
const float ODOM_TRACTION_FACTOR_F
ros::Subscriber e_stop_sub
const int i_BATTERY_STATUS_A
const int i_REG_MOTOR_TEMP_LEFT
unsigned char run(bool e_stop_on, bool control_on, double commanded_vel, double measured_vel, double dt)
bool closed_loop_control_on_
ros::WallTimer slow_timer
const float ODOM_ENCODER_COEF_4WD
ros::Subscriber cmd_vel_sub
const int MOTOR_SPEED_LINEAR_COEF_4WD_HS
ros::Publisher medium_rate_pub
ROSCPP_DECL void requestShutdown()
std::vector< unsigned char > serial_medium_buffer_
const int ROBOT_DATA_INDEX_SLOW[]
const int ROBOT_DATA_INDEX_MEDIUM[]
const int i_REG_PWR_TOTAL_CURRENT
const float ODOM_ANGULAR_COEF_2WD
double right_vel_measured_
bool getParam(const std::string &key, std::string &s) const
ros::WallTimer timeout_timer
float odom_covariance_35_
double left_vel_measured_
const int i_REG_POWER_A_CURRENT
void convert(const A &a, B &b)
ros::WallTimer fast_timer
bool sendCommand(int param1, int param2)
ros::Publisher odom_enc_pub
double left_vel_filtered_
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
const float ODOM_TRACTION_FACTOR_4WD
OpenRover(ros::NodeHandle &nh, ros::NodeHandle &nh_priv)
int getParameterData(int parameter)
void publishMedRateData()
const int i_REG_MOTOR_FB_CURRENT_RIGHT
bool publish_med_rate_values_
bool setParameterData(int param1, int param2)
const int MOTOR_SPEED_MIN
ros::Publisher motor_speeds_pub
const float ODOM_ANGULAR_COEF_F
const int i_REG_MOTOR_FB_RPM_LEFT
ROSCPP_DECL void spinOnce()
const int MOTOR_SPEED_LINEAR_COEF_2WD_HS
const int i_BATTERY_STATUS_B
void publishOdometry(float left_vel, float right_vel)
const int FLIPPER_MOTOR_INDEX_
geometry_msgs::Twist cmd_vel_commanded_
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
const int i_BATTERY_CURRENT_A