如何解决错误消息：不匹配 operator< 因为 battery_sub_ 是订阅者，而battery 是双倍
我目前正在做一个需要我创建发布者和订阅者的项目。这个项目的想法是我要订阅无人驾驶车辆的电池信息，一旦车辆的电池电量低于指定的电池电量（30%左右），蜂鸣器就会响，红色LED会点亮。我已经成功订阅了名为 agv_battery_level 的 rostopic，并且我为指定的电池电量创建了一个名为 Battery 的变量。但是，我面临的问题是我无法将订阅者传递给条件语句，因为会弹出错误消息。我想知道如何为 Battery_sub_ 创建变量并将其传递到条件语句中。非常感谢！
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <sstream>
#include <std_msgs/UInt16.h>
#include <stdio.h> /* Standard input/output definitions */
#include <string.h> /* String function definitions */
#include <unistd.h> /* UNIX standard function definitions */
#include <fcntl.h> /* File control definitions */
#include <errno.h> /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */
#define CRC16 0x8005
#define REPLY_SIZE 1
#define TIMEOUT 1000
#define btoa(x) ((x)?"true":"false")
//ROS Parameter Default
#define MAIN_FREQ 1.0 //Controller main frequency
#define MODBUS_CMD_SEND_DELAY_T 0.05 //unit:s,0: not appcation,>0: appcation
#define MODBUS_CMD_RESEND_MAX 0 //0: not appcation,>0: appcation
#define MODBUS_CMD_RESEND_TIME 0.5 //unit:s
#define MODBUS_RECVEIE_TIMEOUT 1.0 //unit:s
#define TF_OUTPUT_ENABLE 0
#define FRAME_BUFF_MAX 256
//------------------------------------------
unsigned char data[6];
//------------------------------------------
ros::Publisher estop_pub_;
//------------------------------------------
//ROS Parameter
double main_freq_=MAIN_FREQ ;
double modbus_cmd_send_delay_t_=MODBUS_CMD_SEND_DELAY_T;
std::string modbus_port_;
std::string modbus_baud_;
std_msgs::String bmsg;
double modbus_cmd_resend_max_=MODBUS_CMD_RESEND_MAX;
double modbus_cmd_resend_time_=MODBUS_CMD_RESEND_TIME;
double modbus_recveie_timeout_=MODBUS_RECVEIE_TIMEOUT;
bool tf_output_enable_=TF_OUTPUT_ENABLE;
double battery;
//---------------------------------------------------
//for UART recveier time-out setting
fd_set rset;
int ret = 0;
struct timeval tv;
//---------------------------------------------------
int rev_frame_size=0;
int frame_sent=0;
int frame_recved=0;
int frame_checked=0;
unsigned char frame_bytes[FRAME_BUFF_MAX];
unsigned int frame_idx=0;
unsigned short frame_crc16=0;
int frame_resend_cnt=0;
int trans_step=0;
int trans_next=0;
static int get_new_cmd=0;
static short trans_vLa=0;
static short trans_vRa=0;
//------------------------------------------
double cmd_vel_v=0;
double cmd_vel_w=0;
static char fault_clear = 1;
static char temp_count = 0;
int count = 0;
int fd; /* File descriptor for the port */
unsigned buffer[1000];
unsigned int buf_cnt=0;
bool frame_get_flag=0;
unsigned int frame_rev_freq_cnt=0;
unsigned char read_bytes[800];
unsigned int read_idx=0;
bool syn_flag=0;
int syn_num=0;
ros::Time current_ctime,last_ctime;
double cdt=0;
bool get_ctime=0;
//Odometry
ros::Time time1,time2,odom_current_time,odom_last_time;
ros::Time current_time;
unsigned short crc_fn(unsigned char *dpacket,unsigned int len) // CRC Function(Error calcualtion)
{
unsigned short crc = 0xffff,poly = 0xa001; //Modbus CRC-16
unsigned short i=0;
for(i=0;i<len;i++)
{
crc^= (unsigned short) dpacket[i];
for(int j=0;j<8;j++)
{
if(crc & 0x01)
{
crc >>= 1;
crc ^= poly;
}
else
crc >>= 1;
}
}
return (crc);
}
void write_modbus(unsigned char *data,unsigned int len)
{
unsigned short crc16;
unsigned char buff[40];
unsigned char crc16_data[2];
unsigned short i=0;
for (i=0;i<len;i++)
buff[i]=data[i];
crc16=crc_fn(&buff[0],len); //modbus CRC16
crc16_data[0]=(unsigned char)(crc16 & 0xff);
crc16_data[1]=(unsigned char)((crc16 >> 8) & 0xff);
buff[len+1-1]=crc16_data[0];
buff[len+2-1]=crc16_data[1];
printf("crc16=%d\n",crc16);
printf("crc16_data[0]=0x%02X,crc16_data[1]=0x%02X\n",crc16_data[0],crc16_data[1]);
printf("Mobus Data Sent:0x%02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X\n",buff[0],buff[1],buff[2],buff[3],buff[4],buff[5],buff[6],buff[7],buff[8],crc16_data[1]);
write(fd,&buff[0],len+2); //with CRC16
}
void battery_charge(void)
{
unsigned char buff_read[6]; // Refer to user manual to obtain number of bytes to send to device
unsigned short motor_speed_address = 0x0001; // Starting register to read from - 6 is battery
buff_read[0] = 0x04; // Device ID - Get from manual
buff_read[1] = 0x05; // Specifies that we want to READ data (device-specific function - check manual)
buff_read[2] = 0x00; // Start address (1st 8 bits)
buff_read[3] = 0x04; // Start address (2nd 8 bits),currently hex(20) = 14
buff_read[4] = 0xFF; // How many bytes to write starting from and including "[2][3]"" (1st 8 bits) - FF to enable,00 to disable charging
buff_read[5] = 0x00; // How many bytes to write (2nd 8 bits),currently unused
write_modbus(&buff_read[0],6);
frame_sent = 1;
frame_checked = 0;
rev_frame_size=7;
current_ctime=ros::Time::now();
if (modbus_cmd_send_delay_t_!=0)
sleep(modbus_cmd_send_delay_t_);
}
void red_light_on(void)
{
unsigned char buff_read[6];
unsigned short motor_speed_address = 0x0001;
buff_read[0] = 0x04;
buff_read[1] = 0x05;
buff_read[2] = 0x00;
buff_read[3] = 0x02;
buff_read[4] = 0xFF;
buff_read[5] = 0x00;
write_modbus(&buff_read[0],6);
frame_sent = 1;
frame_checked = 0;
rev_frame_size=7;
current_ctime=ros::Time::now();
if (modbus_cmd_send_delay_t_!=0)
sleep(modbus_cmd_send_delay_t_);
}
void red_light_off(void)
{
unsigned char buff_read[6];
unsigned short motor_speed_address = 0x0001;
buff_read[0] = 0x04;
buff_read[1] = 0x05;
buff_read[2] = 0x00;
buff_read[3] = 0x02;
buff_read[4] = 0x00;
buff_read[5] = 0x00;
write_modbus(&buff_read[0],6);
frame_sent = 1;
frame_checked = 0;
rev_frame_size=7;
current_ctime=ros::Time::now();
if (modbus_cmd_send_delay_t_!=0)
sleep(modbus_cmd_send_delay_t_);
}
void green_light_off(void)
{
unsigned char buff_read[6];
unsigned short motor_speed_address = 0x0001;
buff_read[0] = 0x04;
buff_read[1] = 0x05;
buff_read[2] = 0x00;
buff_read[3] = 0x01;
buff_read[4] = 0x00;
buff_read[5] = 0x00;
write_modbus(&buff_read[0],6);
frame_sent = 1;
frame_checked = 0;
rev_frame_size=7;
current_ctime=ros::Time::now();
if (modbus_cmd_send_delay_t_!=0)
sleep(modbus_cmd_send_delay_t_);
}
void green_light_on(void)
{
unsigned char buff_read[6];
unsigned short motor_speed_address = 0x0001;
buff_read[0] = 0x04;
buff_read[1] = 0x05;
buff_read[2] = 0x00;
buff_read[3] = 0x01;
buff_read[4] = 0xFF;
buff_read[5] = 0x00;
write_modbus(&buff_read[0],6);
frame_sent = 1;
frame_checked = 0;
rev_frame_size=7;
current_ctime=ros::Time::now();
if (modbus_cmd_send_delay_t_!=0)
sleep(modbus_cmd_send_delay_t_);
}
void yellow_light_on(void)
{
unsigned char buff_read[6];
unsigned short motor_speed_address = 0x0001;
buff_read[0] = 0x04;
buff_read[1] = 0x05;
buff_read[2] = 0x00;
buff_read[3] = 0x00;
buff_read[4] = 0xFF;
buff_read[5] = 0x00;
write_modbus(&buff_read[0],6);
frame_sent = 1;
frame_checked = 0;
rev_frame_size=7;
current_ctime=ros::Time::now();
if (modbus_cmd_send_delay_t_!=0)
sleep(modbus_cmd_send_delay_t_);
}
void yellow_light_off(void)
{
unsigned char buff_read[6];
unsigned short motor_speed_address = 0x0001;
buff_read[0] = 0x04;
buff_read[1] = 0x05;
buff_read[2] = 0x00;
buff_read[3] = 0x00;
buff_read[4] = 0x00;
buff_read[5] = 0x00;
write_modbus(&buff_read[0],6);
frame_sent = 1;
frame_checked = 0;
rev_frame_size=7;
current_ctime=ros::Time::now();
if (modbus_cmd_send_delay_t_!=0)
sleep(modbus_cmd_send_delay_t_);
}
void estop_read(void)
{
unsigned char buff_read[6];
//unsigned short battery_address = 0x0014; // 0014
buff_read[0] = 0x06; //Slave ID Address
buff_read[1] = 0x02; //Function code (0x04: read registers)
buff_read[2] = 0x00;
buff_read[3] = 0x01;
buff_read[4] = 0x00;
buff_read[5] = 0x00;
write_modbus(&buff_read[0],6);
frame_sent = 1;
frame_checked = 0;
rev_frame_size=7;
current_ctime=ros::Time::now();
std_msgs::String msg;
std::stringstream ss;
//double led_points = frame_bytes[4];
//ss << "Estop " << led_points;
//printf("Estop: %f",led_points);
//msg.data = ss.str();
//ROS_INFO("%s",msg.data.c_str());
//estop_pub_.publish(msg);
if (modbus_cmd_send_delay_t_!=0)
sleep(modbus_cmd_send_delay_t_);
}
void buzzer_on(void)
{
unsigned char buff_read[6];
//unsigned short battery_address = 0x0014; // 0014
buff_read[0] = 0x04; //Slave ID Address
buff_read[1] = 0x05; //Function code (0x04: read registers)
buff_read[2] = 0x00;
buff_read[3] = 0x03;
buff_read[4] = 0xFF;
buff_read[5] = 0x00;
write_modbus(&buff_read[0],6);
frame_sent = 1;
frame_checked = 0;
rev_frame_size=7;
current_ctime=ros::Time::now();
if (modbus_cmd_send_delay_t_!=0)
sleep(modbus_cmd_send_delay_t_);
}
void buzzer_off(void)
{
unsigned char buff_read[6];
//unsigned short battery_address = 0x0014; // 0014
buff_read[0] = 0x04; //Slave ID Address
buff_read[1] = 0x05; //Function code (0x04: read registers)
buff_read[2] = 0x00;
buff_read[3] = 0x03;
buff_read[4] = 0x00;
buff_read[5] = 0x00;
write_modbus(&buff_read[0],6);
frame_sent = 1;
frame_checked = 0;
rev_frame_size=7;
current_ctime=ros::Time::now();
if (modbus_cmd_send_delay_t_!=0)
sleep(modbus_cmd_send_delay_t_);
}
void chatterCallback(const std_msgs::UInt16::ConstPtr& msg)
{
if(msg->data==1)
{
green_light_on();
}
else
{
green_light_off();
}
}
void chatterCallred(const std_msgs::UInt16::ConstPtr& msg)
{
if(msg->data==1)
{
red_light_on();
}
else
{
red_light_off();
}
}
void chatterCallyellow(const std_msgs::UInt16::ConstPtr& msg)
{
if(msg->data==1)
{
yellow_light_on();
}
else
{
yellow_light_off();
}
}
void chatterCallbuzzer(const std_msgs::UInt16::ConstPtr& msg)
{
if(msg->data==1)
{
buzzer_on();
}
else
{
buzzer_off();
}
}
void chatterCallBattery(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("I heard the information: [%s]",msg->data.c_str()); //Subscribe to rostopic agv_battery_level and publish it
}
/// read at least frame_size bytes,returns number of bytes read -1 if timeout
int polling_read(int frame_size)
{
const int read_buffer_size = 256;
unsigned char read_buffer[read_buffer_size]; /* Buffer to store the data received */
assert(frame_size >= 0 && frame_size < read_buffer_size);
int next_index = 0;
int bytes_read = 0;
int total_bytes_read = 0;
timeval tv_copy;
tv_copy.tv_sec = 0L;
tv_copy.tv_usec = 250000L; // 100 milliseconds timeout
printf("Start Receiving...\n");
while (ros::ok())
{
//always need to zero it first,then add our new descriptor
FD_ZERO(&rset); /* clear the set */
FD_SET(fd,&rset); /* add our file descriptor to the set */
if (select(fd+1,&rset,NULL,&tv_copy) > 0) // wait for at least 1 byte of data
{
bytes_read = read(fd,&read_buffer[next_index],read_buffer_size - next_index);
if (bytes_read > 0)
{
for(int i=0;i<bytes_read;i++) /* Printing only the received characters*/
{
printf("0x%02X ",read_buffer[i + next_index + i]);
}
printf("...\n");
next_index += bytes_read;
}
}
if (next_index >= rev_frame_size)
{
total_bytes_read = next_index;
frame_idx = 0;
for(int i=0;i<total_bytes_read;i++)
{
frame_bytes[frame_idx]=read_buffer[i];
frame_idx++;
}
printf("Receive Bytes Rxed %d\n",total_bytes_read); /* Print the number of bytes read */
break;
}
if (tv_copy.tv_sec == 0 && tv_copy.tv_usec == 0)
{
// timeout!
printf("Timeout!\n");
total_bytes_read = -1;
break;
}
}
return total_bytes_read;
}
void modbus_response(void)
{
if (frame_sent == 0) //frame transmit from Master,wait slave feedback message
{
// not sure about the intention for this check?
return;
}
int bytes_read = 0;
frame_checked = 0;
last_ctime=ros::Time::now();
// Receive uart data
bytes_read = polling_read(rev_frame_size);
current_ctime=ros::Time::now();
cdt=(current_ctime-last_ctime).toSec();
if (bytes_read >= rev_frame_size) //Frame received then crc16 check
{
unsigned short crc_num;
unsigned char crc_data[2];
frame_recved=1;
frame_idx=0;
frame_crc16=crc_fn(&frame_bytes[0],rev_frame_size-2); //modbus CRC16
crc_data[1]=(unsigned char)(frame_crc16 & 0xff);
crc_data[0]=(unsigned char)((frame_crc16 >> 8) & 0xff);
frame_crc16=((crc_data[1] << 8) &0xFF00) | (crc_data[0] & 0xFF);
crc_num= ((frame_bytes[rev_frame_size-2] << 8) &0xFF00) | (frame_bytes[rev_frame_size-1] & 0xFF);
printf("frame: 0x");
for (int i=0;i<rev_frame_size;i++)
printf("%02X ",frame_bytes[i]);
printf("\n");
printf("frame_crc16=%d(0x%02X%02X),crc_num=%d(0x%02X%02X)\n",frame_crc16,crc_data[1],crc_data[0],crc_num,frame_bytes[rev_frame_size-2],frame_bytes[rev_frame_size-1]);
if (frame_crc16==crc_num)
{
frame_checked=1;
printf("CRC16 Check Pass.\n");
}
else
{
frame_sent=0;
frame_resend_cnt++;
printf("Error: CRC16 Check Fail,Now Starting Resend (frame_resend_cnt=%d,trans_step=%d) Command to Slave!!.\n",frame_resend_cnt,trans_step);
//printf("I am here\n");
//Re-send command count----------------------------------
if (modbus_cmd_resend_max_!=0) //modbus_cmd_resend_max_=0 is not appcation
{
if (frame_resend_cnt>=modbus_cmd_resend_max_)
{
frame_resend_cnt=0;
printf("Error: Modbus Receive Error,Please check your connect!!\n");
}
}
else
frame_resend_cnt=0;
}
}
else
{
frame_sent=0;
frame_resend_cnt++;
printf("Warning: Receiver Time-out:%+3.3f,trans_step=%d)\n",cdt,trans_step);
//Re-send command count----------------------------------
if (modbus_cmd_resend_max_!=0) //modbus_cmd_resend_max_=0 is not appcation
{
if (frame_resend_cnt>=modbus_cmd_resend_max_)
{
frame_resend_cnt=0;
printf("Error: Modbus Receive Error,Please check your connect!!\n");
}
}
else
frame_resend_cnt=0;
}
//------------------------------------------------------
//std_msgs::String msg;
//std::stringstream ss;
if (frame_checked) //Modbus receive and CRC16 check OK!!
{
//if(bytes_read == 7)
//{
// double led_points = frame_bytes[4];
//ss << "Estop " << led_points;
//printf("Estop: %f \n",led_points);
//msg.data = ss.str();
// ROS_INFO("%s",msg.data.c_str());
// estop_pub_.publish(msg);
//chatterCallbuzzer();
// }
last_ctime=ros::Time::now();
cdt=(last_ctime-current_ctime).toSec();
printf("OK: trans_step=%d,frame_checked:ctime=%+3.3f\n",trans_step,cdt);
frame_resend_cnt=0;
trans_step = 0;
//trans_step=trans_next;
frame_sent=0;
cdt=0;
}
}
void uart_setting(void)
{
//Std::String to char* (string)
char *cstr = new char[modbus_port_.length() + 1];
strcpy(cstr,modbus_port_.c_str());
printf("Connecting to %s\n",modbus_port_.c_str());
fd = open(cstr,O_RDWR | O_NOCTTY | O_NDELAY);
if (fd == -1)
{
perror("open_port: Unable to open Modbus Port - ");
exit(-1);
}
else
fcntl(fd,F_SETFL,1);
delete [] cstr; //delete temp char* (string)
struct termios options;
speed_t baud = B9600;
if (modbus_baud_=="115200")
baud = B115200;
else if (modbus_baud_=="57600")
baud = B57600;
else if (modbus_baud_=="38400")
baud = B38400;
else if (modbus_baud_=="19200")
baud = B19200;
else if (modbus_baud_=="9600")
baud = B9600;
tcgetattr(fd,&options); /* Get the original setting */
cfsetispeed(&options,baud); /* Setting the baudrate */
cfsetospeed(&options,baud);
options.c_cflag |= (CLOCAL|CREAD); /* enable the receiver and set local mode */
options.c_cflag &= ~CSIZE; /* setting the character size (8-bits) */
options.c_cflag |= CS8;
options.c_cflag &= ~PARENB; /* Setting Parity Checking: NONE (NO Parity) */
options.c_cflag &= ~CSTOPB;// |= CSTOPB; /* use 2 stop bits when no parity */
options.c_cflag &= ~CRTSCTS; /* Disable Hardware flow control */
options.c_iflag &= ~INPCK; /* Disable Input Parity Checking */
options.c_iflag &= ~(IXON|IXOFF|IXANY); /* Disable software flow control */
options.c_iflag &= ~(IGNPAR|ICRNL);
options.c_oflag &= ~OPOST; /* output raw data */
options.c_lflag &= ~(ICANON|ECHO|ECHOE|ISIG); /* disablestd input */
tcflush(fd,TCIFLUSH); /* clean the current setting */
tcsetattr(fd,TCSANOW,&options); /* Enable the new setting right now */
//Timeout Receiver Setting
tv.tv_sec = modbus_recveie_timeout_; //unit: sec
tv.tv_usec = 0; //unit: usec
}
int main(int argc,char **argv)
{
ros::init(argc,argv,"agv_modbus_link_node");
ros::NodeHandle n;
estop_pub_ = n.advertise<std_msgs::String>("estop",1);
ros::Subscriber green_led_sub_;
green_led_sub_ = n.subscribe<std_msgs::UInt16>("led_green",100,chatterCallback);
ros::Subscriber red_led_sub_;
red_led_sub_ = n.subscribe<std_msgs::UInt16>("led_red",chatterCallred);
ros::Subscriber yellow_led_sub_;
yellow_led_sub_ = n.subscribe<std_msgs::UInt16>("led_yellow",chatterCallyellow);
ros::Subscriber buzzer_sub_;
buzzer_sub_ = n.subscribe<std_msgs::UInt16>("agv_buzzer",chatterCallbuzzer);
ros::Subscriber battery_sub_;
battery_sub_ = n.subscribe<std_msgs::String>("agv_battery_level",1,chatterCallBattery);
n.getParam("/main_freq",main_freq_);
n.getParam("/aiv_led/battery_lvl",battery); //Specified battery info at 30%
//n.getParam("/comm_link/tf_rev_delay_t",tf_rev_delay_t_);
n.getParam("/comm_link/modbus_cmd_send_delay_t",modbus_cmd_send_delay_t_);
n.param("/comm_link/modbus_port",modbus_port_,std::string("/dev/ttyUSB0"));
n.param("/comm_link/modbus_baud",modbus_baud_,std::string("9600"));
n.getParam("/comm_link/modbus_cmd_resend_max",modbus_cmd_resend_max_);
n.getParam("/comm_link/modbus_cmd_resend_time",modbus_cmd_resend_time_);
n.getParam("/comm_link/modbus_recveie_timeout",modbus_recveie_timeout_);
n.param<bool>("/comm_link/tf_output_enable",tf_output_enable_,false);
ros::Rate loop_rate(main_freq_);
get_new_cmd = 1;
uart_setting();
printf("Start modbus!!\n");
if (battery_sub_ < battery) // Error message: no match for operator< battery_sub_ is a subscriber and battery is a double
{
buzzer_on();
red_light_on();
printf("transmitted successfully!");
}
else
{
buzzer_off();
red_light_off();
printf("Failed!,please try again!");
}
while (ros::ok())
{
if (get_new_cmd == 1)
{
//estop_read();
modbus_response();
//printf("Battery information: %.2f \n",battery);
ros::Duration(0.01).sleep();
get_new_cmd = 2;
battery_charge();
}
else if (get_new_cmd == 2)
{
get_new_cmd = 1;
}
/*
else if (get_new_cmd == 3)
{
//estop_read();
//yellow_light_off();
modbus_response();
//printf("Battery level: %f \n",battery_lvl_)
ros::Duration(0.01).sleep();
get_new_cmd = 4;
}
else if (get_new_cmd == 4)
{
ros::Duration(0.01).sleep();
get_new_cmd = 5;
}
else if (get_new_cmd == 5)
{
//estop_read();
//battery_charge();
//green_light_on();
//ros::Duration(0.01).sleep();
get_new_cmd = 6;
}
else if (get_new_cmd == 6)
{
modbus_response();
ros::Duration(0.01).sleep();
get_new_cmd = 7;
}
else if (get_new_cmd == 7)
{
//estop_read();
//green_light_off();
ros::Duration(0.01).sleep();
get_new_cmd = 8;
}
else if (get_new_cmd == 8)
{
modbus_response();
ros::Duration(0.01).sleep();
get_new_cmd = 9;
}
else if (get_new_cmd == 9)
{
//estop_read();
//red_light_on();
ros::Duration(0.01).sleep();
get_new_cmd = 10;
}
else if (get_new_cmd == 10)
{
modbus_response();
ros::Duration(0.1).sleep();
get_new_cmd = 11;
}
else if (get_new_cmd == 11)
{
//estop_read();
//red_light_off();
ros::Duration(0.01).sleep();
get_new_cmd = 12;
}
else if (get_new_cmd == 12)
{
ros::Duration(0.01).sleep();
get_new_cmd = 1;
}
else
{
modbus_response();
}
*/
ros::spinOnce();
loop_rate.sleep();
}
close(fd);
return 0;
}
版权声明：本文内容由互联网用户自发贡献，该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 dio@foxmail.com 举报，一经查实，本站将立刻删除。
